Update Basis/Transform3D/Quaternion to match the engine
Aaron Franke
parent
8670305589
commit
e83d472c00
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue