leaked Ragdoll_c, adapted for `gta-reversed`

Ragdoll_c.cpp

///////////////////////////////////////////////////////////////////////////////
//  
//	FILE: 	"Ragdoll_c.cpp"
//	BY	: 	Tornamic, Mark Nicholson 
//	FOR	:	GTA:SA Community
//	ON	:	07 Oct 2003 - 16 Feb 2024
//	WHAT:	Routines to handle ragdoll physics
//
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
//  INCLUDES
///////////////////////////////////////////////////////////////////////////////

#include "StdInc.h"
#include "Ragdoll_c.h"
#include <BoneNodeManager_c.h>


///////////////////////////////////////////////////////////////////////////////
//  GLOBAL VARIABLES
///////////////////////////////////////////////////////////////////////////////
int aBONETAG_ENUM_TAB[MAX_BONE_NUM] =
{
    BONE_ROOT, // Normal or Root, both are same
    BONE_PELVIS,
    BONE_SPINE,
    BONE_SPINE1,
    BONE_NECK,
    BONE_HEAD,
    BONE_L_BROW,
    BONE_R_BROW,
    BONE_JAW,
    BONE_R_CLAVICLE,
    BONE_R_UPPER_ARM,
    BONE_R_FORE_ARM,
    BONE_R_HAND,
    BONE_R_FINGER,
    BONE_R_FINGER_01,
    BONE_L_CLAVICLE,
    BONE_L_UPPER_ARM,
    BONE_L_FORE_ARM,
    BONE_L_HAND,
    BONE_L_FINGER,
    BONE_L_FINGER_01,
    BONE_L_THIGH,
    BONE_L_CALF,
    BONE_L_FOOT,
    BONE_L_TOE_0,
    BONE_R_THIGH,
    BONE_R_CALF,
    BONE_R_FOOT,
    BONE_R_TOE_0,
    BONE_BELLY,
    BONE_L_BREAST,
    BONE_R_BREAST
};

RagdollManager_c	g_ragdollMan;


///////////////////////////////////////////////////////////////////////////////
//  CLASS Ragdoll_c
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
//  Constructor
///////////////////////////////////////////////////////////////////////////////

Ragdoll_c::Ragdoll_c()
{}


///////////////////////////////////////////////////////////////////////////////
//  Destructor
///////////////////////////////////////////////////////////////////////////////

Ragdoll_c::~Ragdoll_c()
{}


///////////////////////////////////////////////////////////////////////////////
//  Init
///////////////////////////////////////////////////////////////////////////////

bool8			Ragdoll_c::Init(CPed* pPed)
{
    // store the ped pointer
    m_pPed = pPed;

    // stop this ped from being updated via the animation system
    CAnimBlendClumpData* pAnimBlendClump = RpAnimBlendClumpGetData(m_pPed->m_pRwClump);
    AnimBlendFrameData* frameData = pAnimBlendClump->m_FrameDatas;
    float* pVel = (float*)pAnimBlendClump->m_PedPosition;

    for (uint32 i = 0; i < pAnimBlendClump->m_NumFrameData; i++)
    {
        frameData[i].KeyFramesIgnoreNodeOrientation = true;
        frameData[i].KeyFramesIgnoreNodeTranslation = true;
    }

    pVel[0] = pVel[1] = pVel[2] = 0;

    // set up the bone node data
    RpHAnimHierarchy* pHierarchy = GetAnimHierarchyFromSkinClump((RpClump*)m_pPed->m_pRwObject);

    for (int32 i = 0; i < MAX_BONE_NUM; i++)
    {
        int32 boneTag = aBONETAG_ENUM_TAB[i];
        int32 boneId = RpHAnimIDGetIndex(pHierarchy, boneTag);
        RpHAnimBlendInterpFrame* pAnimKeyFrame = frameData[boneId].KeyFrame;

        m_boneNodes[i].Init(boneTag, pAnimKeyFrame);
    }

    // set up the bone hierarchy
    SetupBoneHierarchy();

    m_blend = 1.0f;

    // apply a test force to the head
    /*RwV3d dir = {0.0f, 0.0f, 1.0f};
    for (int32 i=0; i<MAX_BONE_NUM; i++)
    {
    	ApplyForce((eBoneTag)aBONETAG_ENUM_TAB[i], CGeneral::GetRandomNumberInRange(5.0f, 10.0f), dir);
    }*/

    return true;
}


///////////////////////////////////////////////////////////////////////////////
//  Exit
///////////////////////////////////////////////////////////////////////////////

void			Ragdoll_c::Exit()
{
    // give this ped control back 
    CAnimBlendClumpData* pAnimBlendClump = RpAnimBlendClumpGetData(m_pPed->m_pRwClump);
    AnimBlendFrameData* frameData = pAnimBlendClump->m_FrameDatas;

    for (uint32 i = 0; i < pAnimBlendClump->m_NumFrameData; i++)
    {
        frameData[i].KeyFramesIgnoreNodeOrientation = false;
        frameData[i].KeyFramesIgnoreNodeTranslation = false;
    }

    // let the ped be rendered again
    m_pPed->bDontRender = false;
}


///////////////////////////////////////////////////////////////////////////////
//  Update
///////////////////////////////////////////////////////////////////////////////

void			Ragdoll_c::Update(float deltaTime)
{
    // update the bone nodes	
    m_pRootBoneNode->CalcWldMat((RwMatrix*)m_pPed->m_matrix);

    ResolveSystem(deltaTime);

    // limit the bone rotations
    // for (int32 i=1; i<MAX_BONE_NUM; i++)
    // {
    // 	m_boneNodes[i].Limit(m_blend);
    // }

    // blend into the animation
    for (int32 i = 0; i < MAX_BONE_NUM; i++)
    {
        m_boneNodes[i].BlendKeyframe(deltaTime);
    }
}


///////////////////////////////////////////////////////////////////////////////
//  SetBlend
///////////////////////////////////////////////////////////////////////////////

void			Ragdoll_c::SetBlend(float blend)
{
    m_blend = blend;
}


///////////////////////////////////////////////////////////////////////////////
//  ApplyForce
///////////////////////////////////////////////////////////////////////////////

void			Ragdoll_c::ApplyForce(eBoneTag32 boneTag, float force, RwV3d dir)
{
    // get curr bone node
    BoneNode_c* pBoneNode = &m_boneNodes[BoneNode_c::GetIdFromBoneTag(boneTag)];

    // update it's velocity
    pBoneNode->m_Pos.x += dir.x * force;
    pBoneNode->m_Pos.y += dir.y * force;
    pBoneNode->m_Pos.z += dir.z * force;
}


///////////////////////////////////////////////////////////////////////////////
//  ResolveSystem
///////////////////////////////////////////////////////////////////////////////

void			Ragdoll_c::ResolveSystem(float deltaTime)
{
    printf("INFO: RAGDOLL - ResolveSystem()\n");

    // apply gravity to bone nodes	
    for (int32 i = 0; i < MAX_BONE_NUM; i++)
    {
        m_boneNodes[i].m_Pos.z -= 9.81f * deltaTime;
    }

    // update the bone node rotations 
    for (int32 i = 0; i < MAX_BONE_NUM; i++)
    {
        if (m_boneNodes[i].GetBoneTag() != BONE_HEAD)
        {
            continue;
        }

        // calculate vector from parent node to this node
        RwV3d parentToNodeVec;
        RwV3dSub(&parentToNodeVec, RwMatrixGetPos(&m_boneNodes[i].GetMatrix()), RwMatrixGetPos(&m_boneNodes[i].GetParent()->GetMatrix()));

        // get the cross product of this and the velocity vector
        RwV3d axis;
        RwV3dCrossProduct(&axis, &parentToNodeVec, &m_boneNodes[i].m_Pos);

        // calculate the rotational speed
        float speed = RwV3dNormalize(&axis, &axis);

        if (speed > 0.01f)
        {
            // put this axis into parent space
            RtQuat parentQuat, invParentQuat;
            if (m_boneNodes[i].GetParent())
            {
                RtQuatConvertFromMatrix(&parentQuat, &m_boneNodes[i].GetParent()->GetMatrix());
            }
            else
            {
                RtQuatConvertFromMatrix(&parentQuat, (RwMatrix*)m_pPed->m_matrix);
            }

            RtQuatReciprocal(&invParentQuat, &parentQuat);
            RwV3d relativeAxis;
            RtQuatTransformVectors(&relativeAxis, &axis, 1, &invParentQuat);

            printf("%.2f %.2f %.2f %.2f - parent quat\n", parentQuat.imag.x, parentQuat.imag.y, parentQuat.imag.z, parentQuat.real);
            printf("%.2f %.2f %.2f %.2f - inv parent quat\n", invParentQuat.imag.x, invParentQuat.imag.y, invParentQuat.imag.z, invParentQuat.real);

            // apply the rotation to the current bone's parent
            RtQuatRotate(&m_boneNodes[i].GetParent()->GetOrientation(), &relativeAxis, speed, rwCOMBINEPOSTCONCAT);

            // limit the rotation
//			m_boneNodes[i].Limit();

            // update the world matrices down the tree from the current bone 
            if (m_boneNodes[i].GetParent())
            {
                m_boneNodes[i].CalcWldMat(&m_boneNodes[i].GetParent()->GetMatrix());
            }
            else
            {
                m_boneNodes[i].CalcWldMat((RwMatrix*)m_pPed->m_matrix);
            }
        }
    }

    // apply friction to the bone node velocities
    for (int32 i = 0; i < MAX_BONE_NUM; i++)
    {
        m_boneNodes[i].m_Pos.x *= 0.98f;
    }
}


///////////////////////////////////////////////////////////////////////////////
//  SetupBoneHierarchy
///////////////////////////////////////////////////////////////////////////////

void			Ragdoll_c::SetupBoneHierarchy()
{
    // get a pointer to the root bone node
    m_pRootBoneNode = &m_boneNodes[aBONETAG_ENUM_TAB[BONE_ROOT]];

    // create the bone hierarchy
    for (int32 i = 0; i < MAX_BONE_NUM; i++)
    {
        if (BoneNodeManager_c::ms_boneInfos[i].m_prev > -1)
        {
            m_boneNodes[BoneNode_c::GetIdFromBoneTag(BoneNodeManager_c::ms_boneInfos[i].m_prev)].AddChild(&m_boneNodes[i]);
        }
    }
}

///////////////////////////////////////////////////////////////////////////////
//  CLASS RagdollManager_c
///////////////////////////////////////////////////////////////////////////////


///////////////////////////////////////////////////////////////////////////////
//  Constructor
///////////////////////////////////////////////////////////////////////////////

RagdollManager_c::RagdollManager_c()
{}


///////////////////////////////////////////////////////////////////////////////
//  Destructor
///////////////////////////////////////////////////////////////////////////////

RagdollManager_c::~RagdollManager_c()
{}


///////////////////////////////////////////////////////////////////////////////
//  Init
///////////////////////////////////////////////////////////////////////////////

bool8			RagdollManager_c::Init()
{
    // add the ragdolls to the pool
    for (int32 i = 0; i < NUM_RAGDOLLS; i++)
    {
        m_ragdollPool.AddItem(&m_ragdolls[i]);
    }

    return true;
}


///////////////////////////////////////////////////////////////////////////////
//  Exit
///////////////////////////////////////////////////////////////////////////////

void			RagdollManager_c::Exit()
{
    // delete any active ragdolls	
    Ragdoll_c* pRagdoll = (Ragdoll_c*)m_ragdollList.GetHead();

    while (pRagdoll)
    {
        pRagdoll->Exit();
        pRagdoll = (Ragdoll_c*)m_ragdollList.GetNext(pRagdoll);
    }

    m_ragdollList.RemoveAll();
    m_ragdollPool.RemoveAll();
}


///////////////////////////////////////////////////////////////////////////////
//  Reset
///////////////////////////////////////////////////////////////////////////////

void			RagdollManager_c::Reset()
{
    Exit();
    Init();
}


///////////////////////////////////////////////////////////////////////////////
//  Update
///////////////////////////////////////////////////////////////////////////////

void			RagdollManager_c::Update(float deltaTime)
{
   // DEV_LOG("RagdollManager_c::Update({})", deltaTime);
    Ragdoll_c* pRagdoll = (Ragdoll_c*)m_ragdollList.GetHead();

    while (pRagdoll)
    {
        pRagdoll->Update(deltaTime);

        pRagdoll = (Ragdoll_c*)m_ragdollList.GetNext(pRagdoll);
    }
}


///////////////////////////////////////////////////////////////////////////////
//  AddRagdoll
///////////////////////////////////////////////////////////////////////////////

Ragdoll_c* RagdollManager_c::AddRagdoll(CPed* pPed)
{
    Ragdoll_c* pRagdoll = (Ragdoll_c*)m_ragdollPool.RemoveHead();

    if (pRagdoll)
    {
        pRagdoll->Init(pPed);
        m_ragdollList.AddItem(pRagdoll);
    }

    return pRagdoll;
}


///////////////////////////////////////////////////////////////////////////////
//  RemoveRagdoll
///////////////////////////////////////////////////////////////////////////////

void			RagdollManager_c::RemoveRagdoll(Ragdoll_c* pRagdoll)
{
    pRagdoll->Exit();
    m_ragdollList.RemoveItem(pRagdoll);
    m_ragdollPool.AddItem(pRagdoll);
}

Ragdoll_c.h

#pragma once
#include <BoneNode_c.h>

class Ragdoll_c : public ListItem_c<Ragdoll_c>
{
public:
    Ragdoll_c();
    ~Ragdoll_c();

    bool8 Init(CPed* pPed);
    void Exit();
    void Update(float deltaTime);
    void SetBlend(float blend);
    void ApplyForce(eBoneTag32 boneTag, float force, RwV3d dir);
    void ResolveSystem(float deltaTime);
    void SetupBoneHierarchy();

public:
    CPed* m_pPed;
    float m_blend;
    BoneNode_c m_boneNodes[MAX_BONE_NUM];
    BoneNode_c* m_pRootBoneNode;
};

class RagdollManager_c
{
public:
    static inline constexpr int NUM_RAGDOLLS = 50;
    RagdollManager_c();
    ~RagdollManager_c();

    bool8 Init();
    void Exit();
    void Reset();
    void Update(float deltaTime);
    Ragdoll_c* AddRagdoll(CPed* pPed);
    void RemoveRagdoll(Ragdoll_c* pRagdoll);

private:
    Ragdoll_c m_ragdolls[NUM_RAGDOLLS];
    TList_c<Ragdoll_c> m_ragdollPool;
    TList_c<Ragdoll_c> m_ragdollList;
};
extern RagdollManager_c	g_ragdollMan;
extern int aBONETAG_ENUM_TAB[MAX_BONE_NUM];