Hi, I've been trying to build a physics engine for my own game for a while. But I face an issue which I'm unable to find a solution. They sink through the floor(Plane Collider). Can anyone give me a hand here? Without a stable simulation, I can't complete the game. Pls. Sphere-Sphere and Sphere-Plane are working fine, others not ok. Also friction doesnt work either

#include "Arbiter.h"


Arbiter::Arbiter()
{
}

Arbiter::Arbiter(RigidBody* b1, RigidBody* b2)
{
	if (b1 < b2)
	{
		body1 = b1;
		body2 = b2;
	}
	else
	{
		body1 = b2;
		body2 = b1;
	}
	restituition = body1->GetRestitution() * body2->GetRestitution();
	friction = sqrt(body1->GetStaticFriction() * body2->GetStaticFriction());

	auto colA = body1->GetCollider();
	auto colB = body2->GetCollider();

	colA->TestIntersection(colB, contacts);
	numContacts = contacts.size();
}

void Arbiter::UpdateManifold(vector<Contact>& newContacts, int numNewContacts)
{
	vector<Contact> mergedContacts;

	for (int i = 0; i < numNewContacts; ++i)
	{
		Contact& cNew = newContacts[i];
		int k = -1;
		for (int j = 0; j < numContacts; ++j)
		{
			Contact& cOld = contacts[j];
			if (cNew.contactID == cOld.contactID)
			{
				k = j;
				break;
			}
		}

		if (k > -1)
		{
			Contact& cOld = contacts[k];
			Contact c;
			c.contactID = cNew.contactID;
			c.bias = cNew.bias;
			c.massNormal = cNew.massNormal;
			c.massTangent[0] = cNew.massTangent[0];
			c.massTangent[1] = cNew.massTangent[1];
			c.normal = cNew.normal;
			c.separation = cNew.separation;
			c.point = cNew.point;

			c.Pn = cOld.Pn;
			c.Pt[0] = cOld.Pt[0];
			c.Pt[1] = cOld.Pt[1];

			mergedContacts.push_back(c);
		}
		else
		{
			mergedContacts.push_back(cNew);
		}
	}

	contacts.clear();
	for (int i = 0; i < numNewContacts; ++i)
		contacts.push_back(mergedContacts[i]);

	numContacts = numNewContacts;
}


void Arbiter::PreStep(float inv_dt)
{
	XMVECTOR ContactTangent[2];

	for (int i = 0; i < numContacts; ++i)
	{
		Contact& c = contacts[i];

		XMVECTOR ContactPos = c.point;

		XMVECTOR VelocityA = body1->GetVelocityAtPoint(ContactPos);
		XMVECTOR VelocityB = body2->GetVelocityAtPoint(ContactPos);
		XMVECTOR dVel = VelocityB - VelocityA;

		XMVECTOR rA = (ContactPos - body1->GetPosition());
		XMVECTOR rB = (ContactPos - body2->GetPosition());

		XMVECTOR ContactNormal = c.normal;
		float Val = XMVector3Dot((body2->GetPosition() - body1->GetPosition()), ContactNormal).m128_f32[0];
		if (Val < 0.0f)
			ContactNormal *= -1;

		c.bias += -0.2f * inv_dt * min(0.0f, c.separation + 0.05f); 

		float Cdot = XMVector3Dot(dVel, ContactNormal).m128_f32[0];
		if (Cdot < -1.0f)
			c.bias += -restituition * Cdot; 

		float term1 =  body1->GetMass()>0.0f ? (1.0f / body1->GetMass()) : 0.0f;
		float term2 =  body2->GetMass()>0.0f ? (1.0f / body2->GetMass()) : 0.0f;

		XMVECTOR rnA = XMVector3Cross(rA, ContactNormal);
		XMVECTOR rnB = XMVector3Cross(rB, ContactNormal);

		float K = term1 + term2 + XMVector3Dot(rnA, XMVector4Transform(rnA, body1->GetIInverse())).m128_f32[0] + XMVector3Dot(rnB, XMVector4Transform(rnB, body2->GetIInverse())).m128_f32[0];
		c.massNormal = K > 0.0f ? 1.0f / K : 0.0f;

		ContactTangent[0] = XMVector3Orthogonal(ContactNormal);
		ContactTangent[1] = XMVector3Cross(ContactTangent[0], ContactNormal);;

		XMVECTOR rt1A = XMVector3Cross(rA, ContactTangent[0]);
		XMVECTOR rt1B = XMVector3Cross(rB, ContactTangent[0]);

		XMVECTOR rt2A = XMVector3Cross(rA, ContactTangent[1]);
		XMVECTOR rt2B = XMVector3Cross(rB, ContactTangent[1]);

		float K1 =	term1 + term2 + XMVector3Dot(rt1A, XMVector4Transform(rt1A, body1->GetIInverse())).m128_f32[0] + XMVector3Dot(rt1B, XMVector4Transform(rt1B, body2->GetIInverse())).m128_f32[0];
		float K2 = 	term1 + term2 + XMVector3Dot(rt2A, XMVector4Transform(rt2A, body1->GetIInverse())).m128_f32[0] + XMVector3Dot(rt2B, XMVector4Transform(rt2B, body2->GetIInverse())).m128_f32[0];

		c.massTangent[0] = K1 > 0.0f ? 1.0f / K1 : 0.0f;
		c.massTangent[1] = K2 > 0.0f ? 1.0f / K2 : 0.0f;

		XMVECTOR P = c.Pn * ContactNormal + c.Pt[0] * ContactTangent[0] + c.Pt[1] * ContactTangent[1];

		body1->AddImpulse(-P);
		body1->AddImpulsiveTorque(-XMVector3Cross(rA, P));
		body2->AddImpulse(P);
		body2->AddImpulsiveTorque(XMVector3Cross(rB, P));
	}
}

