From 1d8182b9d4d9c70d01f80bd745fbca6125cf31e7 Mon Sep 17 00:00:00 2001 From: Relintai Date: Mon, 14 Mar 2022 16:48:25 +0100 Subject: [PATCH] Started work on Basis. --- core/math/{basis.cpp => basis.c} | 18 ++- core/math/basis.h | 225 ++++++++++++++++++------------- core/math/vector3.h | 6 + 3 files changed, 146 insertions(+), 103 deletions(-) rename core/math/{basis.cpp => basis.c} (99%) diff --git a/core/math/basis.cpp b/core/math/basis.c similarity index 99% rename from core/math/basis.cpp rename to core/math/basis.c index b4bd043..e5ea29e 100644 --- a/core/math/basis.cpp +++ b/core/math/basis.c @@ -31,11 +31,13 @@ #include "basis.h" #include "core/math/math_funcs.h" -#include "core/print_string.h" +//#include "core/print_string.h" #define cofac(row1, col1, row2, col2) \ (elements[row1][col1] * elements[row2][col2] - elements[row1][col2] * elements[row2][col1]) +/* + void Basis::from_z(const Vector3 &p_z) { if (Math::abs(p_z.z) > (real_t)Math_SQRT12) { // choose p in y-z plane @@ -767,7 +769,7 @@ Quat Basis::get_quat() const { #ifdef MATH_CHECKS ERR_FAIL_COND_V_MSG(!is_rotation(), Quat(), "Basis must be normalized in order to be casted to a Quaternion. Use get_rotation_quat() or call orthonormalized() if the Basis contains linearly independent vectors."); #endif - /* Allow getting a quaternion from an unnormalized transform */ + // Allow getting a quaternion from an unnormalized transform Basis m = *this; real_t trace = m.elements[0][0] + m.elements[1][1] + m.elements[2][2]; real_t temp[4]; @@ -861,11 +863,11 @@ void Basis::set_orthogonal_index(int p_index) { } void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { - /* checking this is a bad idea, because obtaining from scaled transform is a valid use case -#ifdef MATH_CHECKS - ERR_FAIL_COND(!is_rotation()); -#endif -*/ + // checking this is a bad idea, because obtaining from scaled transform is a valid use case +//#ifdef MATH_CHECKS +// ERR_FAIL_COND(!is_rotation()); +//#endif + real_t angle, x, y, z; // variables for result real_t epsilon = 0.01; // margin to allow for rounding errors real_t epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees @@ -1021,3 +1023,5 @@ Basis Basis::slerp(const Basis &p_to, const real_t &p_weight) const { return b; } + +*/ diff --git a/core/math/basis.h b/core/math/basis.h index 434e082..c36c1cf 100644 --- a/core/math/basis.h +++ b/core/math/basis.h @@ -34,21 +34,133 @@ #include "core/math/quat.h" #include "core/math/vector3.h" -class _NO_DISCARD_CLASS_ Basis { -public: - Vector3 elements[3] = { - Vector3(1, 0, 0), - Vector3(0, 1, 0), - Vector3(0, 0, 1) - }; +typedef struct _NO_DISCARD_CLASS_ Basis { + Vector3 elements[3]; +} Basis; - _FORCE_INLINE_ const Vector3 &operator[](int axis) const { - return elements[axis]; - } - _FORCE_INLINE_ Vector3 &operator[](int axis) { - return elements[axis]; - } +extern _FORCE_INLINE_ Basis basis_create() { + Basis b; + vector3_set(&b.elements[0], 1, 0, 0); + vector3_set(&b.elements[1], 0, 1, 0); + vector3_set(&b.elements[2], 0, 0, 1); + + return b; +} + +extern _FORCE_INLINE_ Basis basis_createv(const Vector3 *row0, const Vector3 *row1, const Vector3 *row2) { + Basis b; + + b.elements[0] = *row0; + b.elements[1] = *row1; + b.elements[2] = *row2; + + return b; +} + +/* +Basis(const Quat &p_quat) { + set_quat(p_quat); +} +Basis(const Quat &p_quat, const Vector3 &p_scale) { + set_quat_scale(p_quat, p_scale); +} + +Basis(const Vector3 &p_euler) { + set_euler(p_euler); +} +Basis(const Vector3 &p_euler, const Vector3 &p_scale) { + set_euler_scale(p_euler, p_scale); +} + +Basis(const Vector3 &p_axis, real_t p_phi) { + set_axis_angle(p_axis, p_phi); +} +Basis(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) { + set_axis_angle_scale(p_axis, p_phi, p_scale); +} +*/ + +extern _FORCE_INLINE_ Basis basis_creater(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) { + Basis b; + b.elements[0].coord[0] = xx; + b.elements[0].coord[1] = xy; + b.elements[0].coord[2] = xz; + b.elements[1].coord[0] = yx; + b.elements[1].coord[1] = yy; + b.elements[1].coord[2] = yz; + b.elements[2].coord[0] = zx; + b.elements[2].coord[1] = zy; + b.elements[2].coord[2] = zz; + return b; +} + +extern _FORCE_INLINE_ const Vector3 basis_get_axis(const Basis *b, int p_axis) { + // get actual basis axis (elements is transposed for performance) + return vector3_create(b->elements[0].coord[p_axis], b->elements[1].coord[p_axis], b->elements[2].coord[p_axis]); +} + +extern _FORCE_INLINE_ void basis_set_axis(Basis *b, int p_axis, const Vector3 *p_value) { + // get actual basis axis (elements is transposed for performance) + b->elements[0].coord[p_axis] = p_value->x; + b->elements[1].coord[p_axis] = p_value->y; + b->elements[2].coord[p_axis] = p_value->z; +} + +extern _FORCE_INLINE_ void basis_setr(Basis *b, real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) { + b->elements[0].coord[0] = xx; + b->elements[0].coord[1] = xy; + b->elements[0].coord[2] = xz; + b->elements[1].coord[0] = yx; + b->elements[1].coord[1] = yy; + b->elements[1].coord[2] = yz; + b->elements[2].coord[0] = zx; + b->elements[2].coord[1] = zy; + b->elements[2].coord[2] = zz; +} + +extern _FORCE_INLINE_ void basis_setv(Basis *b, const Vector3 *p_x, const Vector3 *p_y, const Vector3 *p_z) { + basis_set_axis(b, 0, p_x); + basis_set_axis(b, 1, p_y); + basis_set_axis(b, 2, p_z); +} + +extern _FORCE_INLINE_ Vector3 basis_get_column(const Basis *b, int i) { + return vector3_create(b->elements[0].coord[i], b->elements[1].coord[i], b->elements[2].coord[i]); +} +extern _FORCE_INLINE_ Vector3 basis_get_row(const Basis *b, int i) { + return vector3_create(b->elements[i].coord[0], b->elements[i].coord[1], b->elements[i].coord[2]); +} +extern _FORCE_INLINE_ Vector3 basis_get_main_diagonal(const Basis *b) { + return vector3_create(b->elements[0].coord[0], b->elements[1].coord[1], b->elements[2].coord[2]); +} + +extern _FORCE_INLINE_ void basis_set_row(Basis *b, int i, const Vector3 *p_row) { + b->elements[i].coord[0] = p_row->x; + b->elements[i].coord[1] = p_row->y; + b->elements[i].coord[2] = p_row->z; +} + +extern _FORCE_INLINE_ void basis_set_zero(Basis *b) { + vector3_zero(&b->elements[0]); + vector3_zero(&b->elements[1]); + vector3_zero(&b->elements[2]); +} + +extern _FORCE_INLINE_ Basis basis_transpose_xform(const Basis *b, const Basis *m) { + return basis_creater( + b->elements[0].x * m->elements[0].x + b->elements[1].x * m->elements[1].x + b->elements[2].x * m->elements[2].x, + b->elements[0].x * m->elements[0].y + b->elements[1].x * m->elements[1].y + b->elements[2].x * m->elements[2].y, + b->elements[0].x * m->elements[0].z + b->elements[1].x * m->elements[1].z + b->elements[2].x * m->elements[2].z, + b->elements[0].y * m->elements[0].x + b->elements[1].y * m->elements[1].x + b->elements[2].y * m->elements[2].x, + b->elements[0].y * m->elements[0].y + b->elements[1].y * m->elements[1].y + b->elements[2].y * m->elements[2].y, + b->elements[0].y * m->elements[0].z + b->elements[1].y * m->elements[1].z + b->elements[2].y * m->elements[2].z, + b->elements[0].z * m->elements[0].x + b->elements[1].z * m->elements[1].x + b->elements[2].z * m->elements[2].x, + b->elements[0].z * m->elements[0].y + b->elements[1].z * m->elements[1].y + b->elements[2].z * m->elements[2].y, + b->elements[0].z * m->elements[0].z + b->elements[1].z * m->elements[1].z + b->elements[2].z * m->elements[2].z); +} + +/* void invert(); void transpose(); @@ -59,17 +171,6 @@ public: void from_z(const Vector3 &p_z); - _FORCE_INLINE_ Vector3 get_axis(int p_axis) const { - // get actual basis axis (elements is transposed for performance) - return Vector3(elements[0][p_axis], elements[1][p_axis], elements[2][p_axis]); - } - _FORCE_INLINE_ void set_axis(int p_axis, const Vector3 &p_value) { - // get actual basis axis (elements is transposed for performance) - elements[0][p_axis] = p_value.x; - elements[1][p_axis] = p_value.y; - elements[2][p_axis] = p_value.z; - } - void rotate(const Vector3 &p_axis, real_t p_phi); Basis rotated(const Vector3 &p_axis, real_t p_phi) const; @@ -175,62 +276,7 @@ public: operator String() const; - /* create / set */ - - _FORCE_INLINE_ void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) { - elements[0][0] = xx; - elements[0][1] = xy; - elements[0][2] = xz; - elements[1][0] = yx; - elements[1][1] = yy; - elements[1][2] = yz; - elements[2][0] = zx; - elements[2][1] = zy; - elements[2][2] = zz; - } - _FORCE_INLINE_ void set(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) { - set_axis(0, p_x); - set_axis(1, p_y); - set_axis(2, p_z); - } - _FORCE_INLINE_ Vector3 get_column(int i) const { - return Vector3(elements[0][i], elements[1][i], elements[2][i]); - } - - _FORCE_INLINE_ Vector3 get_row(int i) const { - 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; - elements[i][2] = p_row.z; - } - - _FORCE_INLINE_ void set_zero() { - elements[0].zero(); - elements[1].zero(); - elements[2].zero(); - } - - _FORCE_INLINE_ Basis transpose_xform(const Basis &m) const { - return Basis( - elements[0].x * m[0].x + elements[1].x * m[1].x + elements[2].x * m[2].x, - elements[0].x * m[0].y + elements[1].x * m[1].y + elements[2].x * m[2].y, - elements[0].x * m[0].z + elements[1].x * m[1].z + elements[2].x * m[2].z, - elements[0].y * m[0].x + elements[1].y * m[1].x + elements[2].y * m[2].x, - elements[0].y * m[0].y + elements[1].y * m[1].y + elements[2].y * m[2].y, - elements[0].y * m[0].z + elements[1].y * m[1].z + elements[2].y * m[2].z, - elements[0].z * m[0].x + elements[1].z * m[1].x + elements[2].z * m[2].x, - elements[0].z * m[0].y + elements[1].z * m[1].y + elements[2].z * m[2].y, - elements[0].z * m[0].z + elements[1].z * m[1].z + elements[2].z * m[2].z); - } - Basis(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) { - set(xx, xy, xz, yx, yy, yz, zx, zy, zz); - } + // create / set void orthonormalize(); Basis orthonormalized() const; @@ -254,23 +300,8 @@ public: operator Quat() const { return get_quat(); } - Basis(const Quat &p_quat) { set_quat(p_quat); } - Basis(const Quat &p_quat, const Vector3 &p_scale) { set_quat_scale(p_quat, p_scale); } - Basis(const Vector3 &p_euler) { set_euler(p_euler); } - Basis(const Vector3 &p_euler, const Vector3 &p_scale) { set_euler_scale(p_euler, p_scale); } - - Basis(const Vector3 &p_axis, real_t p_phi) { set_axis_angle(p_axis, p_phi); } - Basis(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) { set_axis_angle_scale(p_axis, p_phi, p_scale); } - - _FORCE_INLINE_ Basis(const Vector3 &row0, const Vector3 &row1, const Vector3 &row2) { - elements[0] = row0; - elements[1] = row1; - elements[2] = row2; - } - - _FORCE_INLINE_ Basis() {} -}; +//-------- _FORCE_INLINE_ void Basis::operator*=(const Basis &p_matrix) { set( @@ -350,4 +381,6 @@ Basis Basis::lerp(const Basis &p_to, const real_t &p_weight) const { return b; } +*/ + #endif // BASIS_H diff --git a/core/math/vector3.h b/core/math/vector3.h index f4645f3..8a98b6b 100644 --- a/core/math/vector3.h +++ b/core/math/vector3.h @@ -90,6 +90,12 @@ extern _FORCE_INLINE_ void vector3_set(Vector3 *self, real_t p_value_x, real_t p self->z = p_value_z; } +extern _FORCE_INLINE_ void vector3_setv(Vector3 *self, const Vector3 *other) { + self->x = other->x; + self->y = other->y; + self->z = other->z; +} + extern _FORCE_INLINE_ Vector3 vector3_create(real_t p_value_x, real_t p_value_y, real_t p_value_z) { Vector3 v;