summaryrefslogtreecommitdiff
path: root/core/math
diff options
context:
space:
mode:
Diffstat (limited to 'core/math')
-rw-r--r--core/math/matrix3.cpp74
-rw-r--r--core/math/matrix3.h67
-rw-r--r--core/math/quat.h4
-rw-r--r--core/math/vector3.h20
4 files changed, 163 insertions, 2 deletions
diff --git a/core/math/matrix3.cpp b/core/math/matrix3.cpp
index a928b4d0e5..44abf8cd36 100644
--- a/core/math/matrix3.cpp
+++ b/core/math/matrix3.cpp
@@ -112,6 +112,80 @@ bool Matrix3::is_rotation() 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 33a5ce8687..0b61e3a56c 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"
/**
@@ -104,6 +106,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);
@@ -139,6 +147,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;
@@ -172,12 +184,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;
@@ -212,6 +234,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 40c048006f..43c2cab9e6 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 14cf1bc6ca..f1f34ce318 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) );
generated by cgit v1.2.3 (git 2.25.1) at 2024-11-23 05:00:06 +0000