float Clamp(float a, float low, float high)
{
	return max(low, min(a, high));
}

void Arbiter::ApplyImpulse()
{
	XMVECTOR ContactTangent[2];

	for (int i = 0; i < numContacts; ++i)
	{
		Contact& c = contacts[i];

		XMVECTOR ContactPos = c.point;

		XMVECTOR VelocityA = body1->GetVelocityAtPoint(ContactPos);
		XMVECTOR VelocityB = body2->GetVelocityAtPoint(ContactPos);
		XMVECTOR dVel = VelocityB - VelocityA;

		XMVECTOR rA = (ContactPos - body1->GetPosition());
		XMVECTOR rB = (ContactPos - body2->GetPosition());

		XMVECTOR& ContactNormal = c.normal;
		float Val = XMVector3Dot((body2->GetPosition() - body1->GetPosition()), ContactNormal).m128_f32[0];
		if (Val < 0.0f)
			ContactNormal *= -1;

		{
			float Cdot = XMVector3Dot(dVel, ContactNormal).m128_f32[0];
			float dPn = c.massNormal * (-Cdot + c.bias);

			float Pn0 = c.Pn;
			c.Pn = max(Pn0 + dPn, 0.0f);
			dPn = c.Pn - Pn0;

			XMVECTOR P = dPn * ContactNormal;

			body1->AddImpulse(-P);
			body1->AddImpulsiveTorque(-XMVector3Cross(rA, P));
			body2->AddImpulse(P);
			body2->AddImpulsiveTorque(XMVector3Cross(rB, P));
		}

		ContactPos = c.point;

		VelocityA = body1->GetVelocityAtPoint(ContactPos);
		VelocityB = body2->GetVelocityAtPoint(ContactPos);
		dVel = VelocityB - VelocityA;

		rA = (ContactPos - body1->GetPosition());
		rB = (ContactPos - body2->GetPosition());

		{
			ContactTangent[0] = XMVector3Orthogonal(ContactNormal);
			ContactTangent[1] = XMVector3Cross(ContactTangent[0], ContactNormal);

			for (int z=0;z<2;z++)
			{
				float vt = XMVector3Dot(dVel, ContactTangent[z]).m128_f32[0];
				float dPt = c.massTangent[z] * (-vt);

				float maxPt = friction * c.Pn;

				float oldTangentImpulse = c.Pt[z];
				c.Pt[z] = Clamp(oldTangentImpulse + dPt, -maxPt, maxPt);
				dPt = c.Pt[z] - oldTangentImpulse;

				XMVECTOR Pt = dPt * ContactTangent[z];

				body1->AddImpulse(-Pt);
				body1->AddImpulsiveTorque(-XMVector3Cross(rA, Pt));
				body2->AddImpulse(Pt);
				body2->AddImpulsiveTorque(XMVector3Cross(rB, Pt));
			}
		}
	}
}

