|
|
@@ -6,133 +6,210 @@
|
|
|
namespace M {
|
|
|
|
|
|
|
|
|
-// constructor []
|
|
|
-inline Quat::Quat():
|
|
|
- x(0.0), y(0.0), z(0.0), w(1.0)
|
|
|
-{}
|
|
|
+//======================================================================================================================
|
|
|
+// Constructors =
|
|
|
+//======================================================================================================================
|
|
|
|
|
|
-// constructor [float]
|
|
|
-inline Quat::Quat(float f):
|
|
|
- x(f), y(f), z(f), w(f)
|
|
|
-{}
|
|
|
+// Default
|
|
|
+inline Quat::Quat()
|
|
|
+{
|
|
|
+ x() = y() = z() = w() = 0.0;
|
|
|
+}
|
|
|
+
|
|
|
+// float
|
|
|
+inline Quat::Quat(float f)
|
|
|
+{
|
|
|
+ x() = y() = z() = w() = f;
|
|
|
+}
|
|
|
|
|
|
-// constructor [float, float, float, float]
|
|
|
-inline Quat::Quat(float x_, float y_, float z_, float w_):
|
|
|
- x(x_), y(y_), z(z_), w(w_)
|
|
|
-{}
|
|
|
+// float, float, float, float
|
|
|
+inline Quat::Quat(float x_, float y_, float z_, float w_)
|
|
|
+{
|
|
|
+ x() = x_;
|
|
|
+ y() = y_;
|
|
|
+ z() = z_;
|
|
|
+ w() = w_;
|
|
|
+}
|
|
|
|
|
|
// constructor [vec2, float, float]
|
|
|
-inline Quat::Quat(const Vec2& v2, float z_, float w_):
|
|
|
- x(v2.x()), y(v2.y()), z(z_), w(w_)
|
|
|
-{}
|
|
|
+inline Quat::Quat(const Vec2& v, float z_, float w_)
|
|
|
+{
|
|
|
+ x() = v.x();
|
|
|
+ y() = v.y();
|
|
|
+ z() = z_;
|
|
|
+ w() = w_;
|
|
|
+}
|
|
|
|
|
|
// constructor [vec3, float]
|
|
|
-inline Quat::Quat(const Vec3& v3, float w_):
|
|
|
- x(v3.x()), y(v3.y()), z(v3.z()), w(w_)
|
|
|
-{}
|
|
|
+inline Quat::Quat(const Vec3& v, float w_)
|
|
|
+{
|
|
|
+ x() = v.x();
|
|
|
+ y() = v.y();
|
|
|
+ z() = v.z();
|
|
|
+ w() = w_;
|
|
|
+}
|
|
|
|
|
|
// constructor [vec4]
|
|
|
-inline Quat::Quat(const Vec4& v4):
|
|
|
- x(v4.x()), y(v4.y()), z(v4.z()), w(v4.w())
|
|
|
-{}
|
|
|
+inline Quat::Quat(const Vec4& v)
|
|
|
+{
|
|
|
+ x() = v.x();
|
|
|
+ y() = v.y();
|
|
|
+ z() = v.z();
|
|
|
+ w() = v.w();
|
|
|
+}
|
|
|
|
|
|
-// constructor [quat]
|
|
|
-inline Quat::Quat(const Quat& b):
|
|
|
- x(b.x), y(b.y), z(b.z), w(b.w)
|
|
|
-{}
|
|
|
+// Copy
|
|
|
+inline Quat::Quat(const Quat& b)
|
|
|
+{
|
|
|
+ x() = b.x();
|
|
|
+ y() = b.y();
|
|
|
+ z() = b.z();
|
|
|
+ w() = b.w();
|
|
|
+}
|
|
|
|
|
|
-// constructor [mat3]
|
|
|
+// mat3
|
|
|
inline Quat::Quat(const Mat3& m3)
|
|
|
{
|
|
|
float trace = m3(0, 0) + m3(1, 1) + m3(2, 2) + 1.0;
|
|
|
if(trace > EPSILON)
|
|
|
{
|
|
|
float s = 0.5 / sqrt(trace);
|
|
|
- w = 0.25 / s;
|
|
|
- x = (m3(2, 1) - m3(1, 2)) * s;
|
|
|
- y = (m3(0, 2) - m3(2, 0)) * s;
|
|
|
- z = (m3(1, 0) - m3(0, 1)) * s;
|
|
|
+ w() = 0.25 / s;
|
|
|
+ x() = (m3(2, 1) - m3(1, 2)) * s;
|
|
|
+ y() = (m3(0, 2) - m3(2, 0)) * s;
|
|
|
+ z() = (m3(1, 0) - m3(0, 1)) * s;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
if(m3(0, 0) > m3(1, 1) && m3(0, 0) > m3(2, 2))
|
|
|
{
|
|
|
float s = 0.5 / sqrt(1.0 + m3(0, 0) - m3(1, 1) - m3(2, 2));
|
|
|
- w = (m3(1, 2) - m3(2, 1)) * s;
|
|
|
- x = 0.25 / s;
|
|
|
- y = (m3(0, 1) + m3(1, 0)) * s;
|
|
|
- z = (m3(0, 2) + m3(2, 0)) * s;
|
|
|
+ w() = (m3(1, 2) - m3(2, 1)) * s;
|
|
|
+ x() = 0.25 / s;
|
|
|
+ y() = (m3(0, 1) + m3(1, 0)) * s;
|
|
|
+ z() = (m3(0, 2) + m3(2, 0)) * s;
|
|
|
}
|
|
|
else if(m3(1, 1) > m3(2, 2))
|
|
|
{
|
|
|
float s = 0.5 / sqrt(1.0 + m3(1, 1) - m3(0, 0) - m3(2, 2));
|
|
|
- w = (m3(0, 2) - m3(2, 0)) * s;
|
|
|
- x = (m3(0, 1) + m3(1, 0)) * s;
|
|
|
- y = 0.25 / s;
|
|
|
- z = (m3(1, 2) + m3(2, 1)) * s;
|
|
|
+ w() = (m3(0, 2) - m3(2, 0)) * s;
|
|
|
+ x() = (m3(0, 1) + m3(1, 0)) * s;
|
|
|
+ y() = 0.25 / s;
|
|
|
+ z() = (m3(1, 2) + m3(2, 1)) * s;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
float s = 0.5 / sqrt(1.0 + m3(2, 2) - m3(0, 0) - m3(1, 1));
|
|
|
- w = (m3(0, 1) - m3(1, 0)) * s;
|
|
|
- x = (m3(0, 2) + m3(2, 0)) * s;
|
|
|
- y = (m3(1, 2) + m3(2, 1)) * s;
|
|
|
- z = 0.25 / s;
|
|
|
+ w() = (m3(0, 1) - m3(1, 0)) * s;
|
|
|
+ x() = (m3(0, 2) + m3(2, 0)) * s;
|
|
|
+ y() = (m3(1, 2) + m3(2, 1)) * s;
|
|
|
+ z() = 0.25 / s;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-// constructor [euler]
|
|
|
+// euler
|
|
|
inline Quat::Quat(const Euler& eu)
|
|
|
{
|
|
|
float cx, sx;
|
|
|
- sinCos(eu.getHeading()*0.5, sx, cx);
|
|
|
+ sinCos(eu.getHeading() * 0.5, sx, cx);
|
|
|
|
|
|
float cy, sy;
|
|
|
- sinCos(eu.getAttitude()*0.5, sy, cy);
|
|
|
+ sinCos(eu.getAttitude() * 0.5, sy, cy);
|
|
|
|
|
|
float cz, sz;
|
|
|
- sinCos(eu.getBank()*0.5, sz, cz);
|
|
|
-
|
|
|
- float cxcy = cx*cy;
|
|
|
- float sxsy = sx*sy;
|
|
|
- x = cxcy*sz + sxsy*cz;
|
|
|
- y = sx*cy*cz + cx*sy*sz;
|
|
|
- z = cx*sy*cz - sx*cy*sz;
|
|
|
- w = cxcy*cz - sxsy*sz;
|
|
|
+ sinCos(eu.getBank() * 0.5, sz, cz);
|
|
|
+
|
|
|
+ float cxcy = cx * cy;
|
|
|
+ float sxsy = sx * sy;
|
|
|
+ x() = cxcy * sz + sxsy * cz;
|
|
|
+ y() = sx * cy * cz + cx * sy * sz;
|
|
|
+ z() = cx * sy * cz - sx * cy * sz;
|
|
|
+ w() = cxcy * cz - sxsy * sz;
|
|
|
}
|
|
|
|
|
|
-// constructor [euler]
|
|
|
+// euler
|
|
|
inline Quat::Quat(const Axisang& axisang)
|
|
|
{
|
|
|
- float lengthsq = axisang.axis.getLengthSquared();
|
|
|
+ float lengthsq = axisang.getAxis().getLengthSquared();
|
|
|
if(isZero(lengthsq))
|
|
|
{
|
|
|
ME = getIdentity();
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
- float rad = axisang.ang * 0.5;
|
|
|
+ float rad = axisang.getAngle() * 0.5;
|
|
|
|
|
|
float sintheta, costheta;
|
|
|
sinCos(rad, sintheta, costheta);
|
|
|
|
|
|
float scalefactor = sintheta / sqrt(lengthsq);
|
|
|
|
|
|
- x = scalefactor * axisang.axis.x();
|
|
|
- y = scalefactor * axisang.axis.y();
|
|
|
- z = scalefactor * axisang.axis.z();
|
|
|
- w = costheta;
|
|
|
+ x() = scalefactor * axisang.getAxis().x();
|
|
|
+ y() = scalefactor * axisang.getAxis().y();
|
|
|
+ z() = scalefactor * axisang.getAxis().z();
|
|
|
+ w() = costheta;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+//======================================================================================================================
|
|
|
+// Accessors =
|
|
|
+//======================================================================================================================
|
|
|
+
|
|
|
+inline float Quat::x() const
|
|
|
+{
|
|
|
+ return vec.x;
|
|
|
}
|
|
|
|
|
|
+
|
|
|
+inline float& Quat::x()
|
|
|
+{
|
|
|
+ return vec.x;
|
|
|
+}
|
|
|
+
|
|
|
+inline float Quat::y() const
|
|
|
+{
|
|
|
+ return vec.y;
|
|
|
+}
|
|
|
+
|
|
|
+inline float& Quat::y()
|
|
|
+{
|
|
|
+ return vec.y;
|
|
|
+}
|
|
|
+
|
|
|
+inline float Quat::z() const
|
|
|
+{
|
|
|
+ return vec.z;
|
|
|
+}
|
|
|
+
|
|
|
+inline float& Quat::z()
|
|
|
+{
|
|
|
+ return vec.z;
|
|
|
+}
|
|
|
+
|
|
|
+inline float Quat::w() const
|
|
|
+{
|
|
|
+ return vec.w;
|
|
|
+}
|
|
|
+
|
|
|
+inline float& Quat::w()
|
|
|
+{
|
|
|
+ return vec.w;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+//======================================================================================================================
|
|
|
+// Operators with same =
|
|
|
+//======================================================================================================================
|
|
|
+
|
|
|
// *
|
|
|
inline Quat Quat::operator *(const Quat& b) const
|
|
|
{
|
|
|
return Quat(
|
|
|
- x * b.w + y * b.z - z * b.y + w * b.x,
|
|
|
- -x * b.z + y * b.w + z * b.x + w * b.y,
|
|
|
- x * b.y - y * b.x + z * b.w + w * b.z,
|
|
|
- -x * b.x - y * b.y - z * b.z + w * b.w
|
|
|
+ x() * b.w() + y() * b.z() - z() * b.y() + w() * b.x(),
|
|
|
+ -x() * b.z() + y() * b.w() + z() * b.x() + w() * b.y(),
|
|
|
+ x() * b.y() - y() * b.x() + z() * b.w() + w() * b.z(),
|
|
|
+ -x() * b.x() - y() * b.y() - z() * b.z() + w() * b.w()
|
|
|
);
|
|
|
}
|
|
|
|
|
|
@@ -146,27 +223,32 @@ inline Quat& Quat::operator *=(const Quat& b)
|
|
|
// ==
|
|
|
inline bool Quat::operator ==(const Quat& b) const
|
|
|
{
|
|
|
- return (isZero(x-b.x) && isZero(y-b.y) && isZero(z-b.z) && isZero(w-b.w)) ? true : false;
|
|
|
+ return isZero(x() - b.x()) && isZero(y() - b.y()) && isZero(z() - b.z()) && isZero(w() - b.w());
|
|
|
}
|
|
|
|
|
|
// !=
|
|
|
inline bool Quat::operator !=(const Quat& b) const
|
|
|
{
|
|
|
- return (isZero(x-b.x) && isZero(y-b.y) && isZero(z-b.z) && isZero(w-b.w)) ? false : true;
|
|
|
+ return !(isZero(x() - b.x()) && isZero(y() - b.y()) && isZero(z() - b.z()) && isZero(w() - b.w()));
|
|
|
}
|
|
|
|
|
|
+
|
|
|
+//======================================================================================================================
|
|
|
+// Other =
|
|
|
+//======================================================================================================================
|
|
|
+
|
|
|
// conjugate
|
|
|
inline void Quat::conjugate()
|
|
|
{
|
|
|
- x = -x;
|
|
|
- y = -y;
|
|
|
- z = -z;
|
|
|
+ x() = -x();
|
|
|
+ y() = -y();
|
|
|
+ z() = -z();
|
|
|
}
|
|
|
|
|
|
// getConjugated
|
|
|
inline Quat Quat::getConjugated() const
|
|
|
{
|
|
|
- return Quat(-x, -y, -z, w);
|
|
|
+ return Quat(-x(), -y(), -z(), w());
|
|
|
}
|
|
|
|
|
|
// Normalized
|
|
|
@@ -184,19 +266,19 @@ inline void Quat::normalize()
|
|
|
// getLength
|
|
|
inline float Quat::getLength() const
|
|
|
{
|
|
|
- return M::sqrt(w*w + x*x + y*y + z*z);
|
|
|
+ return M::sqrt(w() * w() + x() * x() + y() * y() + z() * z());
|
|
|
}
|
|
|
|
|
|
|
|
|
// getInverted
|
|
|
inline Quat Quat::getInverted() const
|
|
|
{
|
|
|
- float norm = w*w + x*x + y*y + z*z;
|
|
|
+ float norm = w() * w() + x() * x() + y() * y() + z() * z();
|
|
|
|
|
|
RASSERT_THROW_EXCEPTION(isZero(norm)); // Norm is zero
|
|
|
|
|
|
float normi = 1.0 / norm;
|
|
|
- return Quat(-normi*x, -normi*y, -normi*z, normi*w);
|
|
|
+ return Quat(-normi * x(), -normi * y(), -normi * z(), normi * w());
|
|
|
}
|
|
|
|
|
|
// invert
|
|
|
@@ -211,9 +293,9 @@ inline void Quat::setFrom2Vec3(const Vec3& from, const Vec3& to)
|
|
|
Vec3 axis(from.cross(to));
|
|
|
ME = Quat(axis.x(), axis.y(), axis.z(), from.dot(to));
|
|
|
normalize();
|
|
|
- w += 1.0;
|
|
|
+ w() += 1.0;
|
|
|
|
|
|
- if(w <= EPSILON)
|
|
|
+ if(w() <= EPSILON)
|
|
|
{
|
|
|
if(from.z() * from.z() > from.x() * from.x())
|
|
|
{
|
|
|
@@ -242,7 +324,7 @@ inline void Quat::rotate(const Quat& b)
|
|
|
// dot
|
|
|
inline float Quat::dot(const Quat& b) const
|
|
|
{
|
|
|
- return w*b.w + x*b.x + y*b.y + z*b.z;
|
|
|
+ return w() * b.w() + x() * b.x() + y() * b.y() + z() * b.z();
|
|
|
}
|
|
|
|
|
|
// SLERP
|
|
|
@@ -250,7 +332,7 @@ inline Quat Quat::slerp(const Quat& q1_, float t) const
|
|
|
{
|
|
|
const Quat& q0 = ME;
|
|
|
Quat q1(q1_);
|
|
|
- float cosHalfTheta = q0.w*q1.w + q0.x*q1.x + q0.y*q1.y + q0.z*q1.z;
|
|
|
+ float cosHalfTheta = q0.w() * q1.w() + q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z();
|
|
|
if(cosHalfTheta < 0.0)
|
|
|
{
|
|
|
q1 = Quat(-Vec4(q1)); // quat changes
|
|
|
@@ -281,8 +363,8 @@ inline Quat Quat::slerp(const Quat& q1_, float t) const
|
|
|
// setIdentity
|
|
|
inline void Quat::setIdentity()
|
|
|
{
|
|
|
- x = y = z = 0.0;
|
|
|
- w = 1.0;
|
|
|
+ x() = y() = z() = 0.0;
|
|
|
+ w() = 1.0;
|
|
|
}
|
|
|
|
|
|
// getIdentity
|
|
|
@@ -295,7 +377,7 @@ inline const Quat& Quat::getIdentity()
|
|
|
// print
|
|
|
inline std::ostream& operator<<(std::ostream& s, const Quat& q)
|
|
|
{
|
|
|
- s << q.w << ' ' << q.x << ' ' << q.y << ' ' << q.z;
|
|
|
+ s << q.w() << ' ' << q.x() << ' ' << q.y() << ' ' << q.z();
|
|
|
return s;
|
|
|
}
|
|
|
|