mirror of https://github.com/ZDoom/qzdoom.git
- quaternion improvements
This commit is contained in:
Gutawer
Christoph Oelckers
parent
9f0c518be9
commit
63c2d93033
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -0,0 +1,352 @@
|
|||
#pragma once
|
||||
|
||||
#include "vectors.h"
|
||||
|
||||
template<typename vec_t>
|
||||
class TQuaternion
|
||||
{
|
||||
public:
|
||||
typedef TVector2<vec_t> Vector2;
|
||||
typedef TVector3<vec_t> 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<const Vector2*>(this);
|
||||
}
|
||||
|
||||
Vector2& XY()
|
||||
{
|
||||
return *reinterpret_cast<Vector2*>(this);
|
||||
}
|
||||
|
||||
// returns the XY fields as a 2D-vector.
|
||||
const Vector3& XYZ() const
|
||||
{
|
||||
return *reinterpret_cast<const Vector3*>(this);
|
||||
}
|
||||
|
||||
Vector3& XYZ()
|
||||
{
|
||||
return *reinterpret_cast<Vector3*>(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<vec_t> operator* (const TQuaternion<vec_t>& q1, const TQuaternion<vec_t>& 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<vec_t> operator* (const TQuaternion<vec_t>& q, const TVector3<vec_t>& 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<vec_t> Conjugate()
|
||||
{
|
||||
return TQuaternion(-X, -Y, -Z, +W);
|
||||
}
|
||||
TQuaternion<vec_t> Inverse()
|
||||
{
|
||||
return Conjugate() / LengthSquared();
|
||||
}
|
||||
|
||||
static TQuaternion<vec_t> AxisAngle(TVector3<vec_t> axis, TAngle<vec_t> 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<vec_t> ret;
|
||||
ret.W = cosTheta;
|
||||
ret.XYZ() = factor * axis;
|
||||
return ret;
|
||||
}
|
||||
static TQuaternion<vec_t> FromAngles(TAngle<vec_t> yaw, TAngle<vec_t> pitch, TAngle<vec_t> 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<vec_t> NLerp(TQuaternion<vec_t> from, TQuaternion<vec_t> to, vec_t t)
|
||||
{
|
||||
return (from * (vec_t{1.0} - t) + to * t).Unit();
|
||||
}
|
||||
static TQuaternion<vec_t> SLerp(TQuaternion<vec_t> from, TQuaternion<vec_t> 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<vec_t>::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<float> FQuaternion;
|
||||
typedef TQuaternion<double> DQuaternion;
|
||||
|
|
@ -1720,361 +1720,12 @@ inline TMatrix3x3<T>::TMatrix3x3(const TVector3<T> &axis, TAngle<T> degrees)
|
|||
Cells[2][2] = T( (t-txx-tyy) + c );
|
||||
}
|
||||
|
||||
|
||||
template<typename vec_t>
|
||||
class TQuaternion
|
||||
{
|
||||
public:
|
||||
typedef TVector2<vec_t> Vector2;
|
||||
typedef TVector3<vec_t> 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<const Vector2*>(this);
|
||||
}
|
||||
|
||||
Vector2& XY()
|
||||
{
|
||||
return *reinterpret_cast<Vector2*>(this);
|
||||
}
|
||||
|
||||
// returns the XY fields as a 2D-vector.
|
||||
const Vector3& XYZ() const
|
||||
{
|
||||
return *reinterpret_cast<const Vector3*>(this);
|
||||
}
|
||||
|
||||
Vector3& XYZ()
|
||||
{
|
||||
return *reinterpret_cast<Vector3*>(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<vec_t> operator* (const TQuaternion<vec_t>& q1, const TQuaternion<vec_t>& 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<vec_t> operator* (const TQuaternion<vec_t>& q, const TVector3<vec_t>& 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<vec_t> Conjugate()
|
||||
{
|
||||
return TQuaternion(-X, -Y, -Z, +W);
|
||||
}
|
||||
TQuaternion<vec_t> Inverse()
|
||||
{
|
||||
return Conjugate() / LengthSquared();
|
||||
}
|
||||
|
||||
static TQuaternion<vec_t> AxisAngle(TVector3<vec_t> axis, TAngle<vec_t> 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<vec_t> ret;
|
||||
ret.W = cosTheta;
|
||||
ret.XYZ() = factor * axis;
|
||||
return ret;
|
||||
}
|
||||
static TQuaternion<vec_t> 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<vec_t>::fromDeg(yaw));
|
||||
auto yRotation = TQuaternion::AxisAngle(Vector3(vec_t{0.0}, vec_t{1.0}, vec_t{0.0}), TAngle<vec_t>::fromDeg(pitch));
|
||||
auto xRotation = TQuaternion::AxisAngle(Vector3(vec_t{1.0}, vec_t{0.0}, vec_t{0.0}), TAngle<vec_t>::fromDeg(roll));
|
||||
return zRotation * yRotation * xRotation;
|
||||
}
|
||||
|
||||
static TQuaternion<vec_t> NLerp(TQuaternion<vec_t> from, TQuaternion<vec_t> to, vec_t t)
|
||||
{
|
||||
return (from * (vec_t{1.0} - t) + to * t).Unit();
|
||||
}
|
||||
static TQuaternion<vec_t> SLerp(TQuaternion<vec_t> from, TQuaternion<vec_t> 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<vec_t>::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<float> FVector2;
|
||||
typedef TVector3<float> FVector3;
|
||||
typedef TVector4<float> FVector4;
|
||||
typedef TRotator<float> FRotator;
|
||||
typedef TMatrix3x3<float> FMatrix3x3;
|
||||
typedef TAngle<float> FAngle;
|
||||
typedef TQuaternion<float> FQuaternion;
|
||||
|
||||
typedef TVector2<double> DVector2;
|
||||
typedef TVector3<double> DVector3;
|
||||
|
|
@ -2082,7 +1733,6 @@ typedef TVector4<double> DVector4;
|
|||
typedef TRotator<double> DRotator;
|
||||
typedef TMatrix3x3<double> DMatrix3x3;
|
||||
typedef TAngle<double> DAngle;
|
||||
typedef TQuaternion<double> DQuaternion;
|
||||
|
||||
constexpr DAngle nullAngle = DAngle::fromDeg(0.);
|
||||
constexpr DAngle minAngle = DAngle::fromDeg(1. / 65536.);
|
||||
|
|
@ -2146,5 +1796,4 @@ protected:
|
|||
float m_d;
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
|
|
|||
Loading…
Reference in New Issue