diff options
Diffstat (limited to 'Client/Source')
26 files changed, 1932 insertions, 0 deletions
diff --git a/Client/Source/Phy2D/Common/Math.cpp b/Client/Source/Phy2D/Common/Math.cpp new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/Client/Source/Phy2D/Common/Math.cpp diff --git a/Client/Source/Phy2D/Common/Math.h b/Client/Source/Phy2D/Common/Math.h new file mode 100644 index 0000000..9c0ff8f --- /dev/null +++ b/Client/Source/Phy2D/Common/Math.h @@ -0,0 +1,191 @@ +#pragma once + +#include <math.h> +#include <assert.h> +#include <stdlib.h> +#include <string> + +#include "Settings.h" + +namespace Phy2D +{ + + struct Vec2 + { + Vec2() {} + Vec2(number x, number y) : x(x), y(y) {} + + void Set(number x_, number y_) { x = x_; y = y_; } + + Vec2 operator -() { return Vec2(-x, -y); } + + void operator += (const Vec2& v) + { + x += v.x; y += v.y; + } + + void operator -= (const Vec2& v) + { + x -= v.x; y -= v.y; + } + + void operator *= (number a) + { + x *= a; y *= a; + } + + number Length() const + { + return SQRT(x * x + y * y); + } + + std::string ToString() + { + return std::to_string((float)x) + "," + std::to_string((float)y); + } + + number x, y; + }; + + struct Mat22 + { + Mat22() {} + Mat22(number angle) + { + number c = COS(angle), s = SIN(angle); + col1.x = c; col2.x = -s; + col1.y = s; col2.y = c; + } + + Mat22(const Vec2& col1, const Vec2& col2) : col1(col1), col2(col2) {} + + Mat22 Transpose() const + { + return Mat22(Vec2(col1.x, col2.x), Vec2(col1.y, col2.y)); + } + + Mat22 Invert() const + { + number a = col1.x, b = col2.x, c = col1.y, d = col2.y; + Mat22 B; + number det = a * d - b * c; + assert(det != 0.0f); + det = (number)1.0f / det; + B.col1.x = det * d; B.col2.x = -det * b; + B.col1.y = -det * c; B.col2.y = det * a; + return B; + } + + Vec2 col1, col2; + }; + + inline number Dot(const Vec2& a, const Vec2& b) + { + return a.x * b.x + a.y * b.y; + } + + inline number Cross(const Vec2& a, const Vec2& b) + { + return a.x * b.y - a.y * b.x; + } + + inline Vec2 Cross(const Vec2& a, number s) + { + return Vec2(s * a.y, -s * a.x); + } + + inline Vec2 Cross(number s, const Vec2& a) + { + return Vec2(-s * a.y, s * a.x); + } + + inline Vec2 operator * (const Mat22& A, const Vec2& v) + { + return Vec2(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y); + } + + inline Vec2 operator + (const Vec2& a, const Vec2& b) + { + return Vec2(a.x + b.x, a.y + b.y); + } + + inline Vec2 operator - (const Vec2& a, const Vec2& b) + { + return Vec2(a.x - b.x, a.y - b.y); + } + + inline Vec2 operator * (number s, const Vec2& v) + { + return Vec2(s * v.x, s * v.y); + } + + inline Mat22 operator + (const Mat22& A, const Mat22& B) + { + return Mat22(A.col1 + B.col1, A.col2 + B.col2); + } + + inline Mat22 operator * (const Mat22& A, const Mat22& B) + { + return Mat22(A * B.col1, A * B.col2); + } + + inline number Abs(number a) + { + return a > 0.0f ? a : -a; + } + + inline Vec2 Abs(const Vec2& a) + { + return Vec2(fabsf(a.x), fabsf(a.y)); + } + + inline Mat22 Abs(const Mat22& A) + { + return Mat22(Abs(A.col1), Abs(A.col2)); + } + + inline number Sign(number x) + { + return (float) x < 0.0f ? -1.0f : 1.0f; + } + + inline number Min(number a, number b) + { + return a < b ? a : b; + } + + inline number Max(number a, number b) + { + return a > b ? a : b; + } + + inline number Clamp(number a, number low, number high) + { + return Max(low, Min(a, high)); + } + + template<typename T> inline void Swap(T& a, T& b) + { + T tmp = a; + a = b; + b = tmp; + } + + //// Random number in range [-1,1] + //inline number Random() + //{ + // number r = (number)rand(); + // r /= RAND_MAX; + // r = (number)2.0f * r - 1.0f; + // return r; + //} + + //inline number Random(number lo, number hi) + //{ + // number r = (number)rand(); + // r /= RAND_MAX; + // r = (hi - lo) * r + lo; + // return r; + //} + +} diff --git a/Client/Source/Phy2D/Common/Settings.h b/Client/Source/Phy2D/Common/Settings.h new file mode 100644 index 0000000..87db164 --- /dev/null +++ b/Client/Source/Phy2D/Common/Settings.h @@ -0,0 +1,57 @@ +#pragma once + +#include "libfixmath/libfixmath/fixmath.h" + +namespace Phy2D +{ + +#define NUMBER_FLOAT false + +#if NUMBER_FLOAT +typedef float number; +#define NUMBER_MAX (FLT_MAX) +#define NUMBER_MIN (FLT_MIN) +#define SQRT(a) (sqrt((a))) +#define SIN(a) (sin((a))) +#define COS(a) (cos((a))) +#else +// 同时一定要开启内联函数扩展，否则执行效率会非常低 +typedef Fix16 number; +#define NUMBER_MAX (fix16_maximum) +#define NUMBER_MIN (fix16_minimum) +#define SQRT(a) ((a).sqrt()) +#define SIN(a) ((a).sin()) +#define COS(a) ((a).cos()) +#endif + +#if false + typedef fix16_t number; + +#define A (a) +#define B (b) + + #define Number(float_value) (fix16_from_float((float_value))) + #define to_float(n) (fix16_to_float((n))) + + inline bool eq(number a, number b) { return (A == B) ;} + inline bool lt(number a, number b) { return (A < B) ;} + inline bool le(number a, number b) { return (A <= B) ;} + inline bool gt(number a, number b) { return (A > B) ;} + inline bool ge(number a, number b) { return (A >= B) ;} + inline number add(number a, number b) { return (fix16_add(A, B)) ;} + inline number sub(number a, number b) { return (fix16_sub(A, B)) ;} + inline number mul(number a, number b) { return (fix16_mul(A, B)) ;} + inline number div(number a, number b) { return (fix16_div(A, B)) ;} + inline number mod(number a, number b) { return (fix16_mod(A, B)) ;} + inline number sin(number a) { return (fix16_sin(A)) ;} + inline number cos(number a) { return (fix16_cos(A)) ;} + inline number tan(number a) { return (fix16_tan(A)) ;} + inline number acos(number a) { return (fix16_acos(A)) ;} + inline number asin(number a) { return (fix16_asin(A)) ;} + inline number atan(number a) { return (fix16_atan(A)) ;} + inline number neg(number a) { return (fix16_sub(0, A)) ;} + inline number abs(number a) { return (fix16_abs(A)) ;} + inline number sqrt(number a) { return fix16_sqrt(a); } +#endif + +}
\ No newline at end of file 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<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 new file mode 100644 index 0000000..d5c652c --- /dev/null +++ b/Client/Source/Phy2D/Dynamic/World.h @@ -0,0 +1,36 @@ +#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; + }; + +} diff --git a/Client/Source/Phy2D/Phy2D.h b/Client/Source/Phy2D/Phy2D.h new file mode 100644 index 0000000..48d2824 --- /dev/null +++ b/Client/Source/Phy2D/Phy2D.h @@ -0,0 +1,8 @@ +#pragma once + + +// 定点数的物理引擎 + +#include "Dynamic/World.h" +#include "Dynamic/Body.h" +#include "Dynamic/Joint.h" diff --git a/Client/Source/Phy2D/README.txt b/Client/Source/Phy2D/README.txt new file mode 100644 index 0000000..416a28b --- /dev/null +++ b/Client/Source/Phy2D/README.txt @@ -0,0 +1 @@ +绠鍖栫殑瀹氱偣鏁2D鐗╃悊寮曟搸
\ No newline at end of file diff --git a/Client/Source/Phy2D/Rendering/Visualize.h b/Client/Source/Phy2D/Rendering/Visualize.h new file mode 100644 index 0000000..70aae79 --- /dev/null +++ b/Client/Source/Phy2D/Rendering/Visualize.h @@ -0,0 +1,4 @@ +#pragma once + +#include "glad/glad.h" + diff --git a/Client/Source/Phy2D/Shapes/p2CircleShape.cpp b/Client/Source/Phy2D/Shapes/p2CircleShape.cpp new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/Client/Source/Phy2D/Shapes/p2CircleShape.cpp diff --git a/Client/Source/Phy2D/Shapes/p2CircleShape.h b/Client/Source/Phy2D/Shapes/p2CircleShape.h new file mode 100644 index 0000000..ded0396 --- /dev/null +++ b/Client/Source/Phy2D/Shapes/p2CircleShape.h @@ -0,0 +1,8 @@ +#pragma once + +#include "p2Shape.h" + +class p2CircleShape : public p2Shape +{ + +}; diff --git a/Client/Source/Phy2D/Shapes/p2PolygonShape.cpp b/Client/Source/Phy2D/Shapes/p2PolygonShape.cpp new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/Client/Source/Phy2D/Shapes/p2PolygonShape.cpp diff --git a/Client/Source/Phy2D/Shapes/p2PolygonShape.h b/Client/Source/Phy2D/Shapes/p2PolygonShape.h new file mode 100644 index 0000000..e69de29 --- /dev/null +++ b/Client/Source/Phy2D/Shapes/p2PolygonShape.h diff --git a/Client/Source/Phy2D/Shapes/p2Shape.h b/Client/Source/Phy2D/Shapes/p2Shape.h new file mode 100644 index 0000000..79a98f0 --- /dev/null +++ b/Client/Source/Phy2D/Shapes/p2Shape.h @@ -0,0 +1,7 @@ +#pragma once + +class p2Shape +{ + +}; + diff --git a/Client/Source/Phy2D/Tests/test.h b/Client/Source/Phy2D/Tests/test.h new file mode 100644 index 0000000..c73a192 --- /dev/null +++ b/Client/Source/Phy2D/Tests/test.h @@ -0,0 +1,6 @@ +#pragma once + +#define TEST_MATH 1 +#define TEST_P2D 2 + +#define TEST TEST_P2D diff --git a/Client/Source/Phy2D/Tests/test_math.cpp b/Client/Source/Phy2D/Tests/test_math.cpp new file mode 100644 index 0000000..e047508 --- /dev/null +++ b/Client/Source/Phy2D/Tests/test_math.cpp @@ -0,0 +1,23 @@ +#include "test.h" +#if TEST == TEST_MATH + +#include <iostream> + +#include "../Common/Math.h" + +using namespace Phy2D; +using namespace std; + +int main(int argc, char **argv) +{ + Vec2 a = Vec2(1.f, 2.f); + Vec2 b = a; + + cout << (a+b).ToString(); + + getchar(); + + return 0; +} + +#endif
\ No newline at end of file diff --git a/Client/Source/Phy2D/Tests/test_p2d.cpp b/Client/Source/Phy2D/Tests/test_p2d.cpp new file mode 100644 index 0000000..9d27db8 --- /dev/null +++ b/Client/Source/Phy2D/Tests/test_p2d.cpp @@ -0,0 +1,625 @@ +#include "test.h" +#if TEST == TEST_P2D + +#include "libfixmath/libfixmath/fixmath.h" +#include <iostream> +#include <time.h> +#include <math.h> +#include "SDL2/SDL.h" +#include "../Rendering/Visualize.h" +#include "imgui/imgui.h" +#include "imgui/backends/imgui_impl_opengl2.h" +#include "imgui/backends/imgui_impl_sdl.h" + +#include "../Phy2D.h" + +using namespace std; +using namespace Phy2D; + +namespace +{ + Body bodies[200]; + Joint joints[100]; + + Body* bomb = NULL; + + float timeStep = 1.0f / 60.0f; + int iterations = 10; + Vec2 gravity(0.0f, -10.0f); + + int numBodies = 0; + int numJoints = 0; + + int demoIndex = 0; + + int width = 1280; + int height = 720; + float zoom = 10.0f; + float pan_y = 8.0f; + + World world(gravity, iterations); +} + +static void DrawBody(Body* body) +{ + Mat22 R(body->rotation); + Vec2 x = body->position; + Vec2 h = 0.5f * body->width; + + Vec2 v1 = x + R * Vec2(-h.x, -h.y); + Vec2 v2 = x + R * Vec2(h.x, -h.y); + Vec2 v3 = x + R * Vec2(h.x, h.y); + Vec2 v4 = x + R * Vec2(-h.x, h.y); + + if (body == bomb) + glColor3f(0.4f, 0.9f, 0.4f); + else + glColor3f(0.8f, 0.8f, 0.9f); + + glBegin(GL_LINE_LOOP); + glVertex2f(v1.x, v1.y); + glVertex2f(v2.x, v2.y); + glVertex2f(v3.x, v3.y); + glVertex2f(v4.x, v4.y); + glEnd(); +} + +static void DrawJoint(Joint* joint) +{ + Body* b1 = joint->body1; + Body* b2 = joint->body2; + + Mat22 R1(b1->rotation); + Mat22 R2(b2->rotation); + + Vec2 x1 = b1->position; + Vec2 p1 = x1 + R1 * joint->localAnchor1; + + Vec2 x2 = b2->position; + Vec2 p2 = x2 + R2 * joint->localAnchor2; + + glColor3f(0.5f, 0.5f, 0.8f); + glBegin(GL_LINES); + glVertex2f(x1.x, x1.y); + glVertex2f(p1.x, p1.y); + glVertex2f(x2.x, x2.y); + glVertex2f(p2.x, p2.y); + glEnd(); +} + +static void Demo1(Body* b, Joint* j) +{ + float x, y; + + b->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); + b->position.Set(0.0f, (number)-0.5f * b->width.y); + world.Add(b); + ++b; ++numBodies; + + b->Set(Vec2(1.0f, 1.0f), 200.0f); + b->position.Set(0.0f, 4.0f); + world.Add(b); + ++b; ++numBodies; +} + +// A simple pendulum +static void Demo2(Body* b, Joint* j) +{ + Body* b1 = b + 0; + b1->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); + b1->friction = 0.2f; + b1->position.Set(0.0f, (number)-0.5f * b1->width.y); + b1->rotation = 0.0f; + world.Add(b1); + + Body* b2 = b + 1; + b2->Set(Vec2(1.0f, 1.0f), 100.0f); + b2->friction = 0.2f; + b2->position.Set(9.0f, 11.0f); + b2->rotation = 0.0f; + world.Add(b2); + + numBodies += 2; + + j->Set(b1, b2, Vec2(0.0f, 11.0f)); + world.Add(j); + + numJoints += 1; +} + +// Varying friction coefficients +static void Demo3(Body* b, Joint* j) +{ + b->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); + b->position.Set(0.0f, (number)-0.5f * b->width.y); + world.Add(b); + ++b; ++numBodies; + + b->Set(Vec2(13.0f, 0.25f), NUMBER_MAX); + b->position.Set(-2.0f, 11.0f); + b->rotation = -0.25f; + world.Add(b); + ++b; ++numBodies; + + b->Set(Vec2(0.25f, 1.0f), NUMBER_MAX); + b->position.Set(5.25f, 9.5f); + world.Add(b); + ++b; ++numBodies; + + b->Set(Vec2(13.0f, 0.25f), NUMBER_MAX); + b->position.Set(2.0f, 7.0f); + b->rotation = 0.25f; + world.Add(b); + ++b; ++numBodies; + + b->Set(Vec2(0.25f, 1.0f), NUMBER_MAX); + b->position.Set(-5.25f, 5.5f); + world.Add(b); + ++b; ++numBodies; + + b->Set(Vec2(13.0f, 0.25f), NUMBER_MAX); + b->position.Set(-2.0f, 3.0f); + b->rotation = -0.25f; + world.Add(b); + ++b; ++numBodies; + + float friction[5] = { 0.75f, 0.5f, 0.35f, 0.1f, 0.0f }; + for (int i = 0; i < 5; ++i) + { + b->Set(Vec2(0.5f, 0.5f), 25.0f); + b->friction = friction[i]; + b->position.Set(-7.5f + 2.0f * i, 14.0f); + world.Add(b); + ++b; ++numBodies; + } +} + +// A vertical stack +static void Demo4(Body* b, Joint* j) +{ + b->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); + b->friction = 0.2f; + b->position.Set(0.0f, (number)-0.5f * b->width.y); + b->rotation = 0.0f; + world.Add(b); + ++b; ++numBodies; + + for (int i = 0; i < 10; ++i) + { + b->Set(Vec2(1.0f, 1.0f), 1.0f); + b->friction = 0.2f; + //float x = Random(-0.1f, 0.1f); + float x = 0; + b->position.Set(x, 0.51f + 1.05f * i); + world.Add(b); + ++b; ++numBodies; + } +} + +// A pyramid +static void Demo5(Body* b, Joint* j) +{ + b->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); + b->friction = 0.2f; + b->position.Set(0.0f, (number)-0.5f * b->width.y); + b->rotation = 0.0f; + world.Add(b); + ++b; ++numBodies; + + Vec2 x(-6.0f, 0.75f); + Vec2 y; + + for (int i = 0; i < 12; ++i) + { + y = x; + + for (int j = i; j < 12; ++j) + { + b->Set(Vec2(1.0f, 1.0f), 10.0f); + b->friction = 0.2f; + b->position = y; + world.Add(b); + ++b; ++numBodies; + + y += Vec2(1.125f, 0.0f); + } + + //x += Vec2(0.5625f, 1.125f); + x += Vec2(0.5625f, 2.0f); + } +} + +// A teeter +static void Demo6(Body* b, Joint* j) +{ + Body* b1 = b + 0; + b1->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); + b1->position.Set(0.0f, (number)-0.5f * b1->width.y); + world.Add(b1); + + Body* b2 = b + 1; + b2->Set(Vec2(12.0f, 0.25f), 100.0f); + b2->position.Set(0.0f, 1.0f); + world.Add(b2); + + Body* b3 = b + 2; + b3->Set(Vec2(0.5f, 0.5f), 25.0f); + b3->position.Set(-5.0f, 2.0f); + world.Add(b3); + + Body* b4 = b + 3; + b4->Set(Vec2(0.5f, 0.5f), 25.0f); + b4->position.Set(-5.5f, 2.0f); + world.Add(b4); + + Body* b5 = b + 4; + b5->Set(Vec2(1.0f, 1.0f), 100.0f); + b5->position.Set(5.5f, 15.0f); + world.Add(b5); + + numBodies += 5; + + j->Set(b1, b2, Vec2(0.0f, 1.0f)); + world.Add(j); + + numJoints += 1; +} + +// A suspension bridge +static void Demo7(Body* b, Joint* j) +{ + b->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); + b->friction = 0.2f; + b->position.Set(0.0f, (number)-0.5f * b->width.y); + b->rotation = 0.0f; + world.Add(b); + ++b; ++numBodies; + + const int numPlanks = 15; + float mass = 50.0f; + + for (int i = 0; i < numPlanks; ++i) + { + b->Set(Vec2(1.0f, 0.25f), mass); + b->friction = 0.2f; + b->position.Set(-8.5f + 1.25f * i, 5.0f); + world.Add(b); + ++b; ++numBodies; + } + + // Tuning + float frequencyHz = 2.0f; + float dampingRatio = 0.7f; + + // frequency in radians + float omega = (number)2.0f * fix16_pi * frequencyHz; + + // damping coefficient + float d = 2.0f * mass * dampingRatio * omega; + + // spring stifness + float k = mass * omega * omega; + + // magic formulas + float softness = 1.0f / (d + timeStep * k); + float biasFactor = timeStep * k / (d + timeStep * k); + + for (int i = 0; i < numPlanks; ++i) + { + j->Set(bodies + i, bodies + i + 1, Vec2(-9.125f + 1.25f * i, 5.0f)); + j->softness = softness; + j->biasFactor = biasFactor; + + world.Add(j); + ++j; ++numJoints; + } + + j->Set(bodies + numPlanks, bodies, Vec2(-9.125f + 1.25f * numPlanks, 5.0f)); + j->softness = softness; + j->biasFactor = biasFactor; + world.Add(j); + ++j; ++numJoints; +} + +// Dominos +static void Demo8(Body* b, Joint* j) +{ + Body* b1 = b; + b->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); + b->position.Set(0.0f, (number)-0.5f * b->width.y); + world.Add(b); + ++b; ++numBodies; + + b->Set(Vec2(12.0f, 0.5f), NUMBER_MAX); + b->position.Set(-1.5f, 10.0f); + world.Add(b); + ++b; ++numBodies; + + for (int i = 0; i < 10; ++i) + { + b->Set(Vec2(0.2f, 2.0f), 10.0f); + b->position.Set(-6.0f + 1.0f * i, 11.125f); + b->friction = 0.1f; + world.Add(b); + ++b; ++numBodies; + } + + b->Set(Vec2(14.0f, 0.5f), NUMBER_MAX); + b->position.Set(1.0f, 6.0f); + b->rotation = 0.3f; + world.Add(b); + ++b; ++numBodies; + + Body* b2 = b; + b->Set(Vec2(0.5f, 3.0f), NUMBER_MAX); + b->position.Set(-7.0f, 4.0f); + world.Add(b); + ++b; ++numBodies; + + Body* b3 = b; + b->Set(Vec2(12.0f, 0.25f), 20.0f); + b->position.Set(-0.9f, 1.0f); + world.Add(b); + ++b; ++numBodies; + + j->Set(b1, b3, Vec2(-2.0f, 1.0f)); + world.Add(j); + ++j; ++numJoints; + + Body* b4 = b; + b->Set(Vec2(0.5f, 0.5f), 10.0f); + b->position.Set(-10.0f, 15.0f); + world.Add(b); + ++b; ++numBodies; + + j->Set(b2, b4, Vec2(-7.0f, 15.0f)); + world.Add(j); + ++j; ++numJoints; + + Body* b5 = b; + b->Set(Vec2(2.0f, 2.0f), 20.0f); + b->position.Set(6.0f, 2.5f); + b->friction = 0.1f; + world.Add(b); + ++b; ++numBodies; + + j->Set(b1, b5, Vec2(6.0f, 2.6f)); + world.Add(j); + ++j; ++numJoints; + + Body* b6 = b; + b->Set(Vec2(2.0f, 0.2f), 10.0f); + b->position.Set(6.0f, 3.6f); + world.Add(b); + ++b; ++numBodies; + + j->Set(b5, b6, Vec2(7.0f, 3.5f)); + world.Add(j); + ++j; ++numJoints; +} + +// A multi-pendulum +static void Demo9(Body* b, Joint* j) +{ + b->Set(Vec2(100.0f, 20.0f), NUMBER_MAX); + b->friction = 0.2f; + b->position.Set(0.0f, (number)-0.5f * b->width.y); + b->rotation = 0.0f; + world.Add(b); + + Body * b1 = b; + ++b; + ++numBodies; + + float mass = 10.0f; + + // Tuning + float frequencyHz = 4.0f; + float dampingRatio = 0.7f; + + // frequency in radians + float omega = (number) 2.0f * fix16_pi * frequencyHz; + + // damping coefficient + float d = 2.0f * mass * dampingRatio * omega; + + // spring stiffness + float k = mass * omega * omega; + + // magic formulas + float softness = 1.0f / (d + timeStep * k); + float biasFactor = timeStep * k / (d + timeStep * k); + + const float y = 12.0f; + + for (int i = 0; i < 15; ++i) + { + Vec2 x(0.5f + i, y); + b->Set(Vec2(0.75f, 0.25f), mass); + b->friction = 0.2f; + b->position = x; + b->rotation = 0.0f; + world.Add(b); + + j->Set(b1, b, Vec2(float(i), y)); + j->softness = softness; + j->biasFactor = biasFactor; + world.Add(j); + + b1 = b; + ++b; + ++numBodies; + ++j; + ++numJoints; + } +} + +static void InitDemo() +{ + world.Clear(); + numBodies = 0; + numJoints = 0; + bomb = NULL; + + demoIndex = 0; + Demo8(bodies, joints); +} + +int main(int argc, char **argv) { + + // Setup SDL + // (Some versions of SDL before <2.0.10 appears to have performance/stalling issues on a minority of Windows systems, + // depending on whether SDL_INIT_GAMECONTROLLER is enabled or disabled.. updating to latest version of SDL is recommended!) + if (SDL_Init(SDL_INIT_VIDEO | SDL_INIT_TIMER | SDL_INIT_GAMECONTROLLER) != 0) + { + printf("Error: %s\n", SDL_GetError()); + return -1; + } + + // Setup window + SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1); + SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE, 24); + SDL_GL_SetAttribute(SDL_GL_STENCIL_SIZE, 8); + SDL_GL_SetAttribute(SDL_GL_CONTEXT_MAJOR_VERSION, 2); + SDL_GL_SetAttribute(SDL_GL_CONTEXT_MINOR_VERSION, 2); + SDL_WindowFlags window_flags = (SDL_WindowFlags)(SDL_WINDOW_OPENGL | SDL_WINDOW_RESIZABLE | SDL_WINDOW_ALLOW_HIGHDPI); + SDL_Window* window = SDL_CreateWindow("Phy2D", SDL_WINDOWPOS_CENTERED, SDL_WINDOWPOS_CENTERED, width, height, window_flags); + SDL_GLContext gl_context = SDL_GL_CreateContext(window); + + if (!gladLoadGLLoader((GLADloadproc)SDL_GL_GetProcAddress)) { + std::cerr << "Failed to initialize the OpenGL context." << std::endl; + exit(1); + } + + SDL_GL_MakeCurrent(window, gl_context); + SDL_GL_SetSwapInterval(1); // Enable vsync + + // Setup Dear ImGui context + IMGUI_CHECKVERSION(); + ImGui::CreateContext(); + ImGuiIO& io = ImGui::GetIO(); (void)io; + //io.ConfigFlags |= ImGuiConfigFlags_NavEnableKeyboard; // Enable Keyboard Controls + //io.ConfigFlags |= ImGuiConfigFlags_NavEnableGamepad; // Enable Gamepad Controls + + // Setup Dear ImGui style + ImGui::StyleColorsDark(); + //ImGui::StyleColorsClassic(); + + // Setup Platform/Renderer backends + ImGui_ImplSDL2_InitForOpenGL(window, gl_context); + ImGui_ImplOpenGL2_Init(); + + // Load Fonts + // - If no fonts are loaded, dear imgui will use the default font. You can also load multiple fonts and use ImGui::PushFont()/PopFont() to select them. + // - AddFontFromFileTTF() will return the ImFont* so you can store it if you need to select the font among multiple. + // - If the file cannot be loaded, the function will return NULL. Please handle those errors in your application (e.g. use an assertion, or display an error and quit). + // - The fonts will be rasterized at a given size (w/ oversampling) and stored into a texture when calling ImFontAtlas::Build()/GetTexDataAsXXXX(), which ImGui_ImplXXXX_NewFrame below will call. + // - Read 'docs/FONTS.md' for more instructions and details. + // - Remember that in C/C++ if you want to include a backslash \ in a string literal you need to write a double backslash \\ ! + //io.Fonts->AddFontDefault(); + //io.Fonts->AddFontFromFileTTF("../../misc/fonts/Roboto-Medium.ttf", 16.0f); + //io.Fonts->AddFontFromFileTTF("../../misc/fonts/Cousine-Regular.ttf", 15.0f); + //io.Fonts->AddFontFromFileTTF("../../misc/fonts/DroidSans.ttf", 16.0f); + //io.Fonts->AddFontFromFileTTF("../../misc/fonts/ProggyTiny.ttf", 10.0f); + //ImFont* font = io.Fonts->AddFontFromFileTTF("c:\\Windows\\Fonts\\ArialUni.ttf", 18.0f, NULL, io.Fonts->GetGlyphRangesJapanese()); + //IM_ASSERT(font != NULL); + + // Our state + bool show_demo_window = true; + bool show_another_window = false; + ImVec4 clear_color = ImVec4(0.45f, 0.55f, 0.60f, 1.00f); + + glViewport(0, 0, (int)io.DisplaySize.x, (int)io.DisplaySize.y); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + + float aspect = float(width) / float(height); + if (width >= height) + { + // aspect >= 1, set the height from -1 to 1, with larger width + glOrtho(-zoom * aspect, zoom * aspect, -zoom + pan_y, zoom + pan_y, -1.0, 1.0); + } + else + { + // aspect < 1, set the width to -1 to 1, with larger height + glOrtho(-zoom, zoom, -zoom / aspect + pan_y, zoom / aspect + pan_y, -1.0, 1.0); + } + + InitDemo(); + + // Main loop + bool done = false; + while (!done) + { + //glClearColor(clear_color.x * clear_color.w, clear_color.y * clear_color.w, clear_color.z * clear_color.w, clear_color.w); + glClearColor(0, 0, 0, 0); + glClear(GL_COLOR_BUFFER_BIT); + + // Poll and handle events (inputs, window resize, etc.) + // You can read the io.WantCaptureMouse, io.WantCaptureKeyboard flags to tell if dear imgui wants to use your inputs. + // - When io.WantCaptureMouse is true, do not dispatch mouse input data to your main application. + // - When io.WantCaptureKeyboard is true, do not dispatch keyboard input data to your main application. + // Generally you may always pass all inputs to dear imgui, and hide them from your application based on those two flags. + SDL_Event event; + while (SDL_PollEvent(&event)) + { + ImGui_ImplSDL2_ProcessEvent(&event); + if (event.type == SDL_QUIT) + done = true; + if (event.type == SDL_WINDOWEVENT && event.window.event == SDL_WINDOWEVENT_CLOSE && event.window.windowID == SDL_GetWindowID(window)) + done = true; + } + + // Start the Dear ImGui frame + ImGui_ImplOpenGL2_NewFrame(); + ImGui_ImplSDL2_NewFrame(); + ImGui::NewFrame(); + + // Rendering + ImGui::Render(); + + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); + + world.Step(timeStep); + + for (int i = 0; i < numBodies; ++i) + DrawBody(bodies + i); + + for (int i = 0; i < numJoints; ++i) + DrawJoint(joints + i); + + glPointSize(4.0f); + glColor3f(1.0f, 0.0f, 0.0f); + glBegin(GL_POINTS); + std::map<ArbiterKey, Arbiter>::const_iterator iter; + for (iter = world.arbiters.begin(); iter != world.arbiters.end(); ++iter) + { + const Arbiter& arbiter = iter->second; + for (int i = 0; i < arbiter.numContacts; ++i) + { + Vec2 p = arbiter.contacts[i].position; + glVertex2f(p.x, p.y); + } + } + glEnd(); + glPointSize(1.0f); + + //glUseProgram(0); // You may want this if using this code in an OpenGL 3+ context where shaders may be bound + ImGui_ImplOpenGL2_RenderDrawData(ImGui::GetDrawData()); + SDL_GL_SwapWindow(window); + } + + // Cleanup + ImGui_ImplOpenGL2_Shutdown(); + ImGui_ImplSDL2_Shutdown(); + ImGui::DestroyContext(); + + SDL_GL_DeleteContext(gl_context); + SDL_DestroyWindow(window); + SDL_Quit(); + + return 0; +} + +#endif
\ No newline at end of file diff --git a/Client/Source/Sand2D/README.txt b/Client/Source/Sand2D/README.txt new file mode 100644 index 0000000..4444510 --- /dev/null +++ b/Client/Source/Sand2D/README.txt @@ -0,0 +1 @@ +2D绮掑瓙妯℃嫙寮曟搸
\ No newline at end of file diff --git a/Client/Source/fixedpoint/test_basic.c b/Client/Source/fixedpoint/test_basic.c new file mode 100644 index 0000000..f10c97d --- /dev/null +++ b/Client/Source/fixedpoint/test_basic.c @@ -0,0 +1,6 @@ +// 测试定点数 + +int main() +{ + return 0; +} diff --git a/Client/Source/main.cpp b/Client/Source/main.cpp new file mode 100644 index 0000000..8d0f257 --- /dev/null +++ b/Client/Source/main.cpp @@ -0,0 +1,7 @@ + + +int main() +{ + + return 0; +}
\ No newline at end of file |