public static bool TrianglesCollide(Polygon A, Polygon B, out Vector2 MTD)
{
MTD = Vector2.Zero;
if (A.Triangles == null)
{
A.Triangulate();
}
if (B.Triangles == null)
{
B.Triangulate();
}
foreach (var triA in A.Triangles)
{
Polygon pA = triA.ToPolygon();
foreach (var triB in B.Triangles)
{
if (Polygon.Collide(pA, triB.ToPolygon(), out MTD))
{
return(true);
}
}
}
return(false);
}
// I can't recall if we use this method anymore :-|
// This probably should be deleted.
public void Collide(Body A, Body B, Triangle tA, Triangle tB)
{
if (A.IsStatic && B.IsStatic)
{
return;
}
Vector2 MTD, N;
float t = Speed;
if (Polygon.Collide(tA, tB, A.Velocity, B.Velocity, out N, ref t))
{
if (t < 0.0f)
{
MTD = N * -t;
//objects are overlapped, push them away.
if (A.IsStatic)
{
B.Mesh.Offset(-MTD);
}
else if (B.IsStatic)
{
A.Mesh.Offset(MTD);
}
else
{
//TODO: Implement Push
var aw = A.Mass / (A.Mass + B.Mass);
A.Mesh.Offset(MTD * aw);
B.Mesh.Offset(MTD * (1 - aw));
}
N = MTD;
N.Normalize();
t = 0.0f;
}
//process collision
var V = A.Velocity - B.Velocity;
var n = Vector2.Dot(V, N);
Vector2 Dn = N * n;
Vector2 Dt = V - Dn;
if (n > 0.0f)
{
Dn = Vector2.Zero;
}
float dt = Dt.LengthSquared();
float m = A.Mass + B.Mass;
float r0 = A.Mass / m;
float r1 = B.Mass / m;
A.Velocity += V * r0;
B.Velocity -= V * r1;
}
}
void UpdateBody(int i)
{
if (MaxUpdatePerBody < ++bodyUpdateCount[i])
{
return;
}
Vector2 a, v, p, r;
var body = Bodies[i];
//if (body.IsStatic) return;
a = body.Force * body.InverseMass -
(body.AttachToGravity ?
new Vector2(Gravity.X * body.GravityNormal.X, Gravity.Y * body.GravityNormal.Y) :
Vector2.Zero) + body.Acceleration;
v = a * Speed + body.Velocity;
Vector2 position, size;
body.Mesh.GetPositionAndSize(out position, out size);
v.X = MathHelper.Clamp(v.X, -body.MaxSpeed.X, body.MaxSpeed.X);
v.Y = MathHelper.Clamp(v.Y, -body.MaxSpeed.Y, body.MaxSpeed.Y);
r = v * Speed;
p = r + position;
body.Mesh.AutoTriangulate = true;
if (!body.Convex && body.Mesh.Triangles == null)
{
body.Mesh.Triangulate();
}
body.Mesh.Offset(r);
if (body.IsStatic)
{
return;
}
List <KeyValuePair <Polygon, Polygon> > polys = new List <KeyValuePair <Polygon, Polygon> >();
if (!body.IsFree && !body.IsStatic)
{
for (int j = 0; j < Bodies.Count; j++)
{
var item = Bodies[j];
if (i == j)
{
continue;
}
if (item.NotCollideWith.Contains(body) ||
body.NotCollideWith.Contains(item))
{
continue;
}
var push = Vector2.Zero;
polys.Clear();
if (item.Convex && body.Convex)
{
if (Polygon.Collide(body.Mesh, item.Mesh, out push))
{
polys.Add(new KeyValuePair <Polygon, Polygon>(body.Mesh, item.Mesh));
}
}
else
{
#region SAT With Triangles
if (body.Convex)
{
if (item.Mesh.Triangles == null)
{
item.Mesh.Triangulate();
}
foreach (var pTri in item.Mesh.Triangles)
{
var temppush = Vector2.Zero;
if (Polygon.Collide(body.Mesh, pTri, out temppush))
{
if (polys.Count == 0)
{
push = temppush;
}
polys.Add(new KeyValuePair <Polygon, Polygon>(body.Mesh, pTri));
}
}
}
else if (item.Convex)
{
foreach (var pTri in body.Mesh.Triangles)
{
var temppush = Vector2.Zero;
if (Polygon.Collide(pTri, item.Mesh, out temppush))
{
if (polys.Count == 0)
{
push = temppush;
}
polys.Add(new KeyValuePair <Polygon, Polygon>(pTri, item.Mesh));
}
}
}
else
{
foreach (var p1 in body.Mesh.Triangles)
{
if (item.Mesh.Triangles == null)
{
item.Mesh.Triangulate();
}
foreach (var p2 in item.Mesh.Triangles)
{
var temppush = Vector2.Zero;
if (Polygon.Collide(p1, p2, out temppush))
{
if (polys.Count == 0)
{
push = temppush;
}
polys.Add(new KeyValuePair <Polygon, Polygon>(p1, p2));
}
}
}
}
#endregion
}
for (int pi = 0; pi < polys.Count; pi++)
{
if (pi == 0 || Polygon.Collide(polys[pi].Key, polys[pi].Value, out push))
{
Vector2 pd = push;
pd.Normalize();
pd = push - AllowedPenetrationDepth * pd;
if (body.PreventSlippingOnSlopes)
{
if (Math.Abs(pd.X) < StickCoef)
{
pd.X = 0;
}
}
if (GeometryHelper.IsNaN(pd))
{
pd = Vector2.Zero;
}
/*
* EXPERIMENT:
* Move both bodies based on their weight
*/
else if (item.IsStatic)
{
if (!item.IsFree)
{
body.Mesh.Offset(pd);
}
if (body.Collide != null)
{
body.Collide(item.Entity, pd);
}
}
else
{
//Lerp between masses
var massSum = body.InverseMass + item.InverseMass;
var bodyW = body.InverseMass / massSum;
var itemW = bodyW - 1.0f;
var bodyPV = pd * bodyW;
var itemPV = pd * itemW;
if (!item.IsFree)
{
UpdateBody(j);
body.Mesh.Offset(bodyPV);
}
if (body.Collide != null)
{
body.Collide(item.Entity, bodyPV);
}
if (item.Collide != null)
{
item.Collide(body.Entity, itemPV);
}
}
}
}
}
}
var newP = body.Mesh.GetPosition();
var movedDistance = newP - p;
if (movedDistance.Length() > _bigNumber) //We don't want no weird teleports, do we?
{
body.Mesh.Offset(-movedDistance); //rollback
}
else
{
v += movedDistance / Speed;
}
body.Velocity = v * DampingCoef;
body.Acceleration = a;
/////////////////////
// Check for NaNs generated by bugs
/////////////////////
if (GeometryHelper.IsNaN(body.Velocity))
{
body.Velocity = Vector2.Zero;
}
if (GeometryHelper.IsNaN(body.Acceleration))
{
body.Acceleration = Vector2.Zero;
}
if (body.MaxSpeed.X >= 0 && Math.Abs(body.Velocity.X) > body.MaxSpeed.X)
{
body.Velocity.X = body.MaxSpeed.X * Math.Sign(body.Velocity.X);
}
if (body.MaxSpeed.Y >= 0 && Math.Abs(body.Velocity.Y) > body.MaxSpeed.Y)
{
body.Velocity.Y = body.MaxSpeed.Y * Math.Sign(body.Velocity.Y);
}
if (body.MaxAcceleration.X >= 0 && Math.Abs(body.Acceleration.X) > body.MaxAcceleration.X)
{
body.Acceleration.X = body.MaxAcceleration.X * Math.Sign(body.Acceleration.X);
}
if (body.MaxAcceleration.Y >= 0 && Math.Abs(body.Velocity.Y) > body.MaxAcceleration.Y)
{
body.Acceleration.Y = body.MaxAcceleration.Y * Math.Sign(body.Acceleration.Y);
}
body.Force = Vector2.Zero;
body.Acceleration = Vector2.Zero;
}