OBB* RagDoll::CreateBoneBox(Bone* parent, Bone *bone, D3DXVECTOR3 size, D3DXQUATERNION rot) {
if (bone == NULL || parent == NULL)
return NULL;
D3DXMATRIX &parentMat = parent->CombinedTransformationMatrix;
D3DXMATRIX &boneMat = bone->CombinedTransformationMatrix;
D3DXVECTOR3 parentPos(parentMat(3, 0), parentMat(3, 1), parentMat(3, 2));
D3DXVECTOR3 bonePos(boneMat(3, 0), boneMat(3, 1), boneMat(3, 2));
D3DXQUATERNION q;
D3DXVECTOR3 p, s;
D3DXMatrixDecompose(&s, &q, &p, &parentMat);
q *= rot;
D3DXQuaternionNormalize(&q, &q);
p = (parentPos + bonePos) * 0.5f;
OBB *obb = new OBB(p, size, q, true);
physicsEngine.GetWorld()->addRigidBody(obb->m_pBody);
m_boxes.push_back(obb);
parent->m_pObb = obb;
parent->m_pivot = obb->SetPivot(parentPos);
return obb;
}
void RagDoll::CreateHinge(Bone* parent, OBB* A, OBB *B,
float upperLimit, float lowerLimit,
D3DXVECTOR3 hingeAxisA, D3DXVECTOR3 hingeAxisB,
bool ignoreCollisions) {
if (parent == NULL || A == NULL || B == NULL)
return;
D3DXMATRIX &parentMat = parent->CombinedTransformationMatrix;
btVector3 hingePos(parentMat(3, 0), parentMat(3, 1), parentMat(3, 2));
D3DXVECTOR3 hingePosDX = D3DXVECTOR3(parentMat(3, 0), parentMat(3, 1), parentMat(3, 2));
btRigidBody *a = A->m_pBody;
btRigidBody *b = B->m_pBody;
btTransform aTrans, bTrans;
a->getMotionState()->getWorldTransform(aTrans);
b->getMotionState()->getWorldTransform(bTrans);
btVector3 aPos = aTrans.getOrigin();
btVector3 bPos = bTrans.getOrigin();
btQuaternion aRot = aTrans.getRotation();
btQuaternion bRot = bTrans.getRotation();
D3DXQUATERNION qa(aRot.x(), aRot.y(), aRot.z(), aRot.w());
D3DXQUATERNION qb(bRot.x(), bRot.y(), bRot.z(), bRot.w());
D3DXMATRIX matPosA, matPosB, matRotA, matRotB, worldA, worldB;
D3DXMatrixTranslation(&matPosA, aPos.x(), aPos.y(), aPos.z());
D3DXMatrixTranslation(&matPosB, bPos.x(), bPos.y(), bPos.z());
D3DXMatrixRotationQuaternion(&matRotA, &qa);
D3DXMatrixRotationQuaternion(&matRotB, &qb);
D3DXMatrixMultiply(&worldA, &matRotA, &matPosA);
D3DXMatrixMultiply(&worldB, &matRotB, &matPosB);
D3DXVECTOR3 offA(0.0f, A->m_size.y * -0.5f, 0.0f);
D3DXVECTOR3 offB(0.0f, B->m_size.y * 0.5f, 0.0f);
D3DXMatrixInverse(&worldA, NULL, &worldA);
D3DXMatrixInverse(&worldB, NULL, &worldB);
D3DXVec3TransformCoord(&offA, &hingePosDX, &worldA);
D3DXVec3TransformCoord(&offB, &hingePosDX, &worldB);
// btVector3 offsetA(aPos.x() - hingePos.x(), aPos.y() - hingePos.y(), aPos.z() - hingePos.z());
// btVector3 offsetB(bPos.x() - hingePos.x(), bPos.y() - hingePos.y(), bPos.z() - hingePos.z());
btVector3 offsetA(offA.x, offA.y, offA.z);
btVector3 offsetB(offB.x, offB.y, offB.z);
aTrans.setIdentity();
bTrans.setIdentity();
aTrans.setOrigin(offsetA);
bTrans.setOrigin(offsetB);
aTrans.getBasis().setEulerZYX(hingeAxisA.x, hingeAxisA.y, hingeAxisA.z);
bTrans.getBasis().setEulerZYX(hingeAxisB.x, hingeAxisB.y, hingeAxisB.z);
btHingeConstraint *hingeC = new btHingeConstraint(*b, *a, bTrans, aTrans);
hingeC->setLimit(lowerLimit, upperLimit);
physicsEngine.GetWorld()->addConstraint(hingeC, ignoreCollisions);
}