From 540f778047ed872371212413ab6771eaaa3e00dc Mon Sep 17 00:00:00 2001 From: RaveYard Date: Sun, 13 Nov 2022 14:08:49 +0100 Subject: [PATCH] Quaternion structure and math ops --- src/common/scripting/backend/codegen.cpp | 33 ++++++++-- src/common/scripting/backend/codegen.h | 2 +- src/common/scripting/core/types.cpp | 2 + src/common/scripting/core/types.h | 1 + src/common/scripting/interface/vmnatives.cpp | 63 ++++++++++++++++++++ src/common/scripting/jit/jit_math.cpp | 56 +++++++++++++++++ src/common/scripting/jit/jitintern.h | 21 ++++++- src/common/scripting/vm/vmexec.h | 17 +++++- src/common/scripting/vm/vmops.h | 4 ++ src/common/utility/vectors.h | 58 +++++++++++++----- wadsrc/static/zscript/engine/base.zs | 10 ++++ 11 files changed, 243 insertions(+), 24 deletions(-) diff --git a/src/common/scripting/backend/codegen.cpp b/src/common/scripting/backend/codegen.cpp index 5bedaea4faa..60e35e2b673 100644 --- a/src/common/scripting/backend/codegen.cpp +++ b/src/common/scripting/backend/codegen.cpp @@ -3093,7 +3093,14 @@ FxExpression *FxMulDiv::Resolve(FCompileContext& ctx) [[fallthrough]]; case '*': - if ((left->IsVector() || left->IsQuaternion()) && right->IsNumeric()) + if (Operator == '*' && left->IsQuaternion() && (right->IsVector3() || right->IsQuaternion())) + { + // quat * vec3 + // quat * quat + ValueType = right->ValueType; + break; + } + else if ((left->IsVector() || left->IsQuaternion()) && right->IsNumeric()) { if (right->IsInteger()) { @@ -3208,10 +3215,26 @@ ExpEmit FxMulDiv::Emit(VMFunctionBuilder *build) ExpEmit op1 = left->Emit(build); ExpEmit op2 = right->Emit(build); - if (IsVector() || IsQuaternion()) + if (Operator == '*' && left->IsQuaternion() && right->IsQuaternion()) + { + op1.Free(build); + op2.Free(build); + ExpEmit to(build, ValueType->GetRegType(), ValueType->GetRegCount()); + build->Emit(OP_MULQQ_RR, to.RegNum, op1.RegNum, op2.RegNum); + return to; + } + else if (Operator == '*' && left->IsQuaternion() && right->IsVector3()) + { + op1.Free(build); + op2.Free(build); + ExpEmit to(build, ValueType->GetRegType(), ValueType->GetRegCount()); + build->Emit(OP_MULQV3_RR, to.RegNum, op1.RegNum, op2.RegNum); + return to; + } + else if (IsVector() || IsQuaternion()) { assert(Operator != '%'); - if (right->IsVector()) + if (left->IsFloat()) { std::swap(op1, op2); } @@ -8385,9 +8408,9 @@ FxExpression *FxMemberFunctionCall::Resolve(FCompileContext& ctx) delete this; return x->Resolve(ctx); } - } - + Self->ValueType = TypeQuaternionStruct; + } else if (Self->ValueType == TypeString) { if (MethodName == NAME_Length) // This is an intrinsic because a dedicated opcode exists for it. diff --git a/src/common/scripting/backend/codegen.h b/src/common/scripting/backend/codegen.h index 364333d85c5..d7bd6058752 100644 --- a/src/common/scripting/backend/codegen.h +++ b/src/common/scripting/backend/codegen.h @@ -344,7 +344,7 @@ class FxExpression bool IsVector2() const { return ValueType == TypeVector2 || ValueType == TypeFVector2; }; bool IsVector3() const { return ValueType == TypeVector3 || ValueType == TypeFVector3; }; bool IsVector4() const { return ValueType == TypeVector4 || ValueType == TypeFVector4; }; - bool IsQuaternion() const { return ValueType == TypeQuaternion || ValueType == TypeFQuaternion; }; + bool IsQuaternion() const { return ValueType == TypeQuaternion || ValueType == TypeFQuaternion || ValueType == TypeQuaternionStruct; }; bool IsBoolCompat() const { return ValueType->isScalar(); } bool IsObject() const { return ValueType->isObjectPointer(); } bool IsArray() const { return ValueType->isArray() || (ValueType->isPointer() && ValueType->toPointer()->PointedType->isArray()); } diff --git a/src/common/scripting/core/types.cpp b/src/common/scripting/core/types.cpp index 046c8358e13..f8387d10d2e 100644 --- a/src/common/scripting/core/types.cpp +++ b/src/common/scripting/core/types.cpp @@ -69,6 +69,7 @@ PStruct* TypeFVector4; PStruct* TypeFQuaternion; PStruct *TypeColorStruct; PStruct *TypeStringStruct; +PStruct* TypeQuaternionStruct; PPointer *TypeNullPtr; PPointer *TypeVoidPtr; @@ -316,6 +317,7 @@ void PType::StaticInit() TypeVoidPtr = NewPointer(TypeVoid, false); TypeColorStruct = NewStruct("@ColorStruct", nullptr); //This name is intentionally obfuscated so that it cannot be used explicitly. The point of this type is to gain access to the single channels of a color value. TypeStringStruct = NewStruct("Stringstruct", nullptr, true); + TypeQuaternionStruct = NewStruct("QuatStruct", nullptr, true); TypeFont = NewPointer(NewStruct("Font", nullptr, true)); #ifdef __BIG_ENDIAN__ TypeColorStruct->AddField(NAME_a, TypeUInt8); diff --git a/src/common/scripting/core/types.h b/src/common/scripting/core/types.h index 862138c1dee..14597ddfd7f 100644 --- a/src/common/scripting/core/types.h +++ b/src/common/scripting/core/types.h @@ -623,6 +623,7 @@ extern PStruct* TypeQuaternion; extern PStruct* TypeFQuaternion; extern PStruct *TypeColorStruct; extern PStruct *TypeStringStruct; +extern PStruct* TypeQuaternionStruct; extern PStatePointer *TypeState; extern PPointer *TypeFont; extern PStateLabel *TypeStateLabel; diff --git a/src/common/scripting/interface/vmnatives.cpp b/src/common/scripting/interface/vmnatives.cpp index 4a0d3758b8a..a078e71b9c6 100644 --- a/src/common/scripting/interface/vmnatives.cpp +++ b/src/common/scripting/interface/vmnatives.cpp @@ -1129,3 +1129,66 @@ DEFINE_FIELD(DStatusBarCore, drawClip); DEFINE_FIELD(DStatusBarCore, fullscreenOffsets); DEFINE_FIELD(DStatusBarCore, defaultScale); DEFINE_FIELD(DHUDFont, mFont); + +// +// Quaternion +DEFINE_ACTION_FUNCTION(_QuatStruct, FromEuler) +{ + 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; +} + +DEFINE_ACTION_FUNCTION(_QuatStruct, AxisAngle) +{ + PARAM_PROLOGUE; + PARAM_FLOAT(x); + PARAM_FLOAT(y); + PARAM_FLOAT(z); + PARAM_FLOAT(angle); + + I_Error("Quat.AxisAngle not implemented"); + ret->SetVector4({ 0, 1, 2, 3 }); // X Y Z W + return 1; +} + +DEFINE_ACTION_FUNCTION(_QuatStruct, Nlerp) +{ + PARAM_PROLOGUE; + PARAM_FLOAT(ax); + PARAM_FLOAT(ay); + PARAM_FLOAT(az); + PARAM_FLOAT(aw); + PARAM_FLOAT(bx); + PARAM_FLOAT(by); + PARAM_FLOAT(bz); + PARAM_FLOAT(bw); + PARAM_FLOAT(f); + + I_Error("Quat.NLerp not implemented"); + ret->SetVector4({ 0, 1, 2, 3 }); // X Y Z W + return 1; +} + +DEFINE_ACTION_FUNCTION(_QuatStruct, Slerp) +{ + PARAM_PROLOGUE; + PARAM_FLOAT(ax); + PARAM_FLOAT(ay); + PARAM_FLOAT(az); + PARAM_FLOAT(aw); + PARAM_FLOAT(bx); + PARAM_FLOAT(by); + PARAM_FLOAT(bz); + PARAM_FLOAT(bw); + PARAM_FLOAT(f); + + I_Error("Quat.SLerp not implemented"); + ret->SetVector4({ 0, 1, 2, 3 }); // X Y Z W + return 1; +} \ No newline at end of file diff --git a/src/common/scripting/jit/jit_math.cpp b/src/common/scripting/jit/jit_math.cpp index c75fa204160..24328b9d314 100644 --- a/src/common/scripting/jit/jit_math.cpp +++ b/src/common/scripting/jit/jit_math.cpp @@ -1606,6 +1606,62 @@ void JitCompiler::EmitEQV4_K() I_Error("EQV4_K is not used."); } +// Quaternion ops +void FuncMULQQ(void *result, double ax, double ay, double az, double aw, double bx, double by, double bz, double bw) +{ + *reinterpret_cast(result) = DQuaternion(ax, ay, az, aw) * DQuaternion(bx, by, bz, bw); +} + +void FuncMULQV3(void *result, double ax, double ay, double az, double aw, double bx, double by, double bz) +{ + *reinterpret_cast(result) = DQuaternion(ax, ay, az, aw) * DVector3(bx, by, bz); +} + +void JitCompiler::EmitMULQQ_RR() +{ + auto stack = GetTemporaryVectorStackStorage(); + auto tmp = newTempIntPtr(); + cc.lea(tmp, stack); + + auto call = CreateCall(FuncMULQQ); + call->setArg(0, tmp); + call->setArg(1, regF[B + 0]); + call->setArg(2, regF[B + 1]); + call->setArg(3, regF[B + 2]); + call->setArg(4, regF[B + 3]); + call->setArg(5, regF[C + 0]); + call->setArg(6, regF[C + 1]); + call->setArg(7, regF[C + 2]); + call->setArg(8, regF[C + 3]); + + cc.movsd(regF[A + 0], asmjit::x86::qword_ptr(tmp, 0)); + cc.movsd(regF[A + 1], asmjit::x86::qword_ptr(tmp, 8)); + cc.movsd(regF[A + 2], asmjit::x86::qword_ptr(tmp, 16)); + cc.movsd(regF[A + 3], asmjit::x86::qword_ptr(tmp, 24)); +} + +void JitCompiler::EmitMULQV3_RR() +{ + auto stack = GetTemporaryVectorStackStorage(); + auto tmp = newTempIntPtr(); + cc.lea(tmp, stack); + + auto call = CreateCall(FuncMULQV3); + call->setArg(0, tmp); + call->setArg(1, regF[B + 0]); + call->setArg(2, regF[B + 1]); + call->setArg(3, regF[B + 2]); + call->setArg(4, regF[B + 3]); + call->setArg(5, regF[C + 0]); + call->setArg(6, regF[C + 1]); + call->setArg(7, regF[C + 2]); + + cc.movsd(regF[A + 0], asmjit::x86::qword_ptr(tmp, 0)); + cc.movsd(regF[A + 1], asmjit::x86::qword_ptr(tmp, 8)); + cc.movsd(regF[A + 2], asmjit::x86::qword_ptr(tmp, 16)); +} + + ///////////////////////////////////////////////////////////////////////////// // Pointer math. diff --git a/src/common/scripting/jit/jitintern.h b/src/common/scripting/jit/jitintern.h index 2a3dda42652..aaf8d70a655 100644 --- a/src/common/scripting/jit/jitintern.h +++ b/src/common/scripting/jit/jitintern.h @@ -186,7 +186,13 @@ class JitCompiler asmjit::CCFuncCall *CreateCall(RetType(*func)(P1 p1, P2 p2, P3 p3, P4 p4, P5 p5, P6 p6)) { return cc.call(asmjit::imm_ptr(reinterpret_cast(static_cast(func))), asmjit::FuncSignature6()); } template - asmjit::CCFuncCall *CreateCall(RetType(*func)(P1 p1, P2 p2, P3 p3, P4 p4, P5 p5, P6 p6, P7 p7)) { return cc.call(asmjit::imm_ptr(reinterpret_cast(static_cast(func))), asmjit::FuncSignature7()); } + asmjit::CCFuncCall* CreateCall(RetType(*func)(P1 p1, P2 p2, P3 p3, P4 p4, P5 p5, P6 p6, P7 p7)) { return cc.call(asmjit::imm_ptr(reinterpret_cast(static_cast(func))), asmjit::FuncSignature7()); } + + template + asmjit::CCFuncCall* CreateCall(RetType(*func)(P1 p1, P2 p2, P3 p3, P4 p4, P5 p5, P6 p6, P7 p7, P8 p8)) { return cc.call(asmjit::imm_ptr(reinterpret_cast(static_cast(func))), asmjit::FuncSignature8()); } + + template + asmjit::CCFuncCall* CreateCall(RetType(*func)(P1 p1, P2 p2, P3 p3, P4 p4, P5 p5, P6 p6, P7 p7, P8 p8, P9 p9)) { return cc.call(asmjit::imm_ptr(reinterpret_cast(static_cast(func))), asmjit::FuncSignature9()); } FString regname; size_t tmpPosInt32, tmpPosInt64, tmpPosIntPtr, tmpPosXmmSd, tmpPosXmmSs, tmpPosXmmPd, resultPosInt32, resultPosIntPtr, resultPosXmmSd; @@ -305,6 +311,19 @@ class JitCompiler TArray labels; + // Get temporary storage enough for DVector4 which is required by operation such as MULQQ and MULQV3 + bool vectorStackAllocated = false; + asmjit::X86Mem vectorStack; + asmjit::X86Mem GetTemporaryVectorStackStorage() + { + if (!vectorStackAllocated) + { + vectorStack = cc.newStack(sizeof(DVector4), alignof(DVector4), "tmpDVector4"); + vectorStackAllocated = true; + } + return vectorStack; + } + const VMOP *pc; VM_UBYTE op; }; diff --git a/src/common/scripting/vm/vmexec.h b/src/common/scripting/vm/vmexec.h index b7428eff22b..1fa8ae44079 100644 --- a/src/common/scripting/vm/vmexec.h +++ b/src/common/scripting/vm/vmexec.h @@ -1887,7 +1887,22 @@ static int ExecScriptFunc(VMFrameStack *stack, VMReturn *ret, int numret) ASSERTF(B+3); ASSERTKF(C+3); fcp = &konstf[C]; goto Do_EQV4; - + OP(MULQV3_RR): + ASSERTF(a + 2); ASSERTF(B + 3); ASSERTF(C + 2); + { + const DQuaternion& q = reinterpret_cast(reg.f[B]); + const DVector3& v = reinterpret_cast(reg.f[C]); + reinterpret_cast(reg.f[A]) = q * v; + } + NEXTOP; + OP(MULQQ_RR): + ASSERTF(a + 3); ASSERTF(B + 3); ASSERTF(C + 3); + { + const DQuaternion& q1 = reinterpret_cast(reg.f[B]); + const DQuaternion& q2 = reinterpret_cast(reg.f[C]); + reinterpret_cast(reg.f[A]) = q1 * q2; + } + NEXTOP; OP(ADDA_RR): ASSERTA(a); ASSERTA(B); ASSERTD(C); c = reg.d[C]; diff --git a/src/common/scripting/vm/vmops.h b/src/common/scripting/vm/vmops.h index 9b9274d145f..1782e78b70c 100644 --- a/src/common/scripting/vm/vmops.h +++ b/src/common/scripting/vm/vmops.h @@ -278,6 +278,10 @@ xx(LENV4, lenv4, RFRV, NOP, 0, 0) // fA = vB.Length xx(EQV4_R, beqv4, CVRR, NOP, 0, 0) // if ((vB == vkC) != A) then pc++ (inexact if A & 33) xx(EQV4_K, beqv4, CVRK, NOP, 0, 0) // this will never be used. +// Quaternion math +xx(MULQQ_RR, mulqq, RVRVRV, NOP, 0, 0) // qA = qB * qC +xx(MULQV3_RR, mulqv3, RVRVRV, NOP, 0, 0) // qA = qB * vC + // Pointer math. xx(ADDA_RR, add, RPRPRI, NOP, 0, 0) // pA = pB + dkC xx(ADDA_RK, add, RPRPKI, ADDA_RR,4, REGT_INT) diff --git a/src/common/utility/vectors.h b/src/common/utility/vectors.h index 019eb9db13d..1d73e3194c7 100644 --- a/src/common/utility/vectors.h +++ b/src/common/utility/vectors.h @@ -724,6 +724,11 @@ struct TVector4 { } + TVector4(const vec_t v[4]) + : TVector4(v[0], v[1], v[2], v[3]) + { + } + void Zero() { Z = Y = X = W = 0; @@ -846,22 +851,6 @@ struct TVector4 return TVector4(v.X * scalar, v.Y * scalar, v.Z * scalar, v.W * scalar); } - // Multiply as Quaternion - TVector4& operator*= (const TVector4& v) - { - *this = *this * v; - return *this; - } - - friend TVector4 operator* (const TVector4& v1, const TVector4& v2) - { - return TVector4(v2.W * v1.X + v2.X * v1.W + v2.Y * v1.Z - v1.Z * v1.Y, - v2.W * v1.Y + v2.Y * v1.W + v2.Z * v1.X - v2.X * v1.Z, - v2.W * v1.Z + v2.Z * v1.W + v2.X * v1.Y - v2.Y * v1.X, - v2.W * v1.W - v2.X * v1.X - v2.Y * v1.Y - v2.Z * v1.Z - ); - } - // Scalar division TVector4 &operator/= (vec_t scalar) { @@ -1727,12 +1716,48 @@ inline TMatrix3x3::TMatrix3x3(const TVector3 &axis, TAngle degrees) } +template +class TQuaternion : public TVector4 +{ +public: + 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& 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); + } +}; + + 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; @@ -1740,6 +1765,7 @@ 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.); diff --git a/wadsrc/static/zscript/engine/base.zs b/wadsrc/static/zscript/engine/base.zs index 821508434f8..277d44a80bb 100644 --- a/wadsrc/static/zscript/engine/base.zs +++ b/wadsrc/static/zscript/engine/base.zs @@ -892,3 +892,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 AxisAngle(Vector3 xyz, double angle); + // native double Length(); + // native Quat Unit(); +}