# Physics Engine Collision Response

I'm making a game engine with my integrated physics engine and I've encountered a problem where rigidbodies tend to over react in collisions and move around without and intention to slow down, I have a video to showcase the movement of the objects. I think that something with the torque is going wrong but I can't really tell. I picked up this book that you might have heard of Physics for Game Developers by David M. Bourg & Bryan Bywalec and followed the "Linear and Angular Impulse" section the book provides (Chapter 5, page 112).
The equations are:

• $$\J\$$: Impulse
• $$\v_r\$$: Relative Velocity
• $$\r_1\$$, $$\r_2\$$: vector from the body’s position to the collision point (1 and 2 for each object)
• $$\n\$$: the normal vector of the collision

\begin{align*} t &= (n \times v_r) \times n \\ t &= \frac{t}{|t|} \\ |J| &= -\frac{{(v_r \cdot n)(e + 1)}}{{\left(\frac{1}{{m_1}} + \frac{1}{{m_2}} + n \cdot \left(\frac{{r_1 \times n}}{{I_1}}\right) \times r_1 + n \cdot \left(\frac{{r_2 \times n}}{{I_2}}\right) \times r_2\right)}} \\ v_1^+ &= v_1^- + \frac{{J \cdot n + (\mu J) \cdot t}}{{m_1}} \\ v_2^+ &= v_2^- + \frac{{-J \cdot n + (\mu J) \cdot t}}{{m_2}} \\ \omega_1^+ &= \omega_1^- + \frac{{r_1 \times (J \cdot n+ (\mu J) \cdot t)}}{{I_{\text{cg}}}} \\ \omega_2^+ &= \omega_2^- + \frac{{r_2 \times (-J \cdot n+ (\mu J) \cdot t)}}{{I_{\text{cg}}}} \end{align*}

Below is the code for handling collision responses. I won't really mention the collision detection code because that is working fine and I’m pretty sure the problem lies in the collision response and not on the collision detection side. Though if after looking at the code and the problem you feel like the collision detection part causes the problem, let me know so I can post that part of the code too. I don't really understand where the problem is and I've tried different resources but nothing worked. The two collision points are on each of the corners of the rectangles. Example in the image below. Any help is appreciated.

