This section allows you to view all posts made by this member. Note that you can only see posts made in areas you currently have access to.
// new module header
#define FLT_MAX 340282346638528859811704183484516925440.0
#define MAX_POINTS 32
#define MAX_ARBITERS 16
#define MAX_BODIES 256
#define MAX_JOINTS 256
#define MAX_CONTACTS 2
managed struct Vec2
{
float x, y;
import static Vec2* New(float x, float y); // $AUTOCOMPLETESTATICONLY$
import void Set(float x, float y);
import void SetV(Vec2* v);
import Vec2* Abs();
import Vec2* Negate();
import Vec2* Minus(Vec2* vb);
import Vec2* Plus(Vec2* vb);
import Vec2* Scale(float a);
import float Length();
import float Dot(Vec2* b);
import float Cross(Vec2* b);
};
managed struct Mat22
{
float col1_x, col1_y, col2_x, col2_y;
import static Mat22* New(float col1_x, float col1_y, float col2_x, float col2_y); // $AUTOCOMPLETESTATICONLY$
import static Mat22* NewFromAngle(float angle); // $AUTOCOMPLETESTATICONLY$
import static Mat22* NewFromVec2(Vec2* col1, Vec2* col2); // $AUTOCOMPLETESTATICONLY$
import Mat22* Transpose();
import Mat22* Invert();
import Mat22* Plus(Mat22* B);
import Mat22* Multiply(Mat22* B);
import Mat22* Abs();
};
managed struct Body
{
import static Body* Create();
float position_x, position_y;
float rotation;
float velocity_x, velocity_y;
float angularVelocity;
float force_x, force_y;
float torque;
float width_x, width_y;
float friction;
float mass, invMass;
float I, invI;
import void Set(Vec2* w, float m);
import void AddForce(const Vec2* f);
};
managed struct Joint
{
import static Joint* Create();
Mat22* M;
Vec2* localAnchor1, localAnchor2;
Vec2* r1, r2;
Vec2* bias;
Vec2* P; // accumulated impulse
Body* body1;
Body* body2;
float biasFactor;
float softness;
import void Set(Body* body1, Body* body2, Vec2* anchor);
import void PreStep(float inv_dt);
import void ApplyImpulse();
};
managed struct FeaturePair
{
static import FeaturePair* Create();
char inEdge1;
char outEdge1;
char inEdge2;
char outEdge2;
import attribute int value;
import int get_value();
import void set_value(int value);
};
managed struct Contact
{
static import Contact* Create();
Vec2* position;
Vec2* normal;
Vec2* r1, r2;
float separation;
float Pn; // accumulated normal impulse
float Pt; // accumulated tangent impulse
float Pnb; // accumulated normal impulse for position bias
float massNormal, massTangent;
float bias;
FeaturePair* feature;
import Contact* CopyTo(Contact* c);
};
managed struct Arbiter
{
static import Arbiter* Create(Body* b1, Body* b2);
import void Update(Contact* contacts[], int numContacts);
import void PreStep(float inv_dt);
import void ApplyImpulse();
Contact* contacts[];
int numContacts;
Body* body1;
Body* body2;
// Combined friction
float friction;
};
managed struct Arbiters
{
import void Add(Arbiter* arb);
import void Remove(Body* b1, Body* b2);
import Arbiter* Get(Body* b1, Body* b2);
import void Clear();
Arbiter* a[MAX_ARBITERS];
int a_count;
import static Arbiters* Create();
};
struct World
{
import void Init(float gravity_x, float gravity_y, int iterations = 10);
import void AddBody(Body* body);
import void AddJoint(Joint* joint);
import void Clear();
import void Step(float dt);
import void BroadPhase();
Body* bodies[MAX_BODIES];
int body_count;
Joint* joints[MAX_JOINTS];
int joint_count;
Arbiters* arbiters;
Vec2* gravity;
int iterations;
static import attribute bool accumulateImpulses;
static import bool get_accumulateImpulses();
static import void set_accumulateImpulses(bool value);
static import attribute bool warmStarting;
static import bool get_warmStarting();
static import void set_warmStarting(bool value);
static import attribute bool positionCorrection;
static import bool get_positionCorrection();
static import void set_positionCorrection(bool value);
};// new module script
// assertion check utility
void assert(bool expr)
{
if (expr == false) AbortGame("Failed assertion!");
}
float _abs(float a)
{
if (a >= 0.0)
return a;
return -a;
}
#region MATH_UTILITIES
static Vec2* Vec2::New(float x, float y) {
Vec2* v = new Vec2;
v.x = x;
v.y = y;
return v;
}
void Vec2::Set(float x, float y) { this.x = x; this.y = y; }
void Vec2::SetV(Vec2* v) { this.x = v.x; this.y = v.y; }
Vec2* Vec2::Negate() { return this.New(-this.x, -this.y); }
Vec2* Vec2::Abs() { return this.New(_abs(this.x), _abs(this.y)); }
Vec2* Vec2::Minus(Vec2* v) { return this.New(this.x - v.x, this.y - v.y); }
Vec2* Vec2::Plus(Vec2* v) { return this.New(this.x + v.x, this.y + v.y); }
Vec2* Vec2::Scale(float a) { return this.New(this.x * a, this.y * a); }
float Vec2::Length(){ return Maths.Sqrt(this.x*this.x + this.y*this.y); }
float Vec2::Dot(Vec2* b) { return this.x * b.x + this.y * b.y; }
float Vec2::Cross(Vec2* b){ return this.x * b.y - this.y * b.x; }
static Mat22* Mat22::New(float col1_x, float col1_y, float col2_x, float col2_y)
{
Mat22* m = new Mat22;
m.col1_x = col1_x; m.col2_x = col2_x;
m.col1_y = col1_y; m.col2_y = col2_y;
return m;
}
static Mat22* Mat22::NewFromAngle(float angle)
{
float c = Maths.Cos(angle), s = Maths.Sin(angle);
return Mat22.New(c, s, -s, c);
}
static Mat22* Mat22::NewFromVec2(Vec2* col1, Vec2* col2)
{
return Mat22.New(col1.x, col1.y, col2.x, col2.y);
}
Mat22* Mat22::Transpose()
{
return this.New(this.col1_x, this.col2_x, this.col1_y, this.col2_y);
}
Mat22* Mat22::Invert()
{
float a = this.col1_x, b = this.col2_x, c = this.col1_y, d = this.col2_y;
float det = a * d - b * c;
assert(det != 0.0);
det = 1.0 / det;
return this.New(det * d, -det * c, -det * b, det * a);
}
Mat22* Mat22::Plus(Mat22* B)
{
return this.New(this.col1_x + B.col1_x, this.col1_y + B.col1_y, this.col2_x + B.col2_x, this.col2_y + B.col2_y);
}
Mat22* Mat22::Abs()
{
return this.New(_abs(this.col1_x), _abs(this.col1_y), _abs(this.col2_x), _abs(this.col2_y));
}
Vec2* CrossV2S(Vec2* a, float s)
{
return Vec2.New(s * a.y, -s * a.x);
}
Vec2* CrossS2V(float s, Vec2* a)
{
return Vec2.New(-s * a.y, s * a.x);
}
Vec2* MultiplyVec2(Mat22* A, Vec2* v)
{
return Vec2.New(A.col1_x * v.x + A.col2_x * v.y, A.col1_y * v.x + A.col2_y * v.y);
}
Mat22* Mat22::Multiply(Mat22* matb)
{
//A * B.col1, A * B.col2);
float m11 = this.col1_x * matb.col1_x + this.col2_x * matb.col1_y;
float m12 = this.col1_y * matb.col1_x + this.col2_y * matb.col1_y;
float m21 = this.col1_x * matb.col2_x + this.col2_x * matb.col2_y;
float m22 = this.col1_y * matb.col2_x + this.col2_y * matb.col2_y;
return this.New(m11, m12, m21, m22);
}
float _sign(float a)
{
if(a < 0.0) return -1.0;
return 1.0;
}
int _maxi(int a, int b)
{
if (a > b)
return a;
return b;
}
int _mini(int a, int b)
{
if (a < b)
return a;
return b;
}
int _clampi(int v, int min, int max)
{
return _mini(max, _maxi(v, min));
}
float _max(float a, float b)
{
if (a > b)
return a;
return b;
}
float _min(float a, float b)
{
if (a < b)
return a;
return b;
}
float _clamp(float v, float min, float max)
{
return _min(max, _max(v, min));
}
// Random number in range [-1,1]
float _Random()
{
float r = IntToFloat(Random(100000));
r /= 100000.0;
return 2.0 * r - 1.0;
}
float _RandomRange(float lo, float hi)
{
float r = IntToFloat(Random(100000));
r /= 100000.0;
return (hi - lo) * r + lo;
}
#endregion //MATH_UTILITIES
bool _accumulateImpulses;
bool _warmStarting;
bool _positionCorrection;
static bool World::get_accumulateImpulses()
{
return _accumulateImpulses;
}
static void World::set_accumulateImpulses(bool value)
{
_accumulateImpulses = value;
}
static bool World::get_warmStarting()
{
return _warmStarting;
}
static void World::set_warmStarting(bool value)
{
_warmStarting = value;
}
static bool World::get_positionCorrection()
{
return _positionCorrection;
}
static void World::set_positionCorrection(bool value)
{
_positionCorrection = value;
}
void FeaturePair::set_value(int v)
{
this.inEdge1 = (v >> 24) & 0xff;
this.inEdge2 = (v >> 16) & 0xff;
this.outEdge1 = (v >> 8) & 0xff;
this.outEdge2 = v & 0xff;
}
int FeaturePair::get_value()
{
return (this.inEdge1 << 24) & 0xff000000 | (this.inEdge2 << 16) & 0x00ff0000 | (this.outEdge1 << 8) & 0x0000ff00 | this.outEdge2;
}
static FeaturePair* FeaturePair::Create()
{
FeaturePair* fp = new FeaturePair;
fp.inEdge1 = 0;
fp.inEdge2 = 0;
fp.outEdge1 = 0;
fp.outEdge2 = 0;
return fp;
}
void World::AddBody(Body* body)
{
this.bodies[this.body_count] = body;
this.body_count++;
}
void World::AddJoint(Joint* joint)
{
this.joints[this.joint_count] = joint;
this.joint_count++;
}
void Arbiters::Add(Arbiter* arb)
{
if(this.a_count < MAX_ARBITERS) {
this.a[this.a_count] = arb;
this.a_count++;
}
}
Contact* Contact::CopyTo(Contact* c)
{
c.bias = this.bias;
if(c.feature == null) {
c.feature = FeaturePair.Create();
}
c.feature.inEdge1 = this.feature.inEdge1;
c.feature.inEdge2 = this.feature.inEdge2;
c.feature.outEdge1 = this.feature.outEdge1;
c.feature.outEdge2 = this.feature.outEdge2;
c.massNormal = this.massNormal;
c.massTangent = this.massTangent;
if(c.normal == null) {
c.normal = Vec2.New(this.normal.x, this.normal.y);
} else {
c.normal.Set(this.normal.x, this.normal.y);
}
c.Pn = this.Pn;
c.Pnb = this.Pnb;
if(c.position == null) {
c.position = Vec2.New(c.position.x, c.position.y);
} else {
c.position.Set(c.position.x, c.position.y);
}
c.Pt = this.Pt;
if(c.r1 == null) {
c.r1 = Vec2.New(this.r1.x, this.r1.y);
} else {
c.r1.Set(this.r1.x, this.r1.y);
}
if(c.r2 == null) {
c.r2 = Vec2.New(this.r2.x, this.r2.y);
} else {
c.r2.Set(this.r2.x, this.r2.y);
}
c.separation = this.separation;
return c;
}
void Arbiters::Remove(Body* b1, Body* b2)
{
int j=0;
int removed = 0;
for(int i=0; i<this.a_count; i++)
{
if(this.a[i].body1 == b1 && this.a[i].body2 == b2 ||
this.a[i].body1 == b2 && this.a[i].body2 == b1 ) {
removed++;
continue;
}
this.a[j]= this.a[i];
j++;
}
this.a_count-=removed;
}
Arbiter* Arbiters::Get(Body* b1, Body* b2)
{
for(int i=0; i<this.a_count; i++)
{
if(this.a[i].body1 == b1 && this.a[i].body2 == b2 ||
this.a[i].body1 == b2 && this.a[i].body2 == b1 ) {
return this.a[i];
}
}
return null;
}
void Arbiters::Clear()
{
for(int i=0; i<this.a_count; i++)
{
this.a[i] = null;
}
this.a_count = 0;
}
static Arbiters* Arbiters::Create()
{
Arbiters* a = new Arbiters;
return a;
}
void World::Clear()
{
for(int i=0; i<this.body_count; i++)
{
this.bodies[i] = null;
}
this.body_count = 0;
for(int i=0; i<this.joint_count; i++)
{
this.joints[i] = null;
}
this.joint_count = 0;
if(this.arbiters != null) {
this.arbiters.Clear();
} else {
this.arbiters = Arbiters.Create();
}
}
static Body* Body::Create()
{
Body* b = new Body;
b.position_x = 0.0;
b.position_y = 0.0;
b.rotation = 0.0;
b.velocity_x = 0.0;
b.velocity_y = 0.0;
b.angularVelocity = 0.0;
b.force_x = 0.0;
b.force_y = 0.0;
b.torque = 0.0;
b.friction = 0.2;
b.width_x = 1.0;
b.width_y = 1.0;
b.mass = FLT_MAX;
b.invMass = 0.0;
b.I = FLT_MAX;
b.invI = 0.0;
return b;
}
void Body::Set(Vec2* w, float m)
{
this.position_x = 0.0;
this.position_y = 0.0;
this.rotation = 0.0;
this.velocity_x = 0.0;
this.velocity_y = 0.0;
this.angularVelocity = 0.0;
this.force_x = 0.0;
this.force_y = 0.0;
this.torque = 0.0;
this.friction = 0.2;
this.width_x = w.x;
this.width_y = w.y;
this.mass = m;
if (this.mass < FLT_MAX)
{
this.invMass = 1.0 / this.mass;
this.I = this.mass * (this.width_x * this.width_x + this.width_y * this.width_y) / 12.0;
this.invI = 1.0 / this.I;
}
else
{
this.invMass = 0.0;
this.I = FLT_MAX;
this.invI = 0.0;
}
}
static Joint* Joint::Create()
{
Joint* jo = new Joint;
jo.body1 = null;
jo.body2 = null;
jo.P = Vec2.New(0.0, 0.0);
jo.biasFactor = 0.2;
jo.softness = 0.0;
jo.M = Mat22.New(0.0, 0.0, 0.0, 0.0);
jo.r1 = Vec2.New(0.0, 0.0);
jo.r2 = Vec2.New(0.0, 0.0);
jo.localAnchor1 = Vec2.New(0.0, 0.0);
jo.localAnchor2 = Vec2.New(0.0, 0.0);
jo.bias = Vec2.New(0.0, 0.0);
return jo;
}
void Joint::Set(Body* b1, Body* b2, Vec2* anchor)
{
this.body1 = b1;
this.body2 = b2;
Mat22* Rot1 = Mat22.NewFromAngle(this.body1.rotation);
Mat22* Rot2 = Mat22.NewFromAngle(this.body2.rotation);
Mat22* Rot1T = Rot1.Transpose();
Mat22* Rot2T = Rot2.Transpose();
this.localAnchor1 = MultiplyVec2(Rot1T, anchor.Minus(Vec2.New(this.body1.position_x, this.body1.position_y)));
this.localAnchor2 = MultiplyVec2(Rot2T, anchor.Minus(Vec2.New(this.body2.position_x, this.body2.position_y)));
if(this.P == null) {
this.P = Vec2.New(0.0, 0.0);
} else {
this.P.Set(0.0, 0.0);
}
this.softness = 0.0;
this.biasFactor = 0.2;
}
void Joint::PreStep(float inv_dt)
{
// Pre-compute anchors, mass matrix, and bias.
Mat22* Rot1 = Mat22.NewFromAngle(this.body1.rotation);
Mat22* Rot2 = Mat22.NewFromAngle(this.body2.rotation);
this.r1 = MultiplyVec2(Rot1, this.localAnchor1);
this.r2 = MultiplyVec2(Rot2, this.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 = Mat22.New(
this.body1.invMass + this.body2.invMass, 0.0,
0.0, this.body1.invMass + this.body2.invMass);
Mat22* K2 = Mat22.New( this.body1.invI * this.r1.y * this.r1.y, -this.body1.invI * this.r1.x * this.r1.y,
-this.body1.invI * this.r1.x * this.r1.y, this.body1.invI * this.r1.x * this.r1.x);
Mat22* K3 = Mat22.New(this.body2.invI * this.r2.y * this.r2.y, -this.body2.invI * this.r2.x * this.r2.y,
-this.body2.invI * this.r2.x * this.r2.y, this.body2.invI * this.r2.x * this.r2.x);
Mat22* K = K1.Plus(K2);
K = K.Plus(K3);
K.col1_x += this.softness;
K.col1_y += this.softness;
this.M = K.Invert();
Vec2* p1 = this.r1.Plus(Vec2.New(this.body1.position_x, this.body1.position_y));
Vec2* p2 = this.r2.Plus(Vec2.New(this.body2.position_x, this.body2.position_y));
Vec2* dp = p2.Minus(p1);
if (_positionCorrection)
{
this.bias = dp.Scale(- this.biasFactor * inv_dt);
}
else
{
if(this.bias == null) {
this.bias = Vec2.New(0.0, 0.0);
} else {
this.bias.Set(0.0, 0.0);
}
}
if (_warmStarting)
{
// Apply accumulated impulse.
this.body1.velocity_x -= this.body1.invMass * this.P.x;
this.body1.velocity_y -= this.body1.invMass * this.P.y;
this.body1.angularVelocity -= this.body1.invI * this.r1.Cross(this.P);
this.body2.velocity_x += this.body2.invMass * this.P.x;
this.body2.velocity_y += this.body2.invMass * this.P.y;
this.body2.angularVelocity += this.body2.invI * this.r2.Cross(this.P);
}
else
{
if(this.P == null) {
this.P = Vec2.New(0.0, 0.0);
} else {
this.P.Set(0.0, 0.0);
}
}
}
void Joint::ApplyImpulse()
{
Vec2* tmpa = CrossS2V(this.body2.angularVelocity, this.r2);
tmpa.x += this.body2.velocity_x;
tmpa.y += this.body2.velocity_y;
tmpa.x -= this.body1.velocity_x;
tmpa.y -= this.body1.velocity_y;
Vec2* tmpb = CrossS2V(this.body1.angularVelocity, this.r1);
Vec2* dv = tmpa.Minus(tmpb);
Vec2* tmp = this.bias.Minus(dv);
tmp = tmp.Minus(this.P.Scale(this.softness));
Vec2* impulse = MultiplyVec2(this.M, tmp);
this.body1.velocity_x -= this.body1.invMass*impulse.x;
this.body1.velocity_y -= this.body1.invMass*impulse.y;
this.body1.angularVelocity -= this.body1.invI * this.r1.Cross(impulse);
this.body2.velocity_x += this.body2.invMass*impulse.x;
this.body2.velocity_y += this.body2.invMass*impulse.y;
this.body2.angularVelocity += this.body2.invI * this.r2.Cross(impulse);
this.P = this.P.Plus(impulse);
}
// 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
};
managed struct ClipVertex
{
static import ClipVertex* Create();
Vec2* v;
FeaturePair* fp;
};
static ClipVertex* ClipVertex::Create()
{
ClipVertex* cv = new ClipVertex;
cv.v = Vec2.New(0.0, 0.0);
cv.fp = FeaturePair.Create();
return cv;
}
static Contact* Contact::Create()
{
Contact* c = new Contact;
c.bias = 0.0;
c.feature = FeaturePair.Create();
c.massNormal = 0.0;
c.massTangent = 0.0;
c.normal = Vec2.New(0.0, 0.0);
c.Pn = 0.0;
c.Pnb = 0.0;
c.position = Vec2.New(0.0, 0.0);
c.Pt = 0.0;
c.r1 = Vec2.New(0.0, 0.0);
c.r2 = Vec2.New(0.0, 0.0);
c.separation = 0.0;
return c;
}
void Flip(FeaturePair* fp)
{
char tmp = fp.inEdge1;
fp.inEdge1 = fp.inEdge2;
fp.inEdge2 = tmp;
tmp = fp.outEdge1;
fp.outEdge1 = fp.outEdge2;
fp.outEdge2 = tmp;
}
int ClipSegmentToLine(ClipVertex* vOut[], ClipVertex* vIn[], Vec2* normal, float offset, char clipEdge)
{
// Start with no output points
int numOut = 0;
// Calculate the distance of end points to the line
float distance0 = normal.Dot(vIn[0].v) - offset;
float distance1 = normal.Dot(vIn[1].v) - offset;
// If the points are behind the plane
if (distance0 <= 0.0) {
vOut[numOut] = vIn[0];
numOut++;
}
if (distance1 <= 0.0) {
vOut[numOut] = vIn[1];
numOut++;
}
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0)
{
// Find intersection point of edge and plane
float interp = distance0 / (distance0 - distance1);
Vec2* tmp_v = vIn[1].v.Minus(vIn[0].v);
tmp_v = tmp_v.Scale(interp);
vOut[numOut].v = vIn[0].v.Plus(tmp_v);
if (distance0 > 0.0)
{
vOut[numOut].fp = vIn[0].fp;
vOut[numOut].fp.inEdge1 = clipEdge;
vOut[numOut].fp.inEdge2 = NO_EDGE;
}
else
{
vOut[numOut].fp = vIn[1].fp;
vOut[numOut].fp.outEdge1 = clipEdge;
vOut[numOut].fp.outEdge2 = NO_EDGE;
}
numOut++;
}
return numOut;
}
void ComputeIncidentEdge(ClipVertex* c[], Vec2* h, Vec2* pos, Mat22* Rot, 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 = MultiplyVec2(RotT, normal);
n = n.Negate();
Vec2* nAbs = n.Abs();
if (nAbs.x > nAbs.y)
{
if (_sign(n.x) > 0.0)
{
c[0].v.Set(h.x, -h.y);
c[0].fp.inEdge2 = EDGE3;
c[0].fp.outEdge2 = EDGE4;
c[1].v.Set(h.x, h.y);
c[1].fp.inEdge2 = EDGE4;
c[1].fp.outEdge2 = EDGE1;
}
else
{
c[0].v.Set(-h.x, h.y);
c[0].fp.inEdge2 = EDGE1;
c[0].fp.outEdge2 = EDGE2;
c[1].v.Set(-h.x, -h.y);
c[1].fp.inEdge2 = EDGE2;
c[1].fp.outEdge2 = EDGE3;
}
}
else
{
if (_sign(n.y) > 0.0)
{
c[0].v.Set(h.x, h.y);
c[0].fp.inEdge2 = EDGE4;
c[0].fp.outEdge2 = EDGE1;
c[1].v.Set(-h.x, h.y);
c[1].fp.inEdge2 = EDGE1;
c[1].fp.outEdge2 = EDGE2;
}
else
{
c[0].v.Set(-h.x, -h.y);
c[0].fp.inEdge2 = EDGE2;
c[0].fp.outEdge2 = EDGE3;
c[1].v.Set(h.x, -h.y);
c[1].fp.inEdge2 = EDGE3;
c[1].fp.outEdge2 = EDGE4;
}
}
c[0].v = pos.Plus(MultiplyVec2(Rot, c[0].v));
c[1].v = pos.Plus(MultiplyVec2(Rot, c[1].v));
}
// The normal points from A to B
int Collide(Contact* contacts[], Body* bodyA, Body* bodyB)
{
// Setup
Vec2* hA = Vec2.New(bodyA.width_x, bodyA.width_y);
hA = hA.Scale(0.5);
Vec2* hB = Vec2.New(bodyB.width_x, bodyB.width_y);
hB = hB.Scale(0.5);
Vec2* posA = Vec2.New(bodyA.position_x, bodyA.position_y);
Vec2* posB = Vec2.New(bodyB.position_x, bodyB.position_y);
Mat22* RotA = Mat22.NewFromAngle(bodyA.rotation);
Mat22* RotB = Mat22.NewFromAngle(bodyB.rotation);
Mat22* RotAT = RotA.Transpose();
Mat22* RotBT = RotB.Transpose();
Vec2* dp = posB.Minus(posA);
Vec2* dA = MultiplyVec2(RotAT, dp);
Vec2* dB = MultiplyVec2(RotBT, dp);
Mat22* C = RotAT.Multiply(RotB);
Mat22* absC = C.Abs();
Mat22* absCT = absC.Transpose();
// Box A faces - Vec2 faceA = Abs(dA) - hA - absC * hB;
Vec2* faceA = dA.Abs();
faceA = faceA.Minus(hA);
faceA = faceA.Minus(MultiplyVec2(absC, hB));
if (faceA.x > 0.0 || faceA.y > 0.0)
return 0;
// Box B faces - Vec2 faceB = Abs(dB) - hB - absCT * hA;
Vec2* faceB = dB.Abs();
faceB = faceB.Minus(hB);
faceB = faceB.Minus(MultiplyVec2(absCT, hA));
if (faceB.x > 0.0 || faceB.y > 0.0)
return 0;
// Find best axis
Axis axis;
float separation;
Vec2* normal;
// Box A faces
axis = FACE_A_X;
separation = faceA.x;
if(dA.x > 0.0) {
normal = Vec2.New(RotA.col1_x, RotA.col1_y);
} else {
normal = Vec2.New(-RotA.col1_x, -RotA.col1_y);
}
float relativeTol = 0.95;
float absoluteTol = 0.01;
if (faceA.y > relativeTol * separation + absoluteTol * hA.y)
{
axis = FACE_A_Y;
separation = faceA.y;
if(dA.y > 0.0) {
normal.Set(RotA.col2_x, RotA.col2_y);
} else {
normal.Set(-RotA.col2_x, -RotA.col2_y);
}
}
// Box B faces
if (faceB.x > relativeTol * separation + absoluteTol * hB.x)
{
axis = FACE_B_X;
separation = faceB.x;
if(dB.x > 0.0) {
normal.Set(RotB.col1_x, RotB.col1_y);
} else {
normal.Set(-RotB.col1_x, -RotB.col1_y);
}
}
if (faceB.y > relativeTol * separation + absoluteTol * hB.y)
{
axis = FACE_B_Y;
separation = faceB.y;
if(dB.y > 0.0) {
normal.Set(RotB.col2_x, RotB.col2_y);
} else {
normal.Set(-RotB.col2_x, -RotB.col2_y);
}
}
// Setup clipping plane data based on the separating axis
Vec2* frontNormal;
Vec2* sideNormal;
ClipVertex* incidentEdge[] = new ClipVertex[2];
incidentEdge[0] = ClipVertex.Create();
incidentEdge[1] = ClipVertex.Create();
float 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 = posA.Dot(frontNormal) + hA.x;
sideNormal = Vec2.New(RotA.col2_x, RotA.col2_y);
float side = posA.Dot(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 = posA.Dot(frontNormal) + hA.y;
sideNormal = Vec2.New(RotA.col1_x, RotA.col1_y);
float side = posA.Dot(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.Negate();
front = posB.Dot(frontNormal) + hB.x;
sideNormal = Vec2.New(RotB.col2_x, RotB.col2_y);
float side = posB.Dot(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.Negate();
front = posB.Dot(frontNormal) + hB.y;
sideNormal = Vec2.New(RotB.col1_x, RotB.col1_y);
float side = posB.Dot(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[] = new ClipVertex[2];
clipPoints1[0] = ClipVertex.Create();
clipPoints1[1] = ClipVertex.Create();
ClipVertex* clipPoints2[] = new ClipVertex[2];
clipPoints2[0] = ClipVertex.Create();
clipPoints2[1] = ClipVertex.Create();
int np;
// Clip to box side 1
np = ClipSegmentToLine(clipPoints1, incidentEdge, sideNormal.Negate(), 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++)
{
float sep = frontNormal.Dot(clipPoints2[i].v) - front;
if (sep <= 0.0)
{
contacts[numContacts].separation = sep;
contacts[numContacts].normal = normal;
// slide contact point onto reference face (easy to cull)
contacts[numContacts].position.SetV(clipPoints2[i].v.Minus(frontNormal.Scale(sep)));
contacts[numContacts].feature = clipPoints2[i].fp;
if (axis == FACE_B_X || axis == FACE_B_Y)
Flip(contacts[numContacts].feature);
numContacts++;
}
}
return numContacts;
}
static Arbiter* Arbiter::Create(Body* b1, Body* b2)
{
Arbiter* a = new Arbiter;
if(b1.mass < FLT_MAX) {
a.body1 = b1;
a.body2 = b2;
} else {
a.body1 = b2;
a.body2 = b1;
}
a.contacts = new Contact[MAX_CONTACTS];
for(int i=0; i<MAX_CONTACTS; i++) {
a.contacts[i] = Contact.Create();
}
a.numContacts = Collide(a.contacts, a.body1, a.body2);
a.friction = Maths.Sqrt(a.body1.friction * a.body2.friction);
return a;
}
void Arbiter::Update(Contact* newContacts[], int numNewContacts)
{
Contact* mergedContacts[2];
mergedContacts[0] = Contact.Create();
mergedContacts[1] = Contact.Create();
for (int i=0; i < numNewContacts; i++)
{
Contact* cNew = newContacts[i];
int k = -1;
for (int j=0; j < this.numContacts; j++)
{
Contact* cOld = this.contacts[j];
if (cNew.feature.inEdge1 == cOld.feature.inEdge1 &&
cNew.feature.inEdge2 == cOld.feature.inEdge2 &&
cNew.feature.outEdge1 == cOld.feature.outEdge1 &&
cNew.feature.outEdge2 == cOld.feature.outEdge2 )
{
k = j;
break;
}
}
if (k > -1)
{
Contact* cOld = this.contacts[k];
mergedContacts[i] = cNew;
if (_warmStarting)
{
cNew.Pn = cOld.Pn;
cNew.Pt = cOld.Pt;
cNew.Pnb = cOld.Pnb;
}
else
{
cNew.Pn = 0.0;
cNew.Pt = 0.0;
cNew.Pnb = 0.0;
}
}
else
{
mergedContacts[i] = newContacts[i];
}
}
for (int i = 0; i < numNewContacts; i++)
this.contacts[i] = mergedContacts[i];
this.numContacts = numNewContacts;
}
void Arbiter::ApplyImpulse()
{
Body* b1 = this.body1;
Body* b2 = this.body2;
for (int i=0; i < this.numContacts; i++)
{
Contact* c = this.contacts[i];
c.r1.x = c.position.x - b1.position_x;
c.r1.y = c.position.y - b1.position_y;
c.r2.x = c.position.x - b2.position_x;
c.r2.y = c.position.y - b2.position_y;
// Relative velocity at contact
Vec2* tmpa = CrossS2V(b2.angularVelocity, c.r2);
tmpa.x += b2.velocity_x;
tmpa.y += b2.velocity_y;
tmpa.x -= b1.velocity_x;
tmpa.y -= b1.velocity_y;
Vec2* tmpb = CrossS2V(b1.angularVelocity, c.r1);
Vec2* dv = tmpa.Minus(tmpb);
// Compute normal impulse
float vn = dv.Dot(c.normal);
float dPn = c.massNormal * (-vn + c.bias);
if (_accumulateImpulses)
{
// Clamp the accumulated impulse
float Pn0 = c.Pn;
c.Pn = _max(Pn0 + dPn, 0.0);
dPn = c.Pn - Pn0;
}
else
{
dPn = _max(dPn, 0.0);
}
// Apply contact impulse
Vec2* Pn = c.normal.Scale(dPn);
b1.velocity_x -= b1.invMass * Pn.x;
b1.velocity_y -= b1.invMass * Pn.y;
b1.angularVelocity -= b1.invI * c.r1.Cross(Pn);
b2.velocity_x += b2.invMass * Pn.x;
b2.velocity_y += b2.invMass * Pn.y;
b2.angularVelocity += b2.invI * c.r2.Cross(Pn);
// Relative velocity at contact
tmpa = CrossS2V(b2.angularVelocity, c.r2);
tmpa.x += b2.velocity_x;
tmpa.y += b2.velocity_y;
tmpa.x -= b1.velocity_x;
tmpa.y -= b1.velocity_y;
tmpb = CrossS2V(b1.angularVelocity, c.r1);
dv = tmpa.Minus(tmpb);
Vec2* tangent = CrossV2S(c.normal, 1.0);
float vt = dv.Dot(tangent);
float dPt = c.massTangent * (-vt);
if (_accumulateImpulses)
{
// Compute friction impulse
float maxPt = this.friction * c.Pn;
// Clamp friction
float oldTangentImpulse = c.Pt;
c.Pt = _clamp(oldTangentImpulse + dPt, -maxPt, maxPt);
dPt = c.Pt - oldTangentImpulse;// _clamp(c.Pt - oldTangentImpulse, -maxPt, maxPt);
}
else
{
float maxPt = this.friction * dPn;
dPt = _clamp(dPt, -maxPt, maxPt);
}
// Apply contact impulse
Vec2* Pt = tangent.Scale(dPt);
b1.velocity_x -= b1.invMass * Pt.x;
b1.velocity_y -= b1.invMass * Pt.y;
b1.angularVelocity -= b1.invI * c.r1.Cross(Pt);
b2.velocity_x += b2.invMass * Pt.x;
b2.velocity_y += b2.invMass * Pt.y;
b2.angularVelocity += b2.invI * c.r2.Cross(Pt);
}
}
void Arbiter::PreStep(float inv_dt)
{
float k_allowedPenetration = 0.01;
float k_biasFactor = 0.0;
if(_positionCorrection) k_biasFactor = 0.2;
for (int i = 0; i < this.numContacts; i++)
{
Contact* c = this.contacts[i];
Vec2* r1 = new Vec2;
Vec2* r2 = new Vec2;
r1.x = c.position.x - this.body1.position_x;
r1.y = c.position.y - this.body1.position_y;
r2.x = c.position.x - this.body2.position_x;
r2.y = c.position.y - this.body2.position_y;
// Precompute normal mass, tangent mass, and bias.
float rn1 = r1.Dot(c.normal);
float rn2 = r2.Dot(c.normal);
float kNormal = this.body1.invMass + this.body2.invMass;
kNormal += this.body1.invI * (r1.Dot(r1) - rn1 * rn1) + this.body2.invI * (r2.Dot(r2) - rn2 * rn2);
c.massNormal = 1.0 / kNormal;
Vec2* tangent = CrossV2S(c.normal, 1.0);
float rt1 = r1.Dot(tangent);
float rt2 = r2.Dot(tangent);
float kTangent = this.body1.invMass + this.body2.invMass;
kTangent += this.body1.invI * (r1.Dot(r1) - rt1 * rt1) + this.body2.invI * (r2.Dot(r2) - rt2 * rt2);
c.massTangent = 1.0 / kTangent;
c.bias = -k_biasFactor * inv_dt * _min(0.0, c.separation + k_allowedPenetration);
if (_accumulateImpulses)
{
// Apply normal + friction impulse
Vec2* ppp = tangent.Scale(c.Pt);
Vec2* pp = c.normal.Scale(c.Pn);
Vec2* P = pp.Plus(ppp);
pp = null; ppp = null;
this.body1.velocity_x -= this.body1.invMass * P.x;
this.body1.velocity_y -= this.body1.invMass * P.y;
this.body1.angularVelocity -= this.body1.invI * r1.Cross(P);
this.body2.velocity_x += this.body2.invMass * P.x;
this.body2.velocity_y += this.body2.invMass * P.y;
this.body2.angularVelocity += this.body2.invI * r2.Cross(P);
}
}
}
void World::BroadPhase()
{
// O(n^2) broad-phase
for (int i = 0; i < this.body_count; i++)
{
Body* bi = this.bodies[i];
for (int j = i + 1; j < this.body_count; j++)
{
Body* bj = this.bodies[j];
if (bi.invMass == 0.0 && bj.invMass == 0.0)
continue;
Arbiter* newArb = Arbiter.Create(bi, bj);
if (newArb.numContacts > 0)
{
Arbiter* arb = this.arbiters.Get(bi, bj);
if (arb == null)
{
this.arbiters.Add(newArb);
}
else
{
arb.Update(newArb.contacts, newArb.numContacts);
}
}
else
{
this.arbiters.Remove(bi, bj);
}
}
}
}
void World::Step(float dt)
{
float inv_dt = 0.0;
if(dt > 0.0) {
inv_dt = 1.0 / dt;
}
// Determine overlapping bodies and update contact points.
this.BroadPhase();
// Integrate forces.
for (int i = 0; i < this.body_count; i++)
{
Body* b = this.bodies[i];
if (b.invMass == 0.0)
continue;
b.velocity_x += dt * (this.gravity.x + b.invMass * b.force_x);
b.velocity_y += dt * (this.gravity.y + b.invMass * b.force_y);
b.angularVelocity += dt * b.invI * b.torque;
}
// Perform pre-steps.
for (int i=0; i< this.arbiters.a_count; i++)
{
Arbiter* arb = this.arbiters.a[i];
arb.PreStep(inv_dt);
}
for (int i = 0; i < this.joint_count; i++)
{
this.joints[i].PreStep(inv_dt);
}
// Perform iterations
for (int i=0; i < this.iterations; i++)
{
for (int j=0; j< this.arbiters.a_count; j++)
{
Arbiter* arb = this.arbiters.a[j];
arb.ApplyImpulse();
}
for (int j=0; j < this.joint_count; j++)
{
this.joints[j].ApplyImpulse();
}
}
// Integrate Velocities
for (int i = 0; i < this.body_count; i++)
{
Body* b = this.bodies[i];
b.position_x += dt * b.velocity_x;
b.position_y += dt * b.velocity_y;
b.rotation += dt * b.angularVelocity;
b.force_x = 0.0;
b.force_y = 0.0;
b.torque = 0.0;
}
}
void World::Init(float gravity_x, float gravity_y, int iterations)
{
this.Clear();
this.gravity = Vec2.New(gravity_x, gravity_y);
this.iterations = iterations;
}
void game_start()
{
_accumulateImpulses = true;
_warmStarting = true;
_positionCorrection = true;
}
// room script file
#define MAX_STUFF 32
#define SCALE 16.0
World world;
float step;
struct Thing {
Body* b;
Overlay* ovr;
DynamicSprite* dnspr;
import void InitFromBody(Body* b);
import void Render();
};
Thing things[MAX_STUFF];
int thing_count;
Overlay* scr_ovr;
DynamicSprite* scr_spr;
void DrawContacts()
{
if(scr_ovr == null) {
scr_spr = DynamicSprite.Create(Screen.Width, Screen.Height);
scr_ovr = Overlay.CreateGraphical(0, 0, scr_spr.Graphic);
scr_ovr.ZOrder = 1000;
}
DrawingSurface* surf = scr_spr.GetDrawingSurface();
surf.Clear(COLOR_TRANSPARENT);
surf.DrawingColor = 61871;
for(int arsi=0; arsi<world.arbiters.a_count; arsi++) {
Arbiter* arb = world.arbiters.a[arsi];
for(int i=0; i<arb.numContacts; i++) {
int cnx = FloatToInt(arb.contacts[i].position.x*SCALE)+Screen.Width/2;
int cny = FloatToInt(arb.contacts[i].position.y*SCALE)+Screen.Height/2;
surf.DrawCircle(cnx, cny, 3);
}
}
}
void Thing::InitFromBody(Body* b) {
this.b = b;
DrawingSurface* surf;
int w = FloatToInt(b.width_x*SCALE);
int h = FloatToInt(b.width_y*SCALE);
this.dnspr = DynamicSprite.Create(w, h);
surf = this.dnspr.GetDrawingSurface();
surf.Clear(256 + Random(65504));
if(w > 2 && h > 2) {
surf.DrawingColor = 256 + Random(65504);
surf.DrawRectangle(1, 1, w-2, h-2);
}
surf.Release();
this.ovr = Overlay.CreateGraphical(0, 0, this.dnspr.Graphic);
this.ovr.X = FloatToInt((this.b.position_x - this.b.width_x/2.0)*SCALE)+Screen.Width/2;
this.ovr.Y = FloatToInt((this.b.position_y - this.b.width_y/2.0)*SCALE)+Screen.Height/2;
}
void Thing::Render() {
int w = FloatToInt((this.b.width_x)*SCALE);
int h = FloatToInt((this.b.width_y)*SCALE);
this.ovr.Graphic = this.dnspr.Graphic;
this.ovr.X = FloatToInt((this.b.position_x - this.b.width_x/2.0)*SCALE)+Screen.Width/2;
this.ovr.Y = FloatToInt((this.b.position_y - this.b.width_y/2.0)*SCALE)+Screen.Height/2;
this.ovr.Width = w;
this.ovr.Height = h;
this.ovr.Rotation = Maths.RadiansToDegrees(this.b.rotation);
}
void NewBody(float x, float y, float width, float height, float mass) {
Body* b = Body.Create();
b.Set(Vec2.New(width, height), mass);
b.position_x = x;
b.position_y = y;
things[thing_count].InitFromBody(b);
thing_count++;
world.AddBody(b);
}
void RenderAll() {
for(int i=0; i<thing_count; i++) {
things[i].Render();
}
DrawContacts();
}
function room_Load()
{
world.Init(0.0, 10.0);
step = 1.0/IntToFloat(GetGameSpeed());
NewBody(0.0, 0.5*20.0, 100.0, 20.0, FLT_MAX);
NewBody(0.0, -4.0, 1.0, 1.0, 200.0);
NewBody(1.0, -6.0, 1.0, 1.0, 200.0);
NewBody(2.0, -5.0, 1.0, 1.0, 200.0);
NewBody(1.5, -2.0, 1.0, 1.0, 800.0);
NewBody(2.4, -9.0, 1.0, 1.0, 200.0);
NewBody(1.4, -10.0, 1.0, 1.0, 200.0);
NewBody(2.8, -12.0, 1.0, 1.0, 200.0);
NewBody(1.8, -14.0, 1.0, 1.0, 200.0);
NewBody(0.8, -12.0, 1.0, 1.0, 200.0);
}
bool start;
function room_RepExec()
{
if(!start) {
Display("START");
start = !start;
}
world.Step(step);
RenderAll();
}
