ragdoll.cpp

用DirectX制作高级动画-[Advanced.Animation.with.DirectX]

#include "Ragdoll.h"

///////////////////////////////////////////////////////////
//
// Protected functions
//
///////////////////////////////////////////////////////////
D3DXVECTOR3 cRagdoll::CrossProduct(D3DXVECTOR3 *v1, 
                                   D3DXVECTOR3 *v2)
{
  D3DXVECTOR3 vecResult;

  // Calculate the cross product
  D3DXVec3Cross(&vecResult, v1, v2);

  // Return the result
  return vecResult;
}

D3DXVECTOR3 cRagdoll::Transform(D3DXVECTOR3 *vecSrc, 
                                D3DXMATRIX  *matSrc,
                                D3DXVECTOR3 *vecTranslate)
{
  D3DXVECTOR3 vecResult;

  // Perform transformation
  D3DXVec3TransformCoord(&vecResult, vecSrc, matSrc);

  // Translate if vector passed
  if(vecTranslate)
    vecResult += (*vecTranslate);

  // Return result
  return vecResult;
}

void cRagdoll::GetBoundingBoxSize(D3DXFRAME_EX *pFrame,
                                  D3DXMESHCONTAINER_EX *pMesh,
                                  D3DXVECTOR3 *vecSize,
                                  D3DXVECTOR3 *vecJointOffset)
{
  // Set default min and max coordinates
  D3DXVECTOR3 vecMin = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
  D3DXVECTOR3 vecMax = D3DXVECTOR3(0.0f, 0.0f, 0.0f);

  // Only process bone vertices if there is a bone to work with
  if(pFrame->Name) {

    // Get a pointer to ID3DXSkinInfo interface for
    // easier handling.
    ID3DXSkinInfo *pSkin = pMesh->pSkinInfo;

    // Search for a bone by same name as frame
    DWORD BoneNum = -1;
    for(DWORD i=0;i<pSkin->GetNumBones();i++) {
      if(!strcmp(pSkin->GetBoneName(i), pFrame->Name)) {
        BoneNum = i;
        break;
      }
    }

    // Process vertices if a bone was found
    if(BoneNum != -1) {

      // Get the number of vertices attached
      DWORD NumVertices = pSkin->GetNumBoneInfluences(BoneNum);
      if(NumVertices) {

        // Get the bone influcences
        DWORD *Vertices = new DWORD[NumVertices];
        float *Weights  = new float[NumVertices];
        pSkin->GetBoneInfluence(BoneNum, Vertices, Weights);

        // Get stride of vertex data
        DWORD Stride = D3DXGetFVFVertexSize(pMesh->MeshData.pMesh->GetFVF());

        // Get bone's offset inversed transformation matrix
        D3DXMATRIX *matInvBone = pSkin->GetBoneOffsetMatrix(BoneNum);

        // Lock vertex buffer and go through all of
        // the vertices that are connected to bone
        char *pVertices;
        pMesh->MeshData.pMesh->LockVertexBuffer(D3DLOCK_READONLY, (void**)&pVertices);
        for(i=0;i<NumVertices;i++) {

          // Get pointer to vertex coordinates
          D3DXVECTOR3 *vecPtr = (D3DXVECTOR3*)(pVertices+Vertices[i]*Stride);

          // Transform vertex by bone offset transformation
          D3DXVECTOR3 vecPos;
          D3DXVec3TransformCoord(&vecPos, vecPtr, matInvBone);
    
          // Get min/max values
          vecMin.x = min(vecMin.x, vecPos.x);
          vecMin.y = min(vecMin.y, vecPos.y);
          vecMin.z = min(vecMin.z, vecPos.z);

          vecMax.x = max(vecMax.x, vecPos.x);
          vecMax.y = max(vecMax.y, vecPos.y);
          vecMax.z = max(vecMax.z, vecPos.z);
        }
        pMesh->MeshData.pMesh->UnlockVertexBuffer();

        // Free resource
        delete [] Vertices;
        delete [] Weights;
      }
    }
  }

  // Factor in child bone connection points to size
  if(pFrame->pFrameFirstChild) {
  
    // Get the bone's inverse transformation to 
    // position child connections.
    D3DXMATRIX matInvFrame;
    D3DXMatrixInverse(&matInvFrame,NULL,&pFrame->matCombined);
  
    // Go through all child frames connected to this frame
    D3DXFRAME_EX *pFrameChild = (D3DXFRAME_EX*)pFrame->pFrameFirstChild;
    while(pFrameChild) {
      // Get the frame's vertex coordinates and transform it
      D3DXVECTOR3 vecPos;
      vecPos = D3DXVECTOR3(pFrameChild->matCombined._41,
                           pFrameChild->matCombined._42,
                           pFrameChild->matCombined._43);
      D3DXVec3TransformCoord(&vecPos, &vecPos, &matInvFrame);

      // Get min/max values
      vecMin.x = min(vecMin.x, vecPos.x);
      vecMin.y = min(vecMin.y, vecPos.y);
      vecMin.z = min(vecMin.z, vecPos.z);

      vecMax.x = max(vecMax.x, vecPos.x);
      vecMax.y = max(vecMax.y, vecPos.y);
      vecMax.z = max(vecMax.z, vecPos.z);

      // Go to next child bone
      pFrameChild = (D3DXFRAME_EX*)pFrameChild->pFrameSibling;
    }
  }

  // Set the bounding box size
  vecSize->x = (float)fabs(vecMax.x - vecMin.x);
  vecSize->y = (float)fabs(vecMax.y - vecMin.y);
  vecSize->z = (float)fabs(vecMax.z - vecMin.z);

  // Make sure each bone has a minimal size
  if(vecSize->x < MINIMUM_BONE_SIZE) {
    vecSize->x = MINIMUM_BONE_SIZE; 
    vecMax.x = MINIMUM_BONE_SIZE*0.5f;
  }
  if(vecSize->y < MINIMUM_BONE_SIZE) {
    vecSize->y = MINIMUM_BONE_SIZE; 
    vecMax.y = MINIMUM_BONE_SIZE*0.5f;
  }
  if(vecSize->z < MINIMUM_BONE_SIZE) {
    vecSize->z = MINIMUM_BONE_SIZE; 
    vecMax.z = MINIMUM_BONE_SIZE*0.5f;
  }

  // Set the bone's offset to center based on half the size 
  // of the bounding box and the max position
  (*vecJointOffset) = ((*vecSize) * 0.5f) - vecMax;
}

