Fixed inertia tensor computation and center of mass

4 files changed, 163 insertions, 2 deletions

diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp
index c30401cc24..77b260dd17 100644
--- a/core/math/matrix3.cpp
+++ b/core/math/matrix3.cpp
@@ -100,6 +100,80 @@ Matrix3 Matrix3::orthonormalized() const {
 }
 
 
+bool Matrix3::is_symmetric() const {
+
+	if (Math::abs(elements[0][1] - elements[1][0]) > CMP_EPSILON)
+		return false;
+	if (Math::abs(elements[0][2] - elements[2][0]) > CMP_EPSILON)
+		return false;
+	if (Math::abs(elements[1][2] - elements[2][1]) > CMP_EPSILON)
+		return false;
+
+	return true;
+}
+
+
+Matrix3 Matrix3::diagonalize() {
+
+	//NOTE: only implemented for symmetric matrices
+	//with the Jacobi iterative method method
+	
+	ERR_FAIL_COND_V(!is_symmetric(), Matrix3());
+
+	const int ite_max = 1024;
+
+	real_t off_matrix_norm_2 = elements[0][1] * elements[0][1] + elements[0][2] * elements[0][2] + elements[1][2] * elements[1][2]; 
+
+	int ite = 0;
+	Matrix3 acc_rot;
+	while (off_matrix_norm_2 > CMP_EPSILON2 && ite++ < ite_max ) {
+		real_t el01_2 = elements[0][1] * elements[0][1];
+		real_t el02_2 = elements[0][2] * elements[0][2];
+		real_t el12_2 = elements[1][2] * elements[1][2];
+		// Find the pivot element
+		int i, j;
+		if (el01_2 > el02_2) {
+			if (el12_2 > el01_2) {
+				i = 1;
+				j = 2;
+			} else {
+				i = 0;
+				j = 1;
+			}	
+		} else {
+			if (el12_2 > el02_2) {
+				i = 1;
+				j = 2;
+			} else {
+				i = 0;
+				j = 2;
+			}
+		}
+
+		// Compute the rotation angle
+	    real_t angle;
+		if (Math::abs(elements[j][j] - elements[i][i]) < CMP_EPSILON) {
+			angle = Math_PI / 4;
+		} else {
+			angle = 0.5 * Math::atan(2 * elements[i][j] / (elements[j][j] - elements[i][i]));		
+		}
+
+		// Compute the rotation matrix
+		Matrix3 rot;
+		rot.elements[i][i] = rot.elements[j][j] = Math::cos(angle);
+		rot.elements[i][j] = - (rot.elements[j][i] = Math::sin(angle));
+
+		// Update the off matrix norm
+		off_matrix_norm_2 -= elements[i][j] * elements[i][j];
+
+		// Apply the rotation
+		*this = rot * *this * rot.transposed();
+		acc_rot = rot * acc_rot;
+	}
+
+	return acc_rot;
+}
+
 Matrix3 Matrix3::inverse() const {
 
 	Matrix3 inv=*this;
diff --git a/core/math/matrix3.h b/core/math/matrix3.h
index 2792200b7d..d9ad3ec2bb 100644
--- a/core/math/matrix3.h
+++ b/core/math/matrix3.h
@@ -26,10 +26,12 @@
 /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
+
+#include "vector3.h"
+
 #ifndef MATRIX3_H
 #define MATRIX3_H
 
-#include "vector3.h"
 #include "quat.h"
 
 /**
@@ -98,6 +100,12 @@ public:
 	_FORCE_INLINE_ Vector3 xform_inv(const Vector3& p_vector) const;
 	_FORCE_INLINE_ void operator*=(const Matrix3& p_matrix);
 	_FORCE_INLINE_ Matrix3 operator*(const Matrix3& p_matrix) const;
+	_FORCE_INLINE_ void operator+=(const Matrix3& p_matrix);
+	_FORCE_INLINE_ Matrix3 operator+(const Matrix3& p_matrix) const;
+	_FORCE_INLINE_ void operator-=(const Matrix3& p_matrix);
+	_FORCE_INLINE_ Matrix3 operator-(const Matrix3& p_matrix) const;
+	_FORCE_INLINE_ void operator*=(real_t p_val);
+	_FORCE_INLINE_ Matrix3 operator*(real_t p_val) const;
 
 	int get_orthogonal_index() const;
 	void set_orthogonal_index(int p_index);
@@ -130,6 +138,10 @@ public:
 
 		return Vector3(elements[i][0],elements[i][1],elements[i][2]);
 	}
+	_FORCE_INLINE_ Vector3 get_main_diagonal() const {
+		return Vector3(elements[0][0],elements[1][1],elements[2][2]);
+	}
+
 	_FORCE_INLINE_ void set_row(int i, const Vector3& p_row) {
 		elements[i][0]=p_row.x;
 		elements[i][1]=p_row.y;
@@ -163,12 +175,22 @@ public:
 	void orthonormalize();
 	Matrix3 orthonormalized() const;
 
+	bool is_symmetric() const;
+	Matrix3 diagonalize();
+
 	operator Quat() const;
 
 	Matrix3(const Quat& p_quat); // euler
 	Matrix3(const Vector3& p_euler); // euler
 	Matrix3(const Vector3& p_axis, real_t p_phi);
 
+	_FORCE_INLINE_ Matrix3(const Vector3& row0, const Vector3& row1, const Vector3& row2)
+	{
+		elements[0]=row0;
+		elements[1]=row1;
+		elements[2]=row2;
+	}
+
 	_FORCE_INLINE_ Matrix3() {
 
 		elements[0][0]=1;
@@ -203,6 +225,49 @@ _FORCE_INLINE_ Matrix3 Matrix3::operator*(const Matrix3& p_matrix) const {
 
 }
 
+
+_FORCE_INLINE_ void Matrix3::operator+=(const Matrix3& p_matrix) {
+
+	elements[0] += p_matrix.elements[0];
+	elements[1] += p_matrix.elements[1];
+	elements[2] += p_matrix.elements[2];
+}
+
+_FORCE_INLINE_ Matrix3 Matrix3::operator+(const Matrix3& p_matrix) const {
+	
+	Matrix3 ret(*this);
+	ret += p_matrix;
+	return ret;
+}
+
+_FORCE_INLINE_ void Matrix3::operator-=(const Matrix3& p_matrix) {
+
+	elements[0] -= p_matrix.elements[0];
+	elements[1] -= p_matrix.elements[1];
+	elements[2] -= p_matrix.elements[2];
+}
+
+_FORCE_INLINE_ Matrix3 Matrix3::operator-(const Matrix3& p_matrix) const {
+	
+	Matrix3 ret(*this);
+	ret -= p_matrix;
+	return ret;
+}
+
+_FORCE_INLINE_ void Matrix3::operator*=(real_t p_val) {
+
+	elements[0]*=p_val;
+	elements[1]*=p_val;
+	elements[2]*=p_val;
+}
+
+_FORCE_INLINE_ Matrix3 Matrix3::operator*(real_t p_val) const {
+
+		Matrix3 ret(*this);
+		ret *= p_val;
+		return ret;
+}
+
 Vector3 Matrix3::xform(const Vector3& p_vector) const {
 
 	return Vector3(
diff --git a/core/math/quat.h b/core/math/quat.h
index 9f4145cddb..6ffdf7a81b 100644
--- a/core/math/quat.h
+++ b/core/math/quat.h
@@ -26,13 +26,15 @@
 /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE     */
 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                */
 /*************************************************************************/
+
+#include "vector3.h"
+
 #ifndef QUAT_H
 #define QUAT_H
 
 #include "math_defs.h"
 #include "math_funcs.h"
 #include "ustring.h"
-#include "vector3.h"
 
 /**
 	@author Juan Linietsky <reduzio@gmail.com>
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) );