From 9e0e01b7f4375063f06e494113187d48614628e0 Mon Sep 17 00:00:00 2001 From: chai Date: Tue, 30 Nov 2021 22:25:37 +0800 Subject: +init --- Client/Source/Phy2D/Dynamic/Arbiter.cpp | 189 ++++++++++++++++++ Client/Source/Phy2D/Dynamic/Arbiter.h | 91 +++++++++ Client/Source/Phy2D/Dynamic/Body.cpp | 47 +++++ Client/Source/Phy2D/Dynamic/Body.h | 34 ++++ Client/Source/Phy2D/Dynamic/Collide.cpp | 326 ++++++++++++++++++++++++++++++++ Client/Source/Phy2D/Dynamic/Joint.cpp | 102 ++++++++++ Client/Source/Phy2D/Dynamic/Joint.h | 34 ++++ Client/Source/Phy2D/Dynamic/World.cpp | 129 +++++++++++++ Client/Source/Phy2D/Dynamic/World.h | 36 ++++ 9 files changed, 988 insertions(+) create mode 100644 Client/Source/Phy2D/Dynamic/Arbiter.cpp create mode 100644 Client/Source/Phy2D/Dynamic/Arbiter.h create mode 100644 Client/Source/Phy2D/Dynamic/Body.cpp create mode 100644 Client/Source/Phy2D/Dynamic/Body.h create mode 100644 Client/Source/Phy2D/Dynamic/Collide.cpp create mode 100644 Client/Source/Phy2D/Dynamic/Joint.cpp create mode 100644 Client/Source/Phy2D/Dynamic/Joint.h create mode 100644 Client/Source/Phy2D/Dynamic/World.cpp create mode 100644 Client/Source/Phy2D/Dynamic/World.h (limited to 'Client/Source/Phy2D/Dynamic') diff --git a/Client/Source/Phy2D/Dynamic/Arbiter.cpp b/Client/Source/Phy2D/Dynamic/Arbiter.cpp new file mode 100644 index 0000000..11288a3 --- /dev/null +++ b/Client/Source/Phy2D/Dynamic/Arbiter.cpp @@ -0,0 +1,189 @@ +#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 new file mode 100644 index 0000000..b02b413 --- /dev/null +++ b/Client/Source/Phy2D/Dynamic/Arbiter.h @@ -0,0 +1,91 @@ +#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 new file mode 100644 index 0000000..73bafe2 --- /dev/null +++ b/Client/Source/Phy2D/Dynamic/Body.cpp @@ -0,0 +1,47 @@ +#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 new file mode 100644 index 0000000..6d86e07 --- /dev/null +++ b/Client/Source/Phy2D/Dynamic/Body.h @@ -0,0 +1,34 @@ +#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 new file mode 100644 index 0000000..647147a --- /dev/null +++ b/Client/Source/Phy2D/Dynamic/Collide.cpp @@ -0,0 +1,326 @@ +#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 new file mode 100644 index 0000000..e0d10f9 --- /dev/null +++ b/Client/Source/Phy2D/Dynamic/Joint.cpp @@ -0,0 +1,102 @@ +#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 new file mode 100644 index 0000000..34d8d8d --- /dev/null +++ b/Client/Source/Phy2D/Dynamic/Joint.h @@ -0,0 +1,34 @@ +#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 new file mode 100644 index 0000000..6b0174c --- /dev/null +++ b/Client/Source/Phy2D/Dynamic/World.cpp @@ -0,0 +1,129 @@ +#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::iterator ArbIter; +typedef pair 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 new file mode 100644 index 0000000..d5c652c --- /dev/null +++ b/Client/Source/Phy2D/Dynamic/World.h @@ -0,0 +1,36 @@ +#pragma once + +#include +#include +#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 bodies; + std::vector joints; + std::map arbiters; + Vec2 gravity; + int iterations; + static bool accumulateImpulses; + static bool warmStarting; + static bool positionCorrection; + }; + +} -- cgit v1.1-26-g67d0