public void AddManifold(Manifold m)
{
contacts.Add(m);
}
public override void CheckCollisionBody(Body other)
{
//if (!active || !other.active) { return; }
//if (exclusions.Contains(other)) return;
if (invmass == 0 && other.invmass == 0)
return;
Manifold m = new Manifold(this, other);
m.Solve();
if (m.contact_count > 0)
{
if (DoExclusionCheck(other)) return;
if (HandlersEnabled)
{
//todo:add to handler list
if (OnCollisionStay != null)
{
OnCollisionStay(parent, other.parent);
}
bool parentEnter = OnCollisionEnter != null;
if (parentEnter || OnCollisionExit != null || OnCollisionFirstEnter != null || OnCollisionAllExit != null)
{
HashSet<Collider> lastframe = previousCollision;
HashSet<Collider> thisframe = currentCollision;
thisframe.Add(other);
if (!lastframe.Contains(other) && parentEnter)
{
OnCollisionEnter(parent, other.parent);
}
}
}
if (other.HandlersEnabled)
{
if (other.OnCollisionStay != null)
{
other.OnCollisionStay(other.parent, parent);
}
bool otherEnter = other.OnCollisionEnter != null;
if (otherEnter || other.OnCollisionExit != null || other.OnCollisionFirstEnter != null || other.OnCollisionAllExit != null)
{
//HashSet<Node> lastframe = other.collision.currentIsCol1 ? other.collision.collisions1 : other.collision.collisions2;
//HashSet<Node> thisframe = !other.collision.currentIsCol1 ? other.collision.collisions1 : other.collision.collisions2;
HashSet<Collider> lastframe = other.previousCollision;
HashSet<Collider> thisframe = other.currentCollision;
thisframe.Add(this);
if (!lastframe.Contains(this) && otherEnter)
{
other.OnCollisionEnter(other.parent, parent);
}
}
}
if (DoExclusionCheckResolution(other)) return;
if (isSolid && other.isSolid)
room.collisionManager.AddManifold(m);
}
}
//only for links (no handlers)
public override void AffectOther(Node other)
{
if (!active) return;
Manifold m = new Manifold(parent.body, other.body);
m.Solve();
if (m.contact_count > 0)
{
room.collisionManager.AddManifold(m);
}
}
public static bool PolygontoPolygon(Manifold m, Collider a, Collider b)
{
Polygon A = (Polygon)a.shape;
Polygon B = (Polygon)b.shape;
m.contact_count = 0;
// Check for a separating axis with A's face planes
int faceA = 0;
double penetrationA = FindAxisLeastPenetration(ref faceA, A, B);
if (penetrationA >= 0.0f)
return false;
// Check for a separating axis with B's face planes
int faceB = 0;
double penetrationB = FindAxisLeastPenetration(ref faceB, B, A);
if (penetrationB >= 0.0f)
return false;
int referenceIndex;
bool flip; // Always point from a to b
Polygon RefPoly; // Reference
Polygon IncPoly; // Incident
// Determine which shape contains reference face
if (GMath.BiasGreaterThan(penetrationA, penetrationB))
{
RefPoly = A;
IncPoly = B;
referenceIndex = faceA;
flip = false;
}
else
{
RefPoly = B;
IncPoly = A;
referenceIndex = faceB;
flip = true;
}
// World space incident face
Vector2[] incidentFace = new Vector2[2];
FindIncidentFace(ref 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
Vector2 v1 = RefPoly.vertices[referenceIndex];
referenceIndex = referenceIndex + 1 == RefPoly.vertexCount ? 0 : referenceIndex + 1;
Vector2 v2 = RefPoly.vertices[referenceIndex];
// Transform vertices to world space
v1 = RefPoly.u * v1 + RefPoly.body.pos;
v2 = RefPoly.u * v2 + RefPoly.body.pos;
// Calculate reference face side normal in world space
Vector2 sidePlaneNormal = (v2 - v1);
VMath.NormalizeSafe(ref sidePlaneNormal);
// Orthogonalize
Vector2 refFaceNormal = new Vector2(sidePlaneNormal.Y, -sidePlaneNormal.X);
// ax + by = c
// c is distance from origin
double refC = Vector2.Dot(refFaceNormal, v1);
double negSide = -Vector2.Dot(sidePlaneNormal, v1);
double posSide = Vector2.Dot(sidePlaneNormal, v2);
// Clip incident face to reference face side planes
if (Clip(-sidePlaneNormal, negSide, ref incidentFace) < 2)
return false; // Due to floating point error, possible to not have required points
if (Clip(sidePlaneNormal, posSide, ref incidentFace) < 2)
return false; // Due to floating point error, possible to not have required points
// Flip
m.normal = flip ? -refFaceNormal : refFaceNormal;
// Keep points behind reference face
int cp = 0; // clipped points behind reference face
double separation = Vector2.Dot(refFaceNormal, incidentFace[0]) - refC;
if (separation <= 0.0f)
{
m.contacts[cp] = incidentFace[0];
m.penetration = -separation;
++cp;
}
else
m.penetration = 0;
separation = Vector2.Dot(refFaceNormal, incidentFace[1]) - refC;
if (separation <= 0.0f)
{
m.contacts[cp] = incidentFace[1];
m.penetration += -separation;
++cp;
// Average penetration
m.penetration /= (double)cp;
}
m.contact_count = cp;
return cp > 0;
}
public static bool PolygontoCircle(Manifold m, Collider a, Collider b)
{
bool ret = CircletoPolygon(m, b, a);
m.normal = -m.normal;
return ret;
}
public static bool CircletoPolygon(Manifold m, Collider a, Collider b)
{
Circle A = (Circle)a.shape;
Polygon B = (Polygon)b.shape;
m.contact_count = 0;
// Transform circle center to Polygon model space
Vector2 center = a.pos;
center = B.u.Transpose() * (center - b.pos);
// Find edge with minimum penetration
// Exact concept as using support points in Polygon vs Polygon
double separation = -float.MaxValue;
int faceNormal = 0;
for (int i = 0; i < B.vertexCount; ++i)
{
double s = Vector2.Dot(B.normals[i], center - B.vertices[i]);
if (s > A.radius)
{
return false;
}
if (s > separation)
{
separation = s;
faceNormal = i;
}
}
// Grab face's vertices
Vector2 v1 = B.vertices[faceNormal];
int i2 = faceNormal + 1 < B.vertexCount ? faceNormal + 1 : 0;
Vector2 v2 = B.vertices[i2];
// Check to see if center is within polygon
if (separation < GMath.EPSILON)
{
m.contact_count = 1;
m.normal = -(B.u * B.normals[faceNormal]);
m.contacts[0] = VMath.MultVectDouble(m.normal, A.radius) + a.pos;
m.penetration = A.radius;
return true;
}
// Determine which voronoi region of the edge center of circle lies within
double dot1 = Vector2.Dot(center - v1, v2 - v1);
double dot2 = Vector2.Dot(center - v2, v1 - v2);
m.penetration = A.radius - separation;
// Closest to v1
if (dot1 <= 0.0f)
{
if (Vector2.DistanceSquared(center, v1) > A.radius * A.radius)
{
return false;
}
m.contact_count = 1;
Vector2 n = v1 - center;
n = B.u * n;
VMath.NormalizeSafe(ref n);
m.normal = n;
v1 = B.u * v1 + b.pos;
m.contacts[0] = v1;
}
// Closest to v2
else if (dot2 <= 0.0f)
{
if (Vector2.DistanceSquared(center, v2) > A.radius * A.radius)
{
return false;
}
m.contact_count = 1;
Vector2 n = v2 - center;
v2 = B.u * v2 + b.pos;
m.contacts[0] = v2;
n = B.u * n;
VMath.NormalizeSafe(ref n);
m.normal = n;
}
// Closest to face
else
{
Vector2 n = B.normals[faceNormal];
if (Vector2.Dot(center - v1, n) > A.radius)
{
return false;
}
n = B.u * n;
m.normal = -n;
m.contacts[0] = VMath.MultVectDouble(m.normal, A.radius) + a.pos;
m.contact_count = 1;
}
return true;
}
public static bool CircletoCircle(Manifold m, Collider a, Collider b)
{
Circle ca = (Circle)a.shape;
Circle cb = (Circle)b.shape;
Vector2 normal = b.pos - a.pos;
float distSquared = normal.LengthSquared();
double radius = a.radius + b.radius;
if (distSquared >= (float)(radius * radius))
{
m.contact_count = 0;
return false;
}
double distance = Math.Sqrt(distSquared);
m.contact_count = 1;
if (distance == 0)
{
m.penetration = ca.radius;
m.normal = new Vector2(1, 0);
m.contacts[0] = a.pos;
}
else
{
m.penetration = radius - distance;
m.normal = VMath.MultVectDouble(normal, 1.0 / distance); //normal / distance;
m.contacts[0] = VMath.MultVectDouble(m.normal, ca.radius) + a.pos; //m.normal * ca.radius + a.body.position;
}
return true;
}
