public Fix RayCastCallback(FixVec2 pointA, FixVec2 pointB, Fix maxFraction, int proxyID, out TFRaycastHit2D hit, TFLayerMask mask)
{
TFRigidbody rigid = bodies[dynamicTree.nodes[proxyID].bodyIndex];
if (!(mask == (mask | (1 << rigid.layer))))
{
// This object is not in the mask, ignore it.
hit = new TFRaycastHit2D();
return(maxFraction);
}
rigid.coll.Raycast(out hit, pointA, pointB, maxFraction);
if (!hit)
{
// We did not hit the body, ignore it and use our max ray length.
return(maxFraction);
}
Fix fraction = hit.fraction;
FixVec2 point = (Fix.one - fraction) * pointA + fraction * pointB;
hit.point = point;
hit.distance = (point - pointA).GetMagnitude();
return(RayCastCallback(rigid, hit.point, hit.normal, hit.fraction));
}
public void HandleCollision(Manifold m, TFRigidbody a, TFRigidbody b)
{
TFCircleCollider A = (TFCircleCollider)a.coll;
TFCircleCollider B = (TFCircleCollider)b.coll;
//Calculate translational vector, which is normal
FixVec2 normal = ((FixVec2)b.Position) - ((FixVec2)a.Position);
Fix distSpr = normal.GetMagnitudeSquared();
Fix radius = A.radius + B.radius;
//Not in contact
if (distSpr >= radius * radius)
{
m.contactCount = 0;
return;
}
Fix distance = FixMath.Sqrt(distSpr);
m.contactCount = 1;
if (distance == Fix.zero)
{
m.penetration = A.radius;
m.normal = new FixVec2(1, 0);
m.contacts[0] = (FixVec2)a.Position;
}
else
{
m.penetration = radius - distance;
m.normal = normal / distance;
m.contacts[0] = m.normal * A.radius + ((FixVec2)a.Position);
}
}
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;
}
}
}
public TFCollision(TFCollider collider)
{
this.collider = collider;
gameObject = collider.gameObject;
rigidbody = collider.body;
transform = collider.tdTransform;
}
public void HandleCollision(Manifold m, TFRigidbody a, TFRigidbody b)
{
ContactCirclePolygon.instance.HandleCollision(m, b, a);
if (m.contactCount > 0)
{
m.normal *= -Fix.one;
}
}
public void RemoveBody(TFRigidbody body)
{
int index = bodies.IndexOf(body);
if (index > -1)
{
bodies.RemoveAt(index);
dynamicTree.DestroyProxy(body.ProxyID);
}
}
private void IntegrateForces(TFRigidbody b, Fix dt)
{
// If the body is static or kinematic, don't apply any forces.
if (b.bodyType == TFBodyType.Static ||
b.bodyType == TFBodyType.Kinematic)
{
return;
}
b.info.velocity += ((b.info.force * b.invMass) + (TFPhysics.instance.settings.gravity * b.gravityScale)) * (dt / 2);
b.info.angularVelocity += b.info.torque * b.invInertia * (dt / 2);
}
public Fix RayCastCallback(FixVec2 pointA, FixVec2 pointB, Fix maxFraction, int proxyID)
{
bool hit = false;
TFRigidbody rigid = bodies[dynamicTree.nodes[proxyID].bodyIndex];
TFRaycastHit2D rHit;
hit = rigid.coll.Raycast(out rHit, pointA, pointB, maxFraction);
if (!hit)
{
// We did not hit the body, ignore it and use our max ray length.
return(maxFraction);
}
Fix fraction = rHit.fraction;
FixVec2 point = (Fix.one - fraction) * pointA + fraction * pointB;
return(Fix.zero);
}
private void IntegrateVelocity(TFRigidbody b, Fix dt)
{
// If the body is static, don't move it.
if (b.bodyType == TFBodyType.Static)
{
return;
}
AABB oldAABB = b.bounds;
FixVec2 offset = b.info.velocity * dt;
b.Position += offset;
b.info.rotation += b.info.angularVelocity * dt;
b.SetRotation(b.info.rotation);
IntegrateForces(b, dt);
// Update the dynamic tree (for Broad Phase).
AABB sweptAABB = AABB.Union(b.bounds, oldAABB);
MoveProxy(b.ProxyID, sweptAABB, offset);
}
private Fix RayCastCallback(TFRigidbody rigidbody, FixVec2 point, FixVec2 normal, Fix fraction)
{
return(1);
}
public void AddBody(TFRigidbody body, Fix aabbFattening)
{
bodies.Add(body);
body.ProxyID = dynamicTree.CreateProxy(body.bounds, bodies.Count - 1, aabbFattening);
MoveProxy(body.ProxyID);
}
/// <summary>
/// Removes a body from the simulation.
/// </summary>
/// <param name="body">The rigidbody to remove.</param>
public static void RemoveBody(TFRigidbody body)
{
physicsScene.RemoveBody(body);
}
/// <summary>
/// Adds a body to the simulation.
/// </summary>
/// <param name="body">The rigidbody to add.</param>
public static void AddBody(TFRigidbody body)
{
physicsScene.AddBody(body, (Fix)0.2f);
}
public void HandleCollision(Manifold m, TFRigidbody a, TFRigidbody b)
{
}
public Manifold(TFRigidbody a, TFRigidbody b)
{
A = a;
B = b;
}
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 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;
}
}
private void NarrowPhase()
{
narrowPhasePairs.Clear();
for (int i = 0; i < broadPhasePairs.Count; i++)
{
broadPhasePairs[i].solve();
if (broadPhasePairs[i].contactCount > 0)
{
broadPhasePairs[i].A.currentlyCollidingWith.Add(broadPhasePairs[i].B.coll);
broadPhasePairs[i].B.currentlyCollidingWith.Add(broadPhasePairs[i].A.coll);
// If either are a trigger, don't bother.
if (broadPhasePairs[i].A.coll.isTrigger ||
broadPhasePairs[i].B.coll.isTrigger)
{
continue;
}
narrowPhasePairs.Add(broadPhasePairs[i]);
}
}
// Integrate forces
for (int i = 0; i < bodies.Count; ++i)
{
IntegrateForces(bodies[i], TFPhysics.instance.settings.deltaTime);
}
// Initialize collision
for (int i = 0; i < narrowPhasePairs.Count; ++i)
{
narrowPhasePairs[i].initialize();
}
// Solve collisions
for (int j = 0; j < TFPhysics.instance.settings.solveCollisionIterations; ++j)
{
for (int i = 0; i < narrowPhasePairs.Count; ++i)
{
narrowPhasePairs[i].ApplyImpulse();
}
}
// Integrate velocities
for (int i = 0; i < bodies.Count; ++i)
{
IntegrateVelocity(bodies[i], TFPhysics.instance.settings.deltaTime);
}
// Correct positions
for (int i = 0; i < narrowPhasePairs.Count; ++i)
{
narrowPhasePairs[i].PositionalCorrection();
}
for (int i = 0; i < bodies.Count; ++i)
{
// Clear all forces
TFRigidbody b = bodies[i];
b.info.force = new FixVec2(0, 0);
b.info.torque = 0;
// Handle events
b.HandlePhysicsEvents();
}
}