#pragma once
#include <windows.h>
#include <vector>
#include <algorithm>

#include <DirectXMath.h>
#include "../Colliders/SphereCollider.h"
#include "../Colliders/CapsuleCollider.h"
#include "../Colliders/PlaneCollider.h"
#include "../../Logger/RELog.h"

using namespace std;
using namespace	DirectX;

class Intersections {
public:
	static bool SphereSphere(SphereCollider *A, SphereCollider *B, vector<Contact>& contacts) 
	{
		XMVECTOR CenterSeparation = B->GetCenter() - A->GetCenter();
		float Separation = XMVector3Length(CenterSeparation).m128_f32[0] - (A->GetRadius() + B->GetRadius());
		bool testPart = (Separation <= 0.0f);

		if (testPart)
		{
			XMVECTOR Normal = XMVector3Normalize(CenterSeparation);
			XMVECTOR EndSurfPointA = A->GetCenter() + Normal * A->GetRadius();
			XMVECTOR EndSurfPointB = B->GetCenter() - Normal * B->GetRadius();
			XMVECTOR ContactPoint = (EndSurfPointA + EndSurfPointB) / 2.0f;

			char cStr[2048];
			sprintf_s(cStr, "[COLLISION] [SPHERE, SPHERE] Normal %.10f %.10f %.10f Point %.10f %.10f %.10f Separation %.10f", Normal.m128_f32[0], Normal.m128_f32[1], Normal.m128_f32[2], ContactPoint.m128_f32[0], ContactPoint.m128_f32[1], ContactPoint.m128_f32[2], Separation);
			string str(cStr);
			RELog::GetInstance()->WriteMessage(str);

			Contact contact;
			contact.point = ContactPoint;
			contact.normal = Normal;
			contact.separation = Separation;
			contact.contactID = 1;
			contacts.push_back(contact);
		}
		return testPart;
	}

	static bool SphereCapsule(SphereCollider *A, CapsuleCollider *B, vector<Contact>& contacts)
	{
		float sigma = XMVector3Dot(A->GetCenter() - B->GetCenter1(), B->GetDirection()).m128_f32[0];
		if (sigma <= 0.0f) sigma = 0.0f;
		else
		if (sigma >= B->GetHeight()) sigma = B->GetHeight();

		XMVECTOR X = B->GetCenter1() + sigma*B->GetDirection();
		float rS = A->GetRadius();
		float rC = B->GetRadius();

		XMVECTOR CenterSeparation = X - A->GetCenter();
		float Separation = XMVector3Length(CenterSeparation).m128_f32[0] - (rS + rC);
		bool testPart = (Separation <= 0.0f);

		if (testPart)
		{
			XMVECTOR Normal = XMVector3Normalize(CenterSeparation);
			XMVECTOR EndSurfPointA = A->GetCenter() + Normal * A->GetRadius();
			XMVECTOR EndSurfPointB = X - Normal * B->GetRadius();
			XMVECTOR ContactPoint = (EndSurfPointA + EndSurfPointB) / 2.0f;

			char cStr[2048];
			sprintf_s(cStr, "[COLLISION] [SPHERE, CAPSULE] Normal %.10f %.10f %.10f Point %.10f %.10f %.10f Separation %.10f", Normal.m128_f32[0], Normal.m128_f32[1], Normal.m128_f32[2], ContactPoint.m128_f32[0], ContactPoint.m128_f32[1], ContactPoint.m128_f32[2], Separation);
			string str(cStr);
			RELog::GetInstance()->WriteMessage(str);

			Contact contact;
			contact.point = ContactPoint;
			contact.normal = Normal;
			contact.separation = Separation;
			contact.contactID = 2;
			contacts.push_back(contact);
		}
		return testPart;
	}

