From 31c45d44ecc12d2d517b4c666f9788adc19be65f Mon Sep 17 00:00:00 2001
From: Jiri Pittner <jiri@pittnerovi.com>
Date: Fri, 28 Oct 2022 16:14:13 +0200
Subject: [PATCH] outer product and intertia tensor in vecmat3

---
 vecmat3.cc | 40 ++++++++++++++++++++++++++++++++++++++++
 vecmat3.h  |  7 +++++++
 2 files changed, 47 insertions(+)

diff --git a/vecmat3.cc b/vecmat3.cc
index 8578bc2..0f0414f 100644
--- a/vecmat3.cc
+++ b/vecmat3.cc
@@ -47,6 +47,46 @@ template<typename T>
 Vec3<T>& Vec3<T>::fast_normalize(void) {normalize(); return *this;};
 
 
+template<typename T>
+Mat3<T> Vec3<T>::outer(const Vec3<T> &rhs) const
+{
+Mat3<T> m;
+m[0][0]=q[0]*rhs.q[0];
+m[0][1]=q[0]*rhs.q[1];
+m[0][2]=q[0]*rhs.q[2];
+
+m[1][0]=q[1]*rhs.q[0];
+m[1][1]=q[1]*rhs.q[1];
+m[1][2]=q[1]*rhs.q[2];
+
+m[2][0]=q[2]*rhs.q[0];
+m[2][1]=q[2]*rhs.q[1];
+m[2][2]=q[2]*rhs.q[2];
+
+return m;
+}
+
+
+template<typename T>
+void Vec3<T>::inertia(Mat3<T> &m, const T weight) const
+{
+T r2 = q[0]*q[0]+q[1]*q[1]+q[2]*q[2];
+
+m[0][0] -= weight*(q[0]*q[0]-r2);
+m[0][1] -= weight*q[0]*q[1];
+m[0][2] -= weight*q[0]*q[2];
+
+m[1][0] -= weight*q[1]*q[0];
+m[1][1] -= weight*(q[1]*q[1]-r2);
+m[1][2] -= weight*q[1]*q[2];
+
+m[2][0] -= weight*q[2]*q[0];
+m[2][1] -= weight*q[2]*q[1];
+m[2][2] -= weight*(q[2]*q[2]-r2);
+}
+
+
+
 template<typename T>
 const Vec3<T> Vec3<T>::operator*(const Mat3<T> &rhs) const
                 {
diff --git a/vecmat3.h b/vecmat3.h
index 67a97e8..a34ca0e 100644
--- a/vecmat3.h
+++ b/vecmat3.h
@@ -86,6 +86,9 @@ public:
 	Vec3& normalize(void) {*this /= norm(); return *this;};
 	Vec3& fast_normalize(void);
 	const Vec3 operator*(const Mat3<T> &rhs) const;
+	Mat3<T> outer(const Vec3 &rhs) const; //tensor product
+	void inertia(Mat3<T> &itensor, const T weight) const; //contribution to inertia tensor
+
 	//C-style IO
 	int fprintf(FILE *f, const char *format) const {return ::fprintf(f,format,q[0],q[1],q[2]);};
         int sprintf(char *f, const char *format) const {return ::sprintf(f,format,q[0],q[1],q[2]);};
@@ -94,6 +97,10 @@ public:
 };
 
 
+template <typename T>
+inline T  hypot3(const Vec3<T> &c, const Vec3<T> &d) {return((c-d).norm());}
+
+
 template <typename T>
 class Mat3
 	{