// Thanks to the book "Physics for Game Developers" from David M. Bourg and Bryan Bywalec.
//Gets executed when a collision occurs.
void SapphirePhysics::RigidBody::OnCollisionRotation(RigidBody* bodyA, RigidBody* bodyB, CollisionData &&CD)
{
const glm::vec3& bodyAPos = bodyA->transform->GetPosition();
const glm::vec3& bodyASize = bodyA->transform->GetSize();
const glm::vec3& bodyBPos = bodyB->transform->GetPosition();
const glm::vec3& bodyBSize = bodyB->transform->GetSize();
float bodyAInvMass = 1.0f / bodyA->Mass.Get();
float bodyBInvMass = 1.0f / bodyB->Mass.Get();
float bodyAInvInertia = 1.0f / ((1.0f / 12.0f) * bodyA->Mass.Get() * (bodyASize.x * bodyASize.x + bodyASize.y * bodyASize.y));
float bodyBInvInertia = 1.0f / ((1.0f / 12.0f) * bodyB->Mass.Get() * (bodyBSize.x * bodyBSize.x + bodyBSize.y * bodyBSize.y));
// float bodyAInvInertia = 1.0f / (bodyASize.x * pow(bodyASize.y, 3));
// float bodyBInvInertia = 1.0f / (bodyBSize.x * pow(bodyBSize.y, 3));
if(bodyA->Static.Get()){
bodyAInvMass = 0.0f;
bodyAInvInertia = 0.0f;
}
if(bodyB->Static.Get()){
bodyBInvMass = 0.0f;
bodyBInvInertia = 0.0f;
}

std::array<glm::vec2, 2> contactList {glm::vec2(0),glm::vec2(0)};
std::array<glm::vec3, 2> raList = {glm::vec3(0),glm::vec3(0)};
std::array<glm::vec3, 2> rbList = {glm::vec3(0),glm::vec3(0)};
std::array<glm::vec3, 2> VpA = {glm::vec3(0),glm::vec3(0)};
std::array<glm::vec3, 2> VpB = {glm::vec3(0),glm::vec3(0)};
std::array<float, 2> JList;
contactList[0] = CD.ContactPoint1;
contactList[1] = CD.ContactPoint2;
//coefficient of restitution.
float e = std::max(bodyA->Restitution.Get(), bodyB->Restitution.Get());
//coefficient of friction.
float cf = 0.2f;

glm::vec3 n = glm::vec3(CD.Normal,0);
n = glm::normalize(n);
for (size_t i = 0; i < CD.ContactPointCount; i++)
{
glm::vec2 Point = contactList[i];
glm::vec3 r1 = glm::vec3(Point - glm::vec2(bodyAPos), 0);
glm::vec3 r2 = glm::vec3(Point - glm::vec2(bodyBPos), 0);
// SapphireEngine::AddLine(glm::vec2(bodyAPos), glm::vec2(bodyAPos) + glm::vec2(r1), glm::vec4(1,0,0,1), 5.0f);
// SapphireEngine::AddLine(glm::vec2(bodyBPos), glm::vec2(bodyBPos) + glm::vec2(r2), glm::vec4(0,1,0,1), 5.0f);
glm::vec3 Vp1 = bodyA->Velocity + glm::cross(bodyA->AngularVelocity, r1);
glm::vec3 Vp2 = bodyB->Velocity + glm::cross(bodyB->AngularVelocity, r2);
glm::vec3 RelativeVelocity = Vp1 - Vp2;
float J = -glm::dot(RelativeVelocity, n) * (e + 1)
/ (bodyAInvMass + bodyBInvMass
+ glm::dot(n, glm::cross(glm::cross(r1, n) * bodyAInvMass, r1))
+ glm::dot(n, glm::cross(glm::cross(r2, n) * bodyBInvInertia, r2)));

JList[i] = J;
raList[i] = r1;
rbList[i] = r2;
VpA[i] = Vp1;
VpB[i] = Vp2;
}
if(bodyB->Static.Get()){
bodyA->transform->Move(glm::vec3(-CD.Normal * CD.Depth,0));
for (size_t i = 0; i < CD.ContactPointCount; i++)
{
glm::vec3 RelVel = VpA[i] - VpB[i];
glm::vec3 t = glm::cross(glm::cross(n, RelVel), n);
if(t != glm::vec3(0))
t = glm::normalize(t);

glm::vec3 frictionImpulse = cf * JList[i]*t / FixedTimeStep;
glm::vec3 maxTangentialFrictionForce = cf * (JList[i] * n) / FixedTimeStep;
glm::vec3 Impulse = JList[i]*n/FixedTimeStep;

if (glm::length(frictionImpulse) > glm::length(maxTangentialFrictionForce)){
frictionImpulse = maxTangentialFrictionForce;
}
bodyA->Forces.push_back(Impulse);
bodyA->Forces.push_back(frictionImpulse);
bodyA->Torques.push_back(glm::cross(raList[i], Impulse));
bodyA->Torques.push_back(glm::cross(raList[i], frictionImpulse));

// bodyA->Velocity += (JList[i] * n + cf * JList[i] * t) * bodyAInvMass;
// bodyA->AngularVelocity += glm::cross(raList[i], JList[i] * n + cf * JList[i] * t) * bodyAInvInertia;
}

}
else if(bodyA->Static.Get()){
bodyB->transform->Move(glm::vec3(CD.Normal * CD.Depth,0));
for (size_t i = 0; i < CD.ContactPointCount; i++)
{
glm::vec3 RelVel = VpA[i] - VpB[i];
glm::vec3 t = glm::cross(glm::cross(n, RelVel), n);
if(t != glm::vec3(0))
t = glm::normalize(t);

glm::vec3 frictionImpulse = cf * JList[i]*t / FixedTimeStep;
glm::vec3 maxTangentialFrictionForce = cf * (JList[i] * n) / FixedTimeStep;
glm::vec3 Impulse = JList[i]*n/FixedTimeStep;

if (glm::length(frictionImpulse) > glm::length(maxTangentialFrictionForce)){
frictionImpulse = maxTangentialFrictionForce;
}
bodyB->Forces.push_back(-Impulse);
bodyB->Forces.push_back(frictionImpulse);
bodyB->Torques.push_back(glm::cross(rbList[i], -Impulse));
bodyB->Torques.push_back(glm::cross(rbList[i], frictionImpulse));
// bodyB->Velocity += (-JList[i] * n + cf * JList[i] * t) * bodyBInvMass;
// bodyB->AngularVelocity += glm::cross(rbList[i], -JList[i] * n + cf * JList[i] * t) * bodyBInvInertia;
}
}
else{
bodyA->transform->Move(glm::vec3(-CD.Normal * CD.Depth / 2.0f,0));
bodyB->transform->Move(glm::vec3(CD.Normal * CD.Depth / 2.0f,0));

// SapphireEngine::AddLine(glm::vec2(bodyAPos), glm::vec2(bodyAPos) + glm::vec2(bodyA->Forces.back()), glm::vec4(1,0,0,1), 5.0f);
// SapphireEngine::AddLine(glm::vec2(bodyBPos), glm::vec2(bodyBPos) + glm::vec2(bodyB->Forces.back()), glm::vec4(0,0,1,1), 5.0f);
for (size_t i = 0; i < CD.ContactPointCount; i++)
{
glm::vec3 RelVel = VpA[i] - VpB[i];
glm::vec3 t = glm::cross(glm::cross(n, RelVel), n);
if(t != glm::vec3(0))
t = glm::normalize(t);

glm::vec3 frictionImpulse = cf * JList[i]*t / FixedTimeStep;
glm::vec3 maxTangentialFrictionForce = cf * (JList[i] * n) / FixedTimeStep;
glm::vec3 Impulse = JList[i]*n/FixedTimeStep;

if (glm::length(frictionImpulse) > glm::length(maxTangentialFrictionForce)){
frictionImpulse = maxTangentialFrictionForce;
}
bodyA->Forces.push_back(Impulse);
bodyB->Forces.push_back(-Impulse);
bodyA->Forces.push_back(frictionImpulse);
bodyB->Forces.push_back(frictionImpulse);

bodyA->Torques.push_back(glm::cross(raList[i], Impulse));
bodyB->Torques.push_back(glm::cross(rbList[i], -Impulse));

bodyA->Torques.push_back(glm::cross(raList[i], frictionImpulse));
bodyB->Torques.push_back(glm::cross(rbList[i], frictionImpulse));
}

}
}

//Gets called every frame
void SapphirePhysics::RigidBody::Update(float DeltaTime)
{
if(Static.Get()) return;
Forces.push_back(glm::vec3(0, SapphirePhysics::CollisionDetection::g.Get(), 0) * Mass.Get());
Accelaration = (SapphireEngine::VectorSum(Forces)/Mass.Get());
AngularAccelaration = SapphireEngine::VectorSum(Torques) / (((1.0f / 12.0f) * Mass.Get() * (transform->GetSize().x * transform->GetSize().x + transform->GetSize().y * transform->GetSize().y)));
Velocity += Accelaration * DeltaTime;
AngularVelocity.z += AngularAccelaration.z * DeltaTime;
transform->Move(Velocity * DeltaTime);
if(Rotate.Get())
transform->Rotate(AngularVelocity.z * DeltaTime);
StartingVelocity = glm::vec3(0);
Forces.clear();
Torques.clear();
}