- add remaining quaternion function implementations

This commit is contained in:
Gutawer 2022-11-19 23:23:10 +00:00 committed by Christoph Oelckers
parent 540f778047
commit 9f0c518be9
5 changed files with 427 additions and 26 deletions

View File

@ -8229,6 +8229,7 @@ FxExpression *FxMemberFunctionCall::Resolve(FCompileContext& ctx)
// because the resulting value type would cause problems in nearly every other place where identifiers are being used.
// [ZZ] substitute ccls for String internal type.
if (id == NAME_String) ccls = TypeStringStruct;
else if (id == NAME_Quat || id == NAME_FQuat) ccls = TypeQuaternionStruct;
else ccls = FindContainerType(id, ctx);
if (ccls != nullptr) static_cast<FxIdentifier *>(Self)->noglobal = true;
}

View File

@ -1132,19 +1132,31 @@ DEFINE_FIELD(DHUDFont, mFont);
//
// Quaternion
DEFINE_ACTION_FUNCTION(_QuatStruct, FromEuler)
void QuatFromAngles(double yaw, double pitch, double roll, DQuaternion* pquat)
{
*pquat = DQuaternion::FromAngles(yaw, pitch, roll);
}
DEFINE_ACTION_FUNCTION_NATIVE(_QuatStruct, FromAngles, QuatFromAngles)
{
PARAM_PROLOGUE;
PARAM_FLOAT(yaw);
PARAM_FLOAT(pitch);
PARAM_FLOAT(roll);
I_Error("Quat.FromEuler not implemented");
ret->SetVector4({0, 1, 2, 3}); // X Y Z W
return 1;
DQuaternion quat;
QuatFromAngles(yaw, pitch, roll, &quat);
ACTION_RETURN_QUAT(quat);
}
DEFINE_ACTION_FUNCTION(_QuatStruct, AxisAngle)
void QuatAxisAngle(double x, double y, double z, double angleDeg, DQuaternion* pquat)
{
auto axis = DVector3(x, y, z);
auto angle = DAngle::fromDeg(angleDeg);
*pquat = DQuaternion::AxisAngle(axis, angle);
}
DEFINE_ACTION_FUNCTION_NATIVE(_QuatStruct, AxisAngle, QuatAxisAngle)
{
PARAM_PROLOGUE;
PARAM_FLOAT(x);
@ -1152,12 +1164,24 @@ DEFINE_ACTION_FUNCTION(_QuatStruct, AxisAngle)
PARAM_FLOAT(z);
PARAM_FLOAT(angle);
I_Error("Quat.AxisAngle not implemented");
ret->SetVector4({ 0, 1, 2, 3 }); // X Y Z W
return 1;
DQuaternion quat;
QuatAxisAngle(x, y, z, angle, &quat);
ACTION_RETURN_QUAT(quat);
}
DEFINE_ACTION_FUNCTION(_QuatStruct, Nlerp)
void QuatNLerp(
double ax, double ay, double az, double aw,
double bx, double by, double bz, double bw,
double t,
DQuaternion* pquat
)
{
auto from = DQuaternion { ax, ay, az, aw };
auto to = DQuaternion { bx, by, bz, bw };
*pquat = DQuaternion::NLerp(from, to, t);
}
DEFINE_ACTION_FUNCTION_NATIVE(_QuatStruct, NLerp, QuatNLerp)
{
PARAM_PROLOGUE;
PARAM_FLOAT(ax);
@ -1168,14 +1192,26 @@ DEFINE_ACTION_FUNCTION(_QuatStruct, Nlerp)
PARAM_FLOAT(by);
PARAM_FLOAT(bz);
PARAM_FLOAT(bw);
PARAM_FLOAT(f);
PARAM_FLOAT(t);
I_Error("Quat.NLerp not implemented");
ret->SetVector4({ 0, 1, 2, 3 }); // X Y Z W
return 1;
DQuaternion quat;
QuatNLerp(ax, ay, az, aw, bx, by, bz, bw, t, &quat);
ACTION_RETURN_QUAT(quat);
}
DEFINE_ACTION_FUNCTION(_QuatStruct, Slerp)
void QuatSLerp(
double ax, double ay, double az, double aw,
double bx, double by, double bz, double bw,
double t,
DQuaternion* pquat
)
{
auto from = DQuaternion { ax, ay, az, aw };
auto to = DQuaternion { bx, by, bz, bw };
*pquat = DQuaternion::SLerp(from, to, t);
}
DEFINE_ACTION_FUNCTION_NATIVE(_QuatStruct, SLerp, QuatSLerp)
{
PARAM_PROLOGUE;
PARAM_FLOAT(ax);
@ -1186,9 +1222,43 @@ DEFINE_ACTION_FUNCTION(_QuatStruct, Slerp)
PARAM_FLOAT(by);
PARAM_FLOAT(bz);
PARAM_FLOAT(bw);
PARAM_FLOAT(f);
PARAM_FLOAT(t);
I_Error("Quat.SLerp not implemented");
ret->SetVector4({ 0, 1, 2, 3 }); // X Y Z W
return 1;
}
DQuaternion quat;
QuatSLerp(ax, ay, az, aw, bx, by, bz, bw, t, &quat);
ACTION_RETURN_QUAT(quat);
}
void QuatConjugate(
double x, double y, double z, double w,
DQuaternion* pquat
)
{
*pquat = DQuaternion(x, y, z, w).Conjugate();
}
DEFINE_ACTION_FUNCTION_NATIVE(_QuatStruct, Conjugate, QuatConjugate)
{
PARAM_SELF_STRUCT_PROLOGUE(DQuaternion);
DQuaternion quat;
QuatConjugate(self->X, self->Y, self->Z, self->W, &quat);
ACTION_RETURN_QUAT(quat);
}
void QuatInverse(
double x, double y, double z, double w,
DQuaternion* pquat
)
{
*pquat = DQuaternion(x, y, z, w).Inverse();
}
DEFINE_ACTION_FUNCTION_NATIVE(_QuatStruct, Inverse, QuatInverse)
{
PARAM_SELF_STRUCT_PROLOGUE(DQuaternion);
DQuaternion quat;
QuatInverse(self->X, self->Y, self->Z, self->W, &quat);
ACTION_RETURN_QUAT(quat);
}

