Added multiplication for matrices and vectors

2 files changed, 49 insertions, 0 deletions

diff --git a/modules/codec/c++/math.hpp b/modules/codec/c++/math.hpp
index 095c8ca..a8bbba1 100644
--- a/modules/codec/c++/math.hpp
+++ b/modules/codec/c++/math.hpp
@@ -53,5 +53,33 @@ data<schema::Vector<T,D>, Encoding> vectorize_data(const data<schema::FixedArray
 
 	return vec_data;
 }
+
+template<typename T, uint64_t M, uint64_t N, uint64_t K, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
+data<schema::Matrix<T,M,N>, Encoding> multiply(const data<schema::Matrix<T,M,K>, Encoding>& left, const data<schema::Matrix<T,K,N>, Encoding>& right){
+	data<schema::Matrix<T,M,N>, Encoding> lr;
+
+	for(uint64_t i = 0u; i < M; ++i){
+		for(uint64_t j = 0u; j < N; ++j){
+			for(uint64_t k = 0u; k < K; ++k){
+				lr.at({{i,j}}) = lr.at({{i,j}}) + left.at({{i,k}}) * right.at({{k,j}});
+			}
+		}
+	}
+
+	return lr;
+}
+
+template<typename T, uint64_t M, uint64_t N, typename Encoding = FORSTIO_DEFAULT_DATA_ENCODING>
+data<schema::Vector<T,M>, Encoding> multiply(const data<schema::Matrix<T,M,N>, Encoding>& left, const data<schema::Vector<T,N>, Encoding>& right){
+	data<schema::Vector<T,M>, Encoding> lr;
+
+	for(uint64_t i = 0u; i < M; ++i){
+		for(uint64_t j = 0u; j < N; ++j){
+			lr.at({{i}}) = lr.at({{i}}) + left.at({{i,j}}) * right.at({{j}});
+		}
+	}
+
+	return lr;
+}
 }
 }
diff --git a/modules/codec/tests/math.cpp b/modules/codec/tests/math.cpp
index 6f60e9c..25608e2 100644
--- a/modules/codec/tests/math.cpp
+++ b/modules/codec/tests/math.cpp
@@ -120,4 +120,25 @@ SAW_TEST("Math/Normalize"){
 	SAW_EXPECT(c.at({{0u}}).get() == 1.0, std::string{"Unexpected value for dot + sqrt "} + std::to_string(c.at({{0u}}).get()) );
 	SAW_EXPECT(c.at({{1u}}).get() == 0.0, std::string{"Unexpected value for dot + sqrt "} + std::to_string(c.at({{1u}}).get()) );
 }
+
+SAW_TEST("Math/Matrix Multiply"){
+	using namespace saw;
+
+	data<sch::Matrix<sch::Int64, 2u, 2u>> a;
+	{
+		a.at({{0u,0u}}) = 1;
+		a.at({{0u,1u}}) = 3;
+		a.at({{1u,0u}}) = 2;
+		a.at({{1u,1u}}) = 4;
+	}
+	data<sch::Matrix<sch::Int64, 2u, 1u>> b;
+	{
+		b.at({{0u,0u}}) = -1;
+		b.at({{1u,0u}}) = 2;
+	}
+	auto c = math::multiply(a,b);
+
+	SAW_EXPECT(c.at({{0u,0u}}).get() == 5, std::string{"Unexpected value for Matrix Multiply (0,0): "} + std::to_string(c.at({{0u,0u}}).get()) );
+	SAW_EXPECT(c.at({{1u,0u}}).get() == 6, std::string{"Unexpected value for Matrix Multiply (1,0): "} + std::to_string(c.at({{1u,0u}}).get()) );
+}
 }