Fixed inertia tensor computation and center of mass

1 files changed, 20 insertions, 0 deletions

diff --git a/core/math/vector3.h b/core/math/vector3.h
index 3f451b0ab7..279891c3b2 100644
--- a/core/math/vector3.h
+++ b/core/math/vector3.h
@@ -34,6 +34,7 @@
 #include "math_funcs.h"
 #include "ustring.h"
 
+class Matrix3;
 
 struct Vector3 {
 
@@ -92,6 +93,8 @@ struct Vector3 {
 
 	_FORCE_INLINE_ Vector3 cross(const Vector3& p_b) const;
 	_FORCE_INLINE_ real_t dot(const Vector3& p_b) const;
+	_FORCE_INLINE_ Matrix3 outer(const Vector3& p_b) const;
+	_FORCE_INLINE_ Matrix3 to_diagonal_matrix() const;
 
 	_FORCE_INLINE_ Vector3 abs() const;
 	_FORCE_INLINE_ Vector3 floor() const;
@@ -144,6 +147,8 @@ struct Vector3 {
 
 #else
 
+#include "matrix3.h"
+
 Vector3 Vector3::cross(const Vector3& p_b) const {
 
 	Vector3 ret (
@@ -160,6 +165,21 @@ real_t Vector3::dot(const Vector3& p_b) const {
 	return x*p_b.x + y*p_b.y + z*p_b.z;
 }
 
+Matrix3 Vector3::outer(const Vector3& p_b) const {
+	
+	Vector3 row0(x*p_b.x, x*p_b.y, x*p_b.z);
+	Vector3 row1(y*p_b.x, y*p_b.y, y*p_b.z);
+	Vector3 row2(z*p_b.x, z*p_b.y, z*p_b.z);
+
+	return Matrix3(row0, row1, row2);
+}
+
+Matrix3 Vector3::to_diagonal_matrix() const {
+	return Matrix3(x, 0, 0,
+				   0, y, 0,
+				   0, 0, z);
+}
+
 Vector3 Vector3::abs() const {
 
 	return Vector3( Math::abs(x), Math::abs(y), Math::abs(z) );