void dRigidbodyNodeInfo::BakeTransform (const dMatrix& transform)
{
// SetTransform (transform.Inverse4x4() * GetTransform() * transform);
dNodeInfo::BakeTransform (transform);
m_centerOfMass = transform.UnrotateVector(m_centerOfMass);
m_massMatrix = transform.UnrotateVector(m_massMatrix);
m_velocity = transform.RotateVector(m_velocity);
}
void CalculateAABB (const NewtonCollision* const collision, const dMatrix& matrix, dVector& minP, dVector& maxP)
{
for (int i = 0; i < 3; i ++) {
dVector support(0.0f);
dVector dir (0.0f);
dir[i] = 1.0f;
dVector localDir (matrix.UnrotateVector (dir));
NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
support = matrix.TransformVector (support);
maxP[i] = support[i];
localDir = localDir.Scale (-1.0f);
NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
support = matrix.TransformVector (support);
minP[i] = support[i];
}
}
void CalculateAABB (const NewtonCollision* const collision, const dMatrix& matrix, dVector& minP, dVector& maxP)
{
dFloat skinThickness = NewtonCollisionGetSkinThickness (collision) * 0.125f;
for (int i = 0; i < 3; i ++) {
dVector support;
dVector dir (0.0f, 0.0f, 0.0f, 0.0f);
dir[i] = 1.0f;
dVector localDir (matrix.UnrotateVector (dir));
NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
support = matrix.TransformVector (support);
maxP[i] = support[i] - skinThickness;
localDir = localDir.Scale (-1.0f);
NewtonCollisionSupportVertex (collision, &localDir[0], &support[0]);
support = matrix.TransformVector (support);
minP[i] = support[i] + skinThickness;
}
}