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; } }
/// <summary> /// handles adjusting deltaMovement if we are going up a slope. /// </summary> /// <returns><c>true</c>, if horizontal slope was handled, <c>false</c> otherwise.</returns> /// <param name="deltaMovement">Delta movement.</param> /// <param name="angle">Angle.</param> bool handleHorizontalSlope(ref FixVec2 deltaMovement, Fix angle) { // disregard 90 degree angles (walls) if (FixMath.Round(angle) == 90) { return(false); } if (angle < slopeLimit) { // we only need to adjust the deltaMovement if we are not jumping // TODO: this uses a magic number which isn't ideal! The alternative is to have the user pass in if there is a jump this frame if (deltaMovement.y < jumpingThreshold) { // apply the slopeModifier to slow our movement up the slope Fix slopeModifier = (Fix)slopeSpeedMultiplier.Evaluate((float)angle); deltaMovement.x *= slopeModifier; // we dont set collisions on the sides for this since a slope is not technically a side collision. // smooth y movement when we climb. we make the y movement equivalent to the actual y location that corresponds // to our new x location using our good friend Pythagoras deltaMovement.y = FixMath.Abs(FixMath.Tan(angle * FixMath.Deg2Rad) * deltaMovement.x); var isGoingRight = deltaMovement.x > 0; // safety check. we fire a ray in the direction of movement just in case the diagonal we calculated above ends up // going through a wall. if the ray hits, we back off the horizontal movement to stay in bounds. var ray = isGoingRight ? _raycastOrigins.bottomRight : _raycastOrigins.bottomLeft; TFRaycastHit2D raycastHit; if (collisionState.wasGroundedLastFrame) { raycastHit = TFPhysics.Raycast((FixVec2)ray, (FixVec2)deltaMovement.Normalized(), deltaMovement.GetMagnitude(), platformMask); } else { raycastHit = TFPhysics.Raycast((FixVec2)ray, (FixVec2)deltaMovement.Normalized(), deltaMovement.GetMagnitude(), platformMask & ~oneWayPlatformMask); } if (raycastHit) { // we crossed an edge when using Pythagoras calculation, so we set the actual delta movement to the ray hit location deltaMovement = raycastHit.point - ray; if (isGoingRight) { deltaMovement.x -= _skinWidth; } else { deltaMovement.x += _skinWidth; } } _isGoingUpSlope = true; collisionState.below = true; collisionState.slopeAngle = -angle; } } else // too steep. get out of here { deltaMovement.x = Fix.zero; } return(true); }
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; }
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); } }