private static bool FindMinSepAxis(
VoltPolygon poly1,
VoltPolygon poly2,
out Axis axis)
{
axis = new Axis(VoltVector2.zero, Fix64.MinValue);
for (int i = 0; i < poly1.countWorld; i++)
{
Axis a = poly1.worldAxes[i];
Fix64 min = Fix64.MaxValue;
for (int j = 0; j < poly2.countWorld; j++)
{
VoltVector2 v = poly2.worldVertices[j];
min = VoltMath.Min(min, VoltVector2.Dot(a.Normal, v));
}
min -= a.Width;
if (min > Fix64.Zero)
{
return(false);
}
if (min > axis.Width)
{
axis = new Axis(a.Normal, min);
}
}
return(true);
}
/// <summary>
/// Returns the index of the axis with the max circle penetration depth.
/// Breaks out if a separating axis is found between the two shapes.
/// Outputs the penetration depth of the circle in the axis (if any).
/// </summary>
internal static int FindAxisMaxPenetration(
VoltVector2 origin,
Fix64 radius,
VoltPolygon poly,
out Fix64 penetration)
{
int index = 0;
int found = 0;
penetration = Fix64.MinValue;
for (int i = 0; i < poly.countWorld; i++)
{
Axis axis = poly.worldAxes[i];
Fix64 dot = VoltVector2.Dot(axis.Normal, origin);
Fix64 dist = dot - axis.Width - radius;
if (dist > Fix64.Zero)
{
return(-1);
}
if (dist > penetration)
{
penetration = dist;
found = index;
}
index++;
}
return(found);
}
/// <summary>
/// Returns the index of the nearest axis on the poly to a point.
/// Outputs the minimum distance between the axis and the point.
/// </summary>
internal static int FindAxisShortestDistance(
VoltVector2 point,
Axis[] axes,
out Fix64 minDistance)
{
int ix = 0;
minDistance = Fix64.MaxValue;
bool inside = true;
for (int i = 0; i < axes.Length; i++)
{
Fix64 dot = VoltVector2.Dot(axes[i].Normal, point);
Fix64 dist = axes[i].Width - dot;
if (dist < Fix64.Zero)
{
inside = false;
}
if (dist < minDistance)
{
minDistance = dist;
ix = i;
}
}
if (inside == true)
{
minDistance = Fix64.Zero;
ix = -1;
}
return(ix);
}
internal Axis BodyToWorldAxis(Axis axis)
{
VoltVector2 normal = axis.Normal.Rotate(this.facing);
Fix64 width = VoltVector2.Dot(normal, this.position) + axis.Width;
return(new Axis(normal, width));
}
/// <summary>
/// A world-space point query, used as a shortcut in collision tests.
/// </summary>
internal bool ContainsPoint(
VoltVector2 worldSpacePoint)
{
for (int i = 0; i < this.countWorld; i++)
{
Axis axis = this.worldAxes[i];
if (VoltVector2.Dot(axis.Normal, worldSpacePoint) > axis.Width)
{
return(false);
}
}
return(true);
}
protected override bool ShapeQueryPoint(
VoltVector2 bodySpacePoint)
{
for (int i = 0; i < this.countBody; i++)
{
Axis axis = this.bodyAxes[i];
if (VoltVector2.Dot(axis.Normal, bodySpacePoint) > axis.Width)
{
return(false);
}
}
return(true);
}
/// <summary>
/// Special case that ignores axes pointing away from the normal.
/// </summary>
internal bool ContainsPointPartial(
VoltVector2 worldSpacePoint,
VoltVector2 worldSpaceNormal)
{
foreach (Axis axis in this.worldAxes)
{
if (VoltVector2.Dot(axis.Normal, worldSpaceNormal) >= Fix64.Zero &&
VoltVector2.Dot(axis.Normal, worldSpacePoint) > axis.Width)
{
return(false);
}
}
return(true);
}
private static void ComputeAxes(
VoltVector2[] vertices,
int count,
ref Axis[] destination)
{
if (destination.Length < count)
{
destination = new Axis[count];
}
for (int i = 0; i < count; i++)
{
VoltVector2 u = vertices[i];
VoltVector2 v = vertices[(i + 1) % count];
VoltVector2 normal = (v - u).Left().normalized;
destination[i] = new Axis(normal, VoltVector2.Dot(normal, u));
}
}
private Fix64 ComputeInertia()
{
Fix64 s1 = Fix64.Zero;
Fix64 s2 = Fix64.Zero;
for (int i = 0; i < this.countBody; i++)
{
VoltVector2 v = this.bodyVertices[i];
VoltVector2 u = this.bodyVertices[(i + 1) % this.countBody];
Fix64 a = VoltMath.Cross(u, v);
Fix64 b = v.sqrMagnitude + u.sqrMagnitude + VoltVector2.Dot(v, u);
s1 += a * b;
s2 += a;
}
return(s1 / ((Fix64)6 * s2));
}
internal void Solve(Manifold manifold)
{
VoltBody bodyA = manifold.ShapeA.Body;
VoltBody bodyB = manifold.ShapeB.Body;
Fix64 elasticity = bodyA.World.Elasticity;
// Calculate relative bias velocity
VoltVector2 vb1 = bodyA.BiasVelocity + (bodyA.BiasRotation * this.toALeft);
VoltVector2 vb2 = bodyB.BiasVelocity + (bodyB.BiasRotation * this.toBLeft);
Fix64 vbn = VoltVector2.Dot((vb1 - vb2), this.normal);
// Calculate and clamp the bias impulse
Fix64 jbn = this.nMass * (vbn - this.bias);
jbn = VoltMath.Max(-this.jBias, jbn);
this.jBias += jbn;
// Apply the bias impulse
this.ApplyNormalBiasImpulse(bodyA, bodyB, jbn);
// Calculate relative velocity
VoltVector2 vr = this.RelativeVelocity(bodyA, bodyB);
Fix64 vrn = VoltVector2.Dot(vr, this.normal);
// Calculate and clamp the normal impulse
Fix64 jn = nMass * (vrn + (this.restitution * elasticity));
jn = VoltMath.Max(-this.cachedNormalImpulse, jn);
this.cachedNormalImpulse += jn;
// Calculate the relative tangent velocity
Fix64 vrt = VoltVector2.Dot(vr, this.normal.Left());
// Calculate and clamp the friction impulse
Fix64 jtMax = manifold.Friction * this.cachedNormalImpulse;
Fix64 jt = vrt * tMass;
Fix64 result = VoltMath.Clamp(this.cachedTangentImpulse + jt, -jtMax, jtMax);
jt = result - this.cachedTangentImpulse;
this.cachedTangentImpulse = result;
// Apply the normal and tangent impulse
this.ApplyContactImpulse(bodyA, bodyB, jn, jt);
}
/// <summary>
/// Checks a ray against a circle with a given origin and square radius.
/// </summary>
internal static bool CircleRayCast(
VoltShape shape,
VoltVector2 shapeOrigin,
Fix64 sqrRadius,
ref VoltRayCast ray,
ref VoltRayResult result)
{
VoltVector2 toOrigin = shapeOrigin - ray.origin;
if (toOrigin.sqrMagnitude < sqrRadius)
{
result.SetContained(shape);
return(true);
}
Fix64 slope = VoltVector2.Dot(toOrigin, ray.direction);
if (slope < Fix64.Zero)
{
return(false);
}
Fix64 sqrSlope = slope * slope;
Fix64 d = sqrRadius + sqrSlope - VoltVector2.Dot(toOrigin, toOrigin);
if (d < Fix64.Zero)
{
return(false);
}
Fix64 dist = slope - VoltMath.Sqrt(d);
if (dist < Fix64.Zero || dist > ray.distance)
{
return(false);
}
// N.B.: For historical raycasts this normal will be wrong!
// Must be either transformed back to world or invalidated later.
VoltVector2 normal = (dist * ray.direction - toOrigin).normalized;
result.Set(shape, dist, normal);
return(true);
}
internal void PreStep(Manifold manifold)
{
VoltBody bodyA = manifold.ShapeA.Body;
VoltBody bodyB = manifold.ShapeB.Body;
this.toA = this.position - bodyA.Position;
this.toB = this.position - bodyB.Position;
this.toALeft = this.toA.Left();
this.toBLeft = this.toB.Left();
this.nMass = Fix64.One / this.KScalar(bodyA, bodyB, this.normal);
this.tMass = Fix64.One / this.KScalar(bodyA, bodyB, this.normal.Left());
this.bias = Contact.BiasDist(penetration);
this.jBias = Fix64.Zero;
this.restitution =
manifold.Restitution *
VoltVector2.Dot(
this.normal,
this.RelativeVelocity(bodyA, bodyB));
}
private bool CircleCastEdges(
ref VoltRayCast bodySpaceRay,
Fix64 radius,
ref VoltRayResult result)
{
int foundIndex = -1;
bool couldBeContained = true;
// Pre-compute and initialize values
Fix64 shortestDist = Fix64.MaxValue;
VoltVector2 v3 = bodySpaceRay.direction.Left();
// Check the edges -- this will be different from the raycast because
// we care about staying within the ends of the edge line segment
for (int i = 0; i < this.countBody; i++)
{
Axis curAxis = this.bodyAxes[i];
// Push the edges out by the radius
VoltVector2 extension = curAxis.Normal * radius;
VoltVector2 a = this.bodyVertices[i] + extension;
VoltVector2 b = this.bodyVertices[(i + 1) % this.countBody] + extension;
// Update the check for containment
if (couldBeContained == true)
{
Fix64 proj =
VoltVector2.Dot(curAxis.Normal, bodySpaceRay.origin) - curAxis.Width;
// The point lies outside of the outer layer
if (proj > radius)
{
couldBeContained = false;
}
// The point lies between the outer and inner layer
else if (proj > Fix64.Zero)
{
// See if the point is within the center Vornoi region of the edge
Fix64 d = VoltMath.Cross(curAxis.Normal, bodySpaceRay.origin);
if (d > VoltMath.Cross(curAxis.Normal, a))
{
couldBeContained = false;
}
if (d < VoltMath.Cross(curAxis.Normal, b))
{
couldBeContained = false;
}
}
}
// For the cast, only consider rays pointing towards the edge
if (VoltVector2.Dot(curAxis.Normal, bodySpaceRay.direction) >= Fix64.Zero)
{
continue;
}
// See:
// https://rootllama.wordpress.com/2014/06/20/ray-line-segment-intersection-test-in-2d/
VoltVector2 v1 = bodySpaceRay.origin - a;
VoltVector2 v2 = b - a;
Fix64 denominator = VoltVector2.Dot(v2, v3);
Fix64 t1 = VoltMath.Cross(v2, v1) / denominator;
Fix64 t2 = VoltVector2.Dot(v1, v3) / denominator;
if ((t2 >= Fix64.Zero) && (t2 <= Fix64.One) && (t1 > Fix64.Zero) && (t1 < shortestDist))
{
// See if the point is outside of any of the axes
shortestDist = t1;
foundIndex = i;
}
}
// Report results
if (couldBeContained == true)
{
result.SetContained(this);
return(true);
}
else if (foundIndex >= 0 && shortestDist <= bodySpaceRay.distance)
{
result.Set(
this,
shortestDist,
this.bodyAxes[foundIndex].Normal);
return(true);
}
return(false);
}
protected override bool ShapeRayCast(
ref VoltRayCast bodySpaceRay,
ref VoltRayResult result)
{
int foundIndex = -1;
Fix64 inner = Fix64.MaxValue;
Fix64 outer = Fix64.Zero;
bool couldBeContained = true;
for (int i = 0; i < this.countBody; i++)
{
Axis curAxis = this.bodyAxes[i];
// Distance between the ray origin and the axis/edge along the
// normal (i.e., shortest distance between ray origin and the edge)
Fix64 proj =
VoltVector2.Dot(curAxis.Normal, bodySpaceRay.origin) - curAxis.Width;
// See if the point is outside of any of the axes
if (proj > Fix64.Zero)
{
couldBeContained = false;
}
// Projection of the ray direction onto the axis normal (use
// negative normal because we want to get the penetration length)
Fix64 slope = VoltVector2.Dot(-curAxis.Normal, bodySpaceRay.direction);
if (slope == Fix64.Zero)
{
continue;
}
Fix64 dist = proj / slope;
// The ray is pointing opposite the edge normal (towards the edge)
if (slope > Fix64.Zero)
{
if (dist > inner)
{
return(false);
}
if (dist > outer)
{
outer = dist;
foundIndex = i;
}
}
// The ray is pointing along the edge normal (away from the edge)
else
{
if (dist < outer)
{
return(false);
}
if (dist < inner)
{
inner = dist;
}
}
}
if (couldBeContained == true)
{
result.SetContained(this);
return(true);
}
else if (foundIndex >= 0 && outer <= bodySpaceRay.distance)
{
result.Set(
this,
outer,
this.bodyAxes[foundIndex].Normal);
return(true);
}
return(false);
}
