ragdoll.cpp

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

                        &State->m_quatOrientation, 
                        &quatOrientation, Bone->m_ResolutionRate);
  }

  // Compute the new matrix-based orientation transformation 
  // based on the quaternion just computed
  D3DXMatrixRotationQuaternion(&State->m_matOrientation, 
                               &State->m_quatOrientation);
  D3DXMatrixTranspose(&State->m_matOrientation,
                      &State->m_matOrientation);

  // Calculate the integrated inverse world inertia tensor 
  D3DXMATRIX matTransposedOrientation;
  D3DXMatrixTranspose(&matTransposedOrientation, &State->m_matOrientation);
  State->m_matInvWorldInertiaTensor = State->m_matOrientation * 
                                      Bone->m_matInvInertiaTensor *
                                      matTransposedOrientation;

  // Calculate new angular velocity 
  State->m_vecAngularVelocity = Transform(&State->m_vecAngularMomentum, 
                                          &State->m_matInvWorldInertiaTensor);
}

DWORD cRagdoll::ProcessCollisions(DWORD BoneNum, cCollision *pCollision)
{
  // Error checking
  if(!pCollision || !pCollision->m_NumObjects || !pCollision->m_Objects)
    return TRUE;

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

  // Get a pointer to the integrated state for easier handling
  cRagdollBoneState *State = &Bone->m_State;

  // Keep count of number of collisions
  DWORD CollisionCount = 0;

  // Keep tally of collision forces
  D3DXVECTOR3 vecLinearVelocity  = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
  D3DXVECTOR3 vecAngularMomentum = D3DXVECTOR3(0.0f, 0.0f, 0.0f);

  // Go through all bone vertices looking for a collision
  for(DWORD i=0;i<8;i++) {

    // Loop through all collision objects
    cCollisionObject *pObj = pCollision->m_Objects;
    while(pObj) {

      // Flag if a collision was detected
      BOOL Collision = FALSE;

      // Normal of collision object
      D3DXVECTOR3 vecCollisionNormal;

      // Distance to push point out of collision object
      float CollisionDistance = 0.0f;

      // Process sphere collision object
      if(pObj->m_Type == COLLISION_SPHERE) {

        // Get position of sphere
        D3DXVECTOR3 vecSphere = pObj->m_vecPos;
  
        // Get distance from center of sphere to point
        D3DXVECTOR3 vecDiff = State->m_vecPoints[i] - vecSphere;
        float Dist = vecDiff.x * vecDiff.x + 
                     vecDiff.y * vecDiff.y + 
                     vecDiff.z * vecDiff.z;

        // Process collision if point within radius
        if(Dist <= (pObj->m_Radius * pObj->m_Radius)) {

          // Compute real distance
          Dist = (float)sqrt(Dist);

          // Store the normalize difference vector as the collision normal
          vecCollisionNormal = vecDiff / Dist;

          // Calculate the distance needed to push point out
          CollisionDistance = pObj->m_Radius - Dist;

          // Mark there was a collision
          Collision = TRUE;
        }
      }

      // Process plane collision object
      if(pObj->m_Type == COLLISION_PLANE) {

        // Get position of plane 
        D3DXPLANE Plane = pObj->m_Plane;

        // Get the plane's normal
        D3DXVECTOR3 vecPlane = D3DXVECTOR3(Plane.a, Plane.b, Plane.c);

        // Get distance from plane to point
        float Dist = D3DXVec3Dot(&State->m_vecPoints[i], 
                                 &vecPlane) + Plane.d;

        // Process collision if point on backside of plane
        if(Dist < 0.0f) {

          // Calculate the distance needed to push point out
          CollisionDistance = -Dist;

          // Set plane's normal 
          vecCollisionNormal = vecPlane;

          // Mark there was a collision
          Collision = TRUE;
        }
      }

      // Process a collision if detected
      if(Collision == TRUE) {

        // Push the object onto the collision object's surface 
        State->m_vecPosition += (vecCollisionNormal * CollisionDistance);

        // Get the point's position and velocity
        D3DXVECTOR3 vecPtoP = State->m_vecPosition - State->m_vecPoints[i];
        D3DXVECTOR3 vecPtoPVelocity = State->m_vecLinearVelocity + 
                                      CrossProduct(&State->m_vecAngularVelocity, 
                                                   &vecPtoP);

        // Get the point's speed relative to the surface
        float PointSpeed = D3DXVec3Dot(&vecCollisionNormal, &vecPtoPVelocity);

        // Increase number of collisions
        CollisionCount++;

        // Calculate the impulse force based on the coefficient
        // of restitution, the speed of the point, and the
        // normal of the colliding object.
        float ImpulseForce = PointSpeed * (-(1.0f + Bone->m_Coefficient));
        float ImpulseDamping = (1.0f / Bone->m_Mass) +
                                   D3DXVec3Dot(&CrossProduct(&Transform(&CrossProduct(&vecPtoP, 
                                                                                      &vecCollisionNormal),
                                                                        &State->m_matInvWorldInertiaTensor),
                                                              &vecPtoP), 
                                               &vecCollisionNormal);
        D3DXVECTOR3 vecImpulse = (ImpulseForce/ImpulseDamping) * vecCollisionNormal;

        // Add forces to running total
        vecLinearVelocity  += vecImpulse;
        vecAngularMomentum += CrossProduct(&vecPtoP, &vecImpulse);
      }

      // Go to next collision object to check
      pObj = pObj->m_Next;
    }
  }

  // Was there any collisions
  if(CollisionCount) {

    // Add averaged forces to integrated state
    State->m_vecLinearVelocity  += ((vecLinearVelocity / Bone->m_Mass) / (float)CollisionCount);
    State->m_vecAngularMomentum += (vecAngularMomentum / (float)CollisionCount);

    // Calculate angular velocity
    State->m_vecAngularVelocity = Transform(&State->m_vecAngularMomentum,
                                             &State->m_matInvWorldInertiaTensor);
  }

  // Return that everything processed okay
  return TRUE;
}

