From 63c2d9303326cd4508ef49498d369cc694571fb2 Mon Sep 17 00:00:00 2001 From: Gutawer Date: Sun, 20 Nov 2022 15:49:22 +0000 Subject: [PATCH] - quaternion improvements --- src/common/scripting/interface/vmnatives.cpp | 2 +- src/common/scripting/vm/vm.h | 1 + src/common/utility/quaternion.h | 352 +++++++++++++++++++ src/common/utility/vectors.h | 351 ------------------ 4 files changed, 354 insertions(+), 352 deletions(-) create mode 100644 src/common/utility/quaternion.h diff --git a/src/common/scripting/interface/vmnatives.cpp b/src/common/scripting/interface/vmnatives.cpp index 55cedc42a..a29a6b711 100644 --- a/src/common/scripting/interface/vmnatives.cpp +++ b/src/common/scripting/interface/vmnatives.cpp @@ -1134,7 +1134,7 @@ DEFINE_FIELD(DHUDFont, mFont); // Quaternion void QuatFromAngles(double yaw, double pitch, double roll, DQuaternion* pquat) { - *pquat = DQuaternion::FromAngles(yaw, pitch, roll); + *pquat = DQuaternion::FromAngles(DAngle::fromDeg(yaw), DAngle::fromDeg(pitch), DAngle::fromDeg(roll)); } DEFINE_ACTION_FUNCTION_NATIVE(_QuatStruct, FromAngles, QuatFromAngles) diff --git a/src/common/scripting/vm/vm.h b/src/common/scripting/vm/vm.h index 396fe2631..6b2612819 100644 --- a/src/common/scripting/vm/vm.h +++ b/src/common/scripting/vm/vm.h @@ -39,6 +39,7 @@ #include "autosegs.h" #include "zstring.h" #include "vectors.h" +#include "quaternion.h" #include "cmdlib.h" #include "engineerrors.h" #include "memarena.h" diff --git a/src/common/utility/quaternion.h b/src/common/utility/quaternion.h new file mode 100644 index 000000000..1437b56be --- /dev/null +++ b/src/common/utility/quaternion.h @@ -0,0 +1,352 @@ +#pragma once + +#include "vectors.h" + +template +class TQuaternion +{ +public: + typedef TVector2 Vector2; + typedef TVector3 Vector3; + + vec_t X, Y, Z, W; + + TQuaternion() = default; + + TQuaternion(vec_t x, vec_t y, vec_t z, vec_t w) + : X(x), Y(y), Z(z), W(w) + { + } + + TQuaternion(vec_t *o) + : X(o[0]), Y(o[1]), Z(o[2]), W(o[3]) + { + } + + TQuaternion(const TQuaternion &other) = default; + + TQuaternion(const Vector3 &v, vec_t s) + : X(v.X), Y(v.Y), Z(v.Z), W(s) + { + } + + TQuaternion(const vec_t v[4]) + : TQuaternion(v[0], v[1], v[2], v[3]) + { + } + + void Zero() + { + Z = Y = X = W = 0; + } + + bool isZero() const + { + return X == 0 && Y == 0 && Z == 0 && W == 0; + } + + TQuaternion &operator= (const TQuaternion &other) = default; + + // Access X and Y and Z as an array + vec_t &operator[] (int index) + { + return (&X)[index]; + } + + const vec_t &operator[] (int index) const + { + return (&X)[index]; + } + + // Test for equality + bool operator== (const TQuaternion &other) const + { + return X == other.X && Y == other.Y && Z == other.Z && W == other.W; + } + + // Test for inequality + bool operator!= (const TQuaternion &other) const + { + return X != other.X || Y != other.Y || Z != other.Z || W != other.W; + } + + // returns the XY fields as a 2D-vector. + const Vector2& XY() const + { + return *reinterpret_cast(this); + } + + Vector2& XY() + { + return *reinterpret_cast(this); + } + + // returns the XY fields as a 2D-vector. + const Vector3& XYZ() const + { + return *reinterpret_cast(this); + } + + Vector3& XYZ() + { + return *reinterpret_cast(this); + } + + + // Test for approximate equality + bool ApproximatelyEquals(const TQuaternion &other) const + { + return fabs(X - other.X) < EQUAL_EPSILON && fabs(Y - other.Y) < EQUAL_EPSILON && fabs(Z - other.Z) < EQUAL_EPSILON && fabs(W - other.W) < EQUAL_EPSILON; + } + + // Test for approximate inequality + bool DoesNotApproximatelyEqual(const TQuaternion &other) const + { + return fabs(X - other.X) >= EQUAL_EPSILON || fabs(Y - other.Y) >= EQUAL_EPSILON || fabs(Z - other.Z) >= EQUAL_EPSILON || fabs(W - other.W) >= EQUAL_EPSILON; + } + + // Unary negation + TQuaternion operator- () const + { + return TQuaternion(-X, -Y, -Z, -W); + } + + // Scalar addition + TQuaternion &operator+= (vec_t scalar) + { + X += scalar, Y += scalar, Z += scalar; W += scalar; + return *this; + } + + friend TQuaternion operator+ (const TQuaternion &v, vec_t scalar) + { + return TQuaternion(v.X + scalar, v.Y + scalar, v.Z + scalar, v.W + scalar); + } + + friend TQuaternion operator+ (vec_t scalar, const TQuaternion &v) + { + return TQuaternion(v.X + scalar, v.Y + scalar, v.Z + scalar, v.W + scalar); + } + + // Scalar subtraction + TQuaternion &operator-= (vec_t scalar) + { + X -= scalar, Y -= scalar, Z -= scalar, W -= scalar; + return *this; + } + + TQuaternion operator- (vec_t scalar) const + { + return TQuaternion(X - scalar, Y - scalar, Z - scalar, W - scalar); + } + + // Scalar multiplication + TQuaternion &operator*= (vec_t scalar) + { + X = vec_t(X *scalar), Y = vec_t(Y * scalar), Z = vec_t(Z * scalar), W = vec_t(W * scalar); + return *this; + } + + friend TQuaternion operator* (const TQuaternion &v, vec_t scalar) + { + return TQuaternion(v.X * scalar, v.Y * scalar, v.Z * scalar, v.W * scalar); + } + + friend TQuaternion operator* (vec_t scalar, const TQuaternion &v) + { + return TQuaternion(v.X * scalar, v.Y * scalar, v.Z * scalar, v.W * scalar); + } + + // Scalar division + TQuaternion &operator/= (vec_t scalar) + { + scalar = 1 / scalar, X = vec_t(X * scalar), Y = vec_t(Y * scalar), Z = vec_t(Z * scalar), W = vec_t(W * scalar); + return *this; + } + + TQuaternion operator/ (vec_t scalar) const + { + scalar = 1 / scalar; + return TQuaternion(X * scalar, Y * scalar, Z * scalar, W * scalar); + } + + // Vector addition + TQuaternion &operator+= (const TQuaternion &other) + { + X += other.X, Y += other.Y, Z += other.Z, W += other.W; + return *this; + } + + TQuaternion operator+ (const TQuaternion &other) const + { + return TQuaternion(X + other.X, Y + other.Y, Z + other.Z, W + other.W); + } + + // Vector subtraction + TQuaternion &operator-= (const TQuaternion &other) + { + X -= other.X, Y -= other.Y, Z -= other.Z, W -= other.W; + return *this; + } + + TQuaternion operator- (const TQuaternion &other) const + { + return TQuaternion(X - other.X, Y - other.Y, Z - other.Z, W - other.W); + } + + // Quaternion length + double Length() const + { + return g_sqrt(X*X + Y*Y + Z*Z + W*W); + } + + double LengthSquared() const + { + return X*X + Y*Y + Z*Z + W*W; + } + + double Sum() const + { + return abs(X) + abs(Y) + abs(Z) + abs(W); + } + + + // Return a unit vector facing the same direction as this one + TQuaternion Unit() const + { + double len = Length(); + if (len != 0) len = 1 / len; + return *this * (vec_t)len; + } + + // Scales this vector into a unit vector + void MakeUnit() + { + double len = Length(); + if (len != 0) len = 1 / len; + *this *= (vec_t)len; + } + + // Resizes this vector to be the specified length (if it is not 0) + TQuaternion &MakeResize(double len) + { + double vlen = Length(); + if (vlen != 0.) + { + double scale = len / vlen; + X = vec_t(X * scale); + Y = vec_t(Y * scale); + Z = vec_t(Z * scale); + W = vec_t(W * scale); + } + return *this; + } + + TQuaternion Resized(double len) const + { + double vlen = Length(); + if (vlen != 0.) + { + double scale = len / vlen; + return{ vec_t(X * scale), vec_t(Y * scale), vec_t(Z * scale), vec_t(W * scale) }; + } + else + { + return *this; + } + } + + // Dot product + vec_t operator | (const TQuaternion &other) const + { + return X*other.X + Y*other.Y + Z*other.Z + W*other.W; + } + + vec_t dot(const TQuaternion &other) const + { + return X*other.X + Y*other.Y + Z*other.Z + W*other.W; + } + + TQuaternion& operator*= (const TQuaternion& q) + { + *this = *this * q; + return *this; + } + + friend TQuaternion operator* (const TQuaternion& q1, const TQuaternion& q2) + { + return TQuaternion( + q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y, + q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X, + q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W, + q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z + ); + } + + // Rotate Vector3 by Quaternion q + friend TVector3 operator* (const TQuaternion& q, const TVector3& v) + { + auto r = TQuaternion({ v.X, v.Y, v.Z, 0 }) * TQuaternion({ -q.X, -q.Y, -q.Z, q.W }); + r = q * r; + return TVector3(r.X, r.Y, r.Z); + } + + TQuaternion Conjugate() + { + return TQuaternion(-X, -Y, -Z, +W); + } + TQuaternion Inverse() + { + return Conjugate() / LengthSquared(); + } + + static TQuaternion AxisAngle(TVector3 axis, TAngle angle) + { + auto lengthSquared = axis.LengthSquared(); + auto halfAngle = angle * 0.5; + auto sinTheta = halfAngle.Sin(); + auto cosTheta = halfAngle.Cos(); + auto factor = sinTheta / g_sqrt(lengthSquared); + TQuaternion ret; + ret.W = cosTheta; + ret.XYZ() = factor * axis; + return ret; + } + static TQuaternion FromAngles(TAngle yaw, TAngle pitch, TAngle roll) + { + auto zRotation = TQuaternion::AxisAngle(Vector3(vec_t{0.0}, vec_t{0.0}, vec_t{1.0}), yaw); + auto yRotation = TQuaternion::AxisAngle(Vector3(vec_t{0.0}, vec_t{1.0}, vec_t{0.0}), pitch); + auto xRotation = TQuaternion::AxisAngle(Vector3(vec_t{1.0}, vec_t{0.0}, vec_t{0.0}), roll); + return zRotation * yRotation * xRotation; + } + + static TQuaternion NLerp(TQuaternion from, TQuaternion to, vec_t t) + { + return (from * (vec_t{1.0} - t) + to * t).Unit(); + } + static TQuaternion SLerp(TQuaternion from, TQuaternion to, vec_t t) + { + auto dot = from.dot(to); + const auto dotThreshold = vec_t{0.9995}; + if (dot < vec_t{0.0}) + { + to = -to; + dot = -dot; + } + if (dot > dotThreshold) + { + return NLerp(from, to, t); + } + else + { + auto robustDot = clamp(dot, vec_t{-1.0}, vec_t{1.0}); + auto theta = TAngle::fromRad(g_acos(robustDot)); + auto scale0 = (theta * (vec_t{1.0} - t)).Sin(); + auto scale1 = (theta * t).Sin(); + return (from * scale0 + to * scale1).Unit(); + } + } +}; + +typedef TQuaternion FQuaternion; +typedef TQuaternion DQuaternion; diff --git a/src/common/utility/vectors.h b/src/common/utility/vectors.h index 4202c27db..439b7c7ec 100644 --- a/src/common/utility/vectors.h +++ b/src/common/utility/vectors.h @@ -1720,361 +1720,12 @@ inline TMatrix3x3::TMatrix3x3(const TVector3 &axis, TAngle degrees) Cells[2][2] = T( (t-txx-tyy) + c ); } - -template -class TQuaternion -{ -public: - typedef TVector2 Vector2; - typedef TVector3 Vector3; - - vec_t X, Y, Z, W; - - TQuaternion() = default; - - TQuaternion(vec_t x, vec_t y, vec_t z, vec_t w) - : X(x), Y(y), Z(z), W(w) - { - } - - TQuaternion(vec_t *o) - : X(o[0]), Y(o[1]), Z(o[2]), W(o[3]) - { - } - - TQuaternion(const TQuaternion &other) = default; - - TQuaternion(const Vector3 &v, vec_t s) - : X(v.X), Y(v.Y), Z(v.Z), W(s) - { - } - - TQuaternion(const vec_t v[4]) - : TQuaternion(v[0], v[1], v[2], v[3]) - { - } - - void Zero() - { - Z = Y = X = W = 0; - } - - bool isZero() const - { - return X == 0 && Y == 0 && Z == 0 && W == 0; - } - - TQuaternion &operator= (const TQuaternion &other) = default; - - // Access X and Y and Z as an array - vec_t &operator[] (int index) - { - return (&X)[index]; - } - - const vec_t &operator[] (int index) const - { - return (&X)[index]; - } - - // Test for equality - bool operator== (const TQuaternion &other) const - { - return X == other.X && Y == other.Y && Z == other.Z && W == other.W; - } - - // Test for inequality - bool operator!= (const TQuaternion &other) const - { - return X != other.X || Y != other.Y || Z != other.Z || W != other.W; - } - - // returns the XY fields as a 2D-vector. - const Vector2& XY() const - { - return *reinterpret_cast(this); - } - - Vector2& XY() - { - return *reinterpret_cast(this); - } - - // returns the XY fields as a 2D-vector. - const Vector3& XYZ() const - { - return *reinterpret_cast(this); - } - - Vector3& XYZ() - { - return *reinterpret_cast(this); - } - - - // Test for approximate equality - bool ApproximatelyEquals(const TQuaternion &other) const - { - return fabs(X - other.X) < EQUAL_EPSILON && fabs(Y - other.Y) < EQUAL_EPSILON && fabs(Z - other.Z) < EQUAL_EPSILON && fabs(W - other.W) < EQUAL_EPSILON; - } - - // Test for approximate inequality - bool DoesNotApproximatelyEqual(const TQuaternion &other) const - { - return fabs(X - other.X) >= EQUAL_EPSILON || fabs(Y - other.Y) >= EQUAL_EPSILON || fabs(Z - other.Z) >= EQUAL_EPSILON || fabs(W - other.W) >= EQUAL_EPSILON; - } - - // Unary negation - TQuaternion operator- () const - { - return TQuaternion(-X, -Y, -Z, -W); - } - - // Scalar addition - TQuaternion &operator+= (vec_t scalar) - { - X += scalar, Y += scalar, Z += scalar; W += scalar; - return *this; - } - - friend TQuaternion operator+ (const TQuaternion &v, vec_t scalar) - { - return TQuaternion(v.X + scalar, v.Y + scalar, v.Z + scalar, v.W + scalar); - } - - friend TQuaternion operator+ (vec_t scalar, const TQuaternion &v) - { - return TQuaternion(v.X + scalar, v.Y + scalar, v.Z + scalar, v.W + scalar); - } - - // Scalar subtraction - TQuaternion &operator-= (vec_t scalar) - { - X -= scalar, Y -= scalar, Z -= scalar, W -= scalar; - return *this; - } - - TQuaternion operator- (vec_t scalar) const - { - return TQuaternion(X - scalar, Y - scalar, Z - scalar, W - scalar); - } - - // Scalar multiplication - TQuaternion &operator*= (vec_t scalar) - { - X = vec_t(X *scalar), Y = vec_t(Y * scalar), Z = vec_t(Z * scalar), W = vec_t(W * scalar); - return *this; - } - - friend TQuaternion operator* (const TQuaternion &v, vec_t scalar) - { - return TQuaternion(v.X * scalar, v.Y * scalar, v.Z * scalar, v.W * scalar); - } - - friend TQuaternion operator* (vec_t scalar, const TQuaternion &v) - { - return TQuaternion(v.X * scalar, v.Y * scalar, v.Z * scalar, v.W * scalar); - } - - // Scalar division - TQuaternion &operator/= (vec_t scalar) - { - scalar = 1 / scalar, X = vec_t(X * scalar), Y = vec_t(Y * scalar), Z = vec_t(Z * scalar), W = vec_t(W * scalar); - return *this; - } - - TQuaternion operator/ (vec_t scalar) const - { - scalar = 1 / scalar; - return TQuaternion(X * scalar, Y * scalar, Z * scalar, W * scalar); - } - - // Vector addition - TQuaternion &operator+= (const TQuaternion &other) - { - X += other.X, Y += other.Y, Z += other.Z, W += other.W; - return *this; - } - - TQuaternion operator+ (const TQuaternion &other) const - { - return TQuaternion(X + other.X, Y + other.Y, Z + other.Z, W + other.W); - } - - // Vector subtraction - TQuaternion &operator-= (const TQuaternion &other) - { - X -= other.X, Y -= other.Y, Z -= other.Z, W -= other.W; - return *this; - } - - TQuaternion operator- (const TQuaternion &other) const - { - return TQuaternion(X - other.X, Y - other.Y, Z - other.Z, W - other.W); - } - - // Quaternion length - double Length() const - { - return g_sqrt(X*X + Y*Y + Z*Z + W*W); - } - - double LengthSquared() const - { - return X*X + Y*Y + Z*Z + W*W; - } - - double Sum() const - { - return abs(X) + abs(Y) + abs(Z) + abs(W); - } - - - // Return a unit vector facing the same direction as this one - TQuaternion Unit() const - { - double len = Length(); - if (len != 0) len = 1 / len; - return *this * (vec_t)len; - } - - // Scales this vector into a unit vector - void MakeUnit() - { - double len = Length(); - if (len != 0) len = 1 / len; - *this *= (vec_t)len; - } - - // Resizes this vector to be the specified length (if it is not 0) - TQuaternion &MakeResize(double len) - { - double vlen = Length(); - if (vlen != 0.) - { - double scale = len / vlen; - X = vec_t(X * scale); - Y = vec_t(Y * scale); - Z = vec_t(Z * scale); - W = vec_t(W * scale); - } - return *this; - } - - TQuaternion Resized(double len) const - { - double vlen = Length(); - if (vlen != 0.) - { - double scale = len / vlen; - return{ vec_t(X * scale), vec_t(Y * scale), vec_t(Z * scale), vec_t(W * scale) }; - } - else - { - return *this; - } - } - - // Dot product - vec_t operator | (const TQuaternion &other) const - { - return X*other.X + Y*other.Y + Z*other.Z + W*other.W; - } - - vec_t dot(const TQuaternion &other) const - { - return X*other.X + Y*other.Y + Z*other.Z + W*other.W; - } - - TQuaternion& operator*= (const TQuaternion& q) - { - *this = *this * q; - return *this; - } - - friend TQuaternion operator* (const TQuaternion& q1, const TQuaternion& q2) - { - return TQuaternion( - q1.W * q2.X + q1.X * q2.W + q1.Y * q2.Z - q1.Z * q2.Y, - q1.W * q2.Y - q1.X * q2.Z + q1.Y * q2.W + q1.Z * q2.X, - q1.W * q2.Z + q1.X * q2.Y - q1.Y * q2.X + q1.Z * q2.W, - q1.W * q2.W - q1.X * q2.X - q1.Y * q2.Y - q1.Z * q2.Z - ); - } - - // Rotate Vector3 by Quaternion q - friend TVector3 operator* (const TQuaternion& q, const TVector3& v) - { - auto r = TQuaternion({ v.X, v.Y, v.Z, 0 }) * TQuaternion({ -q.X, -q.Y, -q.Z, q.W }); - r = q * r; - return TVector3(r.X, r.Y, r.Z); - } - - TQuaternion Conjugate() - { - return TQuaternion(-X, -Y, -Z, +W); - } - TQuaternion Inverse() - { - return Conjugate() / LengthSquared(); - } - - static TQuaternion AxisAngle(TVector3 axis, TAngle angle) - { - auto lengthSquared = axis.LengthSquared(); - auto halfAngle = angle * 0.5; - auto sinTheta = halfAngle.Sin(); - auto cosTheta = halfAngle.Cos(); - auto factor = sinTheta / g_sqrt(lengthSquared); - TQuaternion ret; - ret.W = cosTheta; - ret.XYZ() = factor * axis; - return ret; - } - static TQuaternion FromAngles(double yaw, double pitch, double roll) - { - auto zRotation = TQuaternion::AxisAngle(Vector3(vec_t{0.0}, vec_t{0.0}, vec_t{1.0}), TAngle::fromDeg(yaw)); - auto yRotation = TQuaternion::AxisAngle(Vector3(vec_t{0.0}, vec_t{1.0}, vec_t{0.0}), TAngle::fromDeg(pitch)); - auto xRotation = TQuaternion::AxisAngle(Vector3(vec_t{1.0}, vec_t{0.0}, vec_t{0.0}), TAngle::fromDeg(roll)); - return zRotation * yRotation * xRotation; - } - - static TQuaternion NLerp(TQuaternion from, TQuaternion to, vec_t t) - { - return (from * (vec_t{1.0} - t) + to * t).Unit(); - } - static TQuaternion SLerp(TQuaternion from, TQuaternion to, vec_t t) - { - auto dot = from.dot(to); - const auto dotThreshold = vec_t{0.9995}; - if (dot < vec_t{0.0}) - { - to = -to; - dot = -dot; - } - if (dot > dotThreshold) - { - return NLerp(from, to, t); - } - else - { - auto robustDot = clamp(dot, vec_t{-1.0}, vec_t{1.0}); - auto theta = TAngle::fromRad(g_acos(robustDot)); - auto scale0 = (theta * (vec_t{1.0} - t)).Sin(); - auto scale1 = (theta * t).Sin(); - return (from * scale0 + to * scale1).Unit(); - } - } -}; - - typedef TVector2 FVector2; typedef TVector3 FVector3; typedef TVector4 FVector4; typedef TRotator FRotator; typedef TMatrix3x3 FMatrix3x3; typedef TAngle FAngle; -typedef TQuaternion FQuaternion; typedef TVector2 DVector2; typedef TVector3 DVector3; @@ -2082,7 +1733,6 @@ typedef TVector4 DVector4; typedef TRotator DRotator; typedef TMatrix3x3 DMatrix3x3; typedef TAngle DAngle; -typedef TQuaternion DQuaternion; constexpr DAngle nullAngle = DAngle::fromDeg(0.); constexpr DAngle minAngle = DAngle::fromDeg(1. / 65536.); @@ -2146,5 +1796,4 @@ protected: float m_d; }; - #endif