public static void InfoPushContact(ref cpCollisionInfo info, cpVect p1, cpVect p2, ulong hash)
{
cp.AssertSoft(info.count <= cpArbiter.CP_MAX_CONTACTS_PER_ARBITER, "Internal error: Tried to push too many contacts.");
info.arr.Add(new cpContact(p1, p2, hash));
//info.arr[info.count] = ;
info.count++; // = count++;
}
/// Test if a point lies within a shape.
public static cpContactPointSet Collide(cpShape a, cpShape b, ref List <cpContact> contacts)
{
//cpContact[] contacts = new cpContact[cpArbiter.CP_MAX_CONTACTS_PER_ARBITER];
cpCollisionInfo info = cpCollision.cpCollide(a, b, 0, ref contacts);
cpContactPointSet set = new cpContactPointSet();
set.count = info.count;
set.points = new PointsDistance[set.count];
// cpCollideShapes() may have swapped the contact order. Flip the normal.
bool swapped = (a != info.a);
set.normal = (swapped ? cpVect.cpvneg(info.n) : info.n);
for (int i = 0; i < info.count; i++)
{
cpVect p1 = contacts[i].r1;
cpVect p2 = contacts[i].r2;
set.points[i] = new PointsDistance();
set.points[i].pointA = (swapped ? p2 : p1);
set.points[i].pointB = (swapped ? p1 : p2);
set.points[i].distance = cpVect.cpvdot(cpVect.cpvsub(p2, p1), set.normal);
}
return(set);
}
//MARK: Collision Functions
// Collide circle shapes.
public static void CircleToCircle(cpShape cir1, cpShape cir2, ref cpCollisionInfo info)
{
cpCircleShape c1 = (cpCircleShape)cir1;
cpCircleShape c2 = (cpCircleShape)cir2;
float mindist = c1.r + c2.r;
cpVect delta = cpVect.cpvsub(c2.tc, c1.tc);
float distsq = cpVect.cpvlengthsq(delta);
if (distsq < mindist * mindist)
{
float dist = cp.cpfsqrt(distsq);
cpVect n = info.n = (dist > 0.0f ? cpVect.cpvmult(delta, 1.0f / dist) : cpVect.cpv(1.0f, 0.0f));
InfoPushContact(ref info, cpVect.cpvadd(c1.tc, cpVect.cpvmult(n, c1.r)), cpVect.cpvadd(c2.tc, cpVect.cpvmult(n, -c2.r)), 0);
}
}
public void Update(cpCollisionInfo info, cpSpace space)
{
cpShape a = info.a, b = info.b;
// For collisions between two similar primitive types, the order could have been swapped since the last frame.
this.a = a; this.body_a = a.body;
this.b = b; this.body_b = b.body;
// Iterate over the possible pairs to look for hash value matches.
for (int i = 0; i < info.count; i++)
{
cpContact con = info.arr[i];
// r1 and r2 store absolute offsets at init time.
// Need to convert them to relative offsets.
con.r1 = cpVect.cpvsub(con.r1, a.body.p);
con.r2 = cpVect.cpvsub(con.r2, b.body.p);
// Cached impulses are not zeroed at init time.
con.jnAcc = con.jtAcc = 0.0f;
for (int j = 0; j < this.Count; j++)
{
cpContact old = this.contacts[j];
// This could trigger false positives, but is fairly unlikely nor serious if it does.
if (con.hash == old.hash)
{
// Copy the persistant contact information.
con.jnAcc = old.jnAcc;
con.jtAcc = old.jtAcc;
}
}
}
//TODO: revise
this.contacts = info.arr.ToList();
//this.count = info.count;
this.n = info.n;
this.e = a.e * b.e;
this.u = a.u * b.u;
cpVect surface_vr = cpVect.cpvsub(b.surfaceV, a.surfaceV);
this.surface_vr = cpVect.cpvsub(surface_vr, cpVect.cpvmult(info.n, cpVect.cpvdot(surface_vr, info.n)));
ulong typeA = info.a.type, typeB = info.b.type;
cpCollisionHandler defaultHandler = space.defaultHandler;
cpCollisionHandler handler = this.handler = space.LookupHandler(typeA, typeB, defaultHandler);
// Check if the types match, but don't swap for a default handler which use the wildcard for type A.
bool swapped = this.swapped = (typeA != handler.typeA && handler.typeA != cp.WILDCARD_COLLISION_TYPE);
if (handler != defaultHandler || space.usesWildcards)
{
// The order of the main handler swaps the wildcard handlers too. Uffda.
this.handlerA = space.LookupHandler(swapped ? typeB : typeA, cp.WILDCARD_COLLISION_TYPE, cpCollisionHandler.cpCollisionHandlerDoNothing);
this.handlerB = space.LookupHandler(swapped ? typeA : typeB, cp.WILDCARD_COLLISION_TYPE, cpCollisionHandler.cpCollisionHandlerDoNothing);
}
// mark it as new if it's been cached
if (this.state == cpArbiterState.Cached)
{
this.state = cpArbiterState.FirstCollision;
}
}
public void Update(cpCollisionInfo info, cpSpace space)
{
cpShape a = info.a, b = info.b;
// For collisions between two similar primitive types, the order could have been swapped since the last frame.
this.a = a; this.body_a = a.body;
this.b = b; this.body_b = b.body;
// Iterate over the possible pairs to look for hash value matches.
for (int i = 0; i < info.count; i++)
{
cpContact con = info.arr[i];
// r1 and r2 store absolute offsets at init time.
// Need to convert them to relative offsets.
con.r1 = cpVect.cpvsub(con.r1, a.body.p);
con.r2 = cpVect.cpvsub(con.r2, b.body.p);
// Cached impulses are not zeroed at init time.
con.jnAcc = con.jtAcc = 0.0f;
for (int j = 0; j < this.Count; j++)
{
cpContact old = this.contacts[j];
// This could trigger false positives, but is fairly unlikely nor serious if it does.
if (con.hash == old.hash)
{
// Copy the persistant contact information.
con.jnAcc = old.jnAcc;
con.jtAcc = old.jtAcc;
}
}
}
//TODO: revise
this.contacts = info.arr.ToList();
//this.count = info.count;
this.n = info.n;
this.e = a.e * b.e;
this.u = a.u * b.u;
cpVect surface_vr = cpVect.cpvsub(b.surfaceV, a.surfaceV);
this.surface_vr = cpVect.cpvsub(surface_vr, cpVect.cpvmult(info.n, cpVect.cpvdot(surface_vr, info.n)));
ulong typeA = info.a.type, typeB = info.b.type;
cpCollisionHandler defaultHandler = space.defaultHandler;
cpCollisionHandler handler = this.handler = space.LookupHandler(typeA, typeB, defaultHandler);
// Check if the types match, but don't swap for a default handler which use the wildcard for type A.
bool swapped = this.swapped = (typeA != handler.typeA && handler.typeA != cp.WILDCARD_COLLISION_TYPE);
if (handler != defaultHandler || space.usesWildcards)
{
// The order of the main handler swaps the wildcard handlers too. Uffda.
this.handlerA = space.LookupHandler(swapped ? typeB : typeA, cp.WILDCARD_COLLISION_TYPE, cpCollisionHandler.cpCollisionHandlerDoNothing);
this.handlerB = space.LookupHandler(swapped ? typeA : typeB, cp.WILDCARD_COLLISION_TYPE, cpCollisionHandler.cpCollisionHandlerDoNothing);
}
// mark it as new if it's been cached
if (this.state == cpArbiterState.Cached)
this.state = cpArbiterState.FirstCollision;
}
public static cpCollisionInfo cpCollide(cpShape a, cpShape b, ulong id, ref List<cpContact> contacts)
{
cpCollisionInfo info = new cpCollisionInfo(a, b, id, cpVect.Zero, contacts);
// Make sure the shape types are in order.
if ((int)a.shapeType > (int)b.shapeType)
{
info.a = b;
info.b = a;
}
int idSelected = (int)info.a.shapeType + (int)info.b.shapeType * (int)cpShapeType.NumShapes;
CollisionFuncs[idSelected](info.a, info.b, ref info);
return info;
}
//MARK: Contact Clipping
public static void ContactPoints(Edge e1, Edge e2, ClosestPoints points, ref cpCollisionInfo info)
{
float mindist = e1.r + e2.r;
if (points.d <= mindist)
{
info.n = new cpVect(points.n);
cpVect n = new cpVect(points.n);
// Distances along the axis parallel to n
float d_e1_a = cpVect.cpvcross(e1.a.p, n);
float d_e1_b = cpVect.cpvcross(e1.b.p, n);
float d_e2_a = cpVect.cpvcross(e2.a.p, n);
float d_e2_b = cpVect.cpvcross(e2.b.p, n);
float e1_denom = 1.0f / (d_e1_b - d_e1_a);
float e2_denom = 1.0f / (d_e2_b - d_e2_a);
// Project the endpoints of the two edges onto the opposing edge, clamping them as necessary.
// Compare the projected points to the collision normal to see if the shapes overlap there.
{
cpVect p1 = cpVect.cpvadd(cpVect.cpvmult(n, e1.r), cpVect.cpvlerp(e1.a.p, e1.b.p, cp.cpfclamp01((d_e2_b - d_e1_a) * e1_denom)));
cpVect p2 = cpVect.cpvadd(cpVect.cpvmult(n, -e2.r), cpVect.cpvlerp(e2.a.p, e2.b.p, cp.cpfclamp01((d_e1_a - d_e2_a) * e2_denom)));
float dist = cpVect.cpvdot(cpVect.cpvsub(p2, p1), n);
if (dist <= 0.0f)
{
ulong hash_1a2b = cp.CP_HASH_PAIR(e1.a.hash, e2.b.hash);
InfoPushContact(ref info, p1, p2, hash_1a2b);
}
}
{
cpVect p1 = cpVect.cpvadd(cpVect.cpvmult(n, e1.r), cpVect.cpvlerp(e1.a.p, e1.b.p, cp.cpfclamp01((d_e2_a - d_e1_a) * e1_denom)));
cpVect p2 = cpVect.cpvadd(cpVect.cpvmult(n, -e2.r), cpVect.cpvlerp(e2.a.p, e2.b.p, cp.cpfclamp01((d_e1_b - d_e2_a) * e2_denom)));
float dist = cpVect.cpvdot(cpVect.cpvsub(p2, p1), n);
if (dist <= 0.0f)
{
ulong hash_1b2a = cp.CP_HASH_PAIR(e1.b.hash, e2.a.hash);
InfoPushContact(ref info, p1, p2, hash_1b2a);
}
}
}
}
public static void CollisionError(cpShape circle, cpShape poly, ref cpCollisionInfo info)
{
cp.AssertHard(false, "Internal Error: Shape types are not sorted.");
}
public static void CircleToSegment(cpShape circle1, cpShape segment2, ref cpCollisionInfo info)
{
cpCircleShape circle = (cpCircleShape)circle1;
cpSegmentShape segment = (cpSegmentShape)segment2;
cpVect seg_a = segment.ta;
cpVect seg_b = segment.tb;
cpVect center = circle.tc;
// Find the closest point on the segment to the circle.
cpVect seg_delta = cpVect.cpvsub(seg_b, seg_a);
float closest_t = cp.cpfclamp01(cpVect.cpvdot(seg_delta, cpVect.cpvsub(center, seg_a)) / cpVect.cpvlengthsq(seg_delta));
cpVect closest = cpVect.cpvadd(seg_a, cpVect.cpvmult(seg_delta, closest_t));
// Compare the radii of the two shapes to see if they are colliding.
float mindist = circle.r + segment.r;
cpVect delta = cpVect.cpvsub(closest, center);
float distsq = cpVect.cpvlengthsq(delta);
if (distsq < mindist * mindist)
{
float dist = cp.cpfsqrt(distsq);
// Handle coincident shapes as gracefully as possible.
cpVect n = info.n = (dist > 0 ? cpVect.cpvmult(delta, 1.0f / dist) : segment.tn);
// Reject endcap collisions if tangents are provided.
cpVect rot = segment.body.GetRotation();
if (
(closest_t != 0.0f || cpVect.cpvdot(n, cpVect.cpvrotate(segment.a_tangent, rot)) >= 0.0) &&
(closest_t != 1.0f || cpVect.cpvdot(n, cpVect.cpvrotate(segment.b_tangent, rot)) >= 0.0)
)
{
InfoPushContact(ref info, cpVect.cpvadd(center, cpVect.cpvmult(n, circle.r)), cpVect.cpvadd(closest, cpVect.cpvmult(n, -segment.r)), 0);
}
}
}
// This one is less gross, but still gross.
public static void CircleToPoly(cpShape circle1, cpShape poly2, ref cpCollisionInfo info)
{
cpCircleShape circle = (cpCircleShape)circle1;
cpPolyShape poly = (cpPolyShape)poly2;
SupportContext context = new SupportContext(
circle,
poly,
(s, o) => SupportPoint.CircleSupportPoint(s as cpCircleShape, o),
(s, o) => SupportPoint.PolySupportPoint(s as cpPolyShape, o));
ClosestPoints points = GJK(ref context, ref info.id);
// If the closest points are nearer than the sum of the radii...
if (points.d <= circle.r + poly.r)
{
cpVect n = info.n = points.n;
InfoPushContact(ref info, cpVect.cpvadd(points.a, cpVect.cpvmult(n, circle.r)), cpVect.cpvadd(points.b, cpVect.cpvmult(n, poly.r)), 0);
}
}
public ulong CollideShapes(cpShape a, cpShape b, ulong id)
{
// It would be nicer to use .bind() or something, but this is faster.
//return new Action<object, object>((obj1, obj2) =>
//{// Reject any of the simple cases
if (QueryReject(a, b))
{
return(id);
}
//contactsBuffer.Clear();
List <cpContact> contacts = new List <cpContact>();
// Narrow-phase collision detection.
//int numContacts = cpCollideShapes(a, b, contacts);
cpCollisionInfo info = cpCollision.cpCollide(a, b, id, ref contacts);
if (info.count == 0)
{
return(info.id); // Shapes are not colliding.
}
// Get an arbiter from space.arbiterSet for the two shapes.
// This is where the persistant contact magic comes from.
var arbHash = cp.CP_HASH_PAIR(info.a.hashid, info.b.hashid);
cpArbiter arb;
if (!cachedArbiters.TryGetValue(arbHash, out arb))
{
arb = new cpArbiter(a, b);
cachedArbiters.Add(arbHash, arb);
}
arb.Update(info, this);
cpCollisionHandler handler = arb.handler; //LookupHandler(a.type, b.type, defaultHandler);
// Call the begin function first if it's the first step
if (arb.state == cpArbiterState.FirstCollision && !handler.beginFunc(arb, this, null))
{
arb.Ignore(); // permanently ignore the collision until separation
}
if (
// Ignore the arbiter if it has been flagged
(arb.state != cpArbiterState.Ignore) &&
// Call preSolve
handler.preSolveFunc(arb, this, handler.userData) &&
!(a.sensor || b.sensor) &&
// Process, but don't add collisions for sensors.
!(a.body.m == cp.Infinity && b.body.m == cp.Infinity)
)
{
this.arbiters.Add(arb);
}
else
{
//cpSpacePopContacts(space, numContacts);
arb.contacts.Clear();
// Normally arbiters are set as used after calling the post-solve callback.
// However, post-solve callbacks are not called for sensors or arbiters rejected from pre-solve.
if (arb.state != cpArbiterState.Ignore)
{
arb.state = cpArbiterState.Normal;
}
}
// Time stamp the arbiter so we know it was used recently.
arb.stamp = this.stamp;
// });
return(info.id);
}
