Sara.Sync
/
godot-cpp
mirror of https://github.com/godotengine/godot-cpp
0
1
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #859 from aaronfranke/basis-transform-quat 

Update Basis/Transform3D/Quaternion to match the engine
pull/880/head
Rémi Verschelde 2022-10-04 16:39:46 +02:00
parent 047b08922d e83d472c00
commit d25cae9b61
7 changed files with 859 additions and 776 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
include/godot_cpp/core/math.hpp
View File

@ -537,6 +537,27 @@ inline bool is_zero_approx(double s) {
return abs(s) < CMP_EPSILON; return abs(s) < CMP_EPSILON;
} }
inline float absf(float g) {
union {
float f;
uint32_t i;
} u;
u.f = g;
u.i &= 2147483647u;
return u.f;
}
inline double absd(double g) {
union {
double d;
uint64_t i;
} u;
u.d = g;
u.i &= (uint64_t)9223372036854775807ull;
return u.d;
}
inline double smoothstep(double p_from, double p_to, double p_weight) { inline double smoothstep(double p_from, double p_to, double p_weight) {
if (is_equal_approx(static_cast<real_t>(p_from), static_cast<real_t>(p_to))) { if (is_equal_approx(static_cast<real_t>(p_from), static_cast<real_t>(p_to))) {
return p_from; return p_from;

200
include/godot_cpp/variant/basis.hpp
View File

@ -49,10 +49,10 @@ public:
Vector3(0, 0, 1) Vector3(0, 0, 1)
}; };
inline const Vector3 &operator[](int axis) const { _FORCE_INLINE_ const Vector3 &operator[](int axis) const {
return rows[axis]; return rows[axis];
} }
inline Vector3 &operator[](int axis) { _FORCE_INLINE_ Vector3 &operator[](int axis) {
return rows[axis]; return rows[axis];
} }
@ -62,67 +62,53 @@ public:
Basis inverse() const; Basis inverse() const;
Basis transposed() const; Basis transposed() const;
inline real_t determinant() const; _FORCE_INLINE_ real_t determinant() const;
enum EulerOrder {
EULER_ORDER_XYZ,
EULER_ORDER_XZY,
EULER_ORDER_YXZ,
EULER_ORDER_YZX,
EULER_ORDER_ZXY,
EULER_ORDER_ZYX
};
void from_z(const Vector3 &p_z); void from_z(const Vector3 &p_z);
inline Vector3 get_axis(int p_axis) const { void rotate(const Vector3 &p_axis, real_t p_angle);
// get actual basis axis (rows is transposed for performance) Basis rotated(const Vector3 &p_axis, real_t p_angle) const;
return Vector3(rows[0][p_axis], rows[1][p_axis], rows[2][p_axis]);
}
inline void set_axis(int p_axis, const Vector3 &p_value) {
// get actual basis axis (rows is transposed for performance)
rows[0][p_axis] = p_value.x;
rows[1][p_axis] = p_value.y;
rows[2][p_axis] = p_value.z;
}
void rotate(const Vector3 &p_axis, real_t p_phi); void rotate_local(const Vector3 &p_axis, real_t p_angle);
Basis rotated(const Vector3 &p_axis, real_t p_phi) const; Basis rotated_local(const Vector3 &p_axis, real_t p_angle) const;
void rotate_local(const Vector3 &p_axis, real_t p_phi); void rotate(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ);
Basis rotated_local(const Vector3 &p_axis, real_t p_phi) const; Basis rotated(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ) const;
void rotate(const Vector3 &p_euler); void rotate(const Quaternion &p_quaternion);
Basis rotated(const Vector3 &p_euler) const; Basis rotated(const Quaternion &p_quaternion) const;
void rotate(const Quaternion &p_quat); Vector3 get_euler_normalized(EulerOrder p_order = EULER_ORDER_YXZ) const;
Basis rotated(const Quaternion &p_quat) const;
Vector3 get_rotation_euler() const;
void get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const; void get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const;
void get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) const; void get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) const;
Quaternion get_rotation_quaternion() const; Quaternion get_rotation_quaternion() const;
Vector3 get_rotation() const { return get_rotation_euler(); };
void rotate_to_align(Vector3 p_start_direction, Vector3 p_end_direction);
Vector3 rotref_posscale_decomposition(Basis &rotref) const; Vector3 rotref_posscale_decomposition(Basis &rotref) const;
Vector3 get_euler_xyz() const; Vector3 get_euler(EulerOrder p_order = EULER_ORDER_YXZ) const;
void set_euler_xyz(const Vector3 &p_euler); void set_euler(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ);
static Basis from_euler(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ) {
Vector3 get_euler_xzy() const; Basis b;
void set_euler_xzy(const Vector3 &p_euler); b.set_euler(p_euler, p_order);
return b;
Vector3 get_euler_yzx() const; }
void set_euler_yzx(const Vector3 &p_euler);
Vector3 get_euler_yxz() const;
void set_euler_yxz(const Vector3 &p_euler);
Vector3 get_euler_zxy() const;
void set_euler_zxy(const Vector3 &p_euler);
Vector3 get_euler_zyx() const;
void set_euler_zyx(const Vector3 &p_euler);
Quaternion get_quaternion() const; Quaternion get_quaternion() const;
void set_quaternion(const Quaternion &p_quat); void set_quaternion(const Quaternion &p_quaternion);
Vector3 get_euler() const { return get_euler_yxz(); }
void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); }
void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const; void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const;
void set_axis_angle(const Vector3 &p_axis, real_t p_phi); void set_axis_angle(const Vector3 &p_axis, real_t p_angle);
void scale(const Vector3 &p_scale); void scale(const Vector3 &p_scale);
Basis scaled(const Vector3 &p_scale) const; Basis scaled(const Vector3 &p_scale) const;
@ -130,6 +116,9 @@ public:
void scale_local(const Vector3 &p_scale); void scale_local(const Vector3 &p_scale);
Basis scaled_local(const Vector3 &p_scale) const; Basis scaled_local(const Vector3 &p_scale) const;
void scale_orthogonal(const Vector3 &p_scale);
Basis scaled_orthogonal(const Vector3 &p_scale) const;
void make_scale_uniform(); void make_scale_uniform();
float get_uniform_scale() const; float get_uniform_scale() const;
@ -137,18 +126,18 @@ public:
Vector3 get_scale_abs() const; Vector3 get_scale_abs() const;
Vector3 get_scale_local() const; Vector3 get_scale_local() const;
void set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale); void set_axis_angle_scale(const Vector3 &p_axis, real_t p_angle, const Vector3 &p_scale);
void set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale); void set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale, EulerOrder p_order = EULER_ORDER_YXZ);
void set_quaternion_scale(const Quaternion &p_quat, const Vector3 &p_scale); void set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 &p_scale);
// transposed dot products // transposed dot products
inline real_t tdotx(const Vector3 &v) const { _FORCE_INLINE_ real_t tdotx(const Vector3 &v) const {
return rows[0][0] * v[0] + rows[1][0] * v[1] + rows[2][0] * v[2]; return rows[0][0] * v[0] + rows[1][0] * v[1] + rows[2][0] * v[2];
} }
inline real_t tdoty(const Vector3 &v) const { _FORCE_INLINE_ real_t tdoty(const Vector3 &v) const {
return rows[0][1] * v[0] + rows[1][1] * v[1] + rows[2][1] * v[2]; return rows[0][1] * v[0] + rows[1][1] * v[1] + rows[2][1] * v[2];
} }
inline real_t tdotz(const Vector3 &v) const { _FORCE_INLINE_ real_t tdotz(const Vector3 &v) const {
return rows[0][2] * v[0] + rows[1][2] * v[1] + rows[2][2] * v[2]; return rows[0][2] * v[0] + rows[1][2] * v[1] + rows[2][2] * v[2];
} }
@ -157,26 +146,22 @@ public:
bool operator==(const Basis &p_matrix) const; bool operator==(const Basis &p_matrix) const;
bool operator!=(const Basis &p_matrix) const; bool operator!=(const Basis &p_matrix) const;
inline Vector3 xform(const Vector3 &p_vector) const; _FORCE_INLINE_ Vector3 xform(const Vector3 &p_vector) const;
inline Vector3 xform_inv(const Vector3 &p_vector) const; _FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_vector) const;
inline void operator*=(const Basis &p_matrix); _FORCE_INLINE_ void operator*=(const Basis &p_matrix);
inline Basis operator*(const Basis &p_matrix) const; _FORCE_INLINE_ Basis operator*(const Basis &p_matrix) const;
inline void operator+=(const Basis &p_matrix); _FORCE_INLINE_ void operator+=(const Basis &p_matrix);
inline Basis operator+(const Basis &p_matrix) const; _FORCE_INLINE_ Basis operator+(const Basis &p_matrix) const;
inline void operator-=(const Basis &p_matrix); _FORCE_INLINE_ void operator-=(const Basis &p_matrix);
inline Basis operator-(const Basis &p_matrix) const; _FORCE_INLINE_ Basis operator-(const Basis &p_matrix) const;
inline void operator*=(real_t p_val); _FORCE_INLINE_ void operator*=(const real_t p_val);
inline Basis operator*(real_t p_val) const; _FORCE_INLINE_ Basis operator*(const real_t p_val) const;
int get_orthogonal_index() const;
void set_orthogonal_index(int p_index);
void set_diagonal(const Vector3 &p_diag);
bool is_orthogonal() const; bool is_orthogonal() const;
bool is_diagonal() const; bool is_diagonal() const;
bool is_rotation() const; bool is_rotation() const;
Basis lerp(const Basis &p_to, const real_t &p_weight) const;
Basis slerp(const Basis &p_to, const real_t &p_weight) const; Basis slerp(const Basis &p_to, const real_t &p_weight) const;
void rotate_sh(real_t *p_values); void rotate_sh(real_t *p_values);
@ -184,7 +169,7 @@ public:
/* create / set */ /* create / set */
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) { _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) {
rows[0][0] = xx; rows[0][0] = xx;
rows[0][1] = xy; rows[0][1] = xy;
rows[0][2] = xz; rows[0][2] = xz;
@ -195,35 +180,35 @@ public:
rows[2][1] = zy; rows[2][1] = zy;
rows[2][2] = zz; rows[2][2] = zz;
} }
inline void set(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) { _FORCE_INLINE_ void set_columns(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) {
set_axis(0, p_x); set_column(0, p_x);
set_axis(1, p_y); set_column(1, p_y);
set_axis(2, p_z); set_column(2, p_z);
}
inline Vector3 get_column(int i) const {
return Vector3(rows[0][i], rows[1][i], rows[2][i]);
} }
inline Vector3 get_row(int i) const { _FORCE_INLINE_ Vector3 get_column(int p_index) const {
return Vector3(rows[i][0], rows[i][1], rows[i][2]); // Get actual basis axis column (we store transposed as rows for performance).
return Vector3(rows[0][p_index], rows[1][p_index], rows[2][p_index]);
} }
inline Vector3 get_main_diagonal() const {
_FORCE_INLINE_ void set_column(int p_index, const Vector3 &p_value) {
// Set actual basis axis column (we store transposed as rows for performance).
rows[0][p_index] = p_value.x;
rows[1][p_index] = p_value.y;
rows[2][p_index] = p_value.z;
}
_FORCE_INLINE_ Vector3 get_main_diagonal() const {
return Vector3(rows[0][0], rows[1][1], rows[2][2]); return Vector3(rows[0][0], rows[1][1], rows[2][2]);
} }
inline void set_row(int i, const Vector3 &p_row) { _FORCE_INLINE_ void set_zero() {
rows[i][0] = p_row.x;
rows[i][1] = p_row.y;
rows[i][2] = p_row.z;
}
inline void set_zero() {
rows[0].zero(); rows[0].zero();
rows[1].zero(); rows[1].zero();
rows[2].zero(); rows[2].zero();
} }
inline Basis transpose_xform(const Basis &m) const { _FORCE_INLINE_ Basis transpose_xform(const Basis &m) const {
return Basis( return Basis(
rows[0].x * m[0].x + rows[1].x * m[1].x + rows[2].x * m[2].x, rows[0].x * m[0].x + rows[1].x * m[1].x + rows[2].x * m[2].x,
rows[0].x * m[0].y + rows[1].x * m[1].y + rows[2].x * m[2].y, rows[0].x * m[0].y + rows[1].x * m[1].y + rows[2].x * m[2].y,
@ -242,6 +227,9 @@ public:
void orthonormalize(); void orthonormalize();
Basis orthonormalized() const; Basis orthonormalized() const;
void orthogonalize();
Basis orthogonalized() const;
#ifdef MATH_CHECKS #ifdef MATH_CHECKS
bool is_symmetric() const; bool is_symmetric() const;
#endif #endif
@ -249,69 +237,71 @@ public:
operator Quaternion() const { return get_quaternion(); } operator Quaternion() const { return get_quaternion(); }
Basis(const Quaternion &p_quat) { set_quaternion(p_quat); }; static Basis looking_at(const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0));
Basis(const Quaternion &p_quat, const Vector3 &p_scale) { set_quaternion_scale(p_quat, p_scale); }
Basis(const Vector3 &p_euler) { set_euler(p_euler); } Basis(const Quaternion &p_quaternion) { set_quaternion(p_quaternion); };
Basis(const Vector3 &p_euler, const Vector3 &p_scale) { set_euler_scale(p_euler, p_scale); } Basis(const Quaternion &p_quaternion, const Vector3 &p_scale) { set_quaternion_scale(p_quaternion, 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_angle) { set_axis_angle(p_axis, p_angle); }
Basis(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) { set_axis_angle_scale(p_axis, p_phi, p_scale); } Basis(const Vector3 &p_axis, real_t p_angle, const Vector3 &p_scale) { set_axis_angle_scale(p_axis, p_angle, p_scale); }
static Basis from_scale(const Vector3 &p_scale);
inline Basis(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) { _FORCE_INLINE_ Basis(const Vector3 &p_x_axis, const Vector3 &p_y_axis, const Vector3 &p_z_axis) {
set_axis(0, p_x); set_columns(p_x_axis, p_y_axis, p_z_axis);
set_axis(1, p_y);
set_axis(2, p_z);
} }
inline Basis() {} _FORCE_INLINE_ Basis() {}
private:
// Helper method.
void _set_diagonal(const Vector3 &p_diag);
}; };
inline void Basis::operator*=(const Basis &p_matrix) { _FORCE_INLINE_ void Basis::operator*=(const Basis &p_matrix) {
set( set(
p_matrix.tdotx(rows[0]), p_matrix.tdoty(rows[0]), p_matrix.tdotz(rows[0]), p_matrix.tdotx(rows[0]), p_matrix.tdoty(rows[0]), p_matrix.tdotz(rows[0]),
p_matrix.tdotx(rows[1]), p_matrix.tdoty(rows[1]), p_matrix.tdotz(rows[1]), p_matrix.tdotx(rows[1]), p_matrix.tdoty(rows[1]), p_matrix.tdotz(rows[1]),
p_matrix.tdotx(rows[2]), p_matrix.tdoty(rows[2]), p_matrix.tdotz(rows[2])); p_matrix.tdotx(rows[2]), p_matrix.tdoty(rows[2]), p_matrix.tdotz(rows[2]));
} }
inline Basis Basis::operator*(const Basis &p_matrix) const { _FORCE_INLINE_ Basis Basis::operator*(const Basis &p_matrix) const {
return Basis( return Basis(
p_matrix.tdotx(rows[0]), p_matrix.tdoty(rows[0]), p_matrix.tdotz(rows[0]), p_matrix.tdotx(rows[0]), p_matrix.tdoty(rows[0]), p_matrix.tdotz(rows[0]),
p_matrix.tdotx(rows[1]), p_matrix.tdoty(rows[1]), p_matrix.tdotz(rows[1]), p_matrix.tdotx(rows[1]), p_matrix.tdoty(rows[1]), p_matrix.tdotz(rows[1]),
p_matrix.tdotx(rows[2]), p_matrix.tdoty(rows[2]), p_matrix.tdotz(rows[2])); p_matrix.tdotx(rows[2]), p_matrix.tdoty(rows[2]), p_matrix.tdotz(rows[2]));
} }
inline void Basis::operator+=(const Basis &p_matrix) { _FORCE_INLINE_ void Basis::operator+=(const Basis &p_matrix) {
rows[0] += p_matrix.rows[0]; rows[0] += p_matrix.rows[0];
rows[1] += p_matrix.rows[1]; rows[1] += p_matrix.rows[1];
rows[2] += p_matrix.rows[2]; rows[2] += p_matrix.rows[2];
} }
inline Basis Basis::operator+(const Basis &p_matrix) const { _FORCE_INLINE_ Basis Basis::operator+(const Basis &p_matrix) const {
Basis ret(*this); Basis ret(*this);
ret += p_matrix; ret += p_matrix;
return ret; return ret;
} }
inline void Basis::operator-=(const Basis &p_matrix) { _FORCE_INLINE_ void Basis::operator-=(const Basis &p_matrix) {
rows[0] -= p_matrix.rows[0]; rows[0] -= p_matrix.rows[0];
rows[1] -= p_matrix.rows[1]; rows[1] -= p_matrix.rows[1];
rows[2] -= p_matrix.rows[2]; rows[2] -= p_matrix.rows[2];
} }
inline Basis Basis::operator-(const Basis &p_matrix) const { _FORCE_INLINE_ Basis Basis::operator-(const Basis &p_matrix) const {
Basis ret(*this); Basis ret(*this);
ret -= p_matrix; ret -= p_matrix;
return ret; return ret;
} }
inline void Basis::operator*=(real_t p_val) { _FORCE_INLINE_ void Basis::operator*=(const real_t p_val) {
rows[0] *= p_val; rows[0] *= p_val;
rows[1] *= p_val; rows[1] *= p_val;
rows[2] *= p_val; rows[2] *= p_val;
} }
inline Basis Basis::operator*(real_t p_val) const { _FORCE_INLINE_ Basis Basis::operator*(const real_t p_val) const {
Basis ret(*this); Basis ret(*this);
ret *= p_val; ret *= p_val;
return ret; return ret;

101
include/godot_cpp/variant/quaternion.hpp
View File

@ -28,8 +28,8 @@
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/ /*************************************************************************/
#ifndef GODOT_QUAT_HPP #ifndef GODOT_QUATERNION_HPP
#define GODOT_QUAT_HPP #define GODOT_QUATERNION_HPP
#include <godot_cpp/core/math.hpp> #include <godot_cpp/core/math.hpp>
#include <godot_cpp/variant/vector3.hpp> #include <godot_cpp/variant/vector3.hpp>
@ -52,20 +52,23 @@ public:
real_t components[4] = { 0, 0, 0, 1.0 }; real_t components[4] = { 0, 0, 0, 1.0 };
}; };
inline real_t &operator[](int idx) { _FORCE_INLINE_ real_t &operator[](int idx) {
return components[idx]; return components[idx];
} }
inline const real_t &operator[](int idx) const { _FORCE_INLINE_ const real_t &operator[](int idx) const {
return components[idx]; return components[idx];
} }
inline real_t length_squared() const; _FORCE_INLINE_ real_t length_squared() const;
bool is_equal_approx(const Quaternion &p_quat) const; bool is_equal_approx(const Quaternion &p_quaternion) const;
real_t length() const; real_t length() const;
void normalize(); void normalize();
Quaternion normalized() const; Quaternion normalized() const;
bool is_normalized() const; bool is_normalized() const;
Quaternion inverse() const; Quaternion inverse() const;
inline real_t dot(const Quaternion &p_q) const; Quaternion log() const;
Quaternion exp() const;
_FORCE_INLINE_ real_t dot(const Quaternion &p_q) const;
real_t angle_to(const Quaternion &p_to) const;
Vector3 get_euler_xyz() const; Vector3 get_euler_xyz() const;
Vector3 get_euler_yxz() const; Vector3 get_euler_yxz() const;
@ -73,9 +76,13 @@ public:
Quaternion slerp(const Quaternion &p_to, const real_t &p_weight) const; Quaternion slerp(const Quaternion &p_to, const real_t &p_weight) const;
Quaternion slerpni(const Quaternion &p_to, const real_t &p_weight) const; Quaternion slerpni(const Quaternion &p_to, const real_t &p_weight) const;
Quaternion cubic_slerp(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const; Quaternion spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const;
Quaternion spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight, const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const;
inline void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const { Vector3 get_axis() const;
real_t get_angle() const;
_FORCE_INLINE_ void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
r_angle = 2 * Math::acos(w); r_angle = 2 * Math::acos(w);
real_t r = ((real_t)1) / Math::sqrt(1 - w * w); real_t r = ((real_t)1) / Math::sqrt(1 - w * w);
r_axis.x = x * r; r_axis.x = x * r;
@ -86,44 +93,37 @@ public:
void operator*=(const Quaternion &p_q); void operator*=(const Quaternion &p_q);
Quaternion operator*(const Quaternion &p_q) const; Quaternion operator*(const Quaternion &p_q) const;
Quaternion operator*(const Vector3 &v) const { _FORCE_INLINE_ Vector3 xform(const Vector3 &v) const {
return Quaternion(w * v.x + y * v.z - z * v.y,
w * v.y + z * v.x - x * v.z,
w * v.z + x * v.y - y * v.x,
-x * v.x - y * v.y - z * v.z);
}
inline Vector3 xform(const Vector3 &v) const {
#ifdef MATH_CHECKS #ifdef MATH_CHECKS
ERR_FAIL_COND_V(!is_normalized(), v); ERR_FAIL_COND_V_MSG(!is_normalized(), v, "The quaternion must be normalized.");
#endif #endif
Vector3 u(x, y, z); Vector3 u(x, y, z);
Vector3 uv = u.cross(v); Vector3 uv = u.cross(v);
return v + ((uv * w) + u.cross(uv)) * ((real_t)2); return v + ((uv * w) + u.cross(uv)) * ((real_t)2);
} }
inline Vector3 xform_inv(const Vector3 &v) const { _FORCE_INLINE_ Vector3 xform_inv(const Vector3 &v) const {
return inverse().xform(v); return inverse().xform(v);
} }
inline void operator+=(const Quaternion &p_q); _FORCE_INLINE_ void operator+=(const Quaternion &p_q);
inline void operator-=(const Quaternion &p_q); _FORCE_INLINE_ void operator-=(const Quaternion &p_q);
inline void operator*=(const real_t &s); _FORCE_INLINE_ void operator*=(const real_t &s);
inline void operator/=(const real_t &s); _FORCE_INLINE_ void operator/=(const real_t &s);
inline Quaternion operator+(const Quaternion &q2) const; _FORCE_INLINE_ Quaternion operator+(const Quaternion &q2) const;
inline Quaternion operator-(const Quaternion &q2) const; _FORCE_INLINE_ Quaternion operator-(const Quaternion &q2) const;
inline Quaternion operator-() const; _FORCE_INLINE_ Quaternion operator-() const;
inline Quaternion operator*(const real_t &s) const; _FORCE_INLINE_ Quaternion operator*(const real_t &s) const;
inline Quaternion operator/(const real_t &s) const; _FORCE_INLINE_ Quaternion operator/(const real_t &s) const;
inline bool operator==(const Quaternion &p_quat) const; _FORCE_INLINE_ bool operator==(const Quaternion &p_quaternion) const;
inline bool operator!=(const Quaternion &p_quat) const; _FORCE_INLINE_ bool operator!=(const Quaternion &p_quaternion) const;
operator String() const; operator String() const;
inline Quaternion() {} _FORCE_INLINE_ Quaternion() {}
inline Quaternion(real_t p_x, real_t p_y, real_t p_z, real_t p_w) : _FORCE_INLINE_ Quaternion(real_t p_x, real_t p_y, real_t p_z, real_t p_w) :
x(p_x), x(p_x),
y(p_y), y(p_y),
z(p_z), z(p_z),
@ -141,12 +141,11 @@ public:
w(p_q.w) { w(p_q.w) {
} }
Quaternion &operator=(const Quaternion &p_q) { void operator=(const Quaternion &p_q) {
x = p_q.x; x = p_q.x;
y = p_q.y; y = p_q.y;
z = p_q.z; z = p_q.z;
w = p_q.w; w = p_q.w;
return *this;
} }
Quaternion(const Vector3 &v0, const Vector3 &v1) // shortest arc Quaternion(const Vector3 &v0, const Vector3 &v1) // shortest arc
@ -154,19 +153,19 @@ public:
Vector3 c = v0.cross(v1); Vector3 c = v0.cross(v1);
real_t d = v0.dot(v1); real_t d = v0.dot(v1);
if (d < (real_t)-1.0 + CMP_EPSILON) { if (d < -1.0f + (real_t)CMP_EPSILON) {
x = (real_t)0.0; x = 0;
y = (real_t)1.0; y = 1;
z = (real_t)0.0; z = 0;
w = (real_t)0.0; w = 0;
} else { } else {
real_t s = Math::sqrt(((real_t)1.0 + d) * (real_t)2.0); real_t s = Math::sqrt((1.0f + d) * 2.0f);
real_t rs = (real_t)1.0 / s; real_t rs = 1.0f / s;
x = c.x * rs; x = c.x * rs;
y = c.y * rs; y = c.y * rs;
z = c.z * rs; z = c.z * rs;
w = s * (real_t)0.5; w = s * 0.5f;
} }
} }
}; };
@ -201,7 +200,7 @@ void Quaternion::operator*=(const real_t &s) {
} }
void Quaternion::operator/=(const real_t &s) { void Quaternion::operator/=(const real_t &s) {
*this *= (real_t)1.0 / s; *this *= 1.0f / s;
} }
Quaternion Quaternion::operator+(const Quaternion &q2) const { Quaternion Quaternion::operator+(const Quaternion &q2) const {
@ -224,21 +223,21 @@ Quaternion Quaternion::operator*(const real_t &s) const {
} }
Quaternion Quaternion::operator/(const real_t &s) const { Quaternion Quaternion::operator/(const real_t &s) const {
return *this * ((real_t)1.0 / s); return *this * (1.0f / s);
} }
bool Quaternion::operator==(const Quaternion &p_quat) const { bool Quaternion::operator==(const Quaternion &p_quaternion) const {
return x == p_quat.x && y == p_quat.y && z == p_quat.z && w == p_quat.w; return x == p_quaternion.x && y == p_quaternion.y && z == p_quaternion.z && w == p_quaternion.w;
} }
bool Quaternion::operator!=(const Quaternion &p_quat) const { bool Quaternion::operator!=(const Quaternion &p_quaternion) const {
return x != p_quat.x || y != p_quat.y || z != p_quat.z || w != p_quat.w; return x != p_quaternion.x || y != p_quaternion.y || z != p_quaternion.z || w != p_quaternion.w;
} }
inline Quaternion operator*(const real_t &p_real, const Quaternion &p_quat) { _FORCE_INLINE_ Quaternion operator*(const real_t &p_real, const Quaternion &p_quaternion) {
return p_quat * p_real; return p_quaternion * p_real;
} }
} // namespace godot } // namespace godot
#endif // GODOT_QUAT_HPP #endif // GODOT_QUATERNION_HPP

