It seems that the friction applied has too much magnitude. I guess that could that be caused by an incorrect inertia tensor. But, does anyone see any problem with my formulas?
The function that does collision response is:
Code: Select all
void BeSimulator::doCollisionResponse(Float DeltaTime)
{
ContactIterator ContactItr;
for (ContactItr = m_Contacts.begin(); ContactItr != m_Contacts.end(); ++ContactItr)
{
BeContactConstraint& Contact = *ContactItr;
BeRigidBody& BodyA = *Contact.BodyA();
BeRigidBody& BodyB = *Contact.BodyB();
if (BodyA.IsDynamic() || BodyB.IsDynamic())
{
// We have to calculate the response impulse
BeVector3 VelocityA = BodyA.LinearVelocity() + (BodyA.AngularVelocity() % Contact.RelativePositionA());
BeVector3 VelocityB = BodyB.LinearVelocity() + (BodyB.AngularVelocity() % Contact.RelativePositionB());
Float PreRelativeVel = Contact.CollisionNormal() * (VelocityA - VelocityB);
if (PreRelativeVel < 0.0f)
{
BeVector3 AxN = Contact.RelativePositionA() % Contact.CollisionNormal();
BeVector3 BxN = Contact.RelativePositionB() % Contact.CollisionNormal();
BeVector3 JInvAxN = BodyA.WorldInverseInertia() * AxN;
BeVector3 JInvBxN = BodyB.WorldInverseInertia() * BxN;
Float Restitution = Material(Contact.ShapeA()->MaterialIndex()).Restitution() +
Material(Contact.ShapeB()->MaterialIndex()).Restitution();
Float ImpulseNumer = (-1.0f - Restitution) * PreRelativeVel;
Float ImpulseDenom = BodyA.InverseMass() + BodyB.InverseMass() + (Contact.CollisionNormal() * (JInvAxN % Contact.RelativePositionA())) + (Contact.CollisionNormal() * (JInvBxN % Contact.RelativePositionB()));
Float ImpulseMag = ImpulseNumer/ImpulseDenom;
BeVector3 Impulse = Contact.CollisionNormal() * ImpulseMag;
if (BodyA.IsDynamic())
{
BodyA.AddLinearMomentum(Impulse);
BodyA.AddAngularMomentum(Contact.RelativePositionA() % Impulse);
}
if (BodyB.IsDynamic())
{
BodyB.AddLinearMomentum(-Impulse);
BodyB.AddAngularMomentum(Contact.RelativePositionB() % -Impulse);
}
// Calculate and apply friction
BeVector3 ContactTangent = (Contact.RelativeVelocity() - (Contact.CollisionNormal() * PreRelativeVel));
ContactTangent.ToNormalize();
VelocityA = BodyA.LinearVelocity() + (BodyA.AngularVelocity() % Contact.RelativePositionA());
VelocityB = BodyB.LinearVelocity() + (BodyB.AngularVelocity() % Contact.RelativePositionB());
BeVector3 RelVelocity = VelocityA - VelocityB;
Float RelativeVelTangent = RelVelocity * -ContactTangent;
Float FrictionCoefficient;
if (RelVelocity.Length() > m_RestSpeed)
{
FrictionCoefficient = Material(Contact.ShapeA()->MaterialIndex()).DynamicFriction() *
Material(Contact.ShapeB()->MaterialIndex()).DynamicFriction();
}
else
{
FrictionCoefficient = Material(Contact.ShapeA()->MaterialIndex()).StaticFriction() *
Material(Contact.ShapeB()->MaterialIndex()).StaticFriction();
RelativeVelTangent /= m_RestSpeed;
}
if (RelativeVelTangent < 0.0f)
{
BeVector3 AxT = Contact.RelativePositionA() % ContactTangent;
BeVector3 BxT = Contact.RelativePositionB() % ContactTangent;
BeVector3 JInvAxT = BodyA.WorldInverseInertia() * AxT;
BeVector3 JInvBxT = BodyB.WorldInverseInertia() * BxT;
Float FrictionNum = RelativeVelTangent * FrictionCoefficient;
Float FrictionDen = BodyA.InverseMass() + BodyB.InverseMass() + (JInvAxT % AxT) * ContactTangent + (JInvBxT % BxT) * ContactTangent;
Float FrictionMag = FrictionNum / FrictionDen;
BeVector3 Friction = ContactTangent * FrictionMag;
if (BodyA.IsDynamic())
{
BodyA.AddLinearMomentum(Friction); BodyA.AddAngularMomentum(Contact.RelativePositionA() % Friction);
}
if (BodyB.IsDynamic())
{
BodyB.AddLinearMomentum(-Friction); BodyB.AddAngularMomentum(Contact.RelativePositionB() % Friction);
}
}
}
}
}
m_Contacts.clear();
}