Skip to content

Commit

Permalink
Add motor normalization code
Browse files Browse the repository at this point in the history
  • Loading branch information
@jeremyong
jeremyong committed Oct 28, 2019
1 parent b8e5a7e commit b454eb7
Show file tree
Hide file tree
Showing 4 changed files with 101 additions and 23 deletions.
16 changes: 10 additions & 6 deletions public/gal/engine.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -163,6 +163,13 @@ namespace detail
return compute_entity<reified, V, A>(data, std::make_index_sequence<reified.size.term>());
}
}

template <typename T, typename... Ts>
struct reflect_first
{
using value_t = typename T::value_t;
using algebra_t = typename T::algebra_t;
};
} // namespace detail

template <typename... Data>
Expand Down Expand Up @@ -193,16 +200,16 @@ template <typename L, typename... Data>
[[nodiscard]] static auto compute(L&& lambda, Data const&... input) noexcept
{
constexpr auto ies = detail::ies<Data...>(std::tuple<>{}, std::integral_constant<uint, 0>{});
using rf = detail::reflect_first<Data...>;
using value_t = typename rf::value_t;
using algebra_t = typename rf::algebra_t;
using ie_result_t = decltype(std::apply(lambda, ies));
// Produce a lookup table keyed to the indeterminate id mapping to a union containing either an entity property or
// an evaluated property
if constexpr (detail::is_tuple_v<ie_result_t>)
{
if constexpr (std::tuple_size_v<ie_result_t> != 0)
{
using value_t = typename std::tuple_element_t<0, ie_result_t>::value_t;
using algebra_t = typename std::tuple_element_t<0, ie_result_t>::algebra_t;

std::array<detail::ind_value<value_t>, (Data::ind_count() + ...)> data{};
detail::fill(data.data(), input...);

Expand All @@ -216,9 +223,6 @@ template <typename L, typename... Data>
}
else
{
using value_t = typename ie_result_t::value_t;
using algebra_t = typename ie_result_t::algebra_t;

std::array<detail::ind_value<value_t>, (Data::ind_count() + ...)> data{};
detail::fill(data.data(), input...);
return detail::finalize_entity<algebra_t, value_t, ie_result_t>(data);
Expand Down
11 changes: 1 addition & 10 deletions public/gal/expression.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -65,8 +65,6 @@ constexpr inline frac_t<num, den> frac;
template <expr_op O, typename T1, typename T2 = void>
struct expr
{
using value_t = typename T1::value_t;
using algebra_t = typename T1::algebra_t;
constexpr static expr_op op = O;
using lhs_t = T1;
using rhs_t = T2;
Expand All @@ -75,26 +73,21 @@ struct expr
template <expr_op O, typename T>
struct expr<O, T, void>
{
using value_t = typename T::value_t;
using algebra_t = typename T::algebra_t;
constexpr static expr_op op = O;
using lhs_t = T;
};

template <typename T, uint32_t ID>
struct expr<expr_op::identity, T, std::integral_constant<uint32_t, ID>>
{
constexpr static expr_op op = expr_op::identity;
using value_t = typename T::value_t;
using algebra_t = typename T::algebra_t;
constexpr static expr_op op = expr_op::identity;
constexpr static auto lhs = T::ie(ID);
};

template <typename T, uint8_t... N>
struct expr<expr_op::extract, T, std::integer_sequence<uint8_t, N...>>
{
using value_t = typename T::value_t;
using algebra_t = typename T::algebra_t;
constexpr static expr_op op = expr_op::extract;
using lhs_t = T;
constexpr static std::array<uint8_t, sizeof...(N)> elements = {N...};
Expand All @@ -103,8 +96,6 @@ struct expr<expr_op::extract, T, std::integer_sequence<uint8_t, N...>>
template <typename T, uint8_t G>
struct expr<expr_op::select, T, std::integral_constant<uint8_t, G>>
{
using value_t = typename T::value_t;
using algebra_t = typename T::algebra_t;
constexpr static expr_op op = expr_op::select;
using lhs_t = T;
constexpr static uint8_t grade = G;
Expand Down
82 changes: 75 additions & 7 deletions public/gal/pga.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -224,7 +224,79 @@ namespace pga {

// A motor occupies the even subalgebra
template <typename T = float>
using motor = entity<pga_algebra, T, 0, 0b11, 0b101, 0b110, 0b1001, 0b1010, 0b1100, 0b1111>;
union motor
{
using algebra_t = pga_algebra;
using value_t = T;

std::array<T, 8> data;

[[nodiscard]] constexpr static auto ie(uint32_t id) noexcept
{
return detail::construct_ie<algebra_t>(
id,
std::make_integer_sequence<width_t, 8>{},
std::integer_sequence<uint8_t, 0, 0b11, 0b101, 0b110, 0b1001, 0b1010, 0b1100, 0b1111>{});
}

[[nodiscard]] constexpr static size_t size() noexcept
{
return 8;
}

[[nodiscard]] constexpr static uint32_t ind_count() noexcept
{
return 8;
}

constexpr motor(std::array<T, 8> const& in) noexcept
: data{in}
{}

template <uint8_t... E>
constexpr motor(entity<algebra_t, T, E...> in) noexcept
: data{in.template select<0, 0b11, 0b101, 0b110, 0b1001, 0b1010, 0b1100, 0b1111>()}
{
}

[[nodiscard]] constexpr entity<pga_algebra, T, 0b11, 0b101, 0b110, 0b1001, 0b1010, 0b1100> bivector() const
noexcept
{
return {data[1], data[2], data[3], data[4], data[5], data[6]};
}

void normalize() noexcept
{
auto m2 = compute([](auto m) { return m * ~m; }, *this);
auto u = m2[0];
auto sqrt_u = std::sqrt(u);
auto v = m2[1];
m2[0] = T{1} / sqrt_u;
m2[1] = -v / (2 * sqrt_u * u);
*this = compute([](auto m, auto inv_norm) { return m * inv_norm; }, *this, m2);
}

[[nodiscard]] constexpr T const& operator[](size_t index) const noexcept
{
return data[index];
}

[[nodiscard]] constexpr T& operator[](size_t index) noexcept
{
return data[index];
}

[[nodiscard]] constexpr T get(size_t i) const noexcept
{
// Unused
return NAN;
}
};

template <typename T>
[[nodiscard]] constexpr motor<T> normalize(motor<T> m)
{
}

template <typename T = float>
union plane
Expand Down Expand Up @@ -595,12 +667,8 @@ namespace pga {
auto s1 = m[0]; // <m>_0
auto p1 = m[7]; // <m>_4

entity<pga_algebra, T, 0b11, 0b101, 0b110, 0b1001, 0b1010, 0b1100> l{m.template select<0b11>(),
m.template select<0b101>(),
m.template select<0b110>(),
m.template select<0b1001>(),
m.template select<0b1010>(),
m.template select<0b1100>()};
auto l = m.bivector();

// s + p * I
auto l2 = compute([](auto l) { return l * l; }, l);
auto s = l2.template select<0>();
Expand Down
15 changes: 15 additions & 0 deletions test/test_pga.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,21 @@ TEST_CASE("motors")
{
CHECK_EQ(m[i], doctest::Approx(m2[i]));
}

auto m_norm = compute([](auto m) { return m * ~m; }, m2);
printf("m_norm: %f, %f\n", m_norm[0], m_norm[1]);
CHECK_EQ(m_norm[0], doctest::Approx(1));
CHECK_EQ(m_norm[1], doctest::Approx(0));
}

SUBCASE("normalize-motor")
{
motor<> m{{293.2, -39.3, 59.3, -1.04, 434.3, 23.0, 72.874}};
m.normalize();
auto m_norm = compute([](auto m) { return m * ~m; }, m);
printf("m_norm: %f, %f\n", m_norm[0], m_norm[1]);
CHECK_EQ(m_norm[0], doctest::Approx(1));
CHECK_EQ(m_norm[1], doctest::Approx(0));
}
}

Expand Down

0 comments on commit b454eb7

Please sign in to comment.