From 2e38b32e0f261445c2d0b095c1822fbe6df16e25 Mon Sep 17 00:00:00 2001 From: m4nu3lf Date: Sat, 31 Dec 2016 14:39:25 +0000 Subject: Fixed inertia tensor computation and center of mass --- core/math/matrix3.cpp | 74 +++++++++++++++++++++++++++++++++++++++++++++++++++ core/math/matrix3.h | 67 +++++++++++++++++++++++++++++++++++++++++++++- core/math/quat.h | 4 ++- core/math/vector3.h | 20 ++++++++++++++ core/variant_call.cpp | 5 ++++ 5 files changed, 168 insertions(+), 2 deletions(-) (limited to 'core') 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 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) ); diff --git a/core/variant_call.cpp b/core/variant_call.cpp index cedf21b377..7ea6a3a8c4 100644 --- a/core/variant_call.cpp +++ b/core/variant_call.cpp @@ -370,6 +370,8 @@ static void _call_##m_type##_##m_method(Variant& r_ret,Variant& p_self,const Var VCALL_LOCALMEM4R(Vector3, cubic_interpolate); VCALL_LOCALMEM1R(Vector3, dot); VCALL_LOCALMEM1R(Vector3, cross); + VCALL_LOCALMEM1R(Vector3, outer); + VCALL_LOCALMEM0R(Vector3, to_diagonal_matrix); VCALL_LOCALMEM0R(Vector3, abs); VCALL_LOCALMEM0R(Vector3, floor); VCALL_LOCALMEM0R(Vector3, ceil); @@ -1481,6 +1483,9 @@ _VariantCall::addfunc(Variant::m_vtype,Variant::m_ret,_SCS(#m_method),VCALL(m_cl ADDFUNC4(VECTOR3,VECTOR3,Vector3,cubic_interpolate,VECTOR3,"b",VECTOR3,"pre_a",VECTOR3,"post_b",REAL,"t",varray()); ADDFUNC1(VECTOR3,REAL,Vector3,dot,VECTOR3,"b",varray()); ADDFUNC1(VECTOR3,VECTOR3,Vector3,cross,VECTOR3,"b",varray()); + ADDFUNC1(VECTOR3,MATRIX3,Vector3,outer,VECTOR3,"b",varray()); + ADDFUNC0(VECTOR3,MATRIX3,Vector3,to_diagonal_matrix,varray()); + ADDFUNC0(VECTOR3,VECTOR3,Vector3,abs,varray()); ADDFUNC0(VECTOR3,VECTOR3,Vector3,abs,varray()); ADDFUNC0(VECTOR3,VECTOR3,Vector3,floor,varray()); ADDFUNC0(VECTOR3,VECTOR3,Vector3,ceil,varray()); -- cgit v1.2.3