public Body(Shape shape, float x, float y)
{
this.shape = shape;
position.Set(x, y);
velocity.Set(0, 0);
angularVelocity = 0;
torque = 0;
orient = ImpulseMath.Random(-ImpulseMath.PI, ImpulseMath.PI);
force.Set(0, 0);
staticFriction = 0.5f;
dynamicFriction = 0.3f;
restitution = 0.2f;
shape.body = this;
shape.Initialize();
}
public override void HandleCollision(Manifold m, Body a, Body b)
{
Polygon A = (Polygon)a.shape;
Polygon B = (Polygon)b.shape;
m.contactCount = 0;
// Check for a separating axis with A's face planes
int[] faceA = { 0 };
float penetrationA = FindAxisLeastPenetration(faceA, A, B);
if (penetrationA >= 0.0f)
{
return;
}
// Check for a separating axis with B's face planes
int[] faceB = { 0 };
float penetrationB = FindAxisLeastPenetration(faceB, B, A);
if (penetrationB >= 0.0f)
{
return;
}
int referenceIndex;
bool flip; // Always point from a to b
Polygon RefPoly; // Reference
Polygon IncPoly; // Incident
// Determine which shape contains reference face
if (ImpulseMath.Gt(penetrationA, penetrationB))
{
RefPoly = A;
IncPoly = B;
referenceIndex = faceA[0];
flip = false;
}
else
{
RefPoly = B;
IncPoly = A;
referenceIndex = faceB[0];
flip = true;
}
// World space incident face
Vec2[] incidentFace = Vec2.ArrayOf(2);
FindIncidentFace(incidentFace, RefPoly, IncPoly, referenceIndex);
// y
// ^ .n ^
// +---c ------posPlane--
// x < | i |\
// +---+ c-----negPlane--
// \ v
// r
//
// r : reference face
// i : incident poly
// c : clipped point
// n : incident normal
// Setup reference face vertices
Vec2 v1 = RefPoly.vertices[referenceIndex];
referenceIndex = referenceIndex + 1 == RefPoly.vertexCount ? 0 : referenceIndex + 1;
Vec2 v2 = RefPoly.vertices[referenceIndex];
// Transform vertices to world space
// v1 = RefPoly->u * v1 + RefPoly->body->position;
// v2 = RefPoly->u * v2 + RefPoly->body->position;
v1 = RefPoly.u.Mul(v1).Addi(RefPoly.body.position);
v2 = RefPoly.u.Mul(v2).Addi(RefPoly.body.position);
// Calculate reference face side normal in world space
// Vec2 sidePlaneNormal = (v2 - v1);
// sidePlaneNormal.Normalize( );
Vec2 sidePlaneNormal = v2.Sub(v1);
sidePlaneNormal.Normalize();
// Orthogonalize
// Vec2 refFaceNormal( sidePlaneNormal.y, -sidePlaneNormal.x );
Vec2 refFaceNormal = new Vec2(sidePlaneNormal.y, -sidePlaneNormal.x);
// ax + by = c
// c is distance from origin
// real refC = Dot( refFaceNormal, v1 );
// real negSide = -Dot( sidePlaneNormal, v1 );
// real posSide = Dot( sidePlaneNormal, v2 );
float refC = Vec2.Dot(refFaceNormal, v1);
float negSide = -Vec2.Dot(sidePlaneNormal, v1);
float posSide = Vec2.Dot(sidePlaneNormal, v2);
// Clip incident face to reference face side planes
// if(Clip( -sidePlaneNormal, negSide, incidentFace ) < 2)
if (Clip(sidePlaneNormal.Neg(), negSide, incidentFace) < 2)
{
return; // Due to floating point error, possible to not have required
// points
}
// if(Clip( sidePlaneNormal, posSide, incidentFace ) < 2)
if (Clip(sidePlaneNormal, posSide, incidentFace) < 2)
{
return; // Due to floating point error, possible to not have required
// points
}
// Flip
m.normal.Set(refFaceNormal);
if (flip)
{
m.normal.Negi();
}
// Keep points behind reference face
int cp = 0; // clipped points behind reference face
float separation = Vec2.Dot(refFaceNormal, incidentFace[0]) - refC;
if (separation <= 0.0f)
{
m.contacts[cp].Set(incidentFace[0]);
m.penetration = -separation;
++cp;
}
else
{
m.penetration = 0;
}
separation = Vec2.Dot(refFaceNormal, incidentFace[1]) - refC;
if (separation <= 0.0f)
{
m.contacts[cp].Set(incidentFace[1]);
m.penetration += -separation;
++cp;
// Average penetration
m.penetration /= cp;
}
m.contactCount = cp;
}
public void ApplyImpulse()
{
// Early out and positional correct if both objects have infinite mass
// if(Equal( A->im + B->im, 0 ))
if (ImpulseMath.Equal(A.invMass + B.invMass, 0))
{
InfiniteMassCorrection();
return;
}
for (int i = 0; i < contactCount; ++i)
{
// Calculate radii from COM to contact
// Vec2 ra = contacts[i] - A->position;
// Vec2 rb = contacts[i] - B->position;
Vec2 ra = contacts[i].Sub(A.position);
Vec2 rb = contacts[i].Sub(B.position);
// Relative velocity
// Vec2 rv = B->velocity + Cross( B->angularVelocity, rb ) -
// A->velocity - Cross( A->angularVelocity, ra );
Vec2 rv = B.velocity.Add(Vec2.Cross(B.angularVelocity, rb, new Vec2())).Subi(A.velocity).Subi(Vec2.Cross(A.angularVelocity, ra, new Vec2()));
// Relative velocity along the normal
// real contactVel = Dot( rv, normal );
float contactVel = Vec2.Dot(rv, normal);
// Do not resolve if velocities are separating
if (contactVel > 0)
{
return;
}
// real raCrossN = Cross( ra, normal );
// real rbCrossN = Cross( rb, normal );
// real invMassSum = A->im + B->im + Sqr( raCrossN ) * A->iI + Sqr(
// rbCrossN ) * B->iI;
float raCrossN = Vec2.Cross(ra, normal);
float rbCrossN = Vec2.Cross(rb, normal);
float invMassSum = A.invMass + B.invMass + (raCrossN * raCrossN) * A.invInertia + (rbCrossN * rbCrossN) * B.invInertia;
// Calculate impulse scalar
float j = -(1.0f + e) * contactVel;
j /= invMassSum;
j /= contactCount;
// Apply impulse
Vec2 impulse = normal.Mul(j);
A.ApplyImpulse(impulse.Neg(), ra);
B.ApplyImpulse(impulse, rb);
// Friction impulse
// rv = B->velocity + Cross( B->angularVelocity, rb ) -
// A->velocity - Cross( A->angularVelocity, ra );
rv = B.velocity.Add(Vec2.Cross(B.angularVelocity, rb, new Vec2())).Subi(A.velocity).Subi(Vec2.Cross(A.angularVelocity, ra, new Vec2()));
// Vec2 t = rv - (normal * Dot( rv, normal ));
// t.Normalize( );
Vec2 t = new Vec2(rv);
t.Addsi(normal, -Vec2.Dot(rv, normal));
t.Normalize();
// j tangent magnitude
float jt = -Vec2.Dot(rv, t);
jt /= invMassSum;
jt /= contactCount;
// Don't apply tiny friction impulses
if (ImpulseMath.Equal(jt, 0.0f))
{
return;
}
// Coulumb's law
Vec2 tangentImpulse;
// if(std::abs( jt ) < j * sf)
if (Mathf.Abs(jt) < j * sf)
{
// tangentImpulse = t * jt;
tangentImpulse = t.Mul(jt);
}
else
{
// tangentImpulse = t * -j * df;
tangentImpulse = t.Mul(j).Muli(-df);
}
// Apply friction impulse
// A->ApplyImpulse( -tangentImpulse, ra );
// B->ApplyImpulse( tangentImpulse, rb );
A.ApplyImpulse(tangentImpulse.Neg(), ra);
B.ApplyImpulse(tangentImpulse, rb);
}
}
void InputImpulseEngine(CommonMonoBehaviour.GameInput input)
{
if (!playing)
{
return;
}
if (input.keyDown[0])
{
playing = false;
}
if (input.keyDown[1])
{
if (input.mouseUp[0] || input.mouseDown[0])
{
float hw = ImpulseMath.Random(1.0f, 3.0f);
float hh = ImpulseMath.Random(1.0f, 3.0f);
Body b = impulse.Add(new Polygon(hw, hh), input.rayPosition.x, input.rayPosition.y);
b.SetOrient(0.0f);
}
if (input.mouseUp[1] || input.mouseDown[1])
{
float r = ImpulseMath.Random(1.0f, 5.0f);
int vertCount = 3;
Vec2[] verts = Vec2.ArrayOf(vertCount);
for (int i = 0; i < vertCount; i++)
{
verts[i].Set(ImpulseMath.Random(-r, r), ImpulseMath.Random(-r, r));
}
Body b = impulse.Add(new Polygon(verts), input.rayPosition.x, input.rayPosition.y);
b.SetOrient(ImpulseMath.Random(-ImpulseMath.PI, ImpulseMath.PI));
b.restitution = 0.2f;
b.dynamicFriction = 0.2f;
b.staticFriction = 0.4f;
}
}
else
{
if (input.mouseUp[0] || input.mouseDown[0])
{
float r = ImpulseMath.Random(1.0f, 5.0f);
int vertCount = ImpulseMath.Random(3, Polygon.MAX_POLY_VERTEX_COUNT);
Vec2[] verts = Vec2.ArrayOf(vertCount);
for (int i = 0; i < vertCount; i++)
{
verts[i].Set(ImpulseMath.Random(-r, r), ImpulseMath.Random(-r, r));
}
Body b = impulse.Add(new Polygon(verts), input.rayPosition.x, input.rayPosition.y);
b.SetOrient(ImpulseMath.Random(-ImpulseMath.PI, ImpulseMath.PI));
b.restitution = 0.2f;
b.dynamicFriction = 0.2f;
b.staticFriction = 0.4f;
}
if (input.mouseUp[1] || input.mouseDown[1])
{
float r = ImpulseMath.Random(1.0f, 3.0f);
impulse.Add(new Circle(r), input.rayPosition.x, input.rayPosition.y);
}
//Move your cube GameObject to the point where you clicked
if (PointerTransform)
{
PointerTransform.position = input.rayPosition;
}
}
}
