Finished Quat.h
Karroffel
parent
5d543692eb
commit
15515d10d4
|
|
@ -91,6 +91,31 @@ public:
|
|||
}
|
||||
#undef cofac
|
||||
|
||||
bool isequal_approx(const Basis& a, const Basis& b) const {
|
||||
|
||||
for (int i=0;i<3;i++) {
|
||||
for (int j=0;j<3;j++) {
|
||||
if ((::fabs(a.elements[i][j]-b.elements[i][j]) < CMP_EPSILON) == false)
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
bool is_orthogonal() const
|
||||
{
|
||||
Basis id;
|
||||
Basis m = (*this)*transposed();
|
||||
|
||||
return isequal_approx(id,m);
|
||||
}
|
||||
|
||||
bool is_rotation() const
|
||||
{
|
||||
return ::fabs(determinant()-1) < CMP_EPSILON && is_orthogonal();
|
||||
}
|
||||
|
||||
void transpose()
|
||||
{
|
||||
|
|
@ -141,8 +166,6 @@ public:
|
|||
return Basis(p_axis, p_phi) * (*this);
|
||||
}
|
||||
|
||||
Vector3 get_rotation() const; // need?!
|
||||
|
||||
void scale( const Vector3& p_scale )
|
||||
{
|
||||
elements[0][0]*=p_scale.x;
|
||||
|
|
@ -244,8 +267,6 @@ public:
|
|||
return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2];
|
||||
}
|
||||
|
||||
bool isequal_approx(const Basis& a, const Basis& b) const; // need?
|
||||
|
||||
bool operator==(const Basis& p_matrix) const
|
||||
{
|
||||
for (int i=0;i<3;i++) {
|
||||
|
|
@ -345,10 +366,13 @@ public:
|
|||
|
||||
void set_orthogonal_index(int p_index); // down below
|
||||
|
||||
bool is_orthogonal() const; // need?
|
||||
bool is_rotation() const; // need?
|
||||
|
||||
operator String() const;
|
||||
operator String() const
|
||||
{
|
||||
String s;
|
||||
// @Todo
|
||||
return s;
|
||||
}
|
||||
|
||||
void get_axis_and_angle(Vector3 &r_axis,real_t& r_angle) const;
|
||||
|
||||
|
|
|
|||
|
|
@ -5,6 +5,8 @@
|
|||
|
||||
#include "Vector3.h"
|
||||
|
||||
// #include "Basis.h"
|
||||
|
||||
namespace godot {
|
||||
|
||||
#define CMP_EPSILON 0.00001
|
||||
|
|
@ -16,17 +18,127 @@ public:
|
|||
|
||||
real_t x,y,z,w;
|
||||
|
||||
real_t length_squared() const;
|
||||
real_t length() const;
|
||||
void normalize();
|
||||
Quat normalized() const;
|
||||
Quat inverse() const;
|
||||
real_t dot(const Quat& q) const;
|
||||
void set_euler(const Vector3& p_euler);
|
||||
Vector3 get_euler() const;
|
||||
Quat slerp(const Quat& q, const real_t& t) const;
|
||||
Quat slerpni(const Quat& q, const real_t& t) const;
|
||||
Quat cubic_slerp(const Quat& q, const Quat& prep, const Quat& postq,const real_t& t) const;
|
||||
real_t length_squared() const; // down below
|
||||
real_t length() const
|
||||
{
|
||||
return ::sqrt(length_squared());
|
||||
}
|
||||
|
||||
void normalize()
|
||||
{
|
||||
*this /= length();
|
||||
}
|
||||
|
||||
Quat normalized() const
|
||||
{
|
||||
return *this / length();
|
||||
}
|
||||
|
||||
Quat inverse() const
|
||||
{
|
||||
return Quat( -x, -y, -z, w );
|
||||
}
|
||||
|
||||
real_t dot(const Quat& q) const; // down below
|
||||
void set_euler(const Vector3& p_euler)
|
||||
{
|
||||
real_t half_a1 = p_euler.x * 0.5;
|
||||
real_t half_a2 = p_euler.y * 0.5;
|
||||
real_t half_a3 = p_euler.z * 0.5;
|
||||
|
||||
// R = X(a1).Y(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-2)
|
||||
// a3 is the angle of the first rotation, following the notation in this reference.
|
||||
|
||||
real_t cos_a1 = ::cos(half_a1);
|
||||
real_t sin_a1 = ::sin(half_a1);
|
||||
real_t cos_a2 = ::cos(half_a2);
|
||||
real_t sin_a2 = ::sin(half_a2);
|
||||
real_t cos_a3 = ::cos(half_a3);
|
||||
real_t sin_a3 = ::sin(half_a3);
|
||||
|
||||
set(sin_a1*cos_a2*cos_a3 + sin_a2*sin_a3*cos_a1,
|
||||
-sin_a1*sin_a3*cos_a2 + sin_a2*cos_a1*cos_a3,
|
||||
sin_a1*sin_a2*cos_a3 + sin_a3*cos_a1*cos_a2,
|
||||
-sin_a1*sin_a2*sin_a3 + cos_a1*cos_a2*cos_a3);
|
||||
}
|
||||
|
||||
Vector3 get_euler() const; // down below
|
||||
|
||||
Quat slerp(const Quat& q, const real_t& t) const {
|
||||
|
||||
Quat to1;
|
||||
real_t omega, cosom, sinom, scale0, scale1;
|
||||
|
||||
|
||||
// calc cosine
|
||||
cosom = dot(q);
|
||||
|
||||
// adjust signs (if necessary)
|
||||
if ( cosom <0.0 ) {
|
||||
cosom = -cosom;
|
||||
to1.x = - q.x;
|
||||
to1.y = - q.y;
|
||||
to1.z = - q.z;
|
||||
to1.w = - q.w;
|
||||
} else {
|
||||
to1.x = q.x;
|
||||
to1.y = q.y;
|
||||
to1.z = q.z;
|
||||
to1.w = q.w;
|
||||
}
|
||||
|
||||
|
||||
// calculate coefficients
|
||||
|
||||
if ( (1.0 - cosom) > CMP_EPSILON ) {
|
||||
// standard case (slerp)
|
||||
omega = ::acos(cosom);
|
||||
sinom = ::sin(omega);
|
||||
scale0 = ::sin((1.0 - t) * omega) / sinom;
|
||||
scale1 = ::sin(t * omega) / sinom;
|
||||
} else {
|
||||
// "from" and "to" quaternions are very close
|
||||
// ... so we can do a linear interpolation
|
||||
scale0 = 1.0 - t;
|
||||
scale1 = t;
|
||||
}
|
||||
// calculate final values
|
||||
return Quat(
|
||||
scale0 * x + scale1 * to1.x,
|
||||
scale0 * y + scale1 * to1.y,
|
||||
scale0 * z + scale1 * to1.z,
|
||||
scale0 * w + scale1 * to1.w
|
||||
);
|
||||
}
|
||||
|
||||
Quat slerpni(const Quat& q, const real_t& t) const {
|
||||
|
||||
const Quat &from = *this;
|
||||
|
||||
real_t dot = from.dot(q);
|
||||
|
||||
if (::fabs(dot) > 0.9999) return from;
|
||||
|
||||
real_t theta = ::acos(dot),
|
||||
sinT = 1.0 / ::sin(theta),
|
||||
newFactor = ::sin(t * theta) * sinT,
|
||||
invFactor = ::sin((1.0 - t) * theta) * sinT;
|
||||
|
||||
return Quat(invFactor * from.x + newFactor * q.x,
|
||||
invFactor * from.y + newFactor * q.y,
|
||||
invFactor * from.z + newFactor * q.z,
|
||||
invFactor * from.w + newFactor * q.w);
|
||||
}
|
||||
|
||||
Quat cubic_slerp(const Quat& q, const Quat& prep, const Quat& postq,const real_t& t) const
|
||||
{
|
||||
//the only way to do slerp :|
|
||||
real_t t2 = (1.0-t)*t*2;
|
||||
Quat sp = this->slerp(q,t);
|
||||
Quat sq = prep.slerpni(postq,t);
|
||||
return sp.slerpni(sq,t2);
|
||||
}
|
||||
|
||||
void get_axis_and_angle(Vector3& r_axis, real_t &r_angle) const {
|
||||
r_angle = 2 * ::acos(w);
|
||||
|
|
@ -35,8 +147,8 @@ public:
|
|||
r_axis.z = z / ::sqrt(1-w*w);
|
||||
}
|
||||
|
||||
void operator*=(const Quat& q);
|
||||
Quat operator*(const Quat& q) const;
|
||||
void operator*=(const Quat& q); // down below
|
||||
Quat operator*(const Quat& q) const; // down below
|
||||
|
||||
|
||||
|
||||
|
|
@ -55,6 +167,7 @@ public:
|
|||
return Vector3(q.x,q.y,q.z);
|
||||
}
|
||||
|
||||
// everything's down
|
||||
void operator+=(const Quat& q);
|
||||
void operator-=(const Quat& q);
|
||||
void operator*=(const real_t& s);
|
||||
|
|
@ -69,7 +182,10 @@ public:
|
|||
bool operator==(const Quat& p_quat) const;
|
||||
bool operator!=(const Quat& p_quat) const;
|
||||
|
||||
operator String() const;
|
||||
operator String() const
|
||||
{
|
||||
return String(); // @Todo
|
||||
}
|
||||
|
||||
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;
|
||||
|
|
@ -77,7 +193,19 @@ public:
|
|||
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);
|
||||
Quat(const Vector3& axis, const real_t& angle)
|
||||
{
|
||||
real_t d = axis.length();
|
||||
if (d==0)
|
||||
set(0,0,0,0);
|
||||
else {
|
||||
real_t sin_angle = ::sin(angle * 0.5);
|
||||
real_t cos_angle = ::cos(angle * 0.5);
|
||||
real_t s = sin_angle / d;
|
||||
set(axis.x * s, axis.y * s, axis.z * s,
|
||||
cos_angle);
|
||||
}
|
||||
}
|
||||
|
||||
Quat(const Vector3& v0, const Vector3& v1) // shortest arc
|
||||
{
|
||||
|
|
@ -143,6 +271,13 @@ Quat Quat::operator-(const Quat& q2) const {
|
|||
return Quat( q1.x-q2.x, q1.y-q2.y, q1.z-q2.z, q1.w-q2.w);
|
||||
}
|
||||
|
||||
Quat Quat::operator*(const Quat& q2) const {
|
||||
Quat q1 = *this;
|
||||
q1 *= q2;
|
||||
return q1;
|
||||
}
|
||||
|
||||
|
||||
Quat Quat::operator-() const {
|
||||
const Quat& q2 = *this;
|
||||
return Quat( -q2.x, -q2.y, -q2.z, -q2.w);
|
||||
|
|
@ -166,6 +301,18 @@ bool Quat::operator!=(const Quat& p_quat) const {
|
|||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
#include "Basis.h"
|
||||
|
||||
namespace godot {
|
||||
|
||||
Vector3 Quat::get_euler() const
|
||||
{
|
||||
Basis m(*this);
|
||||
return m.get_euler();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif // QUAT_H
|
||||
|
|
|
|||
Loading…
Reference in New Issue