View File

@ -147,6 +147,14 @@ struct VMReturn
((double *)Location)[2] = val[2];
((double *)Location)[3] = val[3];
}
void SetQuaternion(const DQuaternion &val)
{
assert(RegType == (REGT_FLOAT | REGT_MULTIREG4));
((double *)Location)[0] = val[0];
((double *)Location)[1] = val[1];
((double *)Location)[2] = val[2];
((double *)Location)[3] = val[3];
}
void SetVector(const double val[3])
{
assert(RegType == (REGT_FLOAT|REGT_MULTIREG3));
@ -748,6 +756,8 @@ class AActor;
#define ACTION_RETURN_FLOAT(v) do { double u = v; if (numret > 0) { assert(ret != nullptr); ret->SetFloat(u); return 1; } return 0; } while(0)
#define ACTION_RETURN_VEC2(v) do { DVector2 u = v; if (numret > 0) { assert(ret != nullptr); ret[0].SetVector2(u); return 1; } return 0; } while(0)
#define ACTION_RETURN_VEC3(v) do { DVector3 u = v; if (numret > 0) { assert(ret != nullptr); ret[0].SetVector(u); return 1; } return 0; } while(0)
#define ACTION_RETURN_VEC4(v) do { DVector4 u = v; if (numret > 0) { assert(ret != nullptr); ret[0].SetVector4(u); return 1; } return 0; } while(0)
#define ACTION_RETURN_QUAT(v) do { DQuaternion u = v; if (numret > 0) { assert(ret != nullptr); ret[0].SetQuaternion(u); return 1; } return 0; } while(0)
#define ACTION_RETURN_INT(v) do { int u = v; if (numret > 0) { assert(ret != NULL); ret->SetInt(u); return 1; } return 0; } while(0)
#define ACTION_RETURN_BOOL(v) ACTION_RETURN_INT(v)
#define ACTION_RETURN_STRING(v) do { FString u = v; if (numret > 0) { assert(ret != NULL); ret->SetString(u); return 1; } return 0; } while(0)

View File

@ -992,6 +992,11 @@ struct TVector4
{
return X*other.X + Y*other.Y + Z*other.Z + W*other.W;
}
vec_t dot(const TVector4 &other) const
{
return X*other.X + Y*other.Y + Z*other.Z + W*other.W;
}
};
template<class vec_t>
@ -1717,13 +1722,269 @@ inline TMatrix3x3<T>::TMatrix3x3(const TVector3<T> &axis, TAngle<T> degrees)
template<typename vec_t>
class TQuaternion : public TVector4<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 a, vec_t b, vec_t c, vec_t d) : TVector4<vec_t>(a, b, c, d) {}
TQuaternion(const vec_t* o) : TVector4<vec_t>(o[0], o[1], o[2], o[3]) {}
TQuaternion(const TQuaternion& other) = 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)
{
@ -1748,6 +2009,62 @@ public:
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();
}
}
};

View File

@ -895,10 +895,13 @@ struct Translation version("2.4")
// Convenient way to attach functions to Quat
struct QuatStruct native
{
native static Quat SLerp(Quat from, Quat to, double f);
native static Quat NLerp(Quat from, Quat to, double f);
native static Quat FromEuler(double yaw, double pitch, double roll);
native static Quat SLerp(Quat from, Quat to, double t);
native static Quat NLerp(Quat from, Quat to, double t);
native static Quat FromAngles(double yaw, double pitch, double roll);
native static Quat AxisAngle(Vector3 xyz, double angle);
native Quat Conjugate();
native Quat Inverse();
// native double Length();
// native double LengthSquared();
// native Quat Unit();
}