Code:

        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();        }