I’ve implemented a collision resolution system based on Ian Mellingtons system Cyclone and afterwards based on the GamePhysicsCookBook. Currently the last one is implemented.

For some reason the system behaves oddly. If I set up a test, where the cube drops from a bit up the Y-axis and with 45degree rotation over the x axis it just stays on that edge, then it starts moving around weirdly. That last part is due to friction.

A video sample here

You should be able to see the contactpoints and their normal (up the Y axis)

If I move the object and slam it into a wall, it will end up rotating forever.

It’s not the dampening, since that works fine – apply a force without gravity for instance and everything stops eventually.

It’s not the intertia tensor.

The contactnormal is pointing towards “B”, meaning that it has to be negative for the object to be treated as “A”

The code is as follows:

while (currentiteration < 8) {
                for (int cidx = 0; cidx < contactDataFrame.size(); cidx++) {
                    //apply impulse, frictionless
                    {
                        GVECTOR relativeContactPoint = Math::VectorSubtract(contactDataFrame[cidx].contactpoint, mPhysicsOBJData[0].position);
                        GVECTOR closingVelocity = Math::VectorNegate(Math::VectorAdd(mPhysicsOBJData[0].velocity, Math::Vector3Cross(mPhysicsOBJData[0].rotation, relativeContactPoint)));

                        float dp = Math::Vector3Dot(closingVelocity, Math::VectorNegate(contactDataFrame[cidx].contactnormal)).m128_f32[0];

                        if (dp > 0.0f) {
                            continue;
                        }

                        float numerator = (-(1.0f + restitution)) * dp;
                        
                        GMATRIX tensor = transformedTensors[0];; 
                        GVECTOR torque = Math::Vector3Cross(Math::Vec3MultiplyMatrixRHS(Math::Vector3Cross(relativeContactPoint, Math::VectorNegate(contactDataFrame[cidx].contactnormal)), tensor), relativeContactPoint);

                        float denominator = mPhysicsOBJData[0].inverseMass + Math::Vector3Dot(Math::VectorNegate(contactDataFrame[cidx].contactnormal), torque).m128_f32[0];
                        if (denominator == 0.0f) {
                            PRINT_N("ERROR - denominator J == 0");
                            continue;
                        }

                        float j = numerator / denominator;
                        j /= contactDataFrame[cidx].contactcount;

                        GVECTOR impulse = Math::VecMultiplyScalar(Math::VectorNegate(contactDataFrame[cidx].contactnormal), j);
                
                        mPhysicsOBJData[0].velocity = Math::VectorSubtract(mPhysicsOBJData[0].velocity, Math::VecMultiplyScalar(impulse, mPhysicsOBJData[0].inverseMass));
                        mPhysicsOBJData[0].rotation = Math::VectorSubtract(mPhysicsOBJData[0].rotation, Math::Vec3MultiplyMatrixRHS(Math::Vector3Cross(relativeContactPoint, impulse), tensor));

                        GVECTOR tangentNormal = Math::VectorSubtract(closingVelocity, Math::VecMultiplyScalar(Math::VectorNegate(contactDataFrame[cidx].contactnormal), Math::Vector3Dot(closingVelocity, Math::VectorNegate(contactDataFrame[cidx].contactnormal)).m128_f32[0]));

                        if (sqrt(Math::Vector3Dot(tangentNormal, tangentNormal).m128_f32[0]) == 0.0f) {
                            PRINT_N("ERROR - NO TAGENT VECTOR");
                            continue;
                        }
                        tangentNormal = Math::Vec3Normalize(tangentNormal);

                        numerator = -Math::Vector3Dot(closingVelocity, tangentNormal).m128_f32[0];
                        denominator = mPhysicsOBJData[0].inverseMass + Math::Vector3Dot(tangentNormal, Math::Vector3Cross(Math::Vec3MultiplyMatrixRHS(Math::Vector3Cross(relativeContactPoint, tangentNormal), tensor), relativeContactPoint)).m128_f32[0];
                        if (denominator == 0.0f) {
                            PRINT_N("ERROR - DENOMINATOR TANGET == 0");
                            continue;
                        }
                        float jt = numerator / denominator;
                        jt /= contactDataFrame[cidx].contactcount;

                        if (jt == 0.0f) {
                            PRINT_N("ERROR - NO JT IMPULSE");
                            continue;
                        }

                        float friction = 3.75f;
                        if (jt > j * friction) {
                            jt = j * friction;

                        }
                        else if (jt < -j * friction) {
                            jt = -j * friction;
                        }

                        GVECTOR tagentImpulse = Math::VecMultiplyScalar(tangentNormal, jt);
                        mPhysicsOBJData[0].velocity = Math::VectorSubtract(mPhysicsOBJData[0].velocity, tagentImpulse);
                        mPhysicsOBJData[0].rotation = Math::VectorSubtract(mPhysicsOBJData[0].rotation, Math::Vec3MultiplyMatrixRHS(Math::Vector3Cross(relativeContactPoint, tagentImpulse), tensor));
                    }
                }
                currentiteration++;
}