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); }