public void HandleCollision(Manifold m, TFRigidbody a, TFRigidbody b) { TFEdgeCollider A = (TFEdgeCollider)a.coll; TFCircleCollider B = (TFCircleCollider)b.coll; // No line segments, return out. if (A.vertices.Count < 2) { return; } // Transform circle center to Edge model space FixVec2 circleCenter = A.u.Transposed() * (b.Position - a.Position); // Iterate through all the line segments to find contact point. for (int i = 0; i < A.vertices.Count - 1; i++) { FixVec2 rayDir = (A.vertices[i + 1] - A.vertices[i]); FixVec2 centerRay = (A.vertices[i] - circleCenter); Fix k = rayDir.Dot(rayDir); Fix l = 2 * centerRay.Dot(rayDir); Fix n = centerRay.Dot(centerRay) - (B.radius * B.radius); Fix discriminant = l * l - 4 * k * n; // No intersection. if (discriminant <= Fix.zero) { continue; } discriminant = FixMath.Sqrt(discriminant); Fix t1 = (-l - discriminant) / (2 * k); Fix t2 = (-l + discriminant) / (2 * k); Fix s = FixVec2.Dot(A.normals[i], circleCenter - A.vertices[i]); if (t1 >= Fix.zero && t1 <= Fix.one) { //t1 is the intersection, and it's closer than t2. m.contactCount = 1; m.contacts[0] = (A.u * A.vertices[i] + a.Position) + (t1 * rayDir); m.normal = A.normals[i]; m.penetration = B.radius - s; return; } if (t2 >= Fix.zero && t2 <= Fix.one) { // t1 didn't insection, so we either started inside the circle // or completely past it. m.contactCount = 1; m.contacts[0] = (A.u * A.vertices[i] + a.Position) + (t2 * rayDir); m.normal = A.normals[i]; m.penetration = B.radius - s; return; } } }
private void FindIncidentFace(FixVec2[] v, TFPolygonCollider refPoly, TFPolygonCollider incPoly, int referenceIndex) { FixVec2 referenceNormal = refPoly.normals[referenceIndex]; // Calculate normal in incident's frame of reference referenceNormal = refPoly.u * referenceNormal; // To world space referenceNormal = incPoly.u.Transposed() * referenceNormal; // To incident's model space // Find most anti-normal face on incident polygon int incidentFace = 0; Fix minDot = Fix.MaxValue; for (int i = 0; i < incPoly.VertexCount; ++i) { Fix dot = FixVec2.Dot(referenceNormal, incPoly.normals[i]); if (dot < minDot) { minDot = dot; incidentFace = i; } } // Assign face vertices for incidentFace v[0] = incPoly.u * incPoly.GetVertex(incidentFace) + incPoly.body.info.position; incidentFace = incidentFace + 1 >= (int)incPoly.VertexCount ? 0 : incidentFace + 1; v[1] = incPoly.u * incPoly.GetVertex(incidentFace) + incPoly.body.info.position; }
private int Clip(FixVec2 n, Fix c, FixVec2[] face) { int sp = 0; FixVec2[] o = { face[0], face[1] }; // Retrieve distances from each endpoint to the line Fix d1 = FixVec2.Dot(n, face[0]) - c; Fix d2 = FixVec2.Dot(n, face[1]) - c; // If negative (behind plane) clip if (d1 <= Fix.zero) { o[sp++] = face[0]; } if (d2 <= Fix.zero) { o[sp++] = face[1]; } // If the points are on different sides of the plane if (d1 * d2 < Fix.zero) // less than to ignore -0.0f { // Push interesection point Fix alpha = d1 / (d1 - d2); o[sp] = face[0] + alpha * (face[1] - face[0]); ++sp; } // Assign our new converted values face[0] = o[0]; face[1] = o[1]; return(sp); }
public override bool Raycast(out TFRaycastHit2D hit, FixVec2 pointA, FixVec2 pointB, Fix maxFraction) { hit = default; FixVec2 center = (FixVec2)tdTransform.Position; FixVec2 s = pointA - center; Fix b = FixVec2.Dot(s, s) - radius * radius; // Solve quadratic equation. FixVec2 r = pointB - pointA; Fix c = FixVec2.Dot(s, r); Fix rr = FixVec2.Dot(r, r); Fix sigma = c * c - rr * b; // Check for negative discriminant and short segment. if (sigma < Fix.zero || rr < Fix.Epsilon) { return(false); } // Find the point of intersection on the line with the circle. Fix a = -(c + FixMath.Sqrt(sigma)); // Is the intersection point on the segment? if (Fix.zero <= a && a <= maxFraction * rr) { a /= rr; hit.fraction = a; hit.normal = s + a * r; hit.normal.Normalize(); return(true); } return(false); }
public FixVec2 getSupport(FixVec2 dir) { Fix bestProjection = -Fix.MaxValue; FixVec2 bestVertex = new FixVec2(0, 0); for (int i = 0; i < vertices.Count; ++i) { FixVec2 v = vertices[i]; Fix projection = FixVec2.Dot(v, dir); if (projection > bestProjection) { bestVertex = v; bestProjection = projection; } } return(bestVertex); }
// Calculate the projection of a polygon on an axis and returns it as a [min, max] interval private static void ProjectPolygon(FixVec2 axis, FixPolygon polygon, out Fix min, out Fix max) { // To project a point on an axis use the dot product var d = axis.Dot(polygon.Points[0]); min = d; max = d; for (var i = 0; i < polygon.Points.Length; i++) { d = polygon.Points[i].Dot(axis); if (d < min) { min = d; } else if (d > max) { max = d; } } }
public Fix FindAxisLeastPenetration(int[] faceIndex, TFPolygonCollider A, TFPolygonCollider B) { Fix bestDistance = -Fix.MaxValue; int bestIndex = 0; Mat22 buT; for (int i = 0; i < A.VertexCount; ++i) { // Retrieve a face normal from A FixVec2 nw = A.u * A.normals[i]; // Transform face normal into B's model space buT = B.u; buT.Transpose(); FixVec2 n = buT * nw; // Retrieve support point from B along -n // Vec2 s = B->GetSupport( -n ); FixVec2 s = B.getSupport(-n); // Retrieve vertex on face from A, transform into FixVec2 v = A.GetVertex(i); v = A.u * v + A.body.Position; v -= B.body.info.position; v = buT * v; // Compute penetration distance (in B's model space) Fix d = FixVec2.Dot(n, s - v); // Store greatest distance if (d > bestDistance) { bestDistance = d; bestIndex = i; } } faceIndex[0] = bestIndex; return(bestDistance); }
public void HandleCollision(Manifold m, TFRigidbody a, TFRigidbody b) { TFCircleCollider A = (TFCircleCollider)a.coll; TFPolygonCollider B = (TFPolygonCollider)b.coll; m.contactCount = 0; // Transform circle center to Polygon model space FixVec2 center = B.u.Transposed() * (a.Position - b.Position); // Find edge with minimum penetration // Exact concept as using support points in Polygon vs Polygon Fix separation = -Fix.MaxValue; int faceNormal = 0; for (int i = 0; i < B.VertexCount; ++i) { Fix s = FixVec2.Dot(B.normals[i], center - B.GetVertex(i)); if (s > A.radius) { return; } if (s > separation) { separation = s; faceNormal = i; } } // Grab face's vertices FixVec2 v1 = B.GetVertex(faceNormal); int i2 = (faceNormal + 1) < B.VertexCount ? faceNormal + 1 : 0; FixVec2 v2 = B.GetVertex(i2); // Check to see if center is within polygon if (separation < Fix.Epsilon) { m.contactCount = 1; m.normal = -(B.u * B.normals[faceNormal]); m.contacts[0] = m.normal * A.radius + a.Position; m.penetration = A.radius; return; } // Determine which voronoi region of the edge center of circle lies within Fix dot1 = FixVec2.Dot(center - v1, v2 - v1); Fix dot2 = FixVec2.Dot(center - v2, v1 - v2); m.penetration = A.radius - separation; //Closest to v1 if (dot1 <= Fix.zero) { if ((center - v1).GetMagnitudeSquared() > A.radius * A.radius) { return; } m.contactCount = 1; FixVec2 n = v1 - center; n = B.u * n; n = n.Normalized(); m.normal = n; v1 = B.u * v1 + b.Position; m.contacts[0] = v1; } else if (dot2 <= Fix.zero) { //Closest to v2 if ((center - v2).GetMagnitudeSquared() > A.radius * A.radius) { return; } m.contactCount = 1; FixVec2 n = v2 - center; v2 = B.u * v2 + b.Position; m.contacts[0] = v2; n = B.u * n; n = n.Normalized(); m.normal = n; } else { //Closest to face FixVec2 n = B.normals[faceNormal]; if (FixVec2.Dot(center - v1, n) > A.radius) { return; } n = B.u * n; m.normal = -n; m.contacts[0] = m.normal * A.radius + a.Position; m.contactCount = 1; } }
internal TFRaycastHit2D Raycast(ITreeRaycastCallback callback, FixVec2 pointA, FixVec2 pointB, TFLayerMask mask) { TFRaycastHit2D hit = new TFRaycastHit2D(); FixVec2 r = pointB - pointA; if (r.GetMagnitudeSquared() <= Fix.zero) { return(hit); } r.Normalize(); // v is perpendicular to the segment. FixVec2 v = FixVec2.Cross(Fix.one, r); FixVec2 abs_v = FixVec2.Abs(v); // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) Fix maxFraction = Fix.one; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); FixVec2 t = pointA + maxFraction * (pointB - pointA); segmentAABB.min = FixVec2.Min(pointA, t); segmentAABB.max = FixVec2.Max(pointA, t); Stack <int> stack = new Stack <int>(); stack.Push(rootIndex); List <TFRaycastOutput> hitNodes = new List <TFRaycastOutput>(2); while (stack.Count > 0) { var nodeId = stack.Pop(); if (nodeId == nullNode) { continue; } var node = nodes[nodeId]; if (!node.aabb.Overlaps(segmentAABB)) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) var c = node.aabb.GetCenter(); var h = node.aabb.GetExtents(); var separation = FixMath.Abs(FixVec2.Dot(v, pointA - c)) - FixVec2.Dot(abs_v, h); if (separation > Fix.zero) { continue; } if (node.IsLeaf()) { // If value is >= 0, then we hit the node. TFRaycastHit2D rHit; Fix value = callback.RayCastCallback(pointA, pointB, maxFraction, nodeId, out rHit, mask); if (value == Fix.zero) { // The client has terminated the ray cast. if (rHit) { // We actually hit the node, add it to the list. hitNodes.Add(new TFRaycastOutput(nodeId, rHit)); } break; } if (value == maxFraction) { if (rHit) { // We actually hit the node, add it to the list. hitNodes.Add(new TFRaycastOutput(nodeId, rHit)); } } else if (value > Fix.zero) { if (rHit) { // We actually hit the node, add it to the list. hitNodes.Add(new TFRaycastOutput(nodeId, rHit)); } // Update segment bounding box. maxFraction = value; FixVec2 g = pointA + maxFraction * (pointB - pointA); segmentAABB.min = FixVec2.Min(pointA, g); segmentAABB.max = FixVec2.Max(pointA, g); } } else { stack.Push(node.leftChildIndex); stack.Push(node.rightChildIndex); } } // Decide which node was the closest to the starting point. Fix closestNode = maxFraction; for (int i = 0; i < hitNodes.Count; i++) { if (hitNodes[i].hit.fraction < closestNode) { closestNode = hitNodes[i].hit.fraction; hit = hitNodes[i].hit; } } return(hit); }
public void HandleCollision(Manifold m, TFRigidbody a, TFRigidbody b) { TFPolygonCollider A = (TFPolygonCollider)a.coll; TFPolygonCollider B = (TFPolygonCollider)b.coll; m.contactCount = 0; // Check for a separating axis with A's face planes int[] faceA = { 0 }; Fix penetrationA = FindAxisLeastPenetration(faceA, A, B); if (penetrationA >= Fix.zero) { return; } int[] faceB = { 0 }; Fix penetrationB = FindAxisLeastPenetration(faceB, B, A); if (penetrationB >= Fix.zero) { return; } int referenceIndex; bool flip; //Always point from a to b TFPolygonCollider refPoly; //Reference TFPolygonCollider incPoly; //Incident //Determine which shape contains reference face if (TFPhysics.instance.BiasGreaterThan(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 FixVec2[] incidentFace = new FixVec2[2]; FindIncidentFace(incidentFace, refPoly, incPoly, referenceIndex); // Setup reference face certices FixVec2 v1 = refPoly.GetVertex(referenceIndex); referenceIndex = referenceIndex + 1 == refPoly.VertexCount ? 0 : referenceIndex + 1; FixVec2 v2 = refPoly.GetVertex(referenceIndex); // Transform vertices to world space v1 = refPoly.u * v1 + refPoly.body.info.position; v2 = refPoly.u * v2 + refPoly.body.info.position; //Calculate reference face side normal in world space FixVec2 sidePlaneNormal = v2 - v1; sidePlaneNormal = sidePlaneNormal.Normalized(); // Orthogonalize FixVec2 refFaceNormal = new FixVec2(sidePlaneNormal.Y, -sidePlaneNormal.X); // ax + by = c // c is distance from origin Fix refC = FixVec2.Dot(refFaceNormal, v1); Fix negSide = -FixVec2.Dot(sidePlaneNormal, v1); Fix posSide = FixVec2.Dot(sidePlaneNormal, v2); // Clip incident face to reference face side planes if (Clip(-sidePlaneNormal, negSide, incidentFace) < 2) { return; // Due to floating point error, possible to not have required points } if (Clip(sidePlaneNormal, posSide, incidentFace) < 2) { return; } // Flip m.normal = flip ? -refFaceNormal : refFaceNormal; // Keep points behind reference face int cp = 0; // clipped points behind reference face Fix separation = FixVec2.Dot(refFaceNormal, incidentFace[0]) - refC; if (separation <= Fix.zero) { m.contacts[cp] = incidentFace[0]; m.penetration = -separation; ++cp; } else { m.penetration = 0; } separation = FixVec2.Dot(refFaceNormal, incidentFace[1]) - refC; if (separation <= Fix.zero) { m.contacts[cp] = incidentFace[1]; m.penetration += -separation; ++cp; // Average penetration m.penetration /= cp; } m.contactCount = cp; }
internal void Raycast(ITreeRaycastCallback callback, FixVec2 pointA, FixVec2 pointB) { FixVec2 r = pointB - pointA; if (r.GetMagnitudeSquared() <= Fix.zero) { return; } r.Normalize(); // v is perpendicular to the segment. FixVec2 v = FixVec2.Cross(Fix.one, r); FixVec2 abs_v = FixVec2.Abs(v); // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) Fix maxFraction = Fix.one; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); FixVec2 t = pointA + maxFraction * (pointB - pointA); segmentAABB.min = FixVec2.Min(pointA, t); segmentAABB.max = FixVec2.Max(pointA, t); Stack <int> stack = new Stack <int>(); stack.Push(rootIndex); while (stack.Count > 0) { var nodeId = stack.Pop(); if (nodeId == nullNode) { continue; } var node = nodes[nodeId]; if (!node.aabb.Overlaps(segmentAABB)) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) var c = node.aabb.GetCenter(); var h = node.aabb.GetExtents(); var separation = FixMath.Abs(FixVec2.Dot(v, pointA - c)) - FixVec2.Dot(abs_v, h); if (separation > Fix.zero) { continue; } if (node.IsLeaf()) { Fix value = callback.RayCastCallback(pointA, pointB, maxFraction, nodeId); if (value == Fix.zero) { // The client has terminated the ray cast. return; } if (value > Fix.zero) { // Update segment bounding box. maxFraction = value; FixVec2 g = pointA + maxFraction * (pointB - pointA); segmentAABB.min = FixVec2.Min(pointA, g); segmentAABB.max = FixVec2.Max(pointA, g); } } else { stack.Push(node.leftChildIndex); stack.Push(node.rightChildIndex); } } }
public void ApplyImpulse() { // Early out and positional correct if both objects have infinite mass if (A.invMass + B.invMass == 0) { InfiniteMassCorrection(); return; } for (int i = 0; i < contactCount; ++i) { // Calculate radii from COM to contact FixVec2 ra = contacts[i] - A.Position; FixVec2 rb = contacts[i] - B.Position; //Relative velocity FixVec2 rv = B.info.velocity + FixVec2.Cross(B.info.angularVelocity, rb) - A.info.velocity - FixVec2.Cross(A.info.angularVelocity, ra); //Relative velocity along the normal Fix contactVel = rv.Dot(normal); if (contactVel > 0) { return; } Fix raCrossN = FixVec2.Cross(ra, normal); Fix rbCrossN = FixVec2.Cross(rb, normal); Fix invMassSum = A.invMass + B.invMass + (raCrossN * raCrossN) * A.invInertia + (rbCrossN * rbCrossN) * B.invInertia; // Calculate impulse scalar Fix j = -(Fix.one + e) * contactVel; j /= invMassSum; j /= contactCount; // Apply impulse FixVec2 impulse = normal * j; A.ApplyImpulse(-impulse, ra); B.ApplyImpulse(impulse, rb); // Friction Impulse rv = B.info.velocity + FixVec2.Cross(B.info.angularVelocity, rb) - A.info.velocity - FixVec2.Cross(A.info.angularVelocity, ra); FixVec2 t = rv - (normal * FixVec2.Dot(rv, normal)); t = t.Normalized(); // j tangent magnitude Fix jt = -FixVec2.Dot(rv, t); jt /= invMassSum; jt /= contactCount; //Don't apply tiny friction impulses if (FixMath.Abs(jt) <= Fix.zero) { return; } // Coulumb's law FixVec2 tangentImpulse; if (FixMath.Abs(jt) < j * sf) { tangentImpulse = t * jt; } else { tangentImpulse = t * -j * df; } // Apply friction impulse A.ApplyImpulse(-tangentImpulse, ra); B.ApplyImpulse(tangentImpulse, rb); } }
public override bool Raycast(out TFRaycastHit2D hit, FixVec2 pointA, FixVec2 pointB, Fix maxFraction) { hit = new TFRaycastHit2D(); // Put the ray into the polygon's frame of reference. var p1 = u.Transposed() * (pointA - (FixVec2)tdTransform.Position); var p2 = u.Transposed() * (pointB - (FixVec2)tdTransform.Position); var d = p2 - p1; Fix lower = Fix.zero, upper = maxFraction; var index = -1; for (var i = 0; i < vertices.Count; ++i) { // p = p1 + a * d // dot(normal, p - v) = 0 // dot(normal, p1 - v) + a * dot(normal, d) = 0 var numerator = FixVec2.Dot(normals[i], GetVertex(i) - p1); var denominator = FixVec2.Dot(normals[i], d); if (denominator == Fix.zero) { if (numerator < Fix.zero) { return(false); } } else { // Note: we want this predicate without division: // lower < numerator / denominator, where denominator < 0 // Since denominator < 0, we have to flip the inequality: // lower < numerator / denominator <==> denominator * lower > numerator. if (denominator < Fix.zero && numerator < lower * denominator) { // Increase lower. // The segment enters this half-space. lower = numerator / denominator; index = i; } else if (denominator > Fix.zero && numerator < upper * denominator) { // Decrease upper. // The segment exits this half-space. upper = numerator / denominator; } } // The use of epsilon here causes the assert on lower to trip // in some cases. Apparently the use of epsilon was to make edge // shapes work, but now those are handled separately. //if (upper < lower - b2_epsilon) if (upper < lower) { return(false); } } if (index >= 0) { hit.fraction = lower; hit.normal = tdTransform.Rotation * normals[index]; hit.collider = this; return(true); } return(false); }
// Check if polygon A is going to collide with polygon B for the given velocity public static PolygonCollisionResult CheckCollision(FixPolygon polygonA, FixPolygon polygonB, FixVec2 velocity) { var result = new PolygonCollisionResult { Intersect = true, WillIntersect = true }; var edgeCountA = polygonA.Edges.Length; var edgeCountB = polygonB.Edges.Length; var minIntervalDistance = Fix.MaxValue; var translationAxis = new FixVec2(); // Loop through all the edges of both polygons for (var edgeIndex = 0; edgeIndex < edgeCountA + edgeCountB; edgeIndex++) { var edge = edgeIndex < edgeCountA ? polygonA.Edges[edgeIndex] : polygonB.Edges[edgeIndex - edgeCountA]; // ===== 1. Find if the polygons are currently intersecting ===== // Find the axis perpendicular to the current edge var axis = new FixVec2(-edge.Y, edge.X).Normalize(); // Find the projection of the polygon on the current axis Fix minA; Fix maxA; Fix minB; Fix maxB; ProjectPolygon(axis, polygonA, out minA, out maxA); ProjectPolygon(axis, polygonB, out minB, out maxB); // Check if the polygon projections are currently intersecting if (IntervalDistance(minA, maxA, minB, maxB) > 0) { result.Intersect = false; } // ===== 2. Now find if the polygons *will* intersect ===== // Project the velocity on the current axis var velocityProjection = axis.Dot(velocity); // Get the projection of polygon A during the movement if (velocityProjection < 0) { minA += velocityProjection; } else { maxA += velocityProjection; } // Do the same test as above for the new projection var intervalDistance = IntervalDistance(minA, maxA, minB, maxB); if (intervalDistance > 0) { result.WillIntersect = false; } // If the polygons are not intersecting and won't intersect, exit the loop if (!result.Intersect && !result.WillIntersect) { break; } // Check if the current interval distance is the minimum one. If so store // the interval distance and the current distance. // This will be used to calculate the minimum translation FixVec2 intervalDistance = FixMath.Abs(intervalDistance); if (intervalDistance < minIntervalDistance) { minIntervalDistance = intervalDistance; translationAxis = axis; var d = polygonA.Center - polygonB.Center; if (d.Dot(translationAxis) < 0) { translationAxis = -translationAxis; } } } // The minimum translation FixVec2 can be used to push the polygons apart. // First moves the polygons by their velocity // then move polygonA by MinimumTranslationVector. if (result.WillIntersect) { result.MinimumTranslationVector = translationAxis * minIntervalDistance; } return(result); }