diff options
Diffstat (limited to 'Client/Source/Phy2D/Dynamic/Arbiter.cpp')
-rw-r--r-- | Client/Source/Phy2D/Dynamic/Arbiter.cpp | 189 |
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); - } -} |