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; }
void Awake() { // add our one-way platforms to our normal platform mask so that we can land on them from above platformMask |= oneWayPlatformMask; // cache some components tfTransform = GetComponent <TFTransform>(); boxCollider = GetComponent <TFPolygonCollider>(); rigidBody2D = GetComponent <TFRigidbody>(); if (rigidBody2D) { rigidBody2D.OnTriggerEnter += TFOnTriggerEnter; rigidBody2D.OnTriggerStay += TFOnTriggerStay; rigidBody2D.OnTriggerExit += TFOnTriggerExit; } // here, we trigger our properties that have setters with bodies skinWidth = _skinWidth; // we want to set our CC2D to ignore all collision layers except what is in our triggerMask for (var i = 0; i < 32; i++) { // see if our triggerMask contains this layer and if not ignore it if ((triggerMask.mask & 1 << i) == 0) { Physics2D.IgnoreLayerCollision(gameObject.layer, i); } } }
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; } }
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; }