void cRagdoll::ProcessConnections(DWORD BoneNum)
{
  // Get a pointer to the bone and 
  // parent bone for easier handling
  cRagdollBone *Bone = &m_Bones[BoneNum];
  cRagdollBone *ParentBone = Bone->m_ParentBone;

  // Don't continue if there's no parent bone
  if(!ParentBone)
    return;

  // Get the pointer to the bone's state
  cRagdollBoneState *BState = &Bone->m_State;

  // Get pointer to parent's state
  cRagdollBoneState *PState = &ParentBone->m_State;

  // Get joint connection position and vector to center
  D3DXVECTOR3 vecBonePos = BState->m_vecPoints[8];
  D3DXVECTOR3 vecBtoC    = BState->m_vecPosition - vecBonePos;

  // Get the parent connection point coordinates
  D3DXVECTOR3 vecParentPos = BState->m_vecPoints[9];

  // Calculate a spring vector from point to parent's point
  D3DXVECTOR3 vecSpring = vecBonePos - vecParentPos;

  // Move point to match parent's point and adjust 
  // the angular velocity and momentum
  BState->m_vecPosition -= vecSpring;
  BState->m_vecAngularMomentum -= CrossProduct(&vecBtoC, &vecSpring);
  BState->m_vecAngularVelocity = Transform(&BState->m_vecAngularMomentum,
                                           &BState->m_matInvWorldInertiaTensor);
}

void cRagdoll::TransformPoints(DWORD BoneNum)
{
  // Get pointer to bone and bone's state
  cRagdollBone *Bone = &m_Bones[BoneNum];
  cRagdollBoneState *State = &Bone->m_State;

  // Transform all points
  for(DWORD i=0;i<9;i++)
    State->m_vecPoints[i] = Transform(&Bone->m_vecPoints[i], 
                                      &State->m_matOrientation,
                                      &State->m_vecPosition);

  // Calculate parent connection point coordinates (in world space)
  if(Bone->m_ParentBone) {
    State->m_vecPoints[9] = Transform(&Bone->m_vecParentOffset,
                                      &Bone->m_ParentBone->m_State.m_matOrientation,
                                      &Bone->m_ParentBone->m_State.m_vecPosition);
  }
}

