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