diff options
Diffstat (limited to 'modules/codec/c++/data_math.hpp')
-rw-r--r-- | modules/codec/c++/data_math.hpp | 51 |
1 files changed, 50 insertions, 1 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; + } }; + } |