///////////////////////////////////////////////////////////
//
// Public functions
//
///////////////////////////////////////////////////////////
cRagdoll::cRagdoll()
{
  m_pFrame = NULL;

  m_NumBones = 0;
  m_Bones    = NULL;
}

cRagdoll::~cRagdoll()
{
  Free();
}

BOOL cRagdoll::Create(D3DXFRAME_EX *Frame, 
                      D3DXMESHCONTAINER_EX *Mesh,
                      D3DXMATRIX *matInitialTransformation)
{
  // Error checking
  if(!(m_pFrame = Frame))
    return FALSE;
  if(!Mesh->pSkinInfo || !Mesh->pSkinInfo->GetNumBones())
    return FALSE;

  // Update the frame hierarchy using transformation passed
  m_pFrame->UpdateHierarchy(matInitialTransformation);

  // Get the number of bones (frames) and allocate the ragdoll bone structures
  m_pFrame->Count(&m_NumBones);
  if(!m_NumBones)
    return FALSE;
  m_Bones = new cRagdollBone[m_NumBones]();

  // Go through and setup each bone's data
  DWORD BoneNum = 0;
  BuildBoneData(&BoneNum, m_pFrame, Mesh);

  m_Bones[0].m_State.m_vecAngularMomentum = D3DXVECTOR3(-1.0f, 0.0f, 0.0f);

  // Return a success
  return TRUE;
}

void cRagdoll::Free()
{
  m_pFrame = NULL;

  m_NumBones = 0;
  delete [] m_Bones; 
  m_Bones = NULL;
}

void cRagdoll::Resolve(float Elapsed, 
                       float LinearDamping, 
                       float AngularDamping,
                       D3DXVECTOR3 *vecGravity, 
                       cCollision  *pCollision)
{
  for(DWORD i=0;i<m_NumBones;i++) {
   
    float TimeToProcess = Elapsed;

    while(TimeToProcess > 0.0f) {

      // Set forces to prepare for integration
      SetForces(i, vecGravity, LinearDamping, AngularDamping);

      // Integrate bone movement for time slice
      DWORD NumSteps = 0;
      float TimeStep = TimeToProcess;

      // Dont process more than 10ms at a time
      if(TimeStep >= MAXIMUM_TIME_SLICE)
        TimeStep = MAXIMUM_TIME_SLICE;

      // Integrate the bone motion
      Integrate(i, TimeStep);

      // Transform points
      TransformPoints(i);

      // Check for collisions and resolve them, breaking if
      // all collisions could be handled
      ProcessCollisions(i, pCollision);

      // Transform points
      TransformPoints(i);

      // Process connections to ensure all bones
      // meet at their connection points
      ProcessConnections(i);

      // Go forward one time slice
      TimeToProcess -= TimeStep;
    }
  }
}

void cRagdoll::RebuildHierarchy()
{ 
  if(!m_pFrame)
    return;
  if(!m_NumBones || !m_Bones)
    return;

  // Apply bones' rotational matrices to frames
  for(DWORD i=0;i<m_NumBones;i++) {

    // Transform the joint offset in order to position frame
    D3DXVECTOR3 vecPos;
    D3DXVec3TransformCoord(&vecPos, 
                           &m_Bones[i].m_vecJointOffset, 
                           &m_Bones[i].m_State.m_matOrientation);

    // Add bone's position 
    vecPos += m_Bones[i].m_State.m_vecPosition;

    // Orient and position frame
    m_Bones[i].m_Frame->matCombined = m_Bones[i].m_State.m_matOrientation;
    m_Bones[i].m_Frame->matCombined._41 = vecPos.x;
    m_Bones[i].m_Frame->matCombined._42 = vecPos.y;
    m_Bones[i].m_Frame->matCombined._43 = vecPos.z;
  }
}

DWORD cRagdoll::GetNumBones() 
{ 
  return m_NumBones; 
}

cRagdollBone *cRagdoll::GetBone(DWORD BoneNum) 
{ 
  if(BoneNum < m_NumBones)
    return &m_Bones[BoneNum]; 
  return NULL;
}