diff --git a/src/common/utility/vectors.h b/src/common/utility/vectors.h index a2dfae389..154d42f7c 100644 --- a/src/common/utility/vectors.h +++ b/src/common/utility/vectors.h @@ -1293,6 +1293,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 2f51398ab..2737a1cf8 100644 --- a/src/playsim/actor.h +++ b/src/playsim/actor.h @@ -831,6 +831,7 @@ public: //outmat must be double[16] void GetBoneMatrix(int model_index, int bone_index, bool with_override, double *outMat); + 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); diff --git a/src/playsim/p_actionfunctions.cpp b/src/playsim/p_actionfunctions.cpp index d8e13ec26..613575ea5 100644 --- a/src/playsim/p_actionfunctions.cpp +++ b/src/playsim/p_actionfunctions.cpp @@ -6030,6 +6030,45 @@ DEFINE_ACTION_FUNCTION(AActor, GetNamedBoneTRS) } + + +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); diff --git a/src/playsim/p_mobj.cpp b/src/playsim/p_mobj.cpp index a29c6928e..da81dd83b 100644 --- a/src/playsim/p_mobj.cpp +++ b/src/playsim/p_mobj.cpp @@ -4309,6 +4309,86 @@ void AActor::GetBoneMatrix(int model_index, int bone_index, bool with_override, } } +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) @@ -4334,16 +4414,16 @@ void AActor::GetBonePosition(int model_index, int bone_index, bool with_override boneMatrix.multMatrixPoint(&oldUp.X, &newUp.X); oldPos = FVector4(FVector3(newPos.X, newPos.Y, newPos.Z) / newPos.W, 1.0); - oldFwd = FVector4(FVector3(newFwd.X, newFwd.Y, newFwd.Z) / newFwd.W, 0.0); - oldUp = FVector4(FVector3(newUp.X, newUp.Y, newUp.Z) / newUp.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(&oldFwd.X, &newFwd.X); worldMatrix.multMatrixPoint(&oldUp.X, &newUp.X); pos = DVector3(newPos.X, newPos.Z, newPos.Y); - fwd = DVector3(newFwd.X, newFwd.Z, newFwd.Y); - up = DVector3(newUp.X, newUp.Z, newUp.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 fbff082cc..dc0892a3a 100644 --- a/wadsrc/static/zscript/actors/actor.zs +++ b/wadsrc/static/zscript/actors/actor.zs @@ -1479,9 +1479,13 @@ class Actor : Thinker native 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); + /* 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,1,0), 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,1,0), bool include_offsets = true); + 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, Vector3, Vector3 GetBonePosition(int boneIndex, bool include_offsets = true) { @@ -1489,7 +1493,7 @@ class Actor : Thinker native return a, b, c; } - version("4.15.1") Vector3, Vector3 GetNamedBonePosition(name boneName, bool include_offsets = true) + version("4.15.1") Vector3, Vector3, Vector3 GetNamedBonePosition(name boneName, bool include_offsets = true) { let [a, b, c] = GetBonePosition(GetBoneIndex(boneName), include_offsets); return a, b, c;