void cRagdoll::BuildBoneData(DWORD *BoneNum, 
                             D3DXFRAME_EX *Frame, 
                             D3DXMESHCONTAINER_EX *pMesh,
                             cRagdollBone *ParentBone)
{
  // Don't handle NULL frames
  if(!Frame)
    return;

  // Get pointer to bone for easier handling
  cRagdollBone *Bone = &m_Bones[(*BoneNum)];

  // Set pointer to frame
  Bone->m_Frame = Frame;

  // Get the size of this bone and its joint offset
  GetBoundingBoxSize(Bone->m_Frame,
                     pMesh,
                     &Bone->m_vecSize, 
                     &Bone->m_vecJointOffset);

  // Set mass
  Bone->m_Mass = Bone->m_vecSize.x * Bone->m_vecSize.y * Bone->m_vecSize.z;

  // Mass must be > 0.0
  if(Bone->m_Mass == 0.0f)
    Bone->m_Mass = 1.0f;

  // Set the coefficient of restitution (higher = more bounce)
  Bone->m_Coefficient = 0.4f;

  // Clear force and torque vectors
  Bone->m_vecForce        = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
  Bone->m_vecTorque       = D3DXVECTOR3(0.0f, 0.0f, 0.0f);

  // Set the angular resolution rate
  Bone->m_ResolutionRate = 0.05f;

  // Calculate the inverse body inertia tensor
  float xScalar = Bone->m_vecSize.x * Bone->m_vecSize.x;
  float yScalar = Bone->m_vecSize.y * Bone->m_vecSize.y;
  float zScalar = Bone->m_vecSize.z * Bone->m_vecSize.z;
  D3DXMatrixIdentity(&Bone->m_matInvInertiaTensor);
  Bone->m_matInvInertiaTensor._11 = 1.0f / (Bone->m_Mass * (yScalar + zScalar));
  Bone->m_matInvInertiaTensor._22 = 1.0f / (Bone->m_Mass * (xScalar + zScalar));
  Bone->m_matInvInertiaTensor._33 = 1.0f / (Bone->m_Mass * (xScalar + yScalar));

  // Setup the point's coordinates based on half 
  // the bounding box size
  D3DXVECTOR3 vecHalfSize = Bone->m_vecSize * 0.5f;
  Bone->m_vecPoints[0] = D3DXVECTOR3(-vecHalfSize.x,  vecHalfSize.y, -vecHalfSize.z);
  Bone->m_vecPoints[1] = D3DXVECTOR3(-vecHalfSize.x,  vecHalfSize.y,  vecHalfSize.z);
  Bone->m_vecPoints[2] = D3DXVECTOR3( vecHalfSize.x,  vecHalfSize.y,  vecHalfSize.z);
  Bone->m_vecPoints[3] = D3DXVECTOR3( vecHalfSize.x,  vecHalfSize.y, -vecHalfSize.z);
  Bone->m_vecPoints[4] = D3DXVECTOR3(-vecHalfSize.x, -vecHalfSize.y, -vecHalfSize.z);
  Bone->m_vecPoints[5] = D3DXVECTOR3(-vecHalfSize.x, -vecHalfSize.y,  vecHalfSize.z);
  Bone->m_vecPoints[6] = D3DXVECTOR3( vecHalfSize.x, -vecHalfSize.y,  vecHalfSize.z);
  Bone->m_vecPoints[7] = D3DXVECTOR3( vecHalfSize.x, -vecHalfSize.y, -vecHalfSize.z);

  // Set the joint offset (offset to parent bone connection)
  Bone->m_vecPoints[8] = Bone->m_vecJointOffset;

  // Set the bone's center position based on transformed 
  // joint offset coordinates
  D3DXVec3TransformCoord(&Bone->m_State.m_vecPosition,
                         &(-1.0f * Bone->m_vecJointOffset),
                         &Bone->m_Frame->matCombined);

  // Store state matrix-based orientation. Be sure
  // to remove translation values.
  Bone->m_State.m_matOrientation = Bone->m_Frame->matCombined;
  Bone->m_State.m_matOrientation._41 = 0.0f;
  Bone->m_State.m_matOrientation._42 = 0.0f;
  Bone->m_State.m_matOrientation._43 = 0.0f;

  // Store state quaternion-based orientation
  // We need to inverse it the quaternion due to the fact 
  // that we're using a left-handed coordinates system
  D3DXQuaternionRotationMatrix(&Bone->m_State.m_quatOrientation, 
                               &Bone->m_Frame->matCombined);
  D3DXQuaternionInverse(&Bone->m_State.m_quatOrientation, 
                        &Bone->m_State.m_quatOrientation);

  // Clear angular momentum
  Bone->m_State.m_vecAngularMomentum = D3DXVECTOR3(0.0f, 0.0f, 0.0f);

  // Clear force and angular velocities
  Bone->m_State.m_vecLinearVelocity  = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
  Bone->m_State.m_vecAngularVelocity = D3DXVECTOR3(0.0f, 0.0f, 0.0f);

  // Clear inverse world intertia tensor matrix
  D3DXMatrixIdentity(&Bone->m_State.m_matInvWorldInertiaTensor);

  // Transform points
  for(DWORD j=0;j<9;j++)
    Bone->m_State.m_vecPoints[j] = Transform(&Bone->m_vecPoints[j],
                                             &Bone->m_State.m_matOrientation,
                                             &Bone->m_State.m_vecPosition);

  // Get the difference in orientations from parent bone
  // to bone. This is used to later slerp for angular 
  // resolution. The difference (C) from A to B 
  // is calculated as: C = inverse(A) * B
  if(ParentBone) {
    D3DXQUATERNION quatInv;
    D3DXQuaternionInverse(&quatInv, &ParentBone->m_State.m_quatOrientation);
    Bone->m_quatOrientation = quatInv * Bone->m_State.m_quatOrientation;
  }

  // If there is a parent bone, then set connection position
  if((Bone->m_ParentBone = ParentBone)) {

    // Get the inversed coordinates from the joint connection point
    // to the center of the parent's bone.
    D3DXMATRIX matInv;
    D3DXMatrixInverse(&matInv, NULL, &ParentBone->m_State.m_matOrientation);
    D3DXVECTOR3 vecDiff = Bone->m_State.m_vecPoints[8] - 
                          ParentBone->m_State.m_vecPosition;
    Bone->m_vecParentOffset = Transform(&vecDiff, &matInv);
  }

  // Go to next bone
  (*BoneNum)+=1;

  // Process sibling frames
  if(Frame->pFrameSibling)
    BuildBoneData(BoneNum, (D3DXFRAME_EX*)Frame->pFrameSibling, pMesh, ParentBone);

  // Process child frames
  if(Frame->pFrameFirstChild)
    BuildBoneData(BoneNum, (D3DXFRAME_EX*)Frame->pFrameFirstChild, pMesh, Bone);
}