	static bool SpherePlane(SphereCollider *A, PlaneCollider *B, vector<Contact>& contacts)
	{
		float Separation = (XMVector3Dot(A->GetCenter(), B->GetDirection()).m128_f32[0] - B->GetDistanceFromOrigin()) - A->GetRadius();
		bool test = (Separation <= 0.0f);

		if (test)
		{
			XMVECTOR Normal = XMVector3Normalize(B->GetDirection());
			XMVECTOR ContactPoint = A->GetCenter() - Normal * A->GetRadius();

			char cStr[2048];
			sprintf_s(cStr, "[COLLISION] [SPHERE, PLANE] Normal %.10f %.10f %.10f Point %.10f %.10f %.10f Separation %.10f", Normal.m128_f32[0], Normal.m128_f32[1], Normal.m128_f32[2], ContactPoint.m128_f32[0], ContactPoint.m128_f32[1], ContactPoint.m128_f32[2], Separation);
			string str(cStr);
			RELog::GetInstance()->WriteMessage(str);

			Contact contact;
			contact.point = ContactPoint;
			contact.normal = Normal;
			contact.separation = Separation;
			contact.contactID = 3;
			contacts.push_back(contact);
		}
		return test;
	}

	static bool CapsuleCapsule(CapsuleCollider *A, CapsuleCollider *B, vector<Contact>& contacts)
	{
		XMVECTOR distVec[4];
		distVec[0] = B->GetCenter2() - A->GetCenter1();
		distVec[1] = B->GetCenter2() - A->GetCenter2();
		distVec[2] = B->GetCenter1() - A->GetCenter1();
		distVec[3] = B->GetCenter1() - A->GetCenter2();

		float Distance[4];
		Distance[0] = XMVector3Length(distVec[0]).m128_f32[0];
		Distance[1] = XMVector3Length(distVec[1]).m128_f32[0];
		Distance[2] = XMVector3Length(distVec[2]).m128_f32[0];
		Distance[3] = XMVector3Length(distVec[3]).m128_f32[0];

		float rA = A->GetRadius();
		float rB = B->GetRadius();

		float minDist = Distance[0];
		int index = 0;
		for (int i=1; i<4; i++)
		{
			if (minDist > Distance[i]) {
				minDist = Distance[i];
				index = i;
			}
		}

		XMVECTOR BestCenterA, BestCenterB;
		if (index == 0)
		{
			BestCenterA = A->GetCenter1();
			BestCenterB = B->GetCenter2();
		}
		else if (index == 1)
		{
			BestCenterA = A->GetCenter2();
			BestCenterB = B->GetCenter2();
		}
		else if (index == 2)
		{
			BestCenterA = A->GetCenter1();
			BestCenterB = B->GetCenter1();
		}
		else if (index == 3)
		{
			BestCenterA = A->GetCenter2();
			BestCenterB = B->GetCenter1();
		}

		float sigmaB = XMVector3Dot(BestCenterA - BestCenterB, B->GetDirection()).m128_f32[0];

		XMVECTOR X_B = BestCenterB + sigmaB * B->GetDirection();
		XMVECTOR CenterSeparation = X_B - BestCenterA;

		float phi = XMVector3Dot(X_B - B->GetCenter1(), B->GetDirection()).m128_f32[0];
		bool testPart2 = (phi >= 0.0f && phi <= B->GetHeight());

		float Separation = XMVector3Length(CenterSeparation).m128_f32[0] - (rA + rB);
		bool testPart1 = (Separation <= 0.0f);

		if (testPart1 && testPart2)
		{
			XMVECTOR Normal = XMVector3Normalize(CenterSeparation);
			XMVECTOR EndSurfPointA = BestCenterA + Normal * rA;
			XMVECTOR EndSurfPointB = X_B - Normal * rB;
			XMVECTOR ContactPoint = (EndSurfPointA + EndSurfPointB) / 2.0f;

			char cStr[2048];
			sprintf_s(cStr, "[COLLISION] [CAPSULE, CAPSULE] Normal %.10f %.10f %.10f Point %.10f %.10f %.10f Separation %.10f", Normal.m128_f32[0], Normal.m128_f32[1], Normal.m128_f32[2], ContactPoint.m128_f32[0], ContactPoint.m128_f32[1], ContactPoint.m128_f32[2], Separation);
			string str(cStr);
			RELog::GetInstance()->WriteMessage(str);

			Contact contact;
			contact.point = ContactPoint;
			contact.normal = Normal;
			contact.separation = Separation;
			contact.contactID = 4;
			contacts.push_back(contact);
		}
		return testPart1 && testPart2;
	}