129
include/godot_cpp/variant/transform3d.hpp
View File

@ -54,11 +54,11 @@ public:
void affine_invert(); void affine_invert();
Transform3D affine_inverse() const; Transform3D affine_inverse() const;
Transform3D rotated(const Vector3 &p_axis, real_t p_phi) const; Transform3D rotated(const Vector3 &p_axis, real_t p_angle) const;
Transform3D rotated_local(const Vector3 &p_axis, real_t p_angle) const; Transform3D rotated_local(const Vector3 &p_axis, real_t p_angle) const;
void rotate(const Vector3 &p_axis, real_t p_phi); void rotate(const Vector3 &p_axis, real_t p_angle);
void rotate_basis(const Vector3 &p_axis, real_t p_phi); void rotate_basis(const Vector3 &p_axis, real_t p_angle);
void set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0)); void set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0));
Transform3D looking_at(const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0)) const; Transform3D looking_at(const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0)) const;
@ -67,8 +67,8 @@ public:
Transform3D scaled(const Vector3 &p_scale) const; Transform3D scaled(const Vector3 &p_scale) const;
Transform3D scaled_local(const Vector3 &p_scale) const; Transform3D scaled_local(const Vector3 &p_scale) const;
void scale_basis(const Vector3 &p_scale); void scale_basis(const Vector3 &p_scale);
void translate(real_t p_tx, real_t p_ty, real_t p_tz); void translate_local(real_t p_tx, real_t p_ty, real_t p_tz);
void translate(const Vector3 &p_translation); void translate_local(const Vector3 &p_translation);
Transform3D translated(const Vector3 &p_translation) const; Transform3D translated(const Vector3 &p_translation) const;
Transform3D translated_local(const Vector3 &p_translation) const; Transform3D translated_local(const Vector3 &p_translation) const;
@ -80,29 +80,41 @@ public:
void orthonormalize(); void orthonormalize();
Transform3D orthonormalized() const; Transform3D orthonormalized() const;
void orthogonalize();
Transform3D orthogonalized() const;
bool is_equal_approx(const Transform3D &p_transform) const; bool is_equal_approx(const Transform3D &p_transform) const;
bool operator==(const Transform3D &p_transform) const; bool operator==(const Transform3D &p_transform) const;
bool operator!=(const Transform3D &p_transform) const; bool operator!=(const Transform3D &p_transform) const;
inline Vector3 xform(const Vector3 &p_vector) const; _FORCE_INLINE_ Vector3 xform(const Vector3 &p_vector) const;
inline Vector3 xform_inv(const Vector3 &p_vector) const; _FORCE_INLINE_ AABB xform(const AABB &p_aabb) const;
_FORCE_INLINE_ PackedVector3Array xform(const PackedVector3Array &p_array) const;
inline Plane xform(const Plane &p_plane) const; // NOTE: These are UNSAFE with non-uniform scaling, and will produce incorrect results.
inline Plane xform_inv(const Plane &p_plane) const; // They use the transpose.
// For safe inverse transforms, xform by the affine_inverse.
_FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_vector) const;
_FORCE_INLINE_ AABB xform_inv(const AABB &p_aabb) const;
_FORCE_INLINE_ PackedVector3Array xform_inv(const PackedVector3Array &p_array) const;
inline AABB xform(const AABB &p_aabb) const; // Safe with non-uniform scaling (uses affine_inverse).
inline AABB xform_inv(const AABB &p_aabb) const; _FORCE_INLINE_ Plane xform(const Plane &p_plane) const;
_FORCE_INLINE_ Plane xform_inv(const Plane &p_plane) const;
inline PackedVector3Array xform(const PackedVector3Array &p_array) const; // These fast versions use precomputed affine inverse, and should be used in bottleneck areas where
inline PackedVector3Array xform_inv(const PackedVector3Array &p_array) const; // multiple planes are to be transformed.
_FORCE_INLINE_ Plane xform_fast(const Plane &p_plane, const Basis &p_basis_inverse_transpose) const;
static _FORCE_INLINE_ Plane xform_inv_fast(const Plane &p_plane, const Transform3D &p_inverse, const Basis &p_basis_transpose);
void operator*=(const Transform3D &p_transform); void operator*=(const Transform3D &p_transform);
Transform3D operator*(const Transform3D &p_transform) const; Transform3D operator*(const Transform3D &p_transform) const;
void operator*=(const real_t p_val);
Transform3D operator*(const real_t p_val) const;
Transform3D interpolate_with(const Transform3D &p_transform, real_t p_c) const; Transform3D interpolate_with(const Transform3D &p_transform, real_t p_c) const;
inline Transform3D inverse_xform(const Transform3D &t) const { _FORCE_INLINE_ Transform3D inverse_xform(const Transform3D &t) const {
Vector3 v = t.origin - origin; Vector3 v = t.origin - origin;
return Transform3D(basis.transpose_xform(t.basis), return Transform3D(basis.transpose_xform(t.basis),
basis.xform(v)); basis.xform(v));
@ -123,14 +135,14 @@ public:
Transform3D(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, real_t ox, real_t oy, real_t oz); Transform3D(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, real_t ox, real_t oy, real_t oz);
}; };
inline Vector3 Transform3D::xform(const Vector3 &p_vector) const { _FORCE_INLINE_ Vector3 Transform3D::xform(const Vector3 &p_vector) const {
return Vector3( return Vector3(
basis[0].dot(p_vector) + origin.x, basis[0].dot(p_vector) + origin.x,
basis[1].dot(p_vector) + origin.y, basis[1].dot(p_vector) + origin.y,
basis[2].dot(p_vector) + origin.z); basis[2].dot(p_vector) + origin.z);
} }
inline Vector3 Transform3D::xform_inv(const Vector3 &p_vector) const { _FORCE_INLINE_ Vector3 Transform3D::xform_inv(const Vector3 &p_vector) const {
Vector3 v = p_vector - origin; Vector3 v = p_vector - origin;
return Vector3( return Vector3(
@ -139,34 +151,24 @@ inline Vector3 Transform3D::xform_inv(const Vector3 &p_vector) const {
(basis.rows[0][2] * v.x) + (basis.rows[1][2] * v.y) + (basis.rows[2][2] * v.z)); (basis.rows[0][2] * v.x) + (basis.rows[1][2] * v.y) + (basis.rows[2][2] * v.z));
} }
inline Plane Transform3D::xform(const Plane &p_plane) const { // Neither the plane regular xform or xform_inv are particularly efficient,
Vector3 point = p_plane.normal * p_plane.d; // as they do a basis inverse. For xforming a large number
Vector3 point_dir = point + p_plane.normal; // of planes it is better to pre-calculate the inverse transpose basis once
point = xform(point); // and reuse it for each plane, by using the 'fast' version of the functions.
point_dir = xform(point_dir); _FORCE_INLINE_ Plane Transform3D::xform(const Plane &p_plane) const {
Basis b = basis.inverse();
Vector3 normal = point_dir - point; b.transpose();
normal.normalize(); return xform_fast(p_plane, b);
real_t d = normal.dot(point);
return Plane(normal, d);
} }
inline Plane Transform3D::xform_inv(const Plane &p_plane) const { _FORCE_INLINE_ Plane Transform3D::xform_inv(const Plane &p_plane) const {
Vector3 point = p_plane.normal * p_plane.d; Transform3D inv = affine_inverse();
Vector3 point_dir = point + p_plane.normal; Basis basis_transpose = basis.transposed();
point = xform_inv(point); return xform_inv_fast(p_plane, inv, basis_transpose);
point_dir = xform_inv(point_dir);
Vector3 normal = point_dir - point;
normal.normalize();
real_t d = normal.dot(point);
return Plane(normal, d);
} }
inline AABB Transform3D::xform(const AABB &p_aabb) const { _FORCE_INLINE_ AABB Transform3D::xform(const AABB &p_aabb) const {
/* http://dev.theomader.com/transform-bounding-boxes/ */ /* https://dev.theomader.com/transform-bounding-boxes/ */
Vector3 min = p_aabb.position; Vector3 min = p_aabb.position;
Vector3 max = p_aabb.position + p_aabb.size; Vector3 max = p_aabb.position + p_aabb.size;
Vector3 tmin, tmax; Vector3 tmin, tmax;
@ -190,7 +192,7 @@ inline AABB Transform3D::xform(const AABB &p_aabb) const {
return r_aabb; return r_aabb;
} }
inline AABB Transform3D::xform_inv(const AABB &p_aabb) const { _FORCE_INLINE_ AABB Transform3D::xform_inv(const AABB &p_aabb) const {
/* define vertices */ /* define vertices */
Vector3 vertices[8] = { Vector3 vertices[8] = {
Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z), Vector3(p_aabb.position.x + p_aabb.size.x, p_aabb.position.y + p_aabb.size.y, p_aabb.position.z + p_aabb.size.z),
@ -218,8 +220,11 @@ PackedVector3Array Transform3D::xform(const PackedVector3Array &p_array) const {
PackedVector3Array array; PackedVector3Array array;
array.resize(p_array.size()); array.resize(p_array.size());
const Vector3 *r = p_array.ptr();
Vector3 *w = array.ptrw();
for (int i = 0; i < p_array.size(); ++i) { for (int i = 0; i < p_array.size(); ++i) {
array[i] = xform(p_array[i]); w[i] = xform(r[i]);
} }
return array; return array;
} }
@ -228,12 +233,48 @@ PackedVector3Array Transform3D::xform_inv(const PackedVector3Array &p_array) con
PackedVector3Array array; PackedVector3Array array;
array.resize(p_array.size()); array.resize(p_array.size());
const Vector3 *r = p_array.ptr();
Vector3 *w = array.ptrw();
for (int i = 0; i < p_array.size(); ++i) { for (int i = 0; i < p_array.size(); ++i) {
array[i] = xform_inv(p_array[i]); w[i] = xform_inv(r[i]);
} }
return array; return array;
} }
_FORCE_INLINE_ Plane Transform3D::xform_fast(const Plane &p_plane, const Basis &p_basis_inverse_transpose) const {
// Transform a single point on the plane.
Vector3 point = p_plane.normal * p_plane.d;
point = xform(point);
// Use inverse transpose for correct normals with non-uniform scaling.
Vector3 normal = p_basis_inverse_transpose.xform(p_plane.normal);
normal.normalize();
real_t d = normal.dot(point);
return Plane(normal, d);
}
_FORCE_INLINE_ Plane Transform3D::xform_inv_fast(const Plane &p_plane, const Transform3D &p_inverse, const Basis &p_basis_transpose) {
// Transform a single point on the plane.
Vector3 point = p_plane.normal * p_plane.d;
point = p_inverse.xform(point);
// Note that instead of precalculating the transpose, an alternative
// would be to use the transpose for the basis transform.
// However that would be less SIMD friendly (requiring a swizzle).
// So the cost is one extra precalced value in the calling code.
// This is probably worth it, as this could be used in bottleneck areas. And
// where it is not a bottleneck, the non-fast method is fine.
// Use transpose for correct normals with non-uniform scaling.
Vector3 normal = p_basis_transpose.xform(p_plane.normal);
normal.normalize();
real_t d = normal.dot(point);
return Plane(normal, d);
}
} // namespace godot } // namespace godot
#endif // GODOT_TRANSFORM_HPP #endif // GODOT_TRANSFORM3D_HPP

649
src/variant/basis.cpp
View File

File diff suppressed because it is too large Load Diff

204
src/variant/quaternion.cpp
View File

@ -35,13 +35,18 @@
namespace godot { namespace godot {
real_t Quaternion::angle_to(const Quaternion &p_to) const {
real_t d = dot(p_to);
return Math::acos(CLAMP(d * d * 2 - 1, -1, 1));
}
// get_euler_xyz returns a vector containing the Euler angles in the format // get_euler_xyz returns a vector containing the Euler angles in the format
// (ax,ay,az), where ax is the angle of rotation around x axis, // (ax,ay,az), where ax is the angle of rotation around x axis,
// and similar for other axes. // and similar for other axes.
// This implementation uses XYZ convention (Z is the first rotation). // This implementation uses XYZ convention (Z is the first rotation).
Vector3 Quaternion::get_euler_xyz() const { Vector3 Quaternion::get_euler_xyz() const {
Basis m(*this); Basis m(*this);
return m.get_euler_xyz(); return m.get_euler(Basis::EULER_ORDER_XYZ);
} }
// get_euler_yxz returns a vector containing the Euler angles in the format // get_euler_yxz returns a vector containing the Euler angles in the format
@ -50,17 +55,20 @@ Vector3 Quaternion::get_euler_xyz() const {
// This implementation uses YXZ convention (Z is the first rotation). // This implementation uses YXZ convention (Z is the first rotation).
Vector3 Quaternion::get_euler_yxz() const { Vector3 Quaternion::get_euler_yxz() const {
#ifdef MATH_CHECKS #ifdef MATH_CHECKS
ERR_FAIL_COND_V(!is_normalized(), Vector3(0, 0, 0)); ERR_FAIL_COND_V_MSG(!is_normalized(), Vector3(0, 0, 0), "The quaternion must be normalized.");
#endif #endif
Basis m(*this); Basis m(*this);
return m.get_euler_yxz(); return m.get_euler(Basis::EULER_ORDER_YXZ);
} }
void Quaternion::operator*=(const Quaternion &p_q) { void Quaternion::operator*=(const Quaternion &p_q) {
x = w * p_q.x + x * p_q.w + y * p_q.z - z * p_q.y; real_t xx = w * p_q.x + x * p_q.w + y * p_q.z - z * p_q.y;
y = w * p_q.y + y * p_q.w + z * p_q.x - x * p_q.z; real_t yy = w * p_q.y + y * p_q.w + z * p_q.x - x * p_q.z;
z = w * p_q.z + z * p_q.w + x * p_q.y - y * p_q.x; real_t zz = w * p_q.z + z * p_q.w + x * p_q.y - y * p_q.x;
w = w * p_q.w - x * p_q.x - y * p_q.y - z * p_q.z; w = w * p_q.w - x * p_q.x - y * p_q.y - z * p_q.z;
x = xx;
y = yy;
z = zz;
} }
Quaternion Quaternion::operator*(const Quaternion &p_q) const { Quaternion Quaternion::operator*(const Quaternion &p_q) const {
@ -69,8 +77,8 @@ Quaternion Quaternion::operator*(const Quaternion &p_q) const {
return r; return r;
} }
bool Quaternion::is_equal_approx(const Quaternion &p_quat) const { bool Quaternion::is_equal_approx(const Quaternion &p_quaternion) const {
return Math::is_equal_approx(x, p_quat.x) && Math::is_equal_approx(y, p_quat.y) && Math::is_equal_approx(z, p_quat.z) && Math::is_equal_approx(w, p_quat.w); return Math::is_equal_approx(x, p_quaternion.x) && Math::is_equal_approx(y, p_quaternion.y) && Math::is_equal_approx(z, p_quaternion.z) && Math::is_equal_approx(w, p_quaternion.w);
} }
real_t Quaternion::length() const { real_t Quaternion::length() const {
@ -91,15 +99,32 @@ bool Quaternion::is_normalized() const {
Quaternion Quaternion::inverse() const { Quaternion Quaternion::inverse() const {
#ifdef MATH_CHECKS #ifdef MATH_CHECKS
ERR_FAIL_COND_V(!is_normalized(), Quaternion()); ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The quaternion must be normalized.");
#endif #endif
return Quaternion(-x, -y, -z, w); return Quaternion(-x, -y, -z, w);
} }
Quaternion Quaternion::log() const {
Quaternion src = *this;
Vector3 src_v = src.get_axis() * src.get_angle();
return Quaternion(src_v.x, src_v.y, src_v.z, 0);
}
Quaternion Quaternion::exp() const {
Quaternion src = *this;
Vector3 src_v = Vector3(src.x, src.y, src.z);
real_t theta = src_v.length();
src_v = src_v.normalized();
if (theta < CMP_EPSILON || !src_v.is_normalized()) {
return Quaternion(0, 0, 0, 1);
}
return Quaternion(src_v, theta);
}
Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) const { Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) const {
#ifdef MATH_CHECKS #ifdef MATH_CHECKS
ERR_FAIL_COND_V(!is_normalized(), Quaternion()); ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized.");
ERR_FAIL_COND_V(!p_to.is_normalized(), Quaternion()); ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
#endif #endif
Quaternion to1; Quaternion to1;
real_t omega, cosom, sinom, scale0, scale1; real_t omega, cosom, sinom, scale0, scale1;
@ -108,22 +133,16 @@ Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) con
cosom = dot(p_to); cosom = dot(p_to);
// adjust signs (if necessary) // adjust signs (if necessary)
if (cosom < 0.0) { if (cosom < 0.0f) {
cosom = -cosom; cosom = -cosom;
to1.x = -p_to.x; to1 = -p_to;
to1.y = -p_to.y;
to1.z = -p_to.z;
to1.w = -p_to.w;
} else { } else {
to1.x = p_to.x; to1 = p_to;
to1.y = p_to.y;
to1.z = p_to.z;
to1.w = p_to.w;
} }
// calculate coefficients // calculate coefficients
if ((1.0 - cosom) > CMP_EPSILON) { if ((1.0f - cosom) > (real_t)CMP_EPSILON) {
// standard case (slerp) // standard case (slerp)
omega = Math::acos(cosom); omega = Math::acos(cosom);
sinom = Math::sin(omega); sinom = Math::sin(omega);
@ -132,7 +151,7 @@ Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) con
} else { } else {
// "from" and "to" quaternions are very close // "from" and "to" quaternions are very close
// ... so we can do a linear interpolation // ... so we can do a linear interpolation
scale0 = 1.0 - p_weight; scale0 = 1.0f - p_weight;
scale1 = p_weight; scale1 = p_weight;
} }
// calculate final values // calculate final values
@ -145,21 +164,21 @@ Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) con
Quaternion Quaternion::slerpni(const Quaternion &p_to, const real_t &p_weight) const { Quaternion Quaternion::slerpni(const Quaternion &p_to, const real_t &p_weight) const {
#ifdef MATH_CHECKS #ifdef MATH_CHECKS
ERR_FAIL_COND_V(!is_normalized(), Quaternion()); ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized.");
ERR_FAIL_COND_V(!p_to.is_normalized(), Quaternion()); ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
#endif #endif
const Quaternion &from = *this; const Quaternion &from = *this;
real_t dot = from.dot(p_to); real_t dot = from.dot(p_to);
if (Math::abs(dot) > 0.9999) { if (Math::absf(dot) > 0.9999f) {
return from; return from;
} }
real_t theta = Math::acos(dot), real_t theta = Math::acos(dot),
sinT = 1.0 / Math::sin(theta), sinT = 1.0f / Math::sin(theta),
newFactor = Math::sin(p_weight * theta) * sinT, newFactor = Math::sin(p_weight * theta) * sinT,
invFactor = Math::sin((1.0 - p_weight) * theta) * sinT; invFactor = Math::sin((1.0f - p_weight) * theta) * sinT;
return Quaternion(invFactor * from.x + newFactor * p_to.x, return Quaternion(invFactor * from.x + newFactor * p_to.x,
invFactor * from.y + newFactor * p_to.y, invFactor * from.y + newFactor * p_to.y,
@ -167,25 +186,126 @@ Quaternion Quaternion::slerpni(const Quaternion &p_to, const real_t &p_weight) c
invFactor * from.w + newFactor * p_to.w); invFactor * from.w + newFactor * p_to.w);
} }
Quaternion Quaternion::cubic_slerp(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const { Quaternion Quaternion::spherical_cubic_interpolate(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight) const {
#ifdef MATH_CHECKS #ifdef MATH_CHECKS
ERR_FAIL_COND_V(!is_normalized(), Quaternion()); ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized.");
ERR_FAIL_COND_V(!p_b.is_normalized(), Quaternion()); ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
#endif #endif
//the only way to do slerp :| Quaternion from_q = *this;
real_t t2 = (1.0 - p_weight) * p_weight * 2; Quaternion pre_q = p_pre_a;
Quaternion sp = this->slerp(p_b, p_weight); Quaternion to_q = p_b;
Quaternion sq = p_pre_a.slerpni(p_post_b, p_weight); Quaternion post_q = p_post_b;
return sp.slerpni(sq, t2);
// Align flip phases.
from_q = Basis(from_q).get_rotation_quaternion();
pre_q = Basis(pre_q).get_rotation_quaternion();
to_q = Basis(to_q).get_rotation_quaternion();
post_q = Basis(post_q).get_rotation_quaternion();
// Flip quaternions to shortest path if necessary.
bool flip1 = Math::sign(from_q.dot(pre_q));
pre_q = flip1 ? -pre_q : pre_q;
bool flip2 = Math::sign(from_q.dot(to_q));
to_q = flip2 ? -to_q : to_q;
bool flip3 = flip2 ? to_q.dot(post_q) <= 0 : Math::sign(to_q.dot(post_q));
post_q = flip3 ? -post_q : post_q;
// Calc by Expmap in from_q space.
Quaternion ln_from = Quaternion(0, 0, 0, 0);
Quaternion ln_to = (from_q.inverse() * to_q).log();
Quaternion ln_pre = (from_q.inverse() * pre_q).log();
Quaternion ln_post = (from_q.inverse() * post_q).log();
Quaternion ln = Quaternion(0, 0, 0, 0);
ln.x = Math::cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight);
ln.y = Math::cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight);
ln.z = Math::cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight);
Quaternion q1 = from_q * ln.exp();
// Calc by Expmap in to_q space.
ln_from = (to_q.inverse() * from_q).log();
ln_to = Quaternion(0, 0, 0, 0);
ln_pre = (to_q.inverse() * pre_q).log();
ln_post = (to_q.inverse() * post_q).log();
ln = Quaternion(0, 0, 0, 0);
ln.x = Math::cubic_interpolate(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight);
ln.y = Math::cubic_interpolate(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight);
ln.z = Math::cubic_interpolate(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight);
Quaternion q2 = to_q * ln.exp();
// To cancel error made by Expmap ambiguity, do blends.
return q1.slerp(q2, p_weight);
}
Quaternion Quaternion::spherical_cubic_interpolate_in_time(const Quaternion &p_b, const Quaternion &p_pre_a, const Quaternion &p_post_b, const real_t &p_weight,
const real_t &p_b_t, const real_t &p_pre_a_t, const real_t &p_post_b_t) const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized.");
ERR_FAIL_COND_V_MSG(!p_b.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
#endif
Quaternion from_q = *this;
Quaternion pre_q = p_pre_a;
Quaternion to_q = p_b;
Quaternion post_q = p_post_b;
// Align flip phases.
from_q = Basis(from_q).get_rotation_quaternion();
pre_q = Basis(pre_q).get_rotation_quaternion();
to_q = Basis(to_q).get_rotation_quaternion();
post_q = Basis(post_q).get_rotation_quaternion();
// Flip quaternions to shortest path if necessary.
bool flip1 = Math::sign(from_q.dot(pre_q));
pre_q = flip1 ? -pre_q : pre_q;
bool flip2 = Math::sign(from_q.dot(to_q));
to_q = flip2 ? -to_q : to_q;
bool flip3 = flip2 ? to_q.dot(post_q) <= 0 : Math::sign(to_q.dot(post_q));
post_q = flip3 ? -post_q : post_q;
// Calc by Expmap in from_q space.
Quaternion ln_from = Quaternion(0, 0, 0, 0);
Quaternion ln_to = (from_q.inverse() * to_q).log();
Quaternion ln_pre = (from_q.inverse() * pre_q).log();
Quaternion ln_post = (from_q.inverse() * post_q).log();
Quaternion ln = Quaternion(0, 0, 0, 0);
ln.x = Math::cubic_interpolate_in_time(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
ln.y = Math::cubic_interpolate_in_time(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
ln.z = Math::cubic_interpolate_in_time(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
Quaternion q1 = from_q * ln.exp();
// Calc by Expmap in to_q space.
ln_from = (to_q.inverse() * from_q).log();
ln_to = Quaternion(0, 0, 0, 0);
ln_pre = (to_q.inverse() * pre_q).log();
ln_post = (to_q.inverse() * post_q).log();
ln = Quaternion(0, 0, 0, 0);
ln.x = Math::cubic_interpolate_in_time(ln_from.x, ln_to.x, ln_pre.x, ln_post.x, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
ln.y = Math::cubic_interpolate_in_time(ln_from.y, ln_to.y, ln_pre.y, ln_post.y, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
ln.z = Math::cubic_interpolate_in_time(ln_from.z, ln_to.z, ln_pre.z, ln_post.z, p_weight, p_b_t, p_pre_a_t, p_post_b_t);
Quaternion q2 = to_q * ln.exp();
// To cancel error made by Expmap ambiguity, do blends.
return q1.slerp(q2, p_weight);
} }
Quaternion::operator String() const { Quaternion::operator String() const {
return String::num(x, 5) + ", " + String::num(y, 5) + ", " + String::num(z, 5) + ", " + String::num(w, 5); return "(" + String::num_real(x, false) + ", " + String::num_real(y, false) + ", " + String::num_real(z, false) + ", " + String::num_real(w, false) + ")";
}
Vector3 Quaternion::get_axis() const {
if (Math::abs(w) > 1 - CMP_EPSILON) {
return Vector3(x, y, z);
}
real_t r = ((real_t)1) / Math::sqrt(1 - w * w);
return Vector3(x * r, y * r, z * r);
}
real_t Quaternion::get_angle() const {
return 2 * Math::acos(w);
} }
Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) { Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) {
#ifdef MATH_CHECKS #ifdef MATH_CHECKS
ERR_FAIL_COND(!p_axis.is_normalized()); ERR_FAIL_COND_MSG(!p_axis.is_normalized(), "The axis Vector3 must be normalized.");
#endif #endif
real_t d = p_axis.length(); real_t d = p_axis.length();
if (d == 0) { if (d == 0) {
@ -194,8 +314,8 @@ Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) {
z = 0; z = 0;
w = 0; w = 0;
} else { } else {
real_t sin_angle = Math::sin(p_angle * 0.5); real_t sin_angle = Math::sin(p_angle * 0.5f);
real_t cos_angle = Math::cos(p_angle * 0.5); real_t cos_angle = Math::cos(p_angle * 0.5f);
real_t s = sin_angle / d; real_t s = sin_angle / d;
x = p_axis.x * s; x = p_axis.x * s;
y = p_axis.y * s; y = p_axis.y * s;
@ -209,9 +329,9 @@ Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) {
// and similar for other axes. // and similar for other axes.
// This implementation uses YXZ convention (Z is the first rotation). // This implementation uses YXZ convention (Z is the first rotation).
Quaternion::Quaternion(const Vector3 &p_euler) { Quaternion::Quaternion(const Vector3 &p_euler) {
real_t half_a1 = p_euler.y * 0.5; real_t half_a1 = p_euler.y * 0.5f;
real_t half_a2 = p_euler.x * 0.5; real_t half_a2 = p_euler.x * 0.5f;
real_t half_a3 = p_euler.z * 0.5; real_t half_a3 = p_euler.z * 0.5f;
// R = Y(a1).X(a2).Z(a3) convention for Euler angles. // R = Y(a1).X(a2).Z(a3) convention for Euler angles.
// Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-6) // Conversion to quaternion as listed in https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf (page A-6)

87
src/variant/transform3d.cpp
View File

@ -58,13 +58,13 @@ Transform3D Transform3D::inverse() const {
return ret; return ret;
} }
void Transform3D::rotate(const Vector3 &p_axis, real_t p_phi) { void Transform3D::rotate(const Vector3 &p_axis, real_t p_angle) {
*this = rotated(p_axis, p_phi); *this = rotated(p_axis, p_angle);
} }
Transform3D Transform3D::rotated(const Vector3 &p_axis, real_t p_phi) const { Transform3D Transform3D::rotated(const Vector3 &p_axis, real_t p_angle) const {
// Equivalent to left multiplication // Equivalent to left multiplication
Basis p_basis(p_axis, p_phi); Basis p_basis(p_axis, p_angle);
return Transform3D(p_basis * basis, p_basis.xform(origin)); return Transform3D(p_basis * basis, p_basis.xform(origin));
} }
@ -74,51 +74,29 @@ Transform3D Transform3D::rotated_local(const Vector3 &p_axis, real_t p_angle) co
return Transform3D(basis * p_basis, origin); return Transform3D(basis * p_basis, origin);
} }
void Transform3D::rotate_basis(const Vector3 &p_axis, real_t p_phi) { void Transform3D::rotate_basis(const Vector3 &p_axis, real_t p_angle) {
basis.rotate(p_axis, p_phi); basis.rotate(p_axis, p_angle);
} }
Transform3D Transform3D::looking_at(const Vector3 &p_target, const Vector3 &p_up) const { Transform3D Transform3D::looking_at(const Vector3 &p_target, const Vector3 &p_up) const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V_MSG(origin.is_equal_approx(p_target), Transform3D(), "The transform's origin and target can't be equal.");
#endif
Transform3D t = *this; Transform3D t = *this;
t.set_look_at(origin, p_target, p_up); t.basis = Basis::looking_at(p_target - origin, p_up);
return t; return t;
} }
void Transform3D::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up) { void Transform3D::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up) {
#ifdef MATH_CHECKS #ifdef MATH_CHECKS
ERR_FAIL_COND(p_eye == p_target); ERR_FAIL_COND_MSG(p_eye.is_equal_approx(p_target), "The eye and target vectors can't be equal.");
ERR_FAIL_COND(p_up.length() == 0);
#endif #endif
// RefCounted: MESA source code basis = Basis::looking_at(p_target - p_eye, p_up);
Vector3 v_x, v_y, v_z;
/* Make rotation matrix */
/* Z vector */
v_z = p_eye - p_target;
v_z.normalize();
v_y = p_up;
v_x = v_y.cross(v_z);
#ifdef MATH_CHECKS
ERR_FAIL_COND(v_x.length() == 0);
#endif
/* Recompute Y = Z cross X */
v_y = v_z.cross(v_x);
v_x.normalize();
v_y.normalize();
basis.set(v_x, v_y, v_z);
origin = p_eye; origin = p_eye;
} }
Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t p_c) const { Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t p_c) const {
/* not sure if very "efficient" but good enough? */ Transform3D interp;
Vector3 src_scale = basis.get_scale(); Vector3 src_scale = basis.get_scale();
Quaternion src_rot = basis.get_rotation_quaternion(); Quaternion src_rot = basis.get_rotation_quaternion();
@ -128,7 +106,6 @@ Transform3D Transform3D::interpolate_with(const Transform3D &p_transform, real_t
Quaternion dst_rot = p_transform.basis.get_rotation_quaternion(); Quaternion dst_rot = p_transform.basis.get_rotation_quaternion();
Vector3 dst_loc = p_transform.origin; Vector3 dst_loc = p_transform.origin;
Transform3D interp;
interp.basis.set_quaternion_scale(src_rot.slerp(dst_rot, p_c).normalized(), src_scale.lerp(dst_scale, p_c)); interp.basis.set_quaternion_scale(src_rot.slerp(dst_rot, p_c).normalized(), src_scale.lerp(dst_scale, p_c));
interp.origin = src_loc.lerp(dst_loc, p_c); interp.origin = src_loc.lerp(dst_loc, p_c);
@ -154,11 +131,11 @@ void Transform3D::scale_basis(const Vector3 &p_scale) {
basis.scale(p_scale); basis.scale(p_scale);
} }
void Transform3D::translate(real_t p_tx, real_t p_ty, real_t p_tz) { void Transform3D::translate_local(real_t p_tx, real_t p_ty, real_t p_tz) {
translate(Vector3(p_tx, p_ty, p_tz)); translate_local(Vector3(p_tx, p_ty, p_tz));
} }
void Transform3D::translate(const Vector3 &p_translation) { void Transform3D::translate_local(const Vector3 &p_translation) {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
origin[i] += basis[i].dot(p_translation); origin[i] += basis[i].dot(p_translation);
} }
@ -184,6 +161,16 @@ Transform3D Transform3D::orthonormalized() const {
return _copy; return _copy;
} }
void Transform3D::orthogonalize() {
basis.orthogonalize();
}
Transform3D Transform3D::orthogonalized() const {
Transform3D _copy = *this;
_copy.orthogonalize();
return _copy;
}
bool Transform3D::is_equal_approx(const Transform3D &p_transform) const { bool Transform3D::is_equal_approx(const Transform3D &p_transform) const {
return basis.is_equal_approx(p_transform.basis) && origin.is_equal_approx(p_transform.origin); return basis.is_equal_approx(p_transform.basis) && origin.is_equal_approx(p_transform.origin);
} }
@ -207,8 +194,22 @@ Transform3D Transform3D::operator*(const Transform3D &p_transform) const {
return t; return t;
} }
void Transform3D::operator*=(const real_t p_val) {
origin *= p_val;
basis *= p_val;
}
Transform3D Transform3D::operator*(const real_t p_val) const {
Transform3D ret(*this);
ret *= p_val;
return ret;
}
Transform3D::operator String() const { Transform3D::operator String() const {
return basis.operator String() + " - " + origin.operator String(); return "[X: " + basis.get_column(0).operator String() +
", Y: " + basis.get_column(1).operator String() +
", Z: " + basis.get_column(2).operator String() +
", O: " + origin.operator String() + "]";
} }
Transform3D::Transform3D(const Basis &p_basis, const Vector3 &p_origin) : Transform3D::Transform3D(const Basis &p_basis, const Vector3 &p_origin) :
@ -218,9 +219,9 @@ Transform3D::Transform3D(const Basis &p_basis, const Vector3 &p_origin) :
Transform3D::Transform3D(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z, const Vector3 &p_origin) : Transform3D::Transform3D(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z, const Vector3 &p_origin) :
origin(p_origin) { origin(p_origin) {
basis.set_axis(0, p_x); basis.set_column(0, p_x);
basis.set_axis(1, p_y); basis.set_column(1, p_y);
basis.set_axis(2, p_z); basis.set_column(2, p_z);
} }
Transform3D::Transform3D(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, real_t ox, real_t oy, real_t oz) { Transform3D::Transform3D(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, real_t ox, real_t oy, real_t oz) {