diff options
Diffstat (limited to 'modules/codec/c++')
-rw-r--r-- | modules/codec/c++/data_math.hpp | 51 | ||||
-rw-r--r-- | modules/codec/c++/math.hpp | 12 | ||||
-rw-r--r-- | modules/codec/c++/schema_math.hpp | 8 |
3 files changed, 68 insertions, 3 deletions
diff --git a/modules/codec/c++/data_math.hpp b/modules/codec/c++/data_math.hpp index 3bda481..a9eafff 100644 --- a/modules/codec/c++/data_math.hpp +++ b/modules/codec/c++/data_math.hpp @@ -2,15 +2,64 @@ #include "schema_math.hpp" +#include <cmath> + namespace saw { template<typename T> class data<schema::Quarternion<T>,encode::Native> { +public: + using Schema = schema::Quaternion<T>; private: - std::array<data<T,encode::Native>, 4u> vals_; + data<schema::FixedArray<T,4u>,encode::Native> vals_; + + using NativeType = typename native_data_type<T>::type; public: + data() = default; + + data(data<T,encode::Native> i__, data<T,encode::Native> j__, data<T,encode::Native> k__, data<T,encode::Native> w__): + vals_{i__,j__,k__,w__} + {} + + constexpr void normalize() { + data<T,encode::Native> norm{}; + + for(data<schema::UInt64,encode::Native> i = 0u; i < vals_.size(); ++i){ + norm += vals_.at(i) * vals_.at(i); + } + norm.set(std::sqrt(norm.get())); + for(data<schema::UInt64,encode::Native> i = 0u; i < vals_.size(); ++i){ + vals_.at(i) = vals_.at(i) / norm; + } + } + constexpr data<T,encode::Native>& at(data<schema::UInt64,encode::Native> i){ + return vals_.at(i); + } + constexpr const data<T,encode::Native>& at(data<schema::UInt64,encode::Native> i) const { + return vals_.at(i); + } + + constexpr data<Schema, encode::Native> operator*(const data<schema::Quaternion,encode::Native>& rhs) const { + data<Schema,encode::Native> mult; + + data<schema::UInt64,encode::Native> i_i{0u}; + data<schema::UInt64,encode::Native> j_i{1u}; + data<schema::UInt64,encode::Native> k_i{2u}; + data<schema::UInt64,encode::Native> w_i{3u}; + + mult.at(i_i) = at(w_i) * rhs.at(i_i) + at(i_i) * rhs.at(w_i) + at(j_i) * rhs.at(k_i) - at(k_i) * rhs.at(j_i); + + mult.at(j_i) = at(w_i) * rhs.at(j_i) + at(j_i) * rhs.at(w_i) - at(i_i) * rhs.at(k_i) + at(k_i) * rhs.at(i_i); + + mult.at(k_i) = at(w_i) * rhs.at(k_i) + at(k_i) * rhs.at(w_i) + at(i_i) * rhs.at(j_i) - at(j_i) * rhs.at(i_i); + + mult.at(w_i) = at(w_i) * rhs.at(w_i) - at(i_i) * rhs.at(i_i) - at(j_i) * rhs.at(j_i) - at(k_i) * rhs.at(k_i); + + return mult; + } }; + } diff --git a/modules/codec/c++/math.hpp b/modules/codec/c++/math.hpp new file mode 100644 index 0000000..dd9e49b --- /dev/null +++ b/modules/codec/c++/math.hpp @@ -0,0 +1,12 @@ +#pragma once + +#include "data_math.hpp" + +namespace saw { +namespace math { +template<typename T, typename Encoding = encode::Native> +data<typename T::InnerType,Encoding> norm_2(const data<T,Encoding>& d){ + return {}; +} +} +} diff --git a/modules/codec/c++/schema_math.hpp b/modules/codec/c++/schema_math.hpp index 471409f..203b88a 100644 --- a/modules/codec/c++/schema_math.hpp +++ b/modules/codec/c++/schema_math.hpp @@ -8,9 +8,13 @@ template<typename T> struct Complex {}; template<typename T> -struct Quaternion {}; +struct Quaternion { + static_assert(is_primitive<T>::value, "Quaternions assume a primitive type"); +}; template<typename T, uint64_t D> -struct Euler {}; +struct Euler { + static_assert(is_primitive<T>::value, "Quaternions assume a primitive type"); +}; } } |