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

Update Basis/Transform3D/Quaternion to match the engine

pull/859/head
Aaron Franke 2022-09-19 18:15:04 -05:00
parent 8670305589
commit e83d472c00
No known key found for this signature in database
GPG Key ID: 40A1750B977E56BF
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;
}
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) {
if (is_equal_approx(static_cast<real_t>(p_from), static_cast<real_t>(p_to))) {
return p_from;

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

@ -49,10 +49,10 @@ public:
Vector3(0, 0, 1)
};
inline const Vector3 &operator[](int axis) const {
_FORCE_INLINE_ const Vector3 &operator[](int axis) const {
return rows[axis];
}
inline Vector3 &operator[](int axis) {
_FORCE_INLINE_ Vector3 &operator[](int axis) {
return rows[axis];
}
@ -62,67 +62,53 @@ public:
Basis inverse() 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);
inline Vector3 get_axis(int p_axis) const {
// get actual basis axis (rows is transposed for performance)
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_angle);
Basis rotated(const Vector3 &p_axis, real_t p_angle) const;
void rotate(const Vector3 &p_axis, real_t p_phi);
Basis rotated(const Vector3 &p_axis, real_t p_phi) const;
void rotate_local(const Vector3 &p_axis, real_t p_angle);
Basis rotated_local(const Vector3 &p_axis, real_t p_angle) const;
void rotate_local(const Vector3 &p_axis, real_t p_phi);
Basis rotated_local(const Vector3 &p_axis, real_t p_phi) const;
void rotate(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ);
Basis rotated(const Vector3 &p_euler, EulerOrder p_order = EULER_ORDER_YXZ) const;
void rotate(const Vector3 &p_euler);
Basis rotated(const Vector3 &p_euler) const;
void rotate(const Quaternion &p_quaternion);
Basis rotated(const Quaternion &p_quaternion) const;
void rotate(const Quaternion &p_quat);
Basis rotated(const Quaternion &p_quat) const;
Vector3 get_rotation_euler() const;
Vector3 get_euler_normalized(EulerOrder p_order = EULER_ORDER_YXZ) 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;
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 get_euler_xyz() const;
void set_euler_xyz(const Vector3 &p_euler);
Vector3 get_euler_xzy() const;
void set_euler_xzy(const Vector3 &p_euler);
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);
Vector3 get_euler(EulerOrder p_order = EULER_ORDER_YXZ) const;
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) {
Basis b;
b.set_euler(p_euler, p_order);
return b;
}
Quaternion get_quaternion() const;
void set_quaternion(const Quaternion &p_quat);
Vector3 get_euler() const { return get_euler_yxz(); }
void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); }
void set_quaternion(const Quaternion &p_quaternion);
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);
Basis scaled(const Vector3 &p_scale) const;
@ -130,6 +116,9 @@ public:
void scale_local(const Vector3 &p_scale);
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();
float get_uniform_scale() const;
@ -137,18 +126,18 @@ public:
Vector3 get_scale_abs() 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_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale);
void set_quaternion_scale(const Quaternion &p_quat, 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, EulerOrder p_order = EULER_ORDER_YXZ);
void set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 &p_scale);
// 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];
}
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];
}
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];
}
@ -157,26 +146,22 @@ public:
bool operator==(const Basis &p_matrix) const;
bool operator!=(const Basis &p_matrix) const;
inline Vector3 xform(const Vector3 &p_vector) const;
inline Vector3 xform_inv(const Vector3 &p_vector) const;
inline void operator*=(const Basis &p_matrix);
inline Basis operator*(const Basis &p_matrix) const;
inline void operator+=(const Basis &p_matrix);
inline Basis operator+(const Basis &p_matrix) const;
inline void operator-=(const Basis &p_matrix);
inline Basis operator-(const Basis &p_matrix) const;
inline void operator*=(real_t p_val);
inline Basis operator*(real_t p_val) const;
int get_orthogonal_index() const;
void set_orthogonal_index(int p_index);
void set_diagonal(const Vector3 &p_diag);
_FORCE_INLINE_ Vector3 xform(const Vector3 &p_vector) const;
_FORCE_INLINE_ Vector3 xform_inv(const Vector3 &p_vector) const;
_FORCE_INLINE_ void operator*=(const Basis &p_matrix);
_FORCE_INLINE_ Basis operator*(const Basis &p_matrix) const;
_FORCE_INLINE_ void operator+=(const Basis &p_matrix);
_FORCE_INLINE_ Basis operator+(const Basis &p_matrix) const;
_FORCE_INLINE_ void operator-=(const Basis &p_matrix);
_FORCE_INLINE_ Basis operator-(const Basis &p_matrix) const;
_FORCE_INLINE_ void operator*=(const real_t p_val);
_FORCE_INLINE_ Basis operator*(const real_t p_val) const;
bool is_orthogonal() const;
bool is_diagonal() 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;
void rotate_sh(real_t *p_values);
@ -184,7 +169,7 @@ public:
/* 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][1] = xy;
rows[0][2] = xz;
@ -195,35 +180,35 @@ public:
rows[2][1] = zy;
rows[2][2] = zz;
}
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);
}
inline Vector3 get_column(int i) const {
return Vector3(rows[0][i], rows[1][i], rows[2][i]);
_FORCE_INLINE_ void set_columns(const Vector3 &p_x, const Vector3 &p_y, const Vector3 &p_z) {
set_column(0, p_x);
set_column(1, p_y);
set_column(2, p_z);
}
inline Vector3 get_row(int i) const {
return Vector3(rows[i][0], rows[i][1], rows[i][2]);
_FORCE_INLINE_ Vector3 get_column(int p_index) const {
// 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]);
}
inline void set_row(int i, const Vector3 &p_row) {
rows[i][0] = p_row.x;
rows[i][1] = p_row.y;
rows[i][2] = p_row.z;
}
inline void set_zero() {
_FORCE_INLINE_ void set_zero() {
rows[0].zero();
rows[1].zero();
rows[2].zero();
}
inline Basis transpose_xform(const Basis &m) const {
_FORCE_INLINE_ Basis transpose_xform(const Basis &m) const {
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].y + rows[1].x * m[1].y + rows[2].x * m[2].y,
@ -242,6 +227,9 @@ public:
void orthonormalize();
Basis orthonormalized() const;
void orthogonalize();
Basis orthogonalized() const;
#ifdef MATH_CHECKS
bool is_symmetric() const;
#endif
@ -249,69 +237,71 @@ public:
operator Quaternion() const { return get_quaternion(); }
Basis(const Quaternion &p_quat) { set_quaternion(p_quat); };
Basis(const Quaternion &p_quat, const Vector3 &p_scale) { set_quaternion_scale(p_quat, p_scale); }
static Basis looking_at(const Vector3 &p_target, const Vector3 &p_up = Vector3(0, 1, 0));
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 Quaternion &p_quaternion) { set_quaternion(p_quaternion); };
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_phi, const Vector3 &p_scale) { set_axis_angle_scale(p_axis, p_phi, p_scale); }
Basis(const Vector3 &p_axis, real_t p_angle) { set_axis_angle(p_axis, p_angle); }
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) {
set_axis(0, p_x);
set_axis(1, p_y);
set_axis(2, p_z);
_FORCE_INLINE_ Basis(const Vector3 &p_x_axis, const Vector3 &p_y_axis, const Vector3 &p_z_axis) {
set_columns(p_x_axis, p_y_axis, p_z_axis);
}
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(
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[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(
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[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[1] += p_matrix.rows[1];
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);
ret += p_matrix;
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[1] -= p_matrix.rows[1];
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);
ret -= p_matrix;
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[1] *= 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);
ret *= p_val;
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. */
/*************************************************************************/
#ifndef GODOT_QUAT_HPP
#define GODOT_QUAT_HPP
#ifndef GODOT_QUATERNION_HPP
#define GODOT_QUATERNION_HPP
#include <godot_cpp/core/math.hpp>
#include <godot_cpp/variant/vector3.hpp>
@ -52,20 +52,23 @@ public:
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];
}
inline const real_t &operator[](int idx) const {
_FORCE_INLINE_ const real_t &operator[](int idx) const {
return components[idx];
}
inline real_t length_squared() const;
bool is_equal_approx(const Quaternion &p_quat) const;
_FORCE_INLINE_ real_t length_squared() const;
bool is_equal_approx(const Quaternion &p_quaternion) const;
real_t length() const;
void normalize();
Quaternion normalized() const;
bool is_normalized() 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_yxz() const;
@ -73,9 +76,13 @@ public:
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 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);
real_t r = ((real_t)1) / Math::sqrt(1 - w * w);
r_axis.x = x * r;
@ -86,44 +93,37 @@ public:
void operator*=(const Quaternion &p_q);
Quaternion operator*(const Quaternion &p_q) const;
Quaternion operator*(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 {
_FORCE_INLINE_ Vector3 xform(const Vector3 &v) const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V(!is_normalized(), v);
ERR_FAIL_COND_V_MSG(!is_normalized(), v, "The quaternion must be normalized.");
#endif
Vector3 u(x, y, z);
Vector3 uv = u.cross(v);
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);
}
inline void operator+=(const Quaternion &p_q);
inline void operator-=(const Quaternion &p_q);
inline void operator*=(const real_t &s);
inline void operator/=(const real_t &s);
inline Quaternion operator+(const Quaternion &q2) const;
inline Quaternion operator-(const Quaternion &q2) const;
inline Quaternion operator-() const;
inline Quaternion operator*(const real_t &s) const;
inline Quaternion operator/(const real_t &s) const;
_FORCE_INLINE_ void operator+=(const Quaternion &p_q);
_FORCE_INLINE_ void operator-=(const Quaternion &p_q);
_FORCE_INLINE_ void operator*=(const real_t &s);
_FORCE_INLINE_ void operator/=(const real_t &s);
_FORCE_INLINE_ Quaternion operator+(const Quaternion &q2) const;
_FORCE_INLINE_ Quaternion operator-(const Quaternion &q2) const;
_FORCE_INLINE_ Quaternion operator-() const;
_FORCE_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;
inline bool operator!=(const Quaternion &p_quat) const;
_FORCE_INLINE_ bool operator==(const Quaternion &p_quaternion) const;
_FORCE_INLINE_ bool operator!=(const Quaternion &p_quaternion) 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),
y(p_y),
z(p_z),
@ -141,12 +141,11 @@ public:
w(p_q.w) {
}
Quaternion &operator=(const Quaternion &p_q) {
void operator=(const Quaternion &p_q) {
x = p_q.x;
y = p_q.y;
z = p_q.z;
w = p_q.w;
return *this;
}
Quaternion(const Vector3 &v0, const Vector3 &v1) // shortest arc
@ -154,19 +153,19 @@ public:
Vector3 c = v0.cross(v1);
real_t d = v0.dot(v1);
if (d < (real_t)-1.0 + CMP_EPSILON) {
x = (real_t)0.0;
y = (real_t)1.0;
z = (real_t)0.0;
w = (real_t)0.0;
if (d < -1.0f + (real_t)CMP_EPSILON) {
x = 0;
y = 1;
z = 0;
w = 0;
} else {
real_t s = Math::sqrt(((real_t)1.0 + d) * (real_t)2.0);
real_t rs = (real_t)1.0 / s;
real_t s = Math::sqrt((1.0f + d) * 2.0f);
real_t rs = 1.0f / s;
x = c.x * rs;
y = c.y * 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) {
*this *= (real_t)1.0 / s;
*this *= 1.0f / s;
}
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 {
return *this * ((real_t)1.0 / s);
return *this * (1.0f / s);
}
bool Quaternion::operator==(const Quaternion &p_quat) const {
return x == p_quat.x && y == p_quat.y && z == p_quat.z && w == p_quat.w;
bool Quaternion::operator==(const Quaternion &p_quaternion) const {
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 {
return x != p_quat.x || y != p_quat.y || z != p_quat.z || w != p_quat.w;
bool Quaternion::operator!=(const Quaternion &p_quaternion) const {
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) {
return p_quat * p_real;
_FORCE_INLINE_ Quaternion operator*(const real_t &p_real, const Quaternion &p_quaternion) {
return p_quaternion * p_real;
}
} // 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();
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;
void rotate(const Vector3 &p_axis, real_t p_phi);
void rotate_basis(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_angle);
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;
@ -67,8 +67,8 @@ public:
Transform3D scaled(const Vector3 &p_scale) const;
Transform3D scaled_local(const Vector3 &p_scale) const;
void scale_basis(const Vector3 &p_scale);
void translate(real_t p_tx, real_t p_ty, real_t p_tz);
void translate(const Vector3 &p_translation);
void translate_local(real_t p_tx, real_t p_ty, real_t p_tz);
void translate_local(const Vector3 &p_translation);
Transform3D translated(const Vector3 &p_translation) const;
Transform3D translated_local(const Vector3 &p_translation) const;
@ -80,29 +80,41 @@ public:
void orthonormalize();
Transform3D orthonormalized() const;
void orthogonalize();
Transform3D orthogonalized() const;
bool is_equal_approx(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;
inline Vector3 xform_inv(const Vector3 &p_vector) const;
_FORCE_INLINE_ Vector3 xform(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;
inline Plane xform_inv(const Plane &p_plane) const;
// NOTE: These are UNSAFE with non-uniform scaling, and will produce incorrect results.
// 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;
inline AABB xform_inv(const AABB &p_aabb) const;
// Safe with non-uniform scaling (uses affine_inverse).
_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;
inline PackedVector3Array xform_inv(const PackedVector3Array &p_array) const;
// These fast versions use precomputed affine inverse, and should be used in bottleneck areas where
// 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);
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;
inline Transform3D inverse_xform(const Transform3D &t) const {
_FORCE_INLINE_ Transform3D inverse_xform(const Transform3D &t) const {
Vector3 v = t.origin - origin;
return Transform3D(basis.transpose_xform(t.basis),
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);
};
inline Vector3 Transform3D::xform(const Vector3 &p_vector) const {
_FORCE_INLINE_ Vector3 Transform3D::xform(const Vector3 &p_vector) const {
return Vector3(
basis[0].dot(p_vector) + origin.x,
basis[1].dot(p_vector) + origin.y,
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;
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));
}
inline Plane Transform3D::xform(const Plane &p_plane) const {
Vector3 point = p_plane.normal * p_plane.d;
Vector3 point_dir = point + p_plane.normal;
point = xform(point);
point_dir = xform(point_dir);
Vector3 normal = point_dir - point;
normal.normalize();
real_t d = normal.dot(point);
return Plane(normal, d);
// Neither the plane regular xform or xform_inv are particularly efficient,
// as they do a basis inverse. For xforming a large number
// of planes it is better to pre-calculate the inverse transpose basis once
// and reuse it for each plane, by using the 'fast' version of the functions.
_FORCE_INLINE_ Plane Transform3D::xform(const Plane &p_plane) const {
Basis b = basis.inverse();
b.transpose();
return xform_fast(p_plane, b);
}
inline Plane Transform3D::xform_inv(const Plane &p_plane) const {
Vector3 point = p_plane.normal * p_plane.d;
Vector3 point_dir = point + p_plane.normal;
point = xform_inv(point);
point_dir = xform_inv(point_dir);
Vector3 normal = point_dir - point;
normal.normalize();
real_t d = normal.dot(point);
return Plane(normal, d);
_FORCE_INLINE_ Plane Transform3D::xform_inv(const Plane &p_plane) const {
Transform3D inv = affine_inverse();
Basis basis_transpose = basis.transposed();
return xform_inv_fast(p_plane, inv, basis_transpose);
}
inline AABB Transform3D::xform(const AABB &p_aabb) const {
/* http://dev.theomader.com/transform-bounding-boxes/ */
_FORCE_INLINE_ AABB Transform3D::xform(const AABB &p_aabb) const {
/* https://dev.theomader.com/transform-bounding-boxes/ */
Vector3 min = p_aabb.position;
Vector3 max = p_aabb.position + p_aabb.size;
Vector3 tmin, tmax;
@ -190,7 +192,7 @@ inline AABB Transform3D::xform(const AABB &p_aabb) const {
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 */
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),
@ -218,8 +220,11 @@ PackedVector3Array Transform3D::xform(const PackedVector3Array &p_array) const {
PackedVector3Array array;
array.resize(p_array.size());
const Vector3 *r = p_array.ptr();
Vector3 *w = array.ptrw();
for (int i = 0; i < p_array.size(); ++i) {
array[i] = xform(p_array[i]);
w[i] = xform(r[i]);
}
return array;
}
@ -228,12 +233,48 @@ PackedVector3Array Transform3D::xform_inv(const PackedVector3Array &p_array) con
PackedVector3Array array;
array.resize(p_array.size());
const Vector3 *r = p_array.ptr();
Vector3 *w = array.ptrw();
for (int i = 0; i < p_array.size(); ++i) {
array[i] = xform_inv(p_array[i]);
w[i] = xform_inv(r[i]);
}
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
#endif // GODOT_TRANSFORM_HPP
#endif // GODOT_TRANSFORM3D_HPP

643
src/variant/basis.cpp
View File

@ -38,16 +38,16 @@
namespace godot {
void Basis::from_z(const Vector3 &p_z) {
if (Math::abs(p_z.z) > Math_SQRT12) {
if (Math::abs(p_z.z) > (real_t)Math_SQRT12) {
// choose p in y-z plane
real_t a = p_z[1] * p_z[1] + p_z[2] * p_z[2];
real_t k = 1.0 / Math::sqrt(a);
real_t k = 1.0f / Math::sqrt(a);
rows[0] = Vector3(0, -p_z[2] * k, p_z[1] * k);
rows[1] = Vector3(a * k, -p_z[0] * rows[0][2], p_z[0] * rows[0][1]);
} else {
// choose p in x-y plane
real_t a = p_z.x * p_z.x + p_z.y * p_z.y;
real_t k = 1.0 / Math::sqrt(a);
real_t k = 1.0f / Math::sqrt(a);
rows[0] = Vector3(-p_z.y * k, p_z.x * k, 0);
rows[1] = Vector3(-p_z.z * rows[0].y, p_z.z * rows[0].x, a * k);
}
@ -64,7 +64,7 @@ void Basis::invert() {
#ifdef MATH_CHECKS
ERR_FAIL_COND(det == 0);
#endif
real_t s = 1.0 / det;
real_t s = 1.0f / det;
set(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
@ -74,9 +74,9 @@ void Basis::invert() {
void Basis::orthonormalize() {
// Gram-Schmidt Process
Vector3 x = get_axis(0);
Vector3 y = get_axis(1);
Vector3 z = get_axis(2);
Vector3 x = get_column(0);
Vector3 y = get_column(1);
Vector3 z = get_column(2);
x.normalize();
y = (y - x * (x.dot(y)));
@ -84,9 +84,9 @@ void Basis::orthonormalize() {
z = (z - x * (x.dot(z)) - y * (y.dot(z)));
z.normalize();
set_axis(0, x);
set_axis(1, y);
set_axis(2, z);
set_column(0, x);
set_column(1, y);
set_column(2, z);
}
Basis Basis::orthonormalized() const {
@ -95,6 +95,18 @@ Basis Basis::orthonormalized() const {
return c;
}
void Basis::orthogonalize() {
Vector3 scl = get_scale();
orthonormalize();
scale_local(scl);
}
Basis Basis::orthogonalized() const {
Basis c = *this;
c.orthogonalize();
return c;
}
bool Basis::is_orthogonal() const {
Basis identity;
Basis m = (*this) * transposed();
@ -132,7 +144,7 @@ bool Basis::is_symmetric() const {
Basis Basis::diagonalize() {
// NOTE: only implemented for symmetric matrices
// with the Jacobi iterative method method
// with the Jacobi iterative method
#ifdef MATH_CHECKS
ERR_FAIL_COND_V(!is_symmetric(), Basis());
#endif
@ -142,7 +154,7 @@ Basis Basis::diagonalize() {
int ite = 0;
Basis acc_rot;
while (off_matrix_norm_2 > CMP_EPSILON2 && ite++ < ite_max) {
while (off_matrix_norm_2 > (real_t)CMP_EPSILON2 && ite++ < ite_max) {
real_t el01_2 = rows[0][1] * rows[0][1];
real_t el02_2 = rows[0][2] * rows[0][2];
real_t el12_2 = rows[1][2] * rows[1][2];
@ -171,7 +183,7 @@ Basis Basis::diagonalize() {
if (Math::is_equal_approx(rows[j][j], rows[i][i])) {
angle = Math_PI / 4;
} else {
angle = 0.5 * Math::atan(2 * rows[i][j] / (rows[j][j] - rows[i][i]));
angle = 0.5f * Math::atan(2 * rows[i][j] / (rows[j][j] - rows[i][i]));
}
// Compute the rotation matrix
@ -208,6 +220,10 @@ Basis Basis::transposed() const {
return tr;
}
Basis Basis::from_scale(const Vector3 &p_scale) {
return Basis(p_scale.x, 0, 0, 0, p_scale.y, 0, 0, 0, p_scale.z);
}
// Multiplies the matrix from left by the scaling matrix: M -> S.M
// See the comment for Basis::rotated for further explanation.
void Basis::scale(const Vector3 &p_scale) {
@ -234,12 +250,30 @@ void Basis::scale_local(const Vector3 &p_scale) {
*this = scaled_local(p_scale);
}
void Basis::scale_orthogonal(const Vector3 &p_scale) {
*this = scaled_orthogonal(p_scale);
}
Basis Basis::scaled_orthogonal(const Vector3 &p_scale) const {
Basis m = *this;
Vector3 s = Vector3(-1, -1, -1) + p_scale;
Vector3 dots;
Basis b;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
dots[j] += s[i] * abs(m.get_column(i).normalized().dot(b.get_column(j)));
}
}
m.scale_local(Vector3(1, 1, 1) + dots);
return m;
}
float Basis::get_uniform_scale() const {
return (rows[0].length() + rows[1].length() + rows[2].length()) / 3.0;
return (rows[0].length() + rows[1].length() + rows[2].length()) / 3.0f;
}
void Basis::make_scale_uniform() {
float l = (rows[0].length() + rows[1].length() + rows[2].length()) / 3.0;
float l = (rows[0].length() + rows[1].length() + rows[2].length()) / 3.0f;
for (int i = 0; i < 3; i++) {
rows[i].normalize();
rows[i] *= l;
@ -247,10 +281,7 @@ void Basis::make_scale_uniform() {
}
Basis Basis::scaled_local(const Vector3 &p_scale) const {
Basis b;
b.set_diagonal(p_scale);
return (*this) * b;
return (*this) * Basis::from_scale(p_scale);
}
Vector3 Basis::get_scale_abs() const {
@ -261,7 +292,7 @@ Vector3 Basis::get_scale_abs() const {
}
Vector3 Basis::get_scale_local() const {
real_t det_sign = Math::sign(determinant());
real_t det_sign = SIGN(determinant());
return det_sign * Vector3(rows[0].length(), rows[1].length(), rows[2].length());
}
@ -287,11 +318,8 @@ Vector3 Basis::get_scale() const {
// matrix rows.
//
// The rotation part of this decomposition is returned by get_rotation* functions.
real_t det_sign = Math::sign(determinant());
return det_sign * Vector3(
Vector3(rows[0][0], rows[1][0], rows[2][0]).length(),
Vector3(rows[0][1], rows[1][1], rows[2][1]).length(),
Vector3(rows[0][2], rows[1][2], rows[2][2]).length());
real_t det_sign = SIGN(determinant());
return det_sign * get_scale_abs();
}
// Decomposes a Basis into a rotation-reflection matrix (an element of the group O(3)) and a positive scaling matrix as B = O.S.
@ -317,44 +345,44 @@ Vector3 Basis::rotref_posscale_decomposition(Basis &rotref) const {
// Multiplies the matrix from left by the rotation matrix: M -> R.M
// Note that this does *not* rotate the matrix itself.
//
// The main use of Basis is as Transform3D.basis, which is used a the transformation matrix
// The main use of Basis is as Transform.basis, which is used by the transformation matrix
// of 3D object. Rotate here refers to rotation of the object (which is R * (*this)),
// not the matrix itself (which is R * (*this) * R.transposed()).
Basis Basis::rotated(const Vector3 &p_axis, real_t p_phi) const {
return Basis(p_axis, p_phi) * (*this);
Basis Basis::rotated(const Vector3 &p_axis, real_t p_angle) const {
return Basis(p_axis, p_angle) * (*this);
}
void Basis::rotate(const Vector3 &p_axis, real_t p_phi) {
*this = rotated(p_axis, p_phi);
void Basis::rotate(const Vector3 &p_axis, real_t p_angle) {
*this = rotated(p_axis, p_angle);
}
void Basis::rotate_local(const Vector3 &p_axis, real_t p_phi) {
void Basis::rotate_local(const Vector3 &p_axis, real_t p_angle) {
// performs a rotation in object-local coordinate system:
// M -> (M.R.Minv).M = M.R.
*this = rotated_local(p_axis, p_phi);
*this = rotated_local(p_axis, p_angle);
}
Basis Basis::rotated_local(const Vector3 &p_axis, real_t p_phi) const {
return (*this) * Basis(p_axis, p_phi);
Basis Basis::rotated_local(const Vector3 &p_axis, real_t p_angle) const {
return (*this) * Basis(p_axis, p_angle);
}
Basis Basis::rotated(const Vector3 &p_euler) const {
return Basis(p_euler) * (*this);
Basis Basis::rotated(const Vector3 &p_euler, EulerOrder p_order) const {
return Basis::from_euler(p_euler, p_order) * (*this);
}
void Basis::rotate(const Vector3 &p_euler) {
*this = rotated(p_euler);
void Basis::rotate(const Vector3 &p_euler, EulerOrder p_order) {
*this = rotated(p_euler, p_order);
}
Basis Basis::rotated(const Quaternion &p_quat) const {
return Basis(p_quat) * (*this);
Basis Basis::rotated(const Quaternion &p_quaternion) const {
return Basis(p_quaternion) * (*this);
}
void Basis::rotate(const Quaternion &p_quat) {
*this = rotated(p_quat);
void Basis::rotate(const Quaternion &p_quaternion) {
*this = rotated(p_quaternion);
}
Vector3 Basis::get_rotation_euler() const {
Vector3 Basis::get_euler_normalized(EulerOrder p_order) const {
// Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S,
// and returns the Euler angles corresponding to the rotation part, complementing get_scale().
// See the comment in get_scale() for further information.
@ -365,7 +393,7 @@ Vector3 Basis::get_rotation_euler() const {
m.scale(Vector3(-1, -1, -1));
}
return m.get_euler();
return m.get_euler(p_order);
}
Quaternion Basis::get_rotation_quaternion() const {
@ -382,6 +410,18 @@ Quaternion Basis::get_rotation_quaternion() const {
return m.get_quaternion();
}
void Basis::rotate_to_align(Vector3 p_start_direction, Vector3 p_end_direction) {
// Takes two vectors and rotates the basis from the first vector to the second vector.
// Adopted from: https://gist.github.com/kevinmoran/b45980723e53edeb8a5a43c49f134724
const Vector3 axis = p_start_direction.cross(p_end_direction).normalized();
if (axis.length_squared() != 0) {
real_t dot = p_start_direction.dot(p_end_direction);
dot = CLAMP(dot, -1.0f, 1.0f);
const real_t angle_rads = Math::acos(dot);
set_axis_angle(axis, angle_rads);
}
}
void Basis::get_rotation_axis_angle(Vector3 &p_axis, real_t &p_angle) const {
// Assumes that the matrix can be decomposed into a proper rotation and scaling matrix as M = R.S,
// and returns the Euler angles corresponding to the rotation part, complementing get_scale().
@ -412,17 +452,9 @@ void Basis::get_rotation_axis_angle_local(Vector3 &p_axis, real_t &p_angle) cons
p_angle = -p_angle;
}
// get_euler_xyz returns a vector containing the Euler angles in the format
// (a1,a2,a3), where a3 is the angle of the first rotation, and a1 is the last
// (following the convention they are commonly defined in the literature).
//
// The current implementation uses XYZ convention (Z is the first rotation),
// so euler.z is the angle of the (first) rotation around Z axis and so on,
//
// And thus, assuming the matrix is a rotation matrix, this function returns
// the angles in the decomposition R = X(a1).Y(a2).Z(a3) where Z(a) rotates
// around the z-axis by a and so on.
Vector3 Basis::get_euler_xyz() const {
Vector3 Basis::get_euler(EulerOrder p_order) const {
switch (p_order) {
case EULER_ORDER_XYZ: {
// Euler angles in XYZ convention.
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
//
@ -432,10 +464,10 @@ Vector3 Basis::get_euler_xyz() const {
Vector3 euler;
real_t sy = rows[0][2];
if (sy < (1.0 - CMP_EPSILON)) {
if (sy > -(1.0 - CMP_EPSILON)) {
if (sy < (1.0f - (real_t)CMP_EPSILON)) {
if (sy > -(1.0f - (real_t)CMP_EPSILON)) {
// is this a pure Y rotation?
if (rows[1][0] == 0.0 && rows[0][1] == 0.0 && rows[1][2] == 0 && rows[2][1] == 0 && rows[1][1] == 1) {
if (rows[1][0] == 0 && rows[0][1] == 0 && rows[1][2] == 0 && rows[2][1] == 0 && rows[1][1] == 1) {
// return the simplest form (human friendlier in editor and scripts)
euler.x = 0;
euler.y = atan2(rows[0][2], rows[0][0]);
@ -447,41 +479,17 @@ Vector3 Basis::get_euler_xyz() const {
}
} else {
euler.x = Math::atan2(rows[2][1], rows[1][1]);
euler.y = -Math_PI / 2.0;
euler.z = 0.0;
euler.y = -Math_PI / 2.0f;
euler.z = 0.0f;
}
} else {
euler.x = Math::atan2(rows[2][1], rows[1][1]);
euler.y = Math_PI / 2.0;
euler.z = 0.0;
euler.y = Math_PI / 2.0f;
euler.z = 0.0f;
}
return euler;
}
// set_euler_xyz expects a vector containing the Euler angles in the format
// (ax,ay,az), where ax is the angle of rotation around x axis,
// and similar for other axes.
// The current implementation uses XYZ convention (Z is the first rotation).
void Basis::set_euler_xyz(const Vector3 &p_euler) {
real_t c, s;
c = Math::cos(p_euler.x);
s = Math::sin(p_euler.x);
Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c);
c = Math::cos(p_euler.y);
s = Math::sin(p_euler.y);
Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c);
c = Math::cos(p_euler.z);
s = Math::sin(p_euler.z);
Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0);
// optimizer will optimize away all this anyway
*this = xmat * (ymat * zmat);
}
Vector3 Basis::get_euler_xzy() const {
} break;
case EULER_ORDER_XZY: {
// Euler angles in XZY convention.
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
//
@ -491,96 +499,26 @@ Vector3 Basis::get_euler_xzy() const {
Vector3 euler;
real_t sz = rows[0][1];
if (sz < (1.0 - CMP_EPSILON)) {
if (sz > -(1.0 - CMP_EPSILON)) {
if (sz < (1.0f - (real_t)CMP_EPSILON)) {
if (sz > -(1.0f - (real_t)CMP_EPSILON)) {
euler.x = Math::atan2(rows[2][1], rows[1][1]);
euler.y = Math::atan2(rows[0][2], rows[0][0]);
euler.z = Math::asin(-sz);
} else {
// It's -1
euler.x = -Math::atan2(rows[1][2], rows[2][2]);
euler.y = 0.0;
euler.z = Math_PI / 2.0;
euler.y = 0.0f;
euler.z = Math_PI / 2.0f;
}
} else {
// It's 1
euler.x = -Math::atan2(rows[1][2], rows[2][2]);
euler.y = 0.0;
euler.z = -Math_PI / 2.0;
euler.y = 0.0f;
euler.z = -Math_PI / 2.0f;
}
return euler;
}
void Basis::set_euler_xzy(const Vector3 &p_euler) {
real_t c, s;
c = Math::cos(p_euler.x);
s = Math::sin(p_euler.x);
Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c);
c = Math::cos(p_euler.y);
s = Math::sin(p_euler.y);
Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c);
c = Math::cos(p_euler.z);
s = Math::sin(p_euler.z);
Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0);
*this = xmat * zmat * ymat;
}
Vector3 Basis::get_euler_yzx() const {
// Euler angles in YZX convention.
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
//
// rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx
// sz cz*cx -cz*sx
// -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx
Vector3 euler;
real_t sz = rows[1][0];
if (sz < (1.0 - CMP_EPSILON)) {
if (sz > -(1.0 - CMP_EPSILON)) {
euler.x = Math::atan2(-rows[1][2], rows[1][1]);
euler.y = Math::atan2(-rows[2][0], rows[0][0]);
euler.z = Math::asin(sz);
} else {
// It's -1
euler.x = Math::atan2(rows[2][1], rows[2][2]);
euler.y = 0.0;
euler.z = -Math_PI / 2.0;
}
} else {
// It's 1
euler.x = Math::atan2(rows[2][1], rows[2][2]);
euler.y = 0.0;
euler.z = Math_PI / 2.0;
}
return euler;
}
void Basis::set_euler_yzx(const Vector3 &p_euler) {
real_t c, s;
c = Math::cos(p_euler.x);
s = Math::sin(p_euler.x);
Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c);
c = Math::cos(p_euler.y);
s = Math::sin(p_euler.y);
Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c);
c = Math::cos(p_euler.z);
s = Math::sin(p_euler.z);
Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0);
*this = ymat * zmat * xmat;
}
// get_euler_yxz returns a vector containing the Euler angles in the YXZ convention,
// as in first-Z, then-X, last-Y. The angles for X, Y, and Z rotations are returned
// as the x, y, and z components of a Vector3 respectively.
Vector3 Basis::get_euler_yxz() const {
} break;
case EULER_ORDER_YXZ: {
// Euler angles in YXZ convention.
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
//
@ -592,8 +530,8 @@ Vector3 Basis::get_euler_yxz() const {
real_t m12 = rows[1][2];
if (m12 < (1 - CMP_EPSILON)) {
if (m12 > -(1 - CMP_EPSILON)) {
if (m12 < (1 - (real_t)CMP_EPSILON)) {
if (m12 > -(1 - (real_t)CMP_EPSILON)) {
// is this a pure X rotation?
if (rows[1][0] == 0 && rows[0][1] == 0 && rows[0][2] == 0 && rows[2][0] == 0 && rows[0][0] == 1) {
// return the simplest form (human friendlier in editor and scripts)
@ -606,43 +544,48 @@ Vector3 Basis::get_euler_yxz() const {
euler.z = atan2(rows[1][0], rows[1][1]);
}
} else { // m12 == -1
euler.x = Math_PI * 0.5;
euler.x = Math_PI * 0.5f;
euler.y = atan2(rows[0][1], rows[0][0]);
euler.z = 0;
}
} else { // m12 == 1
euler.x = -Math_PI * 0.5;
euler.x = -Math_PI * 0.5f;
euler.y = -atan2(rows[0][1], rows[0][0]);
euler.z = 0;
}
return euler;
} break;
case EULER_ORDER_YZX: {
// Euler angles in YZX convention.
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
//
// rot = cy*cz sy*sx-cy*cx*sz cx*sy+cy*sz*sx
// sz cz*cx -cz*sx
// -cz*sy cy*sx+cx*sy*sz cy*cx-sy*sz*sx
Vector3 euler;
real_t sz = rows[1][0];
if (sz < (1.0f - (real_t)CMP_EPSILON)) {
if (sz > -(1.0f - (real_t)CMP_EPSILON)) {
euler.x = Math::atan2(-rows[1][2], rows[1][1]);
euler.y = Math::atan2(-rows[2][0], rows[0][0]);
euler.z = Math::asin(sz);
} else {
// It's -1
euler.x = Math::atan2(rows[2][1], rows[2][2]);
euler.y = 0.0f;
euler.z = -Math_PI / 2.0f;
}
// set_euler_yxz expects a vector containing the Euler angles in the format
// (ax,ay,az), where ax is the angle of rotation around x axis,
// and similar for other axes.
// The current implementation uses YXZ convention (Z is the first rotation).
void Basis::set_euler_yxz(const Vector3 &p_euler) {
real_t c, s;
c = Math::cos(p_euler.x);
s = Math::sin(p_euler.x);
Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c);
c = Math::cos(p_euler.y);
s = Math::sin(p_euler.y);
Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c);
c = Math::cos(p_euler.z);
s = Math::sin(p_euler.z);
Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0);
// optimizer will optimize away all this anyway
*this = ymat * xmat * zmat;
} else {
// It's 1
euler.x = Math::atan2(rows[2][1], rows[2][2]);
euler.y = 0.0f;
euler.z = Math_PI / 2.0f;
}
Vector3 Basis::get_euler_zxy() const {
return euler;
} break;
case EULER_ORDER_ZXY: {
// Euler angles in ZXY convention.
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
//
@ -651,45 +594,26 @@ Vector3 Basis::get_euler_zxy() const {
// -cx*sy sx cx*cy
Vector3 euler;
real_t sx = rows[2][1];
if (sx < (1.0 - CMP_EPSILON)) {
if (sx > -(1.0 - CMP_EPSILON)) {
if (sx < (1.0f - (real_t)CMP_EPSILON)) {
if (sx > -(1.0f - (real_t)CMP_EPSILON)) {
euler.x = Math::asin(sx);
euler.y = Math::atan2(-rows[2][0], rows[2][2]);
euler.z = Math::atan2(-rows[0][1], rows[1][1]);
} else {
// It's -1
euler.x = -Math_PI / 2.0;
euler.x = -Math_PI / 2.0f;
euler.y = Math::atan2(rows[0][2], rows[0][0]);
euler.z = 0;
}
} else {
// It's 1
euler.x = Math_PI / 2.0;
euler.x = Math_PI / 2.0f;
euler.y = Math::atan2(rows[0][2], rows[0][0]);
euler.z = 0;
}
return euler;
}
void Basis::set_euler_zxy(const Vector3 &p_euler) {
real_t c, s;
c = Math::cos(p_euler.x);
s = Math::sin(p_euler.x);
Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c);
c = Math::cos(p_euler.y);
s = Math::sin(p_euler.y);
Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c);
c = Math::cos(p_euler.z);
s = Math::sin(p_euler.z);
Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0);
*this = zmat * xmat * ymat;
}
Vector3 Basis::get_euler_zyx() const {
} break;
case EULER_ORDER_ZYX: {
// Euler angles in ZYX convention.
// See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix
//
@ -698,42 +622,70 @@ Vector3 Basis::get_euler_zyx() const {
// -sy cy*sx cy*cx
Vector3 euler;
real_t sy = rows[2][0];
if (sy < (1.0 - CMP_EPSILON)) {
if (sy > -(1.0 - CMP_EPSILON)) {
if (sy < (1.0f - (real_t)CMP_EPSILON)) {
if (sy > -(1.0f - (real_t)CMP_EPSILON)) {
euler.x = Math::atan2(rows[2][1], rows[2][2]);
euler.y = Math::asin(-sy);
euler.z = Math::atan2(rows[1][0], rows[0][0]);
} else {
// It's -1
euler.x = 0;
euler.y = Math_PI / 2.0;
euler.y = Math_PI / 2.0f;
euler.z = -Math::atan2(rows[0][1], rows[1][1]);
}
} else {
// It's 1
euler.x = 0;
euler.y = -Math_PI / 2.0;
euler.y = -Math_PI / 2.0f;
euler.z = -Math::atan2(rows[0][1], rows[1][1]);
}
return euler;
} break;
default: {
ERR_FAIL_V_MSG(Vector3(), "Invalid parameter for get_euler(order)");
}
}
return Vector3();
}
void Basis::set_euler_zyx(const Vector3 &p_euler) {
void Basis::set_euler(const Vector3 &p_euler, EulerOrder p_order) {
real_t c, s;
c = Math::cos(p_euler.x);
s = Math::sin(p_euler.x);
Basis xmat(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c);
Basis xmat(1, 0, 0, 0, c, -s, 0, s, c);
c = Math::cos(p_euler.y);
s = Math::sin(p_euler.y);
Basis ymat(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c);
Basis ymat(c, 0, s, 0, 1, 0, -s, 0, c);
c = Math::cos(p_euler.z);
s = Math::sin(p_euler.z);
Basis zmat(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0);
Basis zmat(c, -s, 0, s, c, 0, 0, 0, 1);
switch (p_order) {
case EULER_ORDER_XYZ: {
*this = xmat * (ymat * zmat);
} break;
case EULER_ORDER_XZY: {
*this = xmat * zmat * ymat;
} break;
case EULER_ORDER_YXZ: {
*this = ymat * xmat * zmat;
} break;
case EULER_ORDER_YZX: {
*this = ymat * zmat * xmat;
} break;
case EULER_ORDER_ZXY: {
*this = zmat * xmat * ymat;
} break;
case EULER_ORDER_ZYX: {
*this = zmat * ymat * xmat;
} break;
default: {
ERR_FAIL_MSG("Invalid order parameter for set_euler(vec3,order)");
}
}
}
bool Basis::is_equal_approx(const Basis &p_basis) const {
@ -757,47 +709,38 @@ bool Basis::operator!=(const Basis &p_matrix) const {
}
Basis::operator String() const {
String mtx;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
if (i != 0 || j != 0) {
mtx = mtx + ", ";
}
mtx = mtx + String::num(rows[j][i]); // matrix is stored transposed for performance, so print it transposed
}
}
return mtx;
return "[X: " + get_column(0).operator String() +
", Y: " + get_column(1).operator String() +
", Z: " + get_column(2).operator String() + "]";
}
Quaternion Basis::get_quaternion() const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V(!is_rotation(), Quaternion());
ERR_FAIL_COND_V_MSG(!is_rotation(), Quaternion(), "Basis must be normalized in order to be casted to a Quaternion. Use get_rotation_quaternion() or call orthonormalized() if the Basis contains linearly independent vectors.");
#endif
/* Allow getting a quaternion from an unnormalized transform */
Basis m = *this;
real_t trace = m.rows[0][0] + m.rows[1][1] + m.rows[2][2];
real_t temp[4];
if (trace > 0.0) {
real_t s = Math::sqrt(trace + 1.0);
temp[3] = (s * 0.5);
s = 0.5 / s;
if (trace > 0.0f) {
real_t s = Math::sqrt(trace + 1.0f);
temp[3] = (s * 0.5f);
s = 0.5f / s;
temp[0] = ((m.rows[2][1] - m.rows[1][2]) * s);
temp[1] = ((m.rows[0][2] - m.rows[2][0]) * s);
temp[2] = ((m.rows[1][0] - m.rows[0][1]) * s);
} else {
int i = m.rows[0][0] < m.rows[1][1] ?
(m.rows[1][1] < m.rows[2][2] ? 2 : 1) :
(m.rows[0][0] < m.rows[2][2] ? 2 : 0);
int i = m.rows[0][0] < m.rows[1][1]
? (m.rows[1][1] < m.rows[2][2] ? 2 : 1)
: (m.rows[0][0] < m.rows[2][2] ? 2 : 0);
int j = (i + 1) % 3;
int k = (i + 2) % 3;
real_t s = Math::sqrt(m.rows[i][i] - m.rows[j][j] - m.rows[k][k] + 1.0);
temp[i] = s * 0.5;
s = 0.5 / s;
real_t s = Math::sqrt(m.rows[i][i] - m.rows[j][j] - m.rows[k][k] + 1.0f);
temp[i] = s * 0.5f;
s = 0.5f / s;
temp[3] = (m.rows[k][j] - m.rows[j][k]) * s;
temp[j] = (m.rows[j][i] + m.rows[i][j]) * s;
@ -807,97 +750,34 @@ Quaternion Basis::get_quaternion() const {
return Quaternion(temp[0], temp[1], temp[2], temp[3]);
}
static const Basis _ortho_bases[24] = {
Basis(1, 0, 0, 0, 1, 0, 0, 0, 1),
Basis(0, -1, 0, 1, 0, 0, 0, 0, 1),
Basis(-1, 0, 0, 0, -1, 0, 0, 0, 1),
Basis(0, 1, 0, -1, 0, 0, 0, 0, 1),
Basis(1, 0, 0, 0, 0, -1, 0, 1, 0),
Basis(0, 0, 1, 1, 0, 0, 0, 1, 0),
Basis(-1, 0, 0, 0, 0, 1, 0, 1, 0),
Basis(0, 0, -1, -1, 0, 0, 0, 1, 0),
Basis(1, 0, 0, 0, -1, 0, 0, 0, -1),
Basis(0, 1, 0, 1, 0, 0, 0, 0, -1),
Basis(-1, 0, 0, 0, 1, 0, 0, 0, -1),
Basis(0, -1, 0, -1, 0, 0, 0, 0, -1),
Basis(1, 0, 0, 0, 0, 1, 0, -1, 0),
Basis(0, 0, -1, 1, 0, 0, 0, -1, 0),
Basis(-1, 0, 0, 0, 0, -1, 0, -1, 0),
Basis(0, 0, 1, -1, 0, 0, 0, -1, 0),
Basis(0, 0, 1, 0, 1, 0, -1, 0, 0),
Basis(0, -1, 0, 0, 0, 1, -1, 0, 0),
Basis(0, 0, -1, 0, -1, 0, -1, 0, 0),
Basis(0, 1, 0, 0, 0, -1, -1, 0, 0),
Basis(0, 0, 1, 0, -1, 0, 1, 0, 0),
Basis(0, 1, 0, 0, 0, 1, 1, 0, 0),
Basis(0, 0, -1, 0, 1, 0, 1, 0, 0),
Basis(0, -1, 0, 0, 0, -1, 1, 0, 0)
};
int Basis::get_orthogonal_index() const {
// could be sped up if i come up with a way
Basis orth = *this;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
real_t v = orth[i][j];
if (v > 0.5) {
v = 1.0;
} else if (v < -0.5) {
v = -1.0;
} else {
v = 0;
}
orth[i][j] = v;
}
}
for (int i = 0; i < 24; i++) {
if (_ortho_bases[i] == orth) {
return i;
}
}
return 0;
}
void Basis::set_orthogonal_index(int p_index) {
// there only exist 24 orthogonal bases in r3
ERR_FAIL_INDEX(p_index, 24);
*this = _ortho_bases[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
*/
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
if ((Math::abs(rows[1][0] - rows[0][1]) < epsilon) && (Math::abs(rows[2][0] - rows[0][2]) < epsilon) && (Math::abs(rows[2][1] - rows[1][2]) < epsilon)) {
// singularity found
// first check for identity matrix which must have +1 for all terms
// in leading diagonaland zero in other terms
if ((Math::abs(rows[1][0] + rows[0][1]) < epsilon2) && (Math::abs(rows[2][0] + rows[0][2]) < epsilon2) && (Math::abs(rows[2][1] + rows[1][2]) < epsilon2) && (Math::abs(rows[0][0] + rows[1][1] + rows[2][2] - 3) < epsilon2)) {
// this singularity is identity matrix so angle = 0
// https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
real_t x, y, z; // Variables for result.
if (Math::is_zero_approx(rows[0][1] - rows[1][0]) && Math::is_zero_approx(rows[0][2] - rows[2][0]) && Math::is_zero_approx(rows[1][2] - rows[2][1])) {
// Singularity found.
// First check for identity matrix which must have +1 for all terms in leading diagonal and zero in other terms.
if (is_diagonal() && (Math::abs(rows[0][0] + rows[1][1] + rows[2][2] - 3) < 3 * CMP_EPSILON)) {
// This singularity is identity matrix so angle = 0.
r_axis = Vector3(0, 1, 0);
r_angle = 0;
return;
}
// otherwise this singularity is angle = 180
angle = Math_PI;
// Otherwise this singularity is angle = 180.
real_t xx = (rows[0][0] + 1) / 2;
real_t yy = (rows[1][1] + 1) / 2;
real_t zz = (rows[2][2] + 1) / 2;
real_t xy = (rows[1][0] + rows[0][1]) / 4;
real_t xz = (rows[2][0] + rows[0][2]) / 4;
real_t yz = (rows[2][1] + rows[1][2]) / 4;
if ((xx > yy) && (xx > zz)) { // rows[0][0] is the largest diagonal term
if (xx < epsilon) {
real_t xy = (rows[0][1] + rows[1][0]) / 4;
real_t xz = (rows[0][2] + rows[2][0]) / 4;
real_t yz = (rows[1][2] + rows[2][1]) / 4;
if ((xx > yy) && (xx > zz)) { // rows[0][0] is the largest diagonal term.
if (xx < CMP_EPSILON) {
x = 0;
y = Math_SQRT12;
z = Math_SQRT12;
@ -906,8 +786,8 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
y = xy / x;
z = xz / x;
}
} else if (yy > zz) { // rows[1][1] is the largest diagonal term
if (yy < epsilon) {
} else if (yy > zz) { // rows[1][1] is the largest diagonal term.
if (yy < CMP_EPSILON) {
x = Math_SQRT12;
y = 0;
z = Math_SQRT12;
@ -916,8 +796,8 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
x = xy / y;
z = yz / y;
}
} else { // rows[2][2] is the largest diagonal term so base result on this
if (zz < epsilon) {
} else { // rows[2][2] is the largest diagonal term so base result on this.
if (zz < CMP_EPSILON) {
x = Math_SQRT12;
y = Math_SQRT12;
z = 0;
@ -928,48 +808,50 @@ void Basis::get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
}
}
r_axis = Vector3(x, y, z);
r_angle = angle;
r_angle = Math_PI;
return;
}
// as we have reached here there are no singularities so we can handle normally
real_t s = Math::sqrt((rows[1][2] - rows[2][1]) * (rows[1][2] - rows[2][1]) + (rows[2][0] - rows[0][2]) * (rows[2][0] - rows[0][2]) + (rows[0][1] - rows[1][0]) * (rows[0][1] - rows[1][0])); // s=|axis||sin(angle)|, used to normalise
// As we have reached here there are no singularities so we can handle normally.
double s = Math::sqrt((rows[2][1] - rows[1][2]) * (rows[2][1] - rows[1][2]) + (rows[0][2] - rows[2][0]) * (rows[0][2] - rows[2][0]) + (rows[1][0] - rows[0][1]) * (rows[1][0] - rows[0][1])); // Used to normalise.
angle = Math::acos((rows[0][0] + rows[1][1] + rows[2][2] - 1) / 2);
if (angle < 0) {
s = -s;
if (Math::abs(s) < CMP_EPSILON) {
// Prevent divide by zero, should not happen if matrix is orthogonal and should be caught by singularity test above.
s = 1;
}
x = (rows[2][1] - rows[1][2]) / s;
y = (rows[0][2] - rows[2][0]) / s;
z = (rows[1][0] - rows[0][1]) / s;
r_axis = Vector3(x, y, z);
r_angle = angle;
// CLAMP to avoid NaN if the value passed to acos is not in [0,1].
r_angle = Math::acos(CLAMP((rows[0][0] + rows[1][1] + rows[2][2] - 1) / 2, (real_t)0.0, (real_t)1.0));
}
void Basis::set_quaternion(const Quaternion &p_quat) {
real_t d = p_quat.length_squared();
real_t s = 2.0 / d;
real_t xs = p_quat.x * s, ys = p_quat.y * s, zs = p_quat.z * s;
real_t wx = p_quat.w * xs, wy = p_quat.w * ys, wz = p_quat.w * zs;
real_t xx = p_quat.x * xs, xy = p_quat.x * ys, xz = p_quat.x * zs;
real_t yy = p_quat.y * ys, yz = p_quat.y * zs, zz = p_quat.z * zs;
set(1.0 - (yy + zz), xy - wz, xz + wy,
xy + wz, 1.0 - (xx + zz), yz - wx,
xz - wy, yz + wx, 1.0 - (xx + yy));
void Basis::set_quaternion(const Quaternion &p_quaternion) {
real_t d = p_quaternion.length_squared();
real_t s = 2.0f / d;
real_t xs = p_quaternion.x * s, ys = p_quaternion.y * s, zs = p_quaternion.z * s;
real_t wx = p_quaternion.w * xs, wy = p_quaternion.w * ys, wz = p_quaternion.w * zs;
real_t xx = p_quaternion.x * xs, xy = p_quaternion.x * ys, xz = p_quaternion.x * zs;
real_t yy = p_quaternion.y * ys, yz = p_quaternion.y * zs, zz = p_quaternion.z * zs;
set(1.0f - (yy + zz), xy - wz, xz + wy,
xy + wz, 1.0f - (xx + zz), yz - wx,
xz - wy, yz + wx, 1.0f - (xx + yy));
}
void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_phi) {
void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_angle) {
// Rotation matrix from axis and angle, see https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_angle
#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
Vector3 axis_sq(p_axis.x * p_axis.x, p_axis.y * p_axis.y, p_axis.z * p_axis.z);
real_t cosine = Math::cos(p_phi);
rows[0][0] = axis_sq.x + cosine * (1.0 - axis_sq.x);
rows[1][1] = axis_sq.y + cosine * (1.0 - axis_sq.y);
rows[2][2] = axis_sq.z + cosine * (1.0 - axis_sq.z);
real_t cosine = Math::cos(p_angle);
rows[0][0] = axis_sq.x + cosine * (1.0f - axis_sq.x);
rows[1][1] = axis_sq.y + cosine * (1.0f - axis_sq.y);
rows[2][2] = axis_sq.z + cosine * (1.0f - axis_sq.z);
real_t sine = Math::sin(p_phi);
real_t sine = Math::sin(p_angle);
real_t t = 1 - cosine;
real_t xyzt = p_axis.x * p_axis.y * t;
@ -988,22 +870,24 @@ void Basis::set_axis_angle(const Vector3 &p_axis, real_t p_phi) {
rows[2][1] = xyzt + zyxs;
}
void Basis::set_axis_angle_scale(const Vector3 &p_axis, real_t p_phi, const Vector3 &p_scale) {
set_diagonal(p_scale);
rotate(p_axis, p_phi);
void Basis::set_axis_angle_scale(const Vector3 &p_axis, real_t p_angle, const Vector3 &p_scale) {
_set_diagonal(p_scale);
rotate(p_axis, p_angle);
}
void Basis::set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale) {
set_diagonal(p_scale);
rotate(p_euler);
void Basis::set_euler_scale(const Vector3 &p_euler, const Vector3 &p_scale, EulerOrder p_order) {
_set_diagonal(p_scale);
rotate(p_euler, p_order);
}
void Basis::set_quaternion_scale(const Quaternion &p_quat, const Vector3 &p_scale) {
set_diagonal(p_scale);
rotate(p_quat);
void Basis::set_quaternion_scale(const Quaternion &p_quaternion, const Vector3 &p_scale) {
_set_diagonal(p_scale);
rotate(p_quaternion);
}
void Basis::set_diagonal(const Vector3 &p_diag) {
// This also sets the non-diagonal elements to 0, which is misleading from the
// name, so we want this method to be private. Use `from_scale` externally.
void Basis::_set_diagonal(const Vector3 &p_diag) {
rows[0][0] = p_diag.x;
rows[0][1] = 0;
rows[0][2] = 0;
@ -1017,6 +901,15 @@ void Basis::set_diagonal(const Vector3 &p_diag) {
rows[2][2] = p_diag.z;
}
Basis Basis::lerp(const Basis &p_to, const real_t &p_weight) const {
Basis b;
b.rows[0] = rows[0].lerp(p_to.rows[0], p_weight);
b.rows[1] = rows[1].lerp(p_to.rows[1], p_weight);
b.rows[2] = rows[2].lerp(p_to.rows[2], p_weight);
return b;
}
Basis Basis::slerp(const Basis &p_to, const real_t &p_weight) const {
//consider scale
Quaternion from(*this);
@ -1049,7 +942,7 @@ void Basis::rotate_sh(real_t *p_values) {
const static real_t s_scale_dst2 = s_c3 * s_c_scale_inv;
const static real_t s_scale_dst4 = s_c5 * s_c_scale_inv;
real_t src[9] = { p_values[0], p_values[1], p_values[2], p_values[3], p_values[4], p_values[5], p_values[6], p_values[7], p_values[8] };
const real_t src[9] = { p_values[0], p_values[1], p_values[2], p_values[3], p_values[4], p_values[5], p_values[6], p_values[7], p_values[8] };
real_t m00 = rows[0][0];
real_t m01 = rows[0][1];
@ -1140,4 +1033,22 @@ void Basis::rotate_sh(real_t *p_values) {
p_values[8] = d4 * s_scale_dst4;
}
Basis Basis::looking_at(const Vector3 &p_target, const Vector3 &p_up) {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V_MSG(p_target.is_zero_approx(), Basis(), "The target vector can't be zero.");
ERR_FAIL_COND_V_MSG(p_up.is_zero_approx(), Basis(), "The up vector can't be zero.");
#endif
Vector3 v_z = -p_target.normalized();
Vector3 v_x = p_up.cross(v_z);
#ifdef MATH_CHECKS
ERR_FAIL_COND_V_MSG(v_x.is_zero_approx(), Basis(), "The target vector and up vector can't be parallel to each other.");
#endif
v_x.normalize();
Vector3 v_y = v_z.cross(v_x);
Basis basis;
basis.set_columns(v_x, v_y, v_z);
return basis;
}
} // namespace godot

204
src/variant/quaternion.cpp
View File

@ -35,13 +35,18 @@
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
// (ax,ay,az), where ax is the angle of rotation around x axis,
// and similar for other axes.
// This implementation uses XYZ convention (Z is the first rotation).
Vector3 Quaternion::get_euler_xyz() const {
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
@ -50,17 +55,20 @@ Vector3 Quaternion::get_euler_xyz() const {
// This implementation uses YXZ convention (Z is the first rotation).
Vector3 Quaternion::get_euler_yxz() const {
#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
Basis m(*this);
return m.get_euler_yxz();
return m.get_euler(Basis::EULER_ORDER_YXZ);
}
void Quaternion::operator*=(const Quaternion &p_q) {
x = 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;
z = w * p_q.z + z * p_q.w + x * p_q.y - y * p_q.x;
real_t xx = w * p_q.x + x * p_q.w + y * p_q.z - z * p_q.y;
real_t yy = w * p_q.y + y * p_q.w + z * p_q.x - x * p_q.z;
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;
x = xx;
y = yy;
z = zz;
}
Quaternion Quaternion::operator*(const Quaternion &p_q) const {
@ -69,8 +77,8 @@ Quaternion Quaternion::operator*(const Quaternion &p_q) const {
return r;
}
bool Quaternion::is_equal_approx(const Quaternion &p_quat) 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);
bool Quaternion::is_equal_approx(const Quaternion &p_quaternion) const {
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 {
@ -91,15 +99,32 @@ bool Quaternion::is_normalized() const {
Quaternion Quaternion::inverse() const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V(!is_normalized(), Quaternion());
ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The quaternion must be normalized.");
#endif
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 {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V(!is_normalized(), Quaternion());
ERR_FAIL_COND_V(!p_to.is_normalized(), Quaternion());
ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized.");
ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
#endif
Quaternion to1;
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);
// adjust signs (if necessary)
if (cosom < 0.0) {
if (cosom < 0.0f) {
cosom = -cosom;
to1.x = -p_to.x;
to1.y = -p_to.y;
to1.z = -p_to.z;
to1.w = -p_to.w;
to1 = -p_to;
} else {
to1.x = p_to.x;
to1.y = p_to.y;
to1.z = p_to.z;
to1.w = p_to.w;
to1 = p_to;
}
// calculate coefficients
if ((1.0 - cosom) > CMP_EPSILON) {
if ((1.0f - cosom) > (real_t)CMP_EPSILON) {
// standard case (slerp)
omega = Math::acos(cosom);
sinom = Math::sin(omega);
@ -132,7 +151,7 @@ Quaternion Quaternion::slerp(const Quaternion &p_to, const real_t &p_weight) con
} else {
// "from" and "to" quaternions are very close
// ... so we can do a linear interpolation
scale0 = 1.0 - p_weight;
scale0 = 1.0f - p_weight;
scale1 = p_weight;
}
// 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 {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V(!is_normalized(), Quaternion());
ERR_FAIL_COND_V(!p_to.is_normalized(), Quaternion());
ERR_FAIL_COND_V_MSG(!is_normalized(), Quaternion(), "The start quaternion must be normalized.");
ERR_FAIL_COND_V_MSG(!p_to.is_normalized(), Quaternion(), "The end quaternion must be normalized.");
#endif
const Quaternion &from = *this;
real_t dot = from.dot(p_to);
if (Math::abs(dot) > 0.9999) {
if (Math::absf(dot) > 0.9999f) {
return from;
}
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,
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,
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);
}
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
ERR_FAIL_COND_V(!is_normalized(), Quaternion());
ERR_FAIL_COND_V(!p_b.is_normalized(), Quaternion());
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
//the only way to do slerp :|
real_t t2 = (1.0 - p_weight) * p_weight * 2;
Quaternion sp = this->slerp(p_b, p_weight);
Quaternion sq = p_pre_a.slerpni(p_post_b, p_weight);
return sp.slerpni(sq, t2);
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(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 {
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) {
#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
real_t d = p_axis.length();
if (d == 0) {
@ -194,8 +314,8 @@ Quaternion::Quaternion(const Vector3 &p_axis, real_t p_angle) {
z = 0;
w = 0;
} else {
real_t sin_angle = Math::sin(p_angle * 0.5);
real_t cos_angle = Math::cos(p_angle * 0.5);
real_t sin_angle = Math::sin(p_angle * 0.5f);
real_t cos_angle = Math::cos(p_angle * 0.5f);
real_t s = sin_angle / d;
x = p_axis.x * 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.
// This implementation uses YXZ convention (Z is the first rotation).
Quaternion::Quaternion(const Vector3 &p_euler) {
real_t half_a1 = p_euler.y * 0.5;
real_t half_a2 = p_euler.x * 0.5;
real_t half_a3 = p_euler.z * 0.5;
real_t half_a1 = p_euler.y * 0.5f;
real_t half_a2 = p_euler.x * 0.5f;
real_t half_a3 = p_euler.z * 0.5f;
// 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)

87
src/variant/transform3d.cpp
View File

@ -58,13 +58,13 @@ Transform3D Transform3D::inverse() const {
return ret;
}
void Transform3D::rotate(const Vector3 &p_axis, real_t p_phi) {
*this = rotated(p_axis, p_phi);
void Transform3D::rotate(const Vector3 &p_axis, real_t p_angle) {
*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
Basis p_basis(p_axis, p_phi);
Basis p_basis(p_axis, p_angle);
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);
}
void Transform3D::rotate_basis(const Vector3 &p_axis, real_t p_phi) {
basis.rotate(p_axis, p_phi);
void Transform3D::rotate_basis(const Vector3 &p_axis, real_t p_angle) {
basis.rotate(p_axis, p_angle);
}
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;
t.set_look_at(origin, p_target, p_up);
t.basis = Basis::looking_at(p_target - origin, p_up);
return t;
}
void Transform3D::set_look_at(const Vector3 &p_eye, const Vector3 &p_target, const Vector3 &p_up) {
#ifdef MATH_CHECKS
ERR_FAIL_COND(p_eye == p_target);
ERR_FAIL_COND(p_up.length() == 0);
ERR_FAIL_COND_MSG(p_eye.is_equal_approx(p_target), "The eye and target vectors can't be equal.");
#endif
// RefCounted: MESA source code
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);
basis = Basis::looking_at(p_target - p_eye, p_up);
origin = p_eye;
}
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();
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();
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.origin = src_loc.lerp(dst_loc, p_c);
@ -154,11 +131,11 @@ void Transform3D::scale_basis(const Vector3 &p_scale) {
basis.scale(p_scale);
}
void Transform3D::translate(real_t p_tx, real_t p_ty, real_t p_tz) {
translate(Vector3(p_tx, p_ty, p_tz));
void Transform3D::translate_local(real_t p_tx, real_t p_ty, real_t 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++) {
origin[i] += basis[i].dot(p_translation);
}
@ -184,6 +161,16 @@ Transform3D Transform3D::orthonormalized() const {
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 {
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;
}
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 {
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) :
@ -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) :
origin(p_origin) {
basis.set_axis(0, p_x);
basis.set_axis(1, p_y);
basis.set_axis(2, p_z);
basis.set_column(0, p_x);
basis.set_column(1, p_y);
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) {