public void initialize()
{
// Calculate average bounciness/restitution
e = FixMath.Min(A.Bounciness, B.Bounciness);
// Calculate static & dynamic friction
sf = FixMath.Sqrt(A.StaticFriction * A.StaticFriction + B.StaticFriction * B.StaticFriction);
df = FixMath.Sqrt(A.DynamicFriction * A.DynamicFriction + B.DynamicFriction * B.DynamicFriction);
for (int i = 0; i < contactCount; i++)
{
//Calculate radii from COM to contact
FixVec2 ra = contacts[i] -= A.info.position;
FixVec2 rb = contacts[i] -= B.info.position;
//?
FixVec2 rv = B.info.velocity + FixVec2.Cross(B.info.angularVelocity, rb)
- A.info.velocity - FixVec2.Cross(A.info.angularVelocity, ra);
// Determine if we should perform a resting collision or not
// The idea is if the only thing moving this object is gravity,
// then the collision should be performed without any restitution
if (rv.GetMagnitudeSquared() < TFPhysics.instance.resting)
{
e = 0;
}
}
}
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 ApplyImpulse(FixVec2 impulse, FixVec2 contactVector)
{
info.velocity += impulse * invMass;
info.angularVelocity += invInertia * FixVec2.Cross(contactVector, impulse);
}
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 void SetVertices(List <FixVec2> verts)
{
// Find the right most point on the hull
int rightMost = 0;
Fix highestXCoord = verts[0].x;
for (int i = 1; i < verts.Count; ++i)
{
Fix x = verts[i].x;
if (x > highestXCoord)
{
highestXCoord = x;
rightMost = i;
}
// If matching x then take farthest negative y
else if (x == highestXCoord)
{
if (verts[i].y < verts[rightMost].y)
{
rightMost = i;
}
}
}
int[] hull = new int[MAX_POLY_VERTEX_COUNT];
int outCount = 0;
int indexHull = rightMost;
for (; ;)
{
hull[outCount] = indexHull;
// Search for next index that wraps around the hull
// by computing cross products to find the most counter-clockwise
// vertex in the set, given the previos hull index
int nextHullIndex = 0;
for (int i = 1; i < verts.Count; ++i)
{
// Skip if same coordinate as we need three unique
// points in the set to perform a cross product
if (nextHullIndex == indexHull)
{
nextHullIndex = i;
continue;
}
// Cross every set of three unique vertices
// Record each counter clockwise third vertex and add
// to the output hull
// See : http://www.oocities.org/pcgpe/math2d.html
FixVec2 e1 = verts[nextHullIndex] - verts[hull[outCount]];
FixVec2 e2 = verts[i] - verts[hull[outCount]];
Fix c = FixVec2.Cross(e1, e2);
if (c < Fix.zero)
{
nextHullIndex = i;
}
// Cross product is zero then e vectors are on same line
// therefore want to record vertex farthest along that line
if (c == Fix.zero && e2.GetMagnitudeSquared() > e1.GetMagnitudeSquared())
{
nextHullIndex = i;
}
}
++outCount;
indexHull = nextHullIndex;
// Conclude algorithm upon wrap-around
if (nextHullIndex == rightMost)
{
break;
}
}
// Copy vertices into shape's vertices
for (int i = 0; i < vertices.Count; ++i)
{
vertices[i] = verts[hull[i]];
}
CalculateNormals();
}