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