From af653e8b67ea3e26e21ea98935fd7ba3dad903b6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Ricardo=20Lu=C3=ADs=20Vaz=20Silva?= Date: Thu, 22 May 2025 21:04:21 -0300 Subject: [PATCH] bone getters part 2.5 --- src/common/utility/vectors.h | 15 ++++ src/playsim/actor.h | 3 +- src/playsim/p_actionfunctions.cpp | 85 ++++++++++++++++++---- src/playsim/p_mobj.cpp | 100 ++++++++++++++++++++++++-- wadsrc/static/zscript/actors/actor.zs | 25 ++++--- 5 files changed, 197 insertions(+), 31 deletions(-) diff --git a/src/common/utility/vectors.h b/src/common/utility/vectors.h index d14c24c3a..1d4be750c 100644 --- a/src/common/utility/vectors.h +++ b/src/common/utility/vectors.h @@ -1299,6 +1299,21 @@ public: return TAngle(double(rad * (180.0 / pi::pi()))); } + static constexpr TAngle fromCos(double cos) + { + return fromRad(g_acos(cos)); + } + + static constexpr TAngle fromSin(double sin) + { + return fromRad(g_asin(sin)); + } + + static constexpr TAngle fromTan(double tan) + { + return fromRad(g_atan(tan)); + } + static constexpr TAngle fromBam(int f) { return TAngle(f * (90. / 0x40000000)); diff --git a/src/playsim/actor.h b/src/playsim/actor.h index b0ce878d4..b67bb7dad 100644 --- a/src/playsim/actor.h +++ b/src/playsim/actor.h @@ -819,7 +819,8 @@ public: //outmat must be double[16] void GetBoneMatrix(int model_index, int bone_index, bool with_override, double *outMat); - void GetBonePosition(int model_index, int bone_index, bool with_override, DVector3 &pos, DVector3 &normal); + DVector3 GetBoneEulerAngles(int model_index, int bone_index, bool with_override); + void GetBonePosition(int model_index, int bone_index, bool with_override, DVector3 &pos, DVector3 &fwd, DVector3 &up); void GetObjectToWorldMatrix(double *outMat); static AActor *StaticSpawn (FLevelLocals *Level, PClassActor *type, const DVector3 &pos, replace_t allowreplacement, bool SpawningMapThing = false); diff --git a/src/playsim/p_actionfunctions.cpp b/src/playsim/p_actionfunctions.cpp index bfa2dcc16..613575ea5 100644 --- a/src/playsim/p_actionfunctions.cpp +++ b/src/playsim/p_actionfunctions.cpp @@ -5947,7 +5947,7 @@ DEFINE_ACTION_FUNCTION(AActor, GetNamedBoneBaseRotation) // //================================================ -DEFINE_ACTION_FUNCTION(AActor, GetBone) +DEFINE_ACTION_FUNCTION(AActor, GetBoneTRS) { PARAM_SELF_PROLOGUE(AActor); PARAM_INT(bone_index); @@ -5987,7 +5987,7 @@ DEFINE_ACTION_FUNCTION(AActor, GetBone) return numret; } -DEFINE_ACTION_FUNCTION(AActor, GetNamedBone) +DEFINE_ACTION_FUNCTION(AActor, GetNamedBoneTRS) { PARAM_SELF_PROLOGUE(AActor); PARAM_NAME(bone_name); @@ -6030,6 +6030,45 @@ DEFINE_ACTION_FUNCTION(AActor, GetNamedBone) } + + +DEFINE_ACTION_FUNCTION(AActor, GetBoneEulerAngles) +{ + PARAM_SELF_PROLOGUE(AActor); + PARAM_INT(bone_index); + PARAM_BOOL(with_override); + + FModel * mdl = GetBoneShared(self, 0, bone_index, nullptr); + + if(mdl) + { + ACTION_RETURN_VEC3(self->GetBoneEulerAngles(0, bone_index, with_override)); + } + + + ACTION_RETURN_VEC3(DVector3(0,0,0)); +} + +DEFINE_ACTION_FUNCTION(AActor, GetNamedBoneEulerAngles) +{ + PARAM_SELF_PROLOGUE(AActor); + PARAM_NAME(bone_name); + PARAM_BOOL(with_override); + + int bone_index; + + FModel * mdl = GetBoneShared(self, 0, bone_index, &bone_name); + + if(mdl) + { + ACTION_RETURN_VEC3(self->GetBoneEulerAngles(0, bone_index, with_override)); + } + + ACTION_RETURN_VEC3(DVector3(0,0,0)); +} + + + DEFINE_ACTION_FUNCTION(AActor, TransformByBone) { PARAM_SELF_PROLOGUE(AActor); @@ -6037,24 +6076,33 @@ DEFINE_ACTION_FUNCTION(AActor, TransformByBone) PARAM_FLOAT(pos_x); PARAM_FLOAT(pos_y); PARAM_FLOAT(pos_z); - PARAM_FLOAT(dir_x); - PARAM_FLOAT(dir_y); - PARAM_FLOAT(dir_z); + PARAM_FLOAT(fwd_x); + PARAM_FLOAT(fwd_y); + PARAM_FLOAT(fwd_z); + PARAM_FLOAT(up_x); + PARAM_FLOAT(up_y); + PARAM_FLOAT(up_z); PARAM_BOOL(with_override); FModel * mdl = GetBoneShared(self, 0, bone_index, nullptr); DVector3 position(pos_x, pos_y, pos_z); - DVector3 direction(dir_x, dir_y, dir_z); + DVector3 fwd(fwd_x, fwd_y, fwd_z); + DVector3 up(up_x, up_y, up_z); if(mdl) { - self->GetBonePosition(0, bone_index, with_override, position, direction); + self->GetBonePosition(0, bone_index, with_override, position, fwd, up); + } + + if(numret > 2) + { + ret[2].SetVector(up); } if(numret > 1) { - ret[1].SetVector(direction); + ret[1].SetVector(fwd); } if(numret > 0) @@ -6072,9 +6120,12 @@ DEFINE_ACTION_FUNCTION(AActor, TransformByNamedBone) PARAM_FLOAT(pos_x); PARAM_FLOAT(pos_y); PARAM_FLOAT(pos_z); - PARAM_FLOAT(dir_x); - PARAM_FLOAT(dir_y); - PARAM_FLOAT(dir_z); + PARAM_FLOAT(fwd_x); + PARAM_FLOAT(fwd_y); + PARAM_FLOAT(fwd_z); + PARAM_FLOAT(up_x); + PARAM_FLOAT(up_y); + PARAM_FLOAT(up_z); PARAM_BOOL(with_override); int bone_index; @@ -6082,16 +6133,22 @@ DEFINE_ACTION_FUNCTION(AActor, TransformByNamedBone) FModel * mdl = GetBoneShared(self, 0, bone_index, &bone_name); DVector3 position(pos_x, pos_y, pos_z); - DVector3 direction(dir_x, dir_y, dir_z); + DVector3 fwd(fwd_x, fwd_y, fwd_z); + DVector3 up(up_x, up_y, up_z); if(mdl) { - self->GetBonePosition(0, bone_index, with_override, position, direction); + self->GetBonePosition(0, bone_index, with_override, position, fwd, up); + } + + if(numret > 2) + { + ret[2].SetVector(up); } if(numret > 1) { - ret[1].SetVector(direction); + ret[1].SetVector(fwd); } if(numret > 0) diff --git a/src/playsim/p_mobj.cpp b/src/playsim/p_mobj.cpp index 82db39595..0541f9ed2 100644 --- a/src/playsim/p_mobj.cpp +++ b/src/playsim/p_mobj.cpp @@ -3963,7 +3963,87 @@ void AActor::GetBoneMatrix(int model_index, int bone_index, bool with_override, } } -void AActor::GetBonePosition(int model_index, int bone_index, bool with_override, DVector3 &pos, DVector3 &normal) +DVector3 AActor::GetBoneEulerAngles(int model_index, int bone_index, bool with_override) +{ + if(modelData && modelData->flags & MODELDATA_GET_BONE_INFO) + { + if(picnum.isValid()) return DVector3(0,0,0); // picnum overrides don't render models + + FSpriteModelFrame *smf = FindModelFrame(this, sprite, frame, false); // dropped flag is for voxels + + FVector3 objPos = FVector3(Pos() + WorldOffset); + + VSMatrix boneMatrix = (with_override ? modelData->modelBoneInfo[model_index].positions_with_override : modelData->modelBoneInfo[model_index].positions)[bone_index]; + VSMatrix worldMatrix = smf->ObjectToWorldMatrix(this, objPos.X, objPos.Y, objPos.Z, 1.0); + + FVector4 oldFwd(1.0, 0.0, 0.0, 0.0); + FVector4 newFwd; + FVector4 oldUp(0.0, 1.0, 0.0, 0.0); + FVector4 newUp; + + boneMatrix.multMatrixPoint(&oldFwd.X, &newFwd.X); + boneMatrix.multMatrixPoint(&oldUp.X, &newUp.X); + + oldFwd = FVector4(FVector3(newFwd.X, newFwd.Y, newFwd.Z).Unit(), 0.0); + oldUp = FVector4(FVector3(newUp.X, newUp.Y, newUp.Z).Unit(), 0.0); + + worldMatrix.multMatrixPoint(&oldFwd.X, &newFwd.X); + worldMatrix.multMatrixPoint(&oldUp.X, &newUp.X); + + //billion thanks to https://www.jldoty.com/code/DirectX/YPRfromUF/YPRfromUF.html for helping figure out all this math + + DVector3 fwd = DVector3(newFwd.X, newFwd.Y, newFwd.Z).Unit(); + DVector3 up = DVector3(newUp.X, newUp.Y, newUp.Z).Unit(); + + DVector3 y(0,1,0); + + DAngle yaw = DAngle::fromRad(atan2(fwd.Z, fwd.X)); + DAngle pitch = DAngle::fromSin(-fwd.Y); + DAngle roll; + + if(fwd == y) + { // gimbal lock + yaw = DAngle::fromDeg(0); + roll = DAngle::fromSin(-up.X); + } + else + { + DVector3 right = (up ^ fwd).Unit(); // fwd and up must be perpendicular, thus right must be a unit vector + + DVector3 x0 = (y ^ fwd).Unit(); // right vector with no roll, y and fwd aren't perpendicular, so it must be normalized into a unit vector + DVector3 y0 = (fwd ^ x0).Unit(); // up vector with no roll + + double cos = y0 | up; // cos of roll-less up vector and "rolled" up vector + + double sin; + + double max = std::max(std::abs(x0.Z), std::max(std::abs(x0.X), std::abs(x0.Y))); + if(std::abs(x0.X) == max) + { + assert(x0.X != 0); // at least one of x0.X, x0.Y, x0.Z must be nonzero + sin = ((y0.X * cos) - up.X) / x0.X; + } + else if(std::abs(x0.Y) == max) + { + assert(x0.Y != 0); // at least one of x0.X, x0.Y, x0.Z must be nonzero + sin = ((y0.Y * cos) - up.Y) / x0.Y; + } + else //if(std::abs(x0.Z) == max) + { + assert(x0.Z != 0); // at least one of x0.X, x0.Y, x0.Z must be nonzero + sin = ((y0.Z * cos) - up.Z) / x0.Z; + } + + roll = DAngle::fromSin(sin); + } + + return DVector3(yaw.Degrees(), pitch.Degrees(), roll.Degrees()); + } + + return DVector3(0,0,0); +} + +void AActor::GetBonePosition(int model_index, int bone_index, bool with_override, DVector3 &pos, DVector3 &fwd, DVector3 &up) { if(modelData && modelData->flags & MODELDATA_GET_BONE_INFO) { @@ -3978,20 +4058,26 @@ void AActor::GetBonePosition(int model_index, int bone_index, bool with_override FVector4 oldPos(pos.X, pos.Z, pos.Y, 1.0); FVector4 newPos; - FVector4 oldNormal(normal.X, normal.Z, normal.Y, 0.0); - FVector4 newNormal; + FVector4 oldFwd(fwd.X, fwd.Z, fwd.Y, 0.0); + FVector4 newFwd; + FVector4 oldUp(up.X, up.Z, up.Y, 0.0); + FVector4 newUp; boneMatrix.multMatrixPoint(&oldPos.X, &newPos.X); - boneMatrix.multMatrixPoint(&oldNormal.X, &newNormal.X); + boneMatrix.multMatrixPoint(&oldFwd.X, &newFwd.X); + boneMatrix.multMatrixPoint(&oldUp.X, &newUp.X); oldPos = FVector4(FVector3(newPos.X, newPos.Y, newPos.Z) / newPos.W, 1.0); - oldNormal = FVector4(FVector3(newNormal.X, newNormal.Y, newNormal.Z) / newNormal.W, 0.0); + oldFwd = FVector4(FVector3(newFwd.X, newFwd.Y, newFwd.Z).Unit(), 0.0); + oldUp = FVector4(FVector3(newUp.X, newUp.Y, newUp.Z).Unit(), 0.0); worldMatrix.multMatrixPoint(&oldPos.X, &newPos.X); - worldMatrix.multMatrixPoint(&oldNormal.X, &newNormal.X); + worldMatrix.multMatrixPoint(&oldFwd.X, &newFwd.X); + worldMatrix.multMatrixPoint(&oldUp.X, &newUp.X); pos = DVector3(newPos.X, newPos.Z, newPos.Y); - normal = DVector3(newNormal.X, newNormal.Z, newNormal.Y); + fwd = DVector3(newFwd.X, newFwd.Z, newFwd.Y).Unit(); + up = DVector3(newUp.X, newUp.Z, newUp.Y).Unit(); } } diff --git a/wadsrc/static/zscript/actors/actor.zs b/wadsrc/static/zscript/actors/actor.zs index 43abef7b6..228c8066d 100644 --- a/wadsrc/static/zscript/actors/actor.zs +++ b/wadsrc/static/zscript/actors/actor.zs @@ -1441,21 +1441,28 @@ class Actor : Thinker native // //================================================ - /* rotation, translation, scaling */ - native version("4.15.1") Quat, Vector3, Vector3 GetBone(int boneIndex, bool include_offsets = true); - native version("4.15.1") Quat, Vector3, Vector3 GetNamedBone(Name boneName, bool include_offsets = true); + /* rotation, translation, scaling, doesn't include parent bones */ + native version("4.15.1") Quat, Vector3, Vector3 GetBoneTRS(int boneIndex, bool include_offsets = true); + native version("4.15.1") Quat, Vector3, Vector3 GetNamedBoneTRS(Name boneName, bool include_offsets = true); - native version("4.15.1") Vector3, Vector3 TransformByBone(int boneIndex, Vector3 position, Vector3 direction = (0,0,0), bool include_offsets = true); - native version("4.15.1") Vector3, Vector3 TransformByNamedBone(Name boneName, Vector3 position, Vector3 direction = (0,0,0), bool include_offsets = true); + /* angle, pitch, roll, includes parent bones */ + native version("4.15.1") Vector3 GetBoneEulerAngles(int boneIndex, bool include_offsets = true); + native version("4.15.1") Vector3 GetNamedBoneEulerAngles(Name boneName, bool include_offsets = true); + + //input position/direction vectors are in xzy, model space + native version("4.15.1") Vector3, Vector3, Vector3 TransformByBone(int boneIndex, Vector3 position, Vector3 forward = (1,0,0), Vector3 up = (0,0,1), bool include_offsets = true); + native version("4.15.1") Vector3, Vector3, Vector3 TransformByNamedBone(Name boneName, Vector3 position, Vector3 forward = (1,0,0), Vector3 up = (0,0,1), bool include_offsets = true); - version("4.15.1") Vector3 GetBonePosition(int boneIndex, bool include_offsets = true) + version("4.15.1") Vector3, Vector3, Vector3 GetBonePosition(int boneIndex, bool include_offsets = true) { - return TransformByBone(boneIndex, GetBoneBasePosition(boneIndex), include_offsets:include_offsets); + let [a, b, c] = TransformByBone(boneIndex, GetBoneBasePosition(boneIndex), include_offsets:include_offsets); + return a, b, c; } - version("4.15.1") Vector3 GetNamedBonePosition(name boneName, bool include_offsets = true) + version("4.15.1") Vector3, Vector3, Vector3 GetNamedBonePosition(name boneName, bool include_offsets = true) { - return GetBonePosition(GetBoneIndex(boneName), include_offsets); + let [a, b, c] = GetBonePosition(GetBoneIndex(boneName), include_offsets); + return a, b, c; } //================================================