aboutsummaryrefslogtreecommitdiff
path: root/Client/Source/Phy2D/Dynamic/Arbiter.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'Client/Source/Phy2D/Dynamic/Arbiter.cpp')
-rw-r--r--Client/Source/Phy2D/Dynamic/Arbiter.cpp189
1 files changed, 0 insertions, 189 deletions
diff --git a/Client/Source/Phy2D/Dynamic/Arbiter.cpp b/Client/Source/Phy2D/Dynamic/Arbiter.cpp
deleted file mode 100644
index 4163154..0000000
--- a/Client/Source/Phy2D/Dynamic/Arbiter.cpp
+++ /dev/null
@@ -1,189 +0,0 @@
-#include "Arbiter.h"
-#include "World.h"
-#include "Body.h"
-#include "Joint.h"
-
-using namespace Phy2D;
-
-Arbiter::Arbiter(Body* b1, Body* b2)
-{
- if (b1 < b2)
- {
- body1 = b1;
- body2 = b2;
- }
- else
- {
- body1 = b2;
- body2 = b1;
- }
-
- numContacts = Collide(contacts, body1, body2);
-
- friction = SQRT(body1->friction * body2->friction);
-}
-
-void Arbiter::Update(Contact* newContacts, int numNewContacts)
-{
- Contact mergedContacts[2];
-
- 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->feature.value == cOld->feature.value)
- {
- k = j;
- break;
- }
- }
-
- if (k > -1)
- {
- Contact* c = mergedContacts + i;
- Contact* cOld = contacts + k;
- *c = *cNew;
- if (World::warmStarting)
- {
- c->Pn = cOld->Pn;
- c->Pt = cOld->Pt;
- c->Pnb = cOld->Pnb;
- }
- else
- {
- c->Pn = 0.0f;
- c->Pt = 0.0f;
- c->Pnb = 0.0f;
- }
- }
- else
- {
- mergedContacts[i] = newContacts[i];
- }
- }
-
- for (int i = 0; i < numNewContacts; ++i)
- contacts[i] = mergedContacts[i];
-
- numContacts = numNewContacts;
-}
-
-
-void Arbiter::PreStep(number inv_dt)
-{
- const number k_allowedPenetration = 0.01f;
- number k_biasFactor = World::positionCorrection ? 0.2f : 0.0f;
-
- for (int i = 0; i < numContacts; ++i)
- {
- Contact* c = contacts + i;
-
- Vec2 r1 = c->position - body1->position;
- Vec2 r2 = c->position - body2->position;
-
- // Precompute normal mass, tangent mass, and bias.
- number rn1 = Dot(r1, c->normal);
- number rn2 = Dot(r2, c->normal);
- number kNormal = body1->invMass + body2->invMass;
- kNormal += body1->invI * (Dot(r1, r1) - rn1 * rn1) + body2->invI * (Dot(r2, r2) - rn2 * rn2);
- c->massNormal = (number) 1.0f / kNormal;
-
- Vec2 tangent = Cross(c->normal, 1.0f);
- number rt1 = Dot(r1, tangent);
- number rt2 = Dot(r2, tangent);
- number kTangent = body1->invMass + body2->invMass;
- kTangent += body1->invI * (Dot(r1, r1) - rt1 * rt1) + body2->invI * (Dot(r2, r2) - rt2 * rt2);
- c->massTangent = (number) 1.0f / kTangent;
-
- c->bias = -k_biasFactor * inv_dt * Min(0.0f, c->separation + k_allowedPenetration);
-
- if (World::accumulateImpulses)
- {
- // Apply normal + friction impulse
- Vec2 P = c->Pn * c->normal + c->Pt * tangent;
-
- body1->velocity -= body1->invMass * P;
- body1->angularVelocity -= body1->invI * Cross(r1, P);
-
- body2->velocity += body2->invMass * P;
- body2->angularVelocity += body2->invI * Cross(r2, P);
- }
- }
-}
-
-void Arbiter::ApplyImpulse()
-{
- Body* b1 = body1;
- Body* b2 = body2;
-
- for (int i = 0; i < numContacts; ++i)
- {
- Contact* c = contacts + i;
- c->r1 = c->position - b1->position;
- c->r2 = c->position - b2->position;
-
- // Relative velocity at contact
- Vec2 dv = b2->velocity + Cross(b2->angularVelocity, c->r2) - b1->velocity - Cross(b1->angularVelocity, c->r1);
-
- // Compute normal impulse
- number vn = Dot(dv, c->normal);
-
- number dPn = c->massNormal * (-vn + c->bias);
-
- if (World::accumulateImpulses)
- {
- // Clamp the accumulated impulse
- number Pn0 = c->Pn;
- c->Pn = Max(Pn0 + dPn, 0.0f);
- dPn = c->Pn - Pn0;
- }
- else
- {
- dPn = Max(dPn, 0.0f);
- }
-
- // Apply contact impulse
- Vec2 Pn = dPn * c->normal;
-
- b1->velocity -= b1->invMass * Pn;
- b1->angularVelocity -= b1->invI * Cross(c->r1, Pn);
-
- b2->velocity += b2->invMass * Pn;
- b2->angularVelocity += b2->invI * Cross(c->r2, Pn);
-
- // Relative velocity at contact
- dv = b2->velocity + Cross(b2->angularVelocity, c->r2) - b1->velocity - Cross(b1->angularVelocity, c->r1);
-
- Vec2 tangent = Cross(c->normal, 1.0f);
- number vt = Dot(dv, tangent);
- number dPt = c->massTangent * (-vt);
-
- if (World::accumulateImpulses)
- {
- // Compute friction impulse
- number maxPt = friction * c->Pn;
-
- // Clamp friction
- number oldTangentImpulse = c->Pt;
- c->Pt = Clamp(oldTangentImpulse + dPt, -maxPt, maxPt);
- dPt = c->Pt - oldTangentImpulse;
- }
- else
- {
- number maxPt = friction * dPn;
- dPt = Clamp(dPt, -maxPt, maxPt);
- }
-
- // Apply contact impulse
- Vec2 Pt = dPt * tangent;
-
- b1->velocity -= b1->invMass * Pt;
- b1->angularVelocity -= b1->invI * Cross(c->r1, Pt);
-
- b2->velocity += b2->invMass * Pt;
- b2->angularVelocity += b2->invI * Cross(c->r2, Pt);
- }
-}