Minor work on math stuff

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");
+};
 }
 }