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;
}
}
}
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 GetMagnitudeSquared(float valL, float valR)
{
var fix = new FixVec2((Fix)valL, (Fix)valR);
var magnitude = fix.GetMagnitudeSquared();
var result = (float)magnitude;
Assert.AreEqual((valL * valL) + (valR * valR), result, (Double)Fix.Epsilon);
}
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);
}
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 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();
}