	static bool CapsulePlane(CapsuleCollider *A, PlaneCollider *B, vector<Contact>& contacts)
	{
		float Distance1 = (XMVector3Dot(A->GetCenter1(), B->GetDirection()).m128_f32[0] - B->GetDistanceFromOrigin());
		float Distance2 = (XMVector3Dot(A->GetCenter2(), B->GetDirection()).m128_f32[0] - B->GetDistanceFromOrigin());

		float MinDistance = min(Distance1, Distance2);
		float Separation = (MinDistance - A->GetRadius());
		bool test = (Separation <= 0.0f);

		if (test)
		{
			XMVECTOR Normal = XMVector3Normalize(B->GetDirection());
			XMVECTOR CenterPoint;
			if (Distance1 > Distance2)
			{
				CenterPoint = A->GetCenter2();
				XMVECTOR ContactPoint = CenterPoint - Normal * A->GetRadius();

				char cStr[2048];
				sprintf_s(cStr, "[COLLISION] [CAPSULE, PLANE] Normal %.10f %.10f %.10f Point %.10f %.10f %.10f Separation %.10f", Normal.m128_f32[0], Normal.m128_f32[1], Normal.m128_f32[2], ContactPoint.m128_f32[0], ContactPoint.m128_f32[1], ContactPoint.m128_f32[2], Separation);
				string str(cStr);
				RELog::GetInstance()->WriteMessage(str);

				Contact contact;
				contact.point = ContactPoint;
				contact.normal = Normal;
				contact.separation = Separation;
				contact.contactID = 5;
				contacts.push_back(contact);
			}
			else if (Distance1 < Distance2)
			{
				CenterPoint = A->GetCenter1();
				XMVECTOR ContactPoint = CenterPoint - Normal * A->GetRadius();

				char cStr[2048];
				sprintf_s(cStr, "[COLLISION] [CAPSULE, PLANE] Normal %.10f %.10f %.10f Point %.10f %.10f %.10f Separation %.10f", Normal.m128_f32[0], Normal.m128_f32[1], Normal.m128_f32[2], ContactPoint.m128_f32[0], ContactPoint.m128_f32[1], ContactPoint.m128_f32[2], Separation);
				string str(cStr);
				RELog::GetInstance()->WriteMessage(str);

				Contact contact;
				contact.point = ContactPoint;
				contact.normal = Normal;
				contact.separation = Separation;
				contact.contactID = 5;
				contacts.push_back(contact);
			}
			else if (Distance1 == Distance2)
			{
				{
					XMVECTOR ContactPoint1 = A->GetCenter1() - Normal * A->GetRadius();
					char cStr[2048];
					sprintf_s(cStr, "[COLLISION] [CAPSULE, PLANE] Normal %.10f %.10f %.10f Point %.10f %.10f %.10f Separation %.10f", Normal.m128_f32[0], Normal.m128_f32[1], Normal.m128_f32[2], ContactPoint1.m128_f32[0], ContactPoint1.m128_f32[1], ContactPoint1.m128_f32[2], Separation);
					string str(cStr);
					RELog::GetInstance()->WriteMessage(str);

					Contact contact;
					contact.point = ContactPoint1;
					contact.normal = Normal;
					contact.separation = Separation;
					contact.contactID = 5;
					contacts.push_back(contact);
				}
				{
					XMVECTOR ContactPoint2 = A->GetCenter2() - Normal * A->GetRadius();
					char cStr[2048];
					sprintf_s(cStr, "[COLLISION] [CAPSULE, PLANE] Normal %.10f %.10f %.10f Point %.10f %.10f %.10f Separation %.10f", Normal.m128_f32[0], Normal.m128_f32[1], Normal.m128_f32[2], ContactPoint2.m128_f32[0], ContactPoint2.m128_f32[1], ContactPoint2.m128_f32[2], Separation);
					string str(cStr);
					RELog::GetInstance()->WriteMessage(str);

					Contact contact;
					contact.point = ContactPoint2;
					contact.normal = Normal;
					contact.separation = Separation;
					contact.contactID = 6;
					contacts.push_back(contact);
				}
			}

		}
		return test;
	}
};