mirror of
https://github.com/Relintai/codot.git
synced 2024-11-08 20:22:09 +01:00
Work on quat.
This commit is contained in:
parent
0f4ae261a0
commit
7499e81112
@ -30,8 +30,10 @@
|
|||||||
|
|
||||||
#include "quat.h"
|
#include "quat.h"
|
||||||
|
|
||||||
#include "core/math/basis.h"
|
//#include "core/math/basis.h"
|
||||||
#include "core/print_string.h"
|
//#include "core/print_string.h"
|
||||||
|
|
||||||
|
/*
|
||||||
|
|
||||||
real_t Quat::angle_to(const Quat &p_to) const {
|
real_t Quat::angle_to(const Quat &p_to) const {
|
||||||
real_t d = dot(p_to);
|
real_t d = dot(p_to);
|
||||||
@ -253,3 +255,4 @@ void Quat::set_axis_angle(const Vector3 &axis, const real_t &angle) {
|
|||||||
cos_angle);
|
cos_angle);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
*/
|
||||||
386
core/math/quat.h
386
core/math/quat.h
@ -34,198 +34,260 @@
|
|||||||
#include "core/math/math_defs.h"
|
#include "core/math/math_defs.h"
|
||||||
#include "core/math/math_funcs.h"
|
#include "core/math/math_funcs.h"
|
||||||
#include "core/math/vector3.h"
|
#include "core/math/vector3.h"
|
||||||
#include "core/ustring.h"
|
//#include "core/ustring.h"
|
||||||
|
|
||||||
class _NO_DISCARD_CLASS_ Quat {
|
typedef struct _NO_DISCARD_CLASS_ Quat {
|
||||||
public:
|
|
||||||
real_t x, y, z, w;
|
real_t x, y, z, w;
|
||||||
|
} Quat;
|
||||||
|
|
||||||
_FORCE_INLINE_ real_t length_squared() const;
|
extern _FORCE_INLINE_ void quat_set(Quat *self, real_t p_x, real_t p_y, real_t p_z, real_t p_w) {
|
||||||
bool is_equal_approx(const Quat &p_quat) const;
|
self->x = p_x;
|
||||||
real_t length() const;
|
self->y = p_y;
|
||||||
void normalize();
|
self->z = p_z;
|
||||||
Quat normalized() const;
|
self->w = p_z;
|
||||||
bool is_normalized() const;
|
}
|
||||||
Quat inverse() const;
|
|
||||||
_FORCE_INLINE_ real_t dot(const Quat &p_q) const;
|
|
||||||
real_t angle_to(const Quat &p_to) const;
|
|
||||||
|
|
||||||
void set_euler_xyz(const Vector3 &p_euler);
|
extern _FORCE_INLINE_ Quat quat_create(real_t p_x, real_t p_y, real_t p_z, real_t p_w) {
|
||||||
Vector3 get_euler_xyz() const;
|
Quat q;
|
||||||
void set_euler_yxz(const Vector3 &p_euler);
|
|
||||||
Vector3 get_euler_yxz() const;
|
|
||||||
|
|
||||||
void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); };
|
q.x = p_x;
|
||||||
Vector3 get_euler() const { return get_euler_yxz(); };
|
q.y = p_y;
|
||||||
|
q.z = p_z;
|
||||||
|
q.w = p_z;
|
||||||
|
|
||||||
Quat slerp(const Quat &p_to, const real_t &p_weight) const;
|
return q;
|
||||||
Quat slerpni(const Quat &p_to, const real_t &p_weight) const;
|
}
|
||||||
Quat cubic_slerp(const Quat &p_b, const Quat &p_pre_a, const Quat &p_post_b, const real_t &p_weight) const;
|
|
||||||
|
|
||||||
void set_axis_angle(const Vector3 &axis, const real_t &angle);
|
extern _FORCE_INLINE_ Quat quat_createv(const Quat *other) {
|
||||||
_FORCE_INLINE_ void get_axis_angle(Vector3 &r_axis, real_t &r_angle) const {
|
Quat q;
|
||||||
r_angle = 2 * Math::acos(w);
|
|
||||||
real_t r = ((real_t)1) / Math::sqrt(1 - w * w);
|
|
||||||
r_axis.x = x * r;
|
|
||||||
r_axis.y = y * r;
|
|
||||||
r_axis.z = z * r;
|
|
||||||
}
|
|
||||||
|
|
||||||
void operator*=(const Quat &p_q);
|
q.x = other->x;
|
||||||
Quat operator*(const Quat &p_q) const;
|
q.y = other->y;
|
||||||
|
q.z = other->z;
|
||||||
|
q.w = other->w;
|
||||||
|
|
||||||
Quat operator*(const Vector3 &v) const {
|
return q;
|
||||||
return Quat(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);
|
|
||||||
}
|
|
||||||
|
|
||||||
_FORCE_INLINE_ Vector3 xform(const Vector3 &v) const {
|
real_t quat_dot(const Quat *self, const Quat *p_q) {
|
||||||
#ifdef MATH_CHECKS
|
return self->x * p_q->x + self->y * p_q->y + self->z * p_q->z + self->w * p_q->w;
|
||||||
ERR_FAIL_COND_V_MSG(!is_normalized(), v, "The quaternion must be normalized.");
|
}
|
||||||
#endif
|
real_t quat_dotc(const Quat self, const Quat p_q) {
|
||||||
Vector3 u(x, y, z);
|
return self.x * p_q.x + self.y * p_q.y + self.z * p_q.z + self.w * p_q.w;
|
||||||
Vector3 uv = u.cross(v);
|
}
|
||||||
return v + ((uv * w) + u.cross(uv)) * ((real_t)2);
|
|
||||||
}
|
|
||||||
|
|
||||||
_FORCE_INLINE_ void operator+=(const Quat &p_q);
|
real_t quat_length_squared(const Quat *self) {
|
||||||
_FORCE_INLINE_ void operator-=(const Quat &p_q);
|
return quat_dot(self, self);
|
||||||
_FORCE_INLINE_ void operator*=(const real_t &s);
|
}
|
||||||
_FORCE_INLINE_ void operator/=(const real_t &s);
|
real_t quat_length_squaredc(const Quat self) {
|
||||||
_FORCE_INLINE_ Quat operator+(const Quat &q2) const;
|
return quat_dotc(self, self);
|
||||||
_FORCE_INLINE_ Quat operator-(const Quat &q2) const;
|
}
|
||||||
_FORCE_INLINE_ Quat operator-() const;
|
|
||||||
_FORCE_INLINE_ Quat operator*(const real_t &s) const;
|
|
||||||
_FORCE_INLINE_ Quat operator/(const real_t &s) const;
|
|
||||||
|
|
||||||
_FORCE_INLINE_ bool operator==(const Quat &p_quat) const;
|
extern _FORCE_INLINE_ Quat quat_add(const Quat *self, const Quat *p_q) {
|
||||||
_FORCE_INLINE_ bool operator!=(const Quat &p_quat) const;
|
return quat_create(self->x + p_q->x, self->y + p_q->y, self->z + p_q->z, self->w + p_q->w);
|
||||||
|
}
|
||||||
|
extern _FORCE_INLINE_ void quat_add_eq(Quat *self, const Quat *p_q) {
|
||||||
|
self->x += p_q->x;
|
||||||
|
self->y += p_q->y;
|
||||||
|
self->z += p_q->z;
|
||||||
|
self->w += p_q->w;
|
||||||
|
}
|
||||||
|
|
||||||
operator String() const;
|
extern _FORCE_INLINE_ Quat quat_subv(const Quat *self, const Quat *p_v) {
|
||||||
|
return quat_create(self->x - p_v->x, self->y - p_v->y, self->z - p_v->z, self->w - p_v->w);
|
||||||
|
}
|
||||||
|
extern _FORCE_INLINE_ Quat quat_subvc(const Quat self, const Quat p_v) {
|
||||||
|
return quat_create(self.x - p_v.x, self.y - p_v.y, self.z - p_v.z, self.w - p_v.w);
|
||||||
|
}
|
||||||
|
extern _FORCE_INLINE_ void quat_sub_eqv(Quat *self, const Quat *p_q) {
|
||||||
|
self->x -= p_q->x;
|
||||||
|
self->y -= p_q->y;
|
||||||
|
self->z -= p_q->z;
|
||||||
|
self->w -= p_q->w;
|
||||||
|
}
|
||||||
|
|
||||||
inline void set(real_t p_x, real_t p_y, real_t p_z, real_t p_w) {
|
// Other
|
||||||
x = p_x;
|
extern _FORCE_INLINE_ Quat quat_neg(const Quat *self) {
|
||||||
y = p_y;
|
return quat_create(-(self->x), -(self->y), -(self->z), -(self->w));
|
||||||
z = p_z;
|
}
|
||||||
w = p_w;
|
extern _FORCE_INLINE_ Quat quat_negc(const Quat self) {
|
||||||
}
|
return quat_create(-(self.x), -(self.y), -(self.z), -(self.w));
|
||||||
inline Quat(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),
|
|
||||||
w(p_w) {
|
|
||||||
}
|
|
||||||
Quat(const Vector3 &axis, const real_t &angle) { set_axis_angle(axis, angle); }
|
|
||||||
|
|
||||||
Quat(const Vector3 &euler) { set_euler(euler); }
|
extern _FORCE_INLINE_ Quat quat_mulq(const Quat *self, const Quat *p_v1) {
|
||||||
Quat(const Quat &p_q) :
|
return quat_create(self->x * p_v1->x, self->y * p_v1->y, self->z * p_v1->z, self->w * p_v1->w);
|
||||||
x(p_q.x),
|
}
|
||||||
y(p_q.y),
|
extern _FORCE_INLINE_ Quat quat_mulqc(const Quat self, const Quat p_q) {
|
||||||
z(p_q.z),
|
return quat_create(self.x * p_q.x, self.y * p_q.y, self.z * p_q.z, self.w * p_q.w);
|
||||||
w(p_q.w) {
|
}
|
||||||
}
|
extern _FORCE_INLINE_ void quat_mul_eqq(Quat *self, const Quat *rvalue) {
|
||||||
|
self->x *= rvalue->x;
|
||||||
|
self->y *= rvalue->y;
|
||||||
|
self->z *= rvalue->z;
|
||||||
|
self->w *= rvalue->w;
|
||||||
|
}
|
||||||
|
|
||||||
Quat &operator=(const Quat &p_q) {
|
extern _FORCE_INLINE_ Quat quat_mulv(const Quat *self, const Vector3 *v) {
|
||||||
x = p_q.x;
|
return quat_create(self->w * v->x + self->y * v->z - self->z * v->y,
|
||||||
y = p_q.y;
|
self->w * v->y + self->z * v->x - self->x * v->z,
|
||||||
z = p_q.z;
|
self->w * v->z + self->x * v->y - self->y * v->x,
|
||||||
w = p_q.w;
|
-self->x * v->x - self->y * v->y - self->z * v->z);
|
||||||
return *this;
|
}
|
||||||
}
|
extern _FORCE_INLINE_ Quat quat_mulvc(const Quat self, Vector3 v) {
|
||||||
|
return quat_create(self.w * v.x + self.y * v.z - self.z * v.y,
|
||||||
|
self.w * v.y + self.z * v.x - self.x * v.z,
|
||||||
|
self.w * v.z + self.x * v.y - self.y * v.x,
|
||||||
|
-self.x * v.x - self.y * v.y - self.z * v.z);
|
||||||
|
}
|
||||||
|
|
||||||
Quat(const Vector3 &v0, const Vector3 &v1) // shortest arc
|
extern _FORCE_INLINE_ Quat quat_muls(const Quat *self, const real_t rvalue) {
|
||||||
{
|
return quat_create(self->x * rvalue, self->y * rvalue, self->z * rvalue, self->w * rvalue);
|
||||||
Vector3 c = v0.cross(v1);
|
};
|
||||||
real_t d = v0.dot(v1);
|
extern _FORCE_INLINE_ Quat quat_mulsc(Quat self, const real_t rvalue) {
|
||||||
|
return quat_create(self.x * rvalue, self.y * rvalue, self.z * rvalue, self.w * rvalue);
|
||||||
if (d < -1 + (real_t)CMP_EPSILON) {
|
};
|
||||||
x = 0;
|
extern _FORCE_INLINE_ void quat_mul_eqs(Quat *self, const real_t rvalue) {
|
||||||
y = 1;
|
self->x *= rvalue;
|
||||||
z = 0;
|
self->y *= rvalue;
|
||||||
w = 0;
|
self->z *= rvalue;
|
||||||
} else {
|
self->w *= rvalue;
|
||||||
real_t s = Math::sqrt((1 + d) * 2);
|
|
||||||
real_t rs = 1 / s;
|
|
||||||
|
|
||||||
x = c.x * rs;
|
|
||||||
y = c.y * rs;
|
|
||||||
z = c.z * rs;
|
|
||||||
w = s * 0.5f;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
inline Quat() :
|
|
||||||
x(0),
|
|
||||||
y(0),
|
|
||||||
z(0),
|
|
||||||
w(1) {
|
|
||||||
}
|
|
||||||
};
|
};
|
||||||
|
|
||||||
real_t Quat::dot(const Quat &p_q) const {
|
extern _FORCE_INLINE_ Quat quat_divs(const Quat *self, const real_t rvalue) {
|
||||||
return x * p_q.x + y * p_q.y + z * p_q.z + w * p_q.w;
|
real_t r = 1 / rvalue;
|
||||||
}
|
return quat_create(self->x * r, self->y * r, self->z * r, self->w * r);
|
||||||
|
};
|
||||||
|
extern _FORCE_INLINE_ Quat quat_divsc(Quat self, const real_t rvalue) {
|
||||||
|
real_t r = 1 / rvalue;
|
||||||
|
return quat_create(self.x * r, self.y * r, self.z * r, self.w * r);
|
||||||
|
};
|
||||||
|
extern _FORCE_INLINE_ void quat_div_eqs(Quat *self, const real_t rvalue) {
|
||||||
|
real_t r = 1 / rvalue;
|
||||||
|
|
||||||
real_t Quat::length_squared() const {
|
self->x *= r;
|
||||||
return dot(*this);
|
self->y *= r;
|
||||||
}
|
self->z *= r;
|
||||||
|
self->w *= r;
|
||||||
|
};
|
||||||
|
|
||||||
void Quat::operator+=(const Quat &p_q) {
|
/*
|
||||||
x += p_q.x;
|
|
||||||
y += p_q.y;
|
|
||||||
z += p_q.z;
|
|
||||||
w += p_q.w;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Quat::operator-=(const Quat &p_q) {
|
|
||||||
x -= p_q.x;
|
|
||||||
y -= p_q.y;
|
|
||||||
z -= p_q.z;
|
|
||||||
w -= p_q.w;
|
|
||||||
}
|
|
||||||
|
|
||||||
void Quat::operator*=(const real_t &s) {
|
bool is_equal_approx(const Quat &p_quat) const;
|
||||||
x *= s;
|
real_t length() const;
|
||||||
y *= s;
|
void normalize();
|
||||||
z *= s;
|
Quat normalized() const;
|
||||||
w *= s;
|
bool is_normalized() const;
|
||||||
}
|
Quat inverse() const;
|
||||||
|
|
||||||
void Quat::operator/=(const real_t &s) {
|
real_t angle_to(const Quat &p_to) const;
|
||||||
*this *= 1 / s;
|
|
||||||
}
|
|
||||||
|
|
||||||
Quat Quat::operator+(const Quat &q2) const {
|
void set_euler_xyz(const Vector3 &p_euler);
|
||||||
const Quat &q1 = *this;
|
Vector3 get_euler_xyz() const;
|
||||||
return Quat(q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w);
|
void set_euler_yxz(const Vector3 &p_euler);
|
||||||
}
|
Vector3 get_euler_yxz() const;
|
||||||
|
|
||||||
Quat Quat::operator-(const Quat &q2) const {
|
void set_euler(const Vector3 &p_euler) { set_euler_yxz(p_euler); };
|
||||||
const Quat &q1 = *this;
|
Vector3 get_euler() const { return get_euler_yxz(); };
|
||||||
return Quat(q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w);
|
|
||||||
}
|
|
||||||
|
|
||||||
Quat Quat::operator-() const {
|
Quat slerp(const Quat &p_to, const real_t &p_weight) const;
|
||||||
const Quat &q2 = *this;
|
Quat slerpni(const Quat &p_to, const real_t &p_weight) const;
|
||||||
return Quat(-q2.x, -q2.y, -q2.z, -q2.w);
|
Quat cubic_slerp(const Quat &p_b, const Quat &p_pre_a, const Quat &p_post_b, const real_t &p_weight) const;
|
||||||
}
|
|
||||||
|
|
||||||
Quat Quat::operator*(const real_t &s) const {
|
void set_axis_angle(const Vector3 &axis, const real_t &angle);
|
||||||
return Quat(x * s, y * s, z * s, w * s);
|
_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;
|
||||||
|
r_axis.y = y * r;
|
||||||
|
r_axis.z = z * r;
|
||||||
|
}
|
||||||
|
|
||||||
Quat Quat::operator/(const real_t &s) const {
|
|
||||||
return *this * (1 / s);
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Quat::operator==(const Quat &p_quat) const {
|
|
||||||
return x == p_quat.x && y == p_quat.y && z == p_quat.z && w == p_quat.w;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool Quat::operator!=(const Quat &p_quat) const {
|
_FORCE_INLINE_ Vector3 xform(const Vector3 &v) const {
|
||||||
return x != p_quat.x || y != p_quat.y || z != p_quat.z || w != p_quat.w;
|
#ifdef MATH_CHECKS
|
||||||
}
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
operator String() const;
|
||||||
|
|
||||||
|
inline void set(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;
|
||||||
|
w = p_w;
|
||||||
|
}
|
||||||
|
inline Quat(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),
|
||||||
|
w(p_w) {
|
||||||
|
}
|
||||||
|
Quat(const Vector3 &axis, const real_t &angle) { set_axis_angle(axis, angle); }
|
||||||
|
|
||||||
|
Quat(const Vector3 &euler) { set_euler(euler); }
|
||||||
|
Quat(const Quat &p_q) :
|
||||||
|
x(p_q.x),
|
||||||
|
y(p_q.y),
|
||||||
|
z(p_q.z),
|
||||||
|
w(p_q.w) {
|
||||||
|
}
|
||||||
|
|
||||||
|
Quat &operator=(const Quat &p_q) {
|
||||||
|
x = p_q.x;
|
||||||
|
y = p_q.y;
|
||||||
|
z = p_q.z;
|
||||||
|
w = p_q.w;
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
|
Quat(const Vector3 &v0, const Vector3 &v1) // shortest arc
|
||||||
|
{
|
||||||
|
Vector3 c = v0.cross(v1);
|
||||||
|
real_t d = v0.dot(v1);
|
||||||
|
|
||||||
|
if (d < -1 + (real_t)CMP_EPSILON) {
|
||||||
|
x = 0;
|
||||||
|
y = 1;
|
||||||
|
z = 0;
|
||||||
|
w = 0;
|
||||||
|
} else {
|
||||||
|
real_t s = Math::sqrt((1 + d) * 2);
|
||||||
|
real_t rs = 1 / s;
|
||||||
|
|
||||||
|
x = c.x * rs;
|
||||||
|
y = c.y * rs;
|
||||||
|
z = c.z * rs;
|
||||||
|
w = s * 0.5f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline Quat() :
|
||||||
|
x(0),
|
||||||
|
y(0),
|
||||||
|
z(0),
|
||||||
|
w(1) {
|
||||||
|
}
|
||||||
|
//-----
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
_FORCE_INLINE_ bool operator==(const Quat &p_quat) const;
|
||||||
|
_FORCE_INLINE_ bool operator!=(const Quat &p_quat) const;
|
||||||
|
|
||||||
|
bool Quat::operator==(const Quat &p_quat) const {
|
||||||
|
return x == p_quat.x && y == p_quat.y && z == p_quat.z && w == p_quat.w;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Quat::operator!=(const Quat &p_quat) const {
|
||||||
|
return x != p_quat.x || y != p_quat.y || z != p_quat.z || w != p_quat.w;
|
||||||
|
}
|
||||||
|
*/
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user