diff options
Diffstat (limited to 'Client/Source/Phy2D/Dynamic')
-rw-r--r-- | Client/Source/Phy2D/Dynamic/Arbiter.cpp | 189 | ||||
-rw-r--r-- | Client/Source/Phy2D/Dynamic/Arbiter.h | 91 | ||||
-rw-r--r-- | Client/Source/Phy2D/Dynamic/Body.cpp | 47 | ||||
-rw-r--r-- | Client/Source/Phy2D/Dynamic/Body.h | 34 | ||||
-rw-r--r-- | Client/Source/Phy2D/Dynamic/Collide.cpp | 326 | ||||
-rw-r--r-- | Client/Source/Phy2D/Dynamic/Joint.cpp | 102 | ||||
-rw-r--r-- | Client/Source/Phy2D/Dynamic/Joint.h | 34 | ||||
-rw-r--r-- | Client/Source/Phy2D/Dynamic/World.cpp | 129 | ||||
-rw-r--r-- | Client/Source/Phy2D/Dynamic/World.h | 36 |
9 files changed, 0 insertions, 988 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); - } -} diff --git a/Client/Source/Phy2D/Dynamic/Arbiter.h b/Client/Source/Phy2D/Dynamic/Arbiter.h deleted file mode 100644 index 64bed72..0000000 --- a/Client/Source/Phy2D/Dynamic/Arbiter.h +++ /dev/null @@ -1,91 +0,0 @@ -#pragma once
- -#include "../Common/Math.h" - -namespace Phy2D -{ - - struct Body; - - union FeaturePair - { - struct Edges - { - char inEdge1; - char outEdge1; - char inEdge2; - char outEdge2; - } e; - int value; - }; - - struct Contact - { - Contact() : Pn(0.0f), Pt(0.0f), Pnb(0.0f) {} - - Vec2 position; - Vec2 normal; - Vec2 r1, r2; - number separation; - number Pn; // accumulated normal impulse - number Pt; // accumulated tangent impulse - number Pnb; // accumulated normal impulse for position bias - number massNormal, massTangent; - number bias; - FeaturePair feature; - }; - - struct ArbiterKey - { - ArbiterKey(Body* b1, Body* b2) - { - if (b1 < b2) - { - body1 = b1; body2 = b2; - } - else - { - body1 = b2; body2 = b1; - } - } - - Body* body1; - Body* body2; - }; - - struct Arbiter - { - enum { MAX_POINTS = 2 }; - - Arbiter(Body* b1, Body* b2); - - void Update(Contact* contacts, int numContacts); - - void PreStep(number inv_dt); - void ApplyImpulse(); - - Contact contacts[MAX_POINTS]; - int numContacts; - - Body* body1; - Body* body2; - - // Combined friction - number friction; - }; - - // This is used by std::set - inline bool operator < (const ArbiterKey& a1, const ArbiterKey& a2) - { - if (a1.body1 < a2.body1) - return true; - - if (a1.body1 == a2.body1 && a1.body2 < a2.body2) - return true; - - return false; - } - - int Collide(Contact* contacts, Body* body1, Body* body2); - -}
\ No newline at end of file diff --git a/Client/Source/Phy2D/Dynamic/Body.cpp b/Client/Source/Phy2D/Dynamic/Body.cpp deleted file mode 100644 index 1d896fd..0000000 --- a/Client/Source/Phy2D/Dynamic/Body.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include "Body.h"
- -using namespace Phy2D; - -Body::Body() -{ - position.Set(0.0f, 0.0f); - rotation = 0.0f; - velocity.Set(0.0f, 0.0f); - angularVelocity = 0.0f; - force.Set(0.0f, 0.0f); - torque = 0.0f; - friction = 0.2f; - - width.Set(1.0f, 1.0f); - mass = NUMBER_MAX; - invMass = 0.0f; - I = NUMBER_MAX; - invI = 0.0f; -} - -void Body::Set(const Vec2& w, number m) -{ - position.Set(0.0f, 0.0f); - rotation = 0.0f; - velocity.Set(0.0f, 0.0f); - angularVelocity = 0.0f; - force.Set(0.0f, 0.0f); - torque = 0.0f; - friction = 0.2f; - - width = w; - mass = m; - - if (mass < NUMBER_MAX) - { - invMass = (number)1.0f / mass; - I = mass * (width.x * width.x + width.y * width.y) / 12.0f; - invI = (number)1.0f / I; - } - else - { - invMass = 0.0f; - I = NUMBER_MAX; - invI = 0.0f; - } -} diff --git a/Client/Source/Phy2D/Dynamic/Body.h b/Client/Source/Phy2D/Dynamic/Body.h deleted file mode 100644 index 8c2ac72..0000000 --- a/Client/Source/Phy2D/Dynamic/Body.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once
-
-#include "../Common/Math.h"
- -namespace Phy2D -{ - - struct Body - { - Body(); - void Set(const Vec2& w, number m); - - void AddForce(const Vec2& f) - { - force += f; - } - - Vec2 position; - number rotation; - - Vec2 velocity; - number angularVelocity; - - Vec2 force; - number torque; - - Vec2 width; - - number friction; - number mass, invMass; - number I, invI; - }; - -}
\ No newline at end of file diff --git a/Client/Source/Phy2D/Dynamic/Collide.cpp b/Client/Source/Phy2D/Dynamic/Collide.cpp deleted file mode 100644 index 6849c0e..0000000 --- a/Client/Source/Phy2D/Dynamic/Collide.cpp +++ /dev/null @@ -1,326 +0,0 @@ -#include "Arbiter.h"
-#include "Body.h"
-#include "World.h"
-#include "Joint.h"
-
-using namespace Phy2D;
- -// Box vertex and edge numbering: -// -// ^ y -// | -// e1 -// v2 ------ v1 -// | | -// e2 | | e4 --> x -// | | -// v3 ------ v4 -// e3 - -enum Axis -{ - FACE_A_X, - FACE_A_Y, - FACE_B_X, - FACE_B_Y -}; - -enum EdgeNumbers -{ - NO_EDGE = 0, - EDGE1, - EDGE2, - EDGE3, - EDGE4 -}; - -struct ClipVertex -{ - ClipVertex() { fp.value = 0; } - Vec2 v; - FeaturePair fp; -}; - -void Flip(FeaturePair& fp) -{ - Swap(fp.e.inEdge1, fp.e.inEdge2); - Swap(fp.e.outEdge1, fp.e.outEdge2); -} - -int ClipSegmentToLine(ClipVertex vOut[2], ClipVertex vIn[2], - const Vec2& normal, number offset, char clipEdge) -{ - // Start with no output points - int numOut = 0; - - // Calculate the distance of end points to the line - number distance0 = Dot(normal, vIn[0].v) - offset; - number distance1 = Dot(normal, vIn[1].v) - offset; - - // If the points are behind the plane - if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; - if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; - - // If the points are on different sides of the plane - if (distance0 * distance1 < 0.0f) - { - // Find intersection point of edge and plane - number interp = distance0 / (distance0 - distance1); - vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); - if (distance0 > 0.0f) - { - vOut[numOut].fp = vIn[0].fp; - vOut[numOut].fp.e.inEdge1 = clipEdge; - vOut[numOut].fp.e.inEdge2 = NO_EDGE; - } - else - { - vOut[numOut].fp = vIn[1].fp; - vOut[numOut].fp.e.outEdge1 = clipEdge; - vOut[numOut].fp.e.outEdge2 = NO_EDGE; - } - ++numOut; - } - - return numOut; -} - -static void ComputeIncidentEdge(ClipVertex c[2], const Vec2& h, const Vec2& pos, - const Mat22& Rot, const Vec2& normal) -{ - // The normal is from the reference box. Convert it - // to the incident boxe's frame and flip sign. - Mat22 RotT = Rot.Transpose(); - Vec2 n = -(RotT * normal); - Vec2 nAbs = Abs(n); - - if (nAbs.x > nAbs.y) - { - if (Sign(n.x) > 0.0f) - { - c[0].v.Set(h.x, -h.y); - c[0].fp.e.inEdge2 = EDGE3; - c[0].fp.e.outEdge2 = EDGE4; - - c[1].v.Set(h.x, h.y); - c[1].fp.e.inEdge2 = EDGE4; - c[1].fp.e.outEdge2 = EDGE1; - } - else - { - c[0].v.Set(-h.x, h.y); - c[0].fp.e.inEdge2 = EDGE1; - c[0].fp.e.outEdge2 = EDGE2; - - c[1].v.Set(-h.x, -h.y); - c[1].fp.e.inEdge2 = EDGE2; - c[1].fp.e.outEdge2 = EDGE3; - } - } - else - { - if (Sign(n.y) > 0.0f) - { - c[0].v.Set(h.x, h.y); - c[0].fp.e.inEdge2 = EDGE4; - c[0].fp.e.outEdge2 = EDGE1; - - c[1].v.Set(-h.x, h.y); - c[1].fp.e.inEdge2 = EDGE1; - c[1].fp.e.outEdge2 = EDGE2; - } - else - { - c[0].v.Set(-h.x, -h.y); - c[0].fp.e.inEdge2 = EDGE2; - c[0].fp.e.outEdge2 = EDGE3; - - c[1].v.Set(h.x, -h.y); - c[1].fp.e.inEdge2 = EDGE3; - c[1].fp.e.outEdge2 = EDGE4; - } - } - - c[0].v = pos + Rot * c[0].v; - c[1].v = pos + Rot * c[1].v; -} - -namespace Phy2D -{ - - // The normal points from A to B - int Collide(Contact* contacts, Body* bodyA, Body* bodyB) - { - // Setup - Vec2 hA = 0.5f * bodyA->width; - Vec2 hB = 0.5f * bodyB->width; - - Vec2 posA = bodyA->position; - Vec2 posB = bodyB->position; - - Mat22 RotA(bodyA->rotation), RotB(bodyB->rotation); - - Mat22 RotAT = RotA.Transpose(); - Mat22 RotBT = RotB.Transpose(); - - Vec2 dp = posB - posA; - Vec2 dA = RotAT * dp; - Vec2 dB = RotBT * dp; - - Mat22 C = RotAT * RotB; - Mat22 absC = Abs(C); - Mat22 absCT = absC.Transpose(); - - // Box A faces - Vec2 faceA = Abs(dA) - hA - absC * hB; - if (faceA.x > 0.0f || faceA.y > 0.0f) - return 0; - - // Box B faces - Vec2 faceB = Abs(dB) - absCT * hA - hB; - if (faceB.x > 0.0f || faceB.y > 0.0f) - return 0; - - // Find best axis - Axis axis; - number separation; - Vec2 normal; - - // Box A faces - axis = FACE_A_X; - separation = faceA.x; - normal = dA.x > 0.0f ? RotA.col1 : -RotA.col1; - - const number relativeTol = 0.95f; - const number absoluteTol = 0.01f; - - if (faceA.y > relativeTol * separation + absoluteTol * hA.y) - { - axis = FACE_A_Y; - separation = faceA.y; - normal = dA.y > 0.0f ? RotA.col2 : -RotA.col2; - } - - // Box B faces - if (faceB.x > relativeTol * separation + absoluteTol * hB.x) - { - axis = FACE_B_X; - separation = faceB.x; - normal = dB.x > 0.0f ? RotB.col1 : -RotB.col1; - } - - if (faceB.y > relativeTol * separation + absoluteTol * hB.y) - { - axis = FACE_B_Y; - separation = faceB.y; - normal = dB.y > 0.0f ? RotB.col2 : -RotB.col2; - } - - // Setup clipping plane data based on the separating axis - Vec2 frontNormal, sideNormal; - ClipVertex incidentEdge[2]; - number front, negSide, posSide; - char negEdge, posEdge; - - // Compute the clipping lines and the line segment to be clipped. - switch (axis) - { - case FACE_A_X: - { - frontNormal = normal; - front = Dot(posA, frontNormal) + hA.x; - sideNormal = RotA.col2; - number side = Dot(posA, sideNormal); - negSide = -side + hA.y; - posSide = side + hA.y; - negEdge = EDGE3; - posEdge = EDGE1; - ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal); - } - break; - - case FACE_A_Y: - { - frontNormal = normal; - front = Dot(posA, frontNormal) + hA.y; - sideNormal = RotA.col1; - number side = Dot(posA, sideNormal); - negSide = -side + hA.x; - posSide = side + hA.x; - negEdge = EDGE2; - posEdge = EDGE4; - ComputeIncidentEdge(incidentEdge, hB, posB, RotB, frontNormal); - } - break; - - case FACE_B_X: - { - frontNormal = -normal; - front = Dot(posB, frontNormal) + hB.x; - sideNormal = RotB.col2; - number side = Dot(posB, sideNormal); - negSide = -side + hB.y; - posSide = side + hB.y; - negEdge = EDGE3; - posEdge = EDGE1; - ComputeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal); - } - break; - - case FACE_B_Y: - { - frontNormal = -normal; - front = Dot(posB, frontNormal) + hB.y; - sideNormal = RotB.col1; - number side = Dot(posB, sideNormal); - negSide = -side + hB.x; - posSide = side + hB.x; - negEdge = EDGE2; - posEdge = EDGE4; - ComputeIncidentEdge(incidentEdge, hA, posA, RotA, frontNormal); - } - break; - } - - // clip other face with 5 box planes (1 face plane, 4 edge planes) - - ClipVertex clipPoints1[2]; - ClipVertex clipPoints2[2]; - int np; - - // Clip to box side 1 - np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, negSide, negEdge); - - if (np < 2) - return 0; - - // Clip to negative box side 1 - np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, posSide, posEdge); - - if (np < 2) - return 0; - - // Now clipPoints2 contains the clipping points. - // Due to roundoff, it is possible that clipping removes all points. - - int numContacts = 0; - for (int i = 0; i < 2; ++i) - { - number separation = Dot(frontNormal, clipPoints2[i].v) - front; - - if (separation <= 0) - { - contacts[numContacts].separation = separation; - contacts[numContacts].normal = normal; - // slide contact point onto reference face (easy to cull) - contacts[numContacts].position = clipPoints2[i].v - separation * frontNormal; - contacts[numContacts].feature = clipPoints2[i].fp; - if (axis == FACE_B_X || axis == FACE_B_Y) - Flip(contacts[numContacts].feature); - ++numContacts; - } - } - - return numContacts; - } -}
\ No newline at end of file diff --git a/Client/Source/Phy2D/Dynamic/Joint.cpp b/Client/Source/Phy2D/Dynamic/Joint.cpp deleted file mode 100644 index 95f7c64..0000000 --- a/Client/Source/Phy2D/Dynamic/Joint.cpp +++ /dev/null @@ -1,102 +0,0 @@ -#include "Joint.h"
-#include "Body.h"
-#include "World.h"
-
-using namespace Phy2D;
- -void Joint::Set(Body* b1, Body* b2, const Vec2& anchor) -{ - body1 = b1; - body2 = b2; - - Mat22 Rot1(body1->rotation); - Mat22 Rot2(body2->rotation); - Mat22 Rot1T = Rot1.Transpose(); - Mat22 Rot2T = Rot2.Transpose(); - - localAnchor1 = Rot1T * (anchor - body1->position); - localAnchor2 = Rot2T * (anchor - body2->position); - - P.Set(0.0f, 0.0f); - - softness = 0.0f; - biasFactor = 0.2f; -} - -void Joint::PreStep(number inv_dt) -{ - // Pre-compute anchors, mass matrix, and bias. - Mat22 Rot1(body1->rotation); - Mat22 Rot2(body2->rotation); - - r1 = Rot1 * localAnchor1; - r2 = Rot2 * localAnchor2; - - // deltaV = deltaV0 + K * impulse - // invM = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)] - // = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y] - // [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x] - Mat22 K1; - K1.col1.x = body1->invMass + body2->invMass; K1.col2.x = 0.0f; - K1.col1.y = 0.0f; K1.col2.y = body1->invMass + body2->invMass; - - Mat22 K2; - K2.col1.x = body1->invI * r1.y * r1.y; K2.col2.x = -body1->invI * r1.x * r1.y; - K2.col1.y = -body1->invI * r1.x * r1.y; K2.col2.y = body1->invI * r1.x * r1.x; - - Mat22 K3; - K3.col1.x = body2->invI * r2.y * r2.y; K3.col2.x = -body2->invI * r2.x * r2.y; - K3.col1.y = -body2->invI * r2.x * r2.y; K3.col2.y = body2->invI * r2.x * r2.x; - - Mat22 K = K1 + K2 + K3; - K.col1.x += softness; - K.col2.y += softness; - - M = K.Invert(); - - Vec2 p1 = body1->position + r1; - Vec2 p2 = body2->position + r2; - Vec2 dp = p2 - p1; - - if (World::positionCorrection) - { - bias = -biasFactor * inv_dt * dp; - } - else - { - bias.Set(0.0f, 0.0f); - } - - if (World::warmStarting) - { - // Apply accumulated impulse. - body1->velocity -= body1->invMass * P; - body1->angularVelocity -= body1->invI * Cross(r1, P); - - body2->velocity += body2->invMass * P; - body2->angularVelocity += body2->invI * Cross(r2, P); - } - else - { - P.Set(0.0f, 0.0f); - } -} - -void Joint::ApplyImpulse() -{ - Vec2 dv = body2->velocity + Cross(body2->angularVelocity, r2) - body1->velocity - Cross(body1->angularVelocity, r1); - - Vec2 impulse; - - impulse = M * (bias - dv - softness * P); - - body1->velocity -= body1->invMass * impulse; - body1->angularVelocity -= body1->invI * Cross(r1, impulse); - - body2->velocity += body2->invMass * impulse; - body2->angularVelocity += body2->invI * Cross(r2, impulse); - - P += impulse; -} -
-
diff --git a/Client/Source/Phy2D/Dynamic/Joint.h b/Client/Source/Phy2D/Dynamic/Joint.h deleted file mode 100644 index ee08b40..0000000 --- a/Client/Source/Phy2D/Dynamic/Joint.h +++ /dev/null @@ -1,34 +0,0 @@ -#pragma once
-
-#include "../Common/Math.h"
- -namespace Phy2D -{ - - struct Body; - - struct Joint - { - Joint() : - body1(0), body2(0), - P(0.0f, 0.0f), - biasFactor(0.2f), softness(0.0f) - {} - - void Set(Body* body1, Body* body2, const Vec2& anchor); - - void PreStep(number inv_dt); - void ApplyImpulse(); - - Mat22 M; - Vec2 localAnchor1, localAnchor2; - Vec2 r1, r2; - Vec2 bias; - Vec2 P; // accumulated impulse - Body* body1; - Body* body2; - number biasFactor; - number softness; - }; - -}
diff --git a/Client/Source/Phy2D/Dynamic/World.cpp b/Client/Source/Phy2D/Dynamic/World.cpp deleted file mode 100644 index 4f48b69..0000000 --- a/Client/Source/Phy2D/Dynamic/World.cpp +++ /dev/null @@ -1,129 +0,0 @@ -#include "World.h"
-#include "Body.h"
-#include "Joint.h"
-#include "Arbiter.h"
-
-using namespace Phy2D;
-
- -using std::vector; -using std::map; -using std::pair; - -typedef map<ArbiterKey, Arbiter>::iterator ArbIter; -typedef pair<ArbiterKey, Arbiter> ArbPair; - -bool World::accumulateImpulses = true; -bool World::warmStarting = true; -bool World::positionCorrection = true; - -void World::Add(Body* body) -{ - bodies.push_back(body); -} - -void World::Add(Joint* joint) -{ - joints.push_back(joint); -} - -void World::Clear() -{ - bodies.clear(); - joints.clear(); - arbiters.clear(); -} - -void World::BroadPhase() -{ - // O(n^2) broad-phase - for (int i = 0; i < (int)bodies.size(); ++i) - { - Body* bi = bodies[i]; - - for (int j = i + 1; j < (int)bodies.size(); ++j) - { - Body* bj = bodies[j]; - - if (bi->invMass == 0.0f && bj->invMass == 0.0f) - continue; - - Arbiter newArb(bi, bj); - ArbiterKey key(bi, bj); - - if (newArb.numContacts > 0) - { - ArbIter iter = arbiters.find(key); - if (iter == arbiters.end()) - { - arbiters.insert(ArbPair(key, newArb)); - } - else - { - iter->second.Update(newArb.contacts, newArb.numContacts); - } - } - else - { - arbiters.erase(key); - } - } - } -} - -void World::Step(number dt) -{ - number inv_dt = dt > 0.0f ? (number)1.0f / dt : (number)0.0f; - - // Determine overlapping bodies and update contact points. - BroadPhase(); - - // Integrate forces. - for (int i = 0; i < (int)bodies.size(); ++i) - { - Body* b = bodies[i]; - - if (b->invMass == 0.0f) - continue; - - b->velocity += dt * (gravity + b->invMass * b->force); - b->angularVelocity += dt * b->invI * b->torque; - } - - // Perform pre-steps. - for (ArbIter arb = arbiters.begin(); arb != arbiters.end(); ++arb) - { - arb->second.PreStep(inv_dt); - } - - for (int i = 0; i < (int)joints.size(); ++i) - { - joints[i]->PreStep(inv_dt); - } - - // Perform iterations - for (int i = 0; i < iterations; ++i) - { - for (ArbIter arb = arbiters.begin(); arb != arbiters.end(); ++arb) - { - arb->second.ApplyImpulse(); - } - - for (int j = 0; j < (int)joints.size(); ++j) - { - joints[j]->ApplyImpulse(); - } - } - - // Integrate Velocities - for (int i = 0; i < (int)bodies.size(); ++i) - { - Body* b = bodies[i]; - - b->position += dt * b->velocity; - b->rotation += dt * b->angularVelocity; - - b->force.Set(0.0f, 0.0f); - b->torque = 0.0f; - } -}
diff --git a/Client/Source/Phy2D/Dynamic/World.h b/Client/Source/Phy2D/Dynamic/World.h deleted file mode 100644 index 93c883d..0000000 --- a/Client/Source/Phy2D/Dynamic/World.h +++ /dev/null @@ -1,36 +0,0 @@ -#pragma once
-
-#include <vector>
-#include <map>
-#include "../Common/Math.h"
-#include "Arbiter.h"
-
-namespace Phy2D
-{
- - struct Body; - struct Joint; - - struct World - { - World(Vec2 gravity, int iterations) : gravity(gravity), iterations(iterations) {} - - void Add(Body* body); - void Add(Joint* joint); - void Clear(); - - void Step(number dt); - - void BroadPhase(); - - std::vector<Body*> bodies; - std::vector<Joint*> joints; - std::map<ArbiterKey, Arbiter> arbiters; - Vec2 gravity; - int iterations; - static bool accumulateImpulses; - static bool warmStarting; - static bool positionCorrection; - };
-
-}
|