void cRagdoll::SetForces(DWORD BoneNum,
                         D3DXVECTOR3 *vecGravity, 
                         float LinearDamping, 
                         float AngularDamping)
{
  // Get a pointer to the bone for easier handling
  cRagdollBone *Bone = &m_Bones[BoneNum];

  // Get the pointer to the current state for easier handling
  cRagdollBoneState *BCState = &Bone->m_State;

  // Set gravity and clear torque
  Bone->m_vecForce  = ((*vecGravity) * Bone->m_Mass);
  Bone->m_vecTorque = D3DXVECTOR3(0.0f, 0.0f, 0.0f);

  // Apply damping on force and torque
  Bone->m_vecForce  += (BCState->m_vecLinearVelocity  * LinearDamping);
  Bone->m_vecTorque += (BCState->m_vecAngularVelocity * AngularDamping);
}

void cRagdoll::Integrate(DWORD BoneNum, float Elapsed)
{
  // Get pointer to bone
  cRagdollBone *Bone = &m_Bones[BoneNum];

  // Get pointers to states for easier handling
  cRagdollBoneState *State = &Bone->m_State;

  // Integrate position
  State->m_vecPosition += (Elapsed * State->m_vecLinearVelocity);
    
  // Integrate angular momentum
  State->m_vecAngularMomentum += (Elapsed * Bone->m_vecTorque);

  // Integrate linear velocity
  State->m_vecLinearVelocity += Elapsed * Bone->m_vecForce / Bone->m_Mass;

  // Integrate quaternion orientation
  D3DXVECTOR3 vecVelocity = Elapsed * State->m_vecAngularVelocity;
  State->m_quatOrientation.w -= 0.5f * 
                     (State->m_quatOrientation.x * vecVelocity.x + 
                      State->m_quatOrientation.y * vecVelocity.y + 
                      State->m_quatOrientation.z * vecVelocity.z);
  State->m_quatOrientation.x += 0.5f * 
                     (State->m_quatOrientation.w * vecVelocity.x - 
                      State->m_quatOrientation.z * vecVelocity.y + 
                      State->m_quatOrientation.y * vecVelocity.z);
  State->m_quatOrientation.y += 0.5f * 
                     (State->m_quatOrientation.z * vecVelocity.x + 
                      State->m_quatOrientation.w * vecVelocity.y - 
                      State->m_quatOrientation.x * vecVelocity.z);
  State->m_quatOrientation.z += 0.5f * 
                     (State->m_quatOrientation.x * vecVelocity.y - 
                      State->m_quatOrientation.y * vecVelocity.x + 
                      State->m_quatOrientation.w * vecVelocity.z);

  // Normalize the quaternion (creates a unit quaternion)
  D3DXQuaternionNormalize(&State->m_quatOrientation, 
                          &State->m_quatOrientation);

  // Force rotation resolution
  if(BoneNum && Bone->m_ResolutionRate != 0.0f) {

    // Slerp from current orientation to beginning orientation
    D3DXQUATERNION quatOrientation = Bone->m_ParentBone->m_State.m_quatOrientation *
                                     Bone->m_quatOrientation;
    D3DXQuaternionSlerp(&State->m_quatOrientation,