From 9f0c518be9e6e35a181cc3c613532120b738af36 Mon Sep 17 00:00:00 2001 From: Gutawer Date: Sat, 19 Nov 2022 23:23:10 +0000 Subject: [PATCH] - add remaining quaternion function implementations --- src/common/scripting/backend/codegen.cpp | 1 + src/common/scripting/interface/vmnatives.cpp | 108 ++++-- src/common/scripting/vm/vm.h | 10 + src/common/utility/vectors.h | 325 ++++++++++++++++++- wadsrc/static/zscript/engine/base.zs | 9 +- 5 files changed, 427 insertions(+), 26 deletions(-) diff --git a/src/common/scripting/backend/codegen.cpp b/src/common/scripting/backend/codegen.cpp index 60e35e2b673..493e48b45b0 100644 --- a/src/common/scripting/backend/codegen.cpp +++ b/src/common/scripting/backend/codegen.cpp @@ -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(Self)->noglobal = true; } diff --git a/src/common/scripting/interface/vmnatives.cpp b/src/common/scripting/interface/vmnatives.cpp index a078e71b9c6..55cedc42a7b 100644 --- a/src/common/scripting/interface/vmnatives.cpp +++ b/src/common/scripting/interface/vmnatives.cpp @@ -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); +} + +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(_QuatStruct, Nlerp) +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); + + DQuaternion quat; + QuatSLerp(ax, ay, az, aw, bx, by, bz, bw, t, &quat); + ACTION_RETURN_QUAT(quat); +} - I_Error("Quat.SLerp not implemented"); - ret->SetVector4({ 0, 1, 2, 3 }); // X Y Z W - return 1; -} \ No newline at end of file +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); +} diff --git a/src/common/scripting/vm/vm.h b/src/common/scripting/vm/vm.h index 35db9a73266..396fe263190 100644 --- a/src/common/scripting/vm/vm.h +++ b/src/common/scripting/vm/vm.h @@ -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) diff --git a/src/common/utility/vectors.h b/src/common/utility/vectors.h index 1d73e3194c7..4202c27dba6 100644 --- a/src/common/utility/vectors.h +++ b/src/common/utility/vectors.h @@ -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 @@ -1717,13 +1722,269 @@ inline TMatrix3x3::TMatrix3x3(const TVector3 &axis, TAngle degrees) template -class TQuaternion : public TVector4 +class TQuaternion { public: + typedef TVector2 Vector2; + typedef TVector3 Vector3; + + vec_t X, Y, Z, W; + TQuaternion() = default; - TQuaternion(vec_t a, vec_t b, vec_t c, vec_t d) : TVector4(a, b, c, d) {} - TQuaternion(const vec_t* o) : TVector4(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(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) { @@ -1748,6 +2009,62 @@ class TQuaternion : public TVector4 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(); + } + } }; diff --git a/wadsrc/static/zscript/engine/base.zs b/wadsrc/static/zscript/engine/base.zs index 277d44a80bb..cd2269661b9 100644 --- a/wadsrc/static/zscript/engine/base.zs +++ b/wadsrc/static/zscript/engine/base.zs @@ -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(); }