aboutsummaryrefslogtreecommitdiffstats
path: root/dev/MinGfx/src/quaternion.cc
diff options
context:
space:
mode:
Diffstat (limited to 'dev/MinGfx/src/quaternion.cc')
-rw-r--r--dev/MinGfx/src/quaternion.cc522
1 files changed, 261 insertions, 261 deletions
diff --git a/dev/MinGfx/src/quaternion.cc b/dev/MinGfx/src/quaternion.cc
index 24830c8..42723f1 100644
--- a/dev/MinGfx/src/quaternion.cc
+++ b/dev/MinGfx/src/quaternion.cc
@@ -1,261 +1,261 @@
-/*
-Copyright (c) 2017,2018 Regents of the University of Minnesota.
-All Rights Reserved.
-See corresponding header file for details.
-*/
-
-#define _USE_MATH_DEFINES
-#include "quaternion.h"
-
-#include "gfxmath.h"
-
-namespace mingfx {
-
-
-Quaternion::Quaternion() {
- q[0] = 0.0;
- q[1] = 0.0;
- q[2] = 0.0;
- q[3] = 1.0;
-}
-
-Quaternion::Quaternion(float qx, float qy, float qz, float qw) {
- q[0] = qx;
- q[1] = qy;
- q[2] = qz;
- q[3] = qw;
-}
-
-Quaternion::Quaternion(float *ptr) {
- q[0] = ptr[0];
- q[1] = ptr[1];
- q[2] = ptr[2];
- q[3] = ptr[3];
-}
-
-Quaternion::Quaternion(const Quaternion& other) {
- q[0] = other[0];
- q[1] = other[1];
- q[2] = other[2];
- q[3] = other[3];
-}
-
-Quaternion::~Quaternion() {
-}
-
-bool Quaternion::operator==(const Quaternion& other) const {
- return (fabs(other[0] - q[0]) < MINGFX_MATH_EPSILON &&
- fabs(other[1] - q[1]) < MINGFX_MATH_EPSILON &&
- fabs(other[2] - q[2]) < MINGFX_MATH_EPSILON &&
- fabs(other[3] - q[3]) < MINGFX_MATH_EPSILON);
-}
-
-bool Quaternion::operator!=(const Quaternion& other) const {
- return (fabs(other[0] - q[0]) >= MINGFX_MATH_EPSILON ||
- fabs(other[1] - q[1]) >= MINGFX_MATH_EPSILON ||
- fabs(other[2] - q[2]) >= MINGFX_MATH_EPSILON ||
- fabs(other[3] - q[3]) >= MINGFX_MATH_EPSILON);
-}
-
-Quaternion& Quaternion::operator=(const Quaternion& other) {
- q[0] = other[0];
- q[1] = other[1];
- q[2] = other[2];
- q[3] = other[3];
- return *this;
-}
-
-float Quaternion::operator[](const int i) const {
- if ((i>=0) && (i<=3)) {
- return q[i];
- }
- else {
- // this is an error!
- return 0.0;
- }
-}
-
-float& Quaternion::operator[](const int i) {
- return q[i];
-}
-
-
-const float * Quaternion::value_ptr() const {
- return q;
-}
-
-Quaternion Quaternion::Slerp(const Quaternion &other, float alpha) const {
- // https://en.wikipedia.org/wiki/Slerp
-
- Quaternion v0 = *this;
- Quaternion v1 = other;
-
- // Only unit quaternions are valid rotations.
- // Normalize to avoid undefined behavior.
- v0.Normalize();
- v1.Normalize();
-
- // Compute the cosine of the angle between the two vectors.
- float dot = v0.Dot(v1);
-
- // If the dot product is negative, the quaternions
- // have opposite handed-ness and slerp won't take
- // the shorter path. Fix by reversing one quaternion.
- if (dot < 0.0f) {
- v1 = -v1;
- dot = -dot;
- }
-
- const double DOT_THRESHOLD = 0.9995;
- if (dot > DOT_THRESHOLD) {
- // If the inputs are too close for comfort, linearly interpolate
- // and normalize the result.
-
- Quaternion result = v0 + alpha*(v1 - v0);
- result.Normalize();
- return result;
- }
-
- GfxMath::Clamp(dot, -1, 1); // Robustness: Stay within domain of acos()
- float theta_0 = GfxMath::acos(dot); // theta_0 = angle between input vectors
- float theta = theta_0 * alpha; // theta = angle between v0 and result
-
- float s0 = GfxMath::cos(theta) - dot
- * GfxMath::sin(theta) / GfxMath::sin(theta_0); // == sin(theta_0 - theta) / sin(theta_0)
- float s1 = GfxMath::sin(theta) / GfxMath::sin(theta_0);
-
- return (s0 * v0) + (s1 * v1);
-}
-
-Quaternion Quaternion::Slerp(const Quaternion &a, const Quaternion &b, float alpha) {
- return a.Slerp(b, alpha);
-}
-
-
-std::ostream & operator<< ( std::ostream &os, const Quaternion &q) {
- return os << "<" << q[0] << ", " << q[1] << ", " << q[2] << ", " << q[3] << ")";
-}
-
-std::istream & operator>> ( std::istream &is, Quaternion &q) {
- // format: <qx, qy, qz, qw>
- char dummy;
- return is >> dummy >> q[0] >> dummy >> q[1] >> dummy >> q[2] >> dummy >> q[3] >> dummy;
-}
-
-
-float Quaternion::Dot(const Quaternion& other) const {
- return q[0]*other[0] + q[1]*other[1] + q[2]*other[2] + q[3]*other[3];
-
-}
-
-float Quaternion::Length() const {
- return sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
-}
-
-void Quaternion::Normalize() {
- float sizeSq = + q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
- if (sizeSq < MINGFX_MATH_EPSILON) {
- return; // do nothing to zero quats
- }
- float scaleFactor = (float)1.0/(float)sqrt(sizeSq);
- q[0] *= scaleFactor;
- q[1] *= scaleFactor;
- q[2] *= scaleFactor;
- q[3] *= scaleFactor;
-}
-
-Quaternion Quaternion::ToUnit() const {
- Quaternion qtmp(*this);
- qtmp.Normalize();
- return qtmp;
-}
-
-/// Returns the conjugate of the quaternion.
-Quaternion Quaternion::Conjugate() const {
- return Quaternion(-q[0], -q[1], -q[2], q[3]);
-}
-
-
-Quaternion Quaternion::FromAxisAngle(const Vector3 &axis, float angle) {
- // [qx, qy, qz, qw] = [sin(a/2) * vx, sin(a/2)* vy, sin(a/2) * vz, cos(a/2)]
- float x = GfxMath::sin(angle/2.0) * axis[0];
- float y = GfxMath::sin(angle/2.0) * axis[1];
- float z = GfxMath::sin(angle/2.0) * axis[2];
- float w = GfxMath::cos(angle/2.0);
- return Quaternion(x,y,z,w);
-}
-
-
-Quaternion Quaternion::FromEulerAnglesZYX(const Vector3 &angles) {
- Quaternion rot_x = Quaternion::FromAxisAngle(Vector3::UnitX(), angles[0]);
- Quaternion rot_y = Quaternion::FromAxisAngle(Vector3::UnitY(), angles[1]);
- Quaternion rot_z = Quaternion::FromAxisAngle(Vector3::UnitZ(), angles[2]);
- return rot_z * rot_y * rot_x;
-}
-
-Vector3 Quaternion::ToEulerAnglesZYX() const {
- // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
-
- Vector3 angles;
-
- // roll (x-axis rotation)
- float sinr = +2.0f * (w() * x() + y() * z());
- float cosr = +1.0f - 2.0f * (x() * x() + y() * y());
- angles[0] = std::atan2(sinr, cosr);
-
- // pitch (y-axis rotation)
- float sinp = +2.0f * (w() * y() - z() * x());
- if (std::fabs(sinp) >= 1.f)
- angles[1] = std::copysign(GfxMath::HALF_PI, sinp); // use 90 degrees if out of range
- else
- angles[1] = std::asin(sinp);
-
- // yaw (z-axis rotation)
- float siny = +2.0f * (w() * z() + x() * y());
- float cosy = +1.0f - 2.0f * (y() * y() + z() * z());
- angles[2] = std::atan2(siny, cosy);
-
- return angles;
-}
-
-
-Quaternion operator*(const Quaternion& q1, const Quaternion& q2) {
- float real1 = q1[3];
- Vector3 imag1 = Vector3(q1[0], q1[1], q1[2]);
-
- float real2 = q2[3];
- Vector3 imag2 = Vector3(q2[0], q2[1], q2[2]);
-
- float real = real1*real2 - imag1.Dot(imag2);
- Vector3 imag = real1*imag2 + real2*imag1 + imag1.Cross(imag2);
-
- return Quaternion(imag[0], imag[1], imag[2], real);
-}
-
-
-Quaternion operator/(const Quaternion& q, const float s) {
- const float invS = 1.0f / s;
- return Quaternion(q[0]*invS, q[1]*invS, q[2]*invS, q[3]*invS);
-}
-
-Quaternion operator*(const float s, const Quaternion& q) {
- return Quaternion(q[0]*s, q[1]*s, q[2]*s, q[3]*s);
-}
-
-Quaternion operator*(const Quaternion& q, const float s) {
- return Quaternion(q[0]*s, q[1]*s, q[2]*s, q[3]*s);
-}
-
-Quaternion operator-(const Quaternion& q) {
- return Quaternion(-q[0], -q[1], -q[2], -q[3]);
-}
-
-Quaternion operator+(const Quaternion& q1, const Quaternion& q2) {
- return Quaternion(q1[0] + q2[0], q1[1] + q2[1], q1[2] + q2[2], q1[3] + q2[3]);
-}
-
-Quaternion operator-(const Quaternion& q1, const Quaternion& q2) {
- return Quaternion(q1[0] - q2[0], q1[1] - q2[1], q1[2] - q2[2], q1[3] - q2[3]);
-}
-
-} // end namespace
+/*
+Copyright (c) 2017,2018 Regents of the University of Minnesota.
+All Rights Reserved.
+See corresponding header file for details.
+*/
+
+#define _USE_MATH_DEFINES
+#include "quaternion.h"
+
+#include "gfxmath.h"
+
+namespace mingfx {
+
+
+Quaternion::Quaternion() {
+ q[0] = 0.0;
+ q[1] = 0.0;
+ q[2] = 0.0;
+ q[3] = 1.0;
+}
+
+Quaternion::Quaternion(float qx, float qy, float qz, float qw) {
+ q[0] = qx;
+ q[1] = qy;
+ q[2] = qz;
+ q[3] = qw;
+}
+
+Quaternion::Quaternion(float *ptr) {
+ q[0] = ptr[0];
+ q[1] = ptr[1];
+ q[2] = ptr[2];
+ q[3] = ptr[3];
+}
+
+Quaternion::Quaternion(const Quaternion& other) {
+ q[0] = other[0];
+ q[1] = other[1];
+ q[2] = other[2];
+ q[3] = other[3];
+}
+
+Quaternion::~Quaternion() {
+}
+
+bool Quaternion::operator==(const Quaternion& other) const {
+ return (fabs(other[0] - q[0]) < MINGFX_MATH_EPSILON &&
+ fabs(other[1] - q[1]) < MINGFX_MATH_EPSILON &&
+ fabs(other[2] - q[2]) < MINGFX_MATH_EPSILON &&
+ fabs(other[3] - q[3]) < MINGFX_MATH_EPSILON);
+}
+
+bool Quaternion::operator!=(const Quaternion& other) const {
+ return (fabs(other[0] - q[0]) >= MINGFX_MATH_EPSILON ||
+ fabs(other[1] - q[1]) >= MINGFX_MATH_EPSILON ||
+ fabs(other[2] - q[2]) >= MINGFX_MATH_EPSILON ||
+ fabs(other[3] - q[3]) >= MINGFX_MATH_EPSILON);
+}
+
+Quaternion& Quaternion::operator=(const Quaternion& other) {
+ q[0] = other[0];
+ q[1] = other[1];
+ q[2] = other[2];
+ q[3] = other[3];
+ return *this;
+}
+
+float Quaternion::operator[](const int i) const {
+ if ((i>=0) && (i<=3)) {
+ return q[i];
+ }
+ else {
+ // this is an error!
+ return 0.0;
+ }
+}
+
+float& Quaternion::operator[](const int i) {
+ return q[i];
+}
+
+
+const float * Quaternion::value_ptr() const {
+ return q;
+}
+
+Quaternion Quaternion::Slerp(const Quaternion &other, float alpha) const {
+ // https://en.wikipedia.org/wiki/Slerp
+
+ Quaternion v0 = *this;
+ Quaternion v1 = other;
+
+ // Only unit quaternions are valid rotations.
+ // Normalize to avoid undefined behavior.
+ v0.Normalize();
+ v1.Normalize();
+
+ // Compute the cosine of the angle between the two vectors.
+ float dot = v0.Dot(v1);
+
+ // If the dot product is negative, the quaternions
+ // have opposite handed-ness and slerp won't take
+ // the shorter path. Fix by reversing one quaternion.
+ if (dot < 0.0f) {
+ v1 = -v1;
+ dot = -dot;
+ }
+
+ const double DOT_THRESHOLD = 0.9995;
+ if (dot > DOT_THRESHOLD) {
+ // If the inputs are too close for comfort, linearly interpolate
+ // and normalize the result.
+
+ Quaternion result = v0 + alpha*(v1 - v0);
+ result.Normalize();
+ return result;
+ }
+
+ GfxMath::Clamp(dot, -1, 1); // Robustness: Stay within domain of acos()
+ float theta_0 = GfxMath::acos(dot); // theta_0 = angle between input vectors
+ float theta = theta_0 * alpha; // theta = angle between v0 and result
+
+ float s0 = GfxMath::cos(theta) - dot
+ * GfxMath::sin(theta) / GfxMath::sin(theta_0); // == sin(theta_0 - theta) / sin(theta_0)
+ float s1 = GfxMath::sin(theta) / GfxMath::sin(theta_0);
+
+ return (s0 * v0) + (s1 * v1);
+}
+
+Quaternion Quaternion::Slerp(const Quaternion &a, const Quaternion &b, float alpha) {
+ return a.Slerp(b, alpha);
+}
+
+
+std::ostream & operator<< ( std::ostream &os, const Quaternion &q) {
+ return os << "<" << q[0] << ", " << q[1] << ", " << q[2] << ", " << q[3] << ")";
+}
+
+std::istream & operator>> ( std::istream &is, Quaternion &q) {
+ // format: <qx, qy, qz, qw>
+ char dummy;
+ return is >> dummy >> q[0] >> dummy >> q[1] >> dummy >> q[2] >> dummy >> q[3] >> dummy;
+}
+
+
+float Quaternion::Dot(const Quaternion& other) const {
+ return q[0]*other[0] + q[1]*other[1] + q[2]*other[2] + q[3]*other[3];
+
+}
+
+float Quaternion::Length() const {
+ return sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
+}
+
+void Quaternion::Normalize() {
+ float sizeSq = + q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
+ if (sizeSq < MINGFX_MATH_EPSILON) {
+ return; // do nothing to zero quats
+ }
+ float scaleFactor = (float)1.0/(float)sqrt(sizeSq);
+ q[0] *= scaleFactor;
+ q[1] *= scaleFactor;
+ q[2] *= scaleFactor;
+ q[3] *= scaleFactor;
+}
+
+Quaternion Quaternion::ToUnit() const {
+ Quaternion qtmp(*this);
+ qtmp.Normalize();
+ return qtmp;
+}
+
+/// Returns the conjugate of the quaternion.
+Quaternion Quaternion::Conjugate() const {
+ return Quaternion(-q[0], -q[1], -q[2], q[3]);
+}
+
+
+Quaternion Quaternion::FromAxisAngle(const Vector3 &axis, float angle) {
+ // [qx, qy, qz, qw] = [sin(a/2) * vx, sin(a/2)* vy, sin(a/2) * vz, cos(a/2)]
+ float x = GfxMath::sin(angle/2.0) * axis[0];
+ float y = GfxMath::sin(angle/2.0) * axis[1];
+ float z = GfxMath::sin(angle/2.0) * axis[2];
+ float w = GfxMath::cos(angle/2.0);
+ return Quaternion(x,y,z,w);
+}
+
+
+Quaternion Quaternion::FromEulerAnglesZYX(const Vector3 &angles) {
+ Quaternion rot_x = Quaternion::FromAxisAngle(Vector3::UnitX(), angles[0]);
+ Quaternion rot_y = Quaternion::FromAxisAngle(Vector3::UnitY(), angles[1]);
+ Quaternion rot_z = Quaternion::FromAxisAngle(Vector3::UnitZ(), angles[2]);
+ return rot_z * rot_y * rot_x;
+}
+
+Vector3 Quaternion::ToEulerAnglesZYX() const {
+ // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
+
+ Vector3 angles;
+
+ // roll (x-axis rotation)
+ float sinr = +2.0f * (w() * x() + y() * z());
+ float cosr = +1.0f - 2.0f * (x() * x() + y() * y());
+ angles[0] = std::atan2(sinr, cosr);
+
+ // pitch (y-axis rotation)
+ float sinp = +2.0f * (w() * y() - z() * x());
+ if (std::fabs(sinp) >= 1.f)
+ angles[1] = std::copysign(GfxMath::HALF_PI, sinp); // use 90 degrees if out of range
+ else
+ angles[1] = std::asin(sinp);
+
+ // yaw (z-axis rotation)
+ float siny = +2.0f * (w() * z() + x() * y());
+ float cosy = +1.0f - 2.0f * (y() * y() + z() * z());
+ angles[2] = std::atan2(siny, cosy);
+
+ return angles;
+}
+
+
+Quaternion operator*(const Quaternion& q1, const Quaternion& q2) {
+ float real1 = q1[3];
+ Vector3 imag1 = Vector3(q1[0], q1[1], q1[2]);
+
+ float real2 = q2[3];
+ Vector3 imag2 = Vector3(q2[0], q2[1], q2[2]);
+
+ float real = real1*real2 - imag1.Dot(imag2);
+ Vector3 imag = real1*imag2 + real2*imag1 + imag1.Cross(imag2);
+
+ return Quaternion(imag[0], imag[1], imag[2], real);
+}
+
+
+Quaternion operator/(const Quaternion& q, const float s) {
+ const float invS = 1.0f / s;
+ return Quaternion(q[0]*invS, q[1]*invS, q[2]*invS, q[3]*invS);
+}
+
+Quaternion operator*(const float s, const Quaternion& q) {
+ return Quaternion(q[0]*s, q[1]*s, q[2]*s, q[3]*s);
+}
+
+Quaternion operator*(const Quaternion& q, const float s) {
+ return Quaternion(q[0]*s, q[1]*s, q[2]*s, q[3]*s);
+}
+
+Quaternion operator-(const Quaternion& q) {
+ return Quaternion(-q[0], -q[1], -q[2], -q[3]);
+}
+
+Quaternion operator+(const Quaternion& q1, const Quaternion& q2) {
+ return Quaternion(q1[0] + q2[0], q1[1] + q2[1], q1[2] + q2[2], q1[3] + q2[3]);
+}
+
+Quaternion operator-(const Quaternion& q1, const Quaternion& q2) {
+ return Quaternion(q1[0] - q2[0], q1[1] - q2[1], q1[2] - q2[2], q1[3] - q2[3]);
+}
+
+} // end namespace