mirror of
https://github.com/Relintai/codot.git
synced 2025-04-22 05:41:17 +02:00
Started work on Basis.
This commit is contained in:
parent
9be45046c1
commit
1d8182b9d4
@ -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;
|
||||
}
|
||||
|
||||
*/
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user