/// <summary>
/// Workhorse for circle-circle collisions, compares origin distance
/// to the sum of the two circles' radii, returns a Manifold.
/// </summary>
///
private static Manifold TestCircles(
VoltWorld world,
VoltCircle shapeA,
VoltShape shapeB,
TSVector2 overrideBCenter, // For testing vertices in circles
FP overrideBRadius)
{
TSVector2 r = overrideBCenter - shapeA.worldSpaceOrigin;
FP min = shapeA.radius + overrideBRadius;
FP distSq = r.sqrMagnitude;
if (distSq >= min * min)
{
return(null);
}
FP dist = TSMath.Sqrt(distSq);
FP distInv = 1.0f / dist;
TSVector2 pos =
shapeA.worldSpaceOrigin +
(0.5f + distInv * (shapeA.radius - min / 2.0f)) * r;
// Build the collision Manifold
Manifold manifold =
world.AllocateManifold().Assign(world, shapeA, shapeB);
manifold.AddContact(pos, distInv * r, dist - min);
return(manifold);
}
private static bool FindMinSepAxis(
VoltPolygon poly1,
VoltPolygon poly2,
out Axis axis)
{
axis = new Axis(TSVector2.zero, FP.NegativeInfinity);
for (int i = 0; i < poly1.countWorld; i++)
{
Axis a = poly1.worldAxes[i];
FP min = FP.PositiveInfinity;
for (int j = 0; j < poly2.countWorld; j++)
{
TSVector2 v = poly2.worldVertices[j];
min = TSMath.Min(min, TSVector2.Dot(a.Normal, v));
}
min -= a.Width;
if (min > 0)
{
return(false);
}
if (min > axis.Width)
{
axis = new Axis(a.Normal, min);
}
}
return(true);
}
public static void CatmullRom(ref TSVector2 value1, ref TSVector2 value2, ref TSVector2 value3, ref TSVector2 value4,
FP amount, out TSVector2 result)
{
result = new TSVector2(
TSMath.CatmullRom(value1.x, value2.x, value3.x, value4.x, amount),
TSMath.CatmullRom(value1.y, value2.y, value3.y, value4.y, amount));
}
public static void Barycentric(ref TSVector2 value1, ref TSVector2 value2, ref TSVector2 value3, FP amount1,
FP amount2, out TSVector2 result)
{
result = new TSVector2(
TSMath.Barycentric(value1.x, value2.x, value3.x, amount1, amount2),
TSMath.Barycentric(value1.y, value2.y, value3.y, amount1, amount2));
}
public static TSVector2 Lerp(TSVector2 value1, TSVector2 value2, FP amount)
{
amount = TSMath.Clamp(amount, 0, 1);
return(new TSVector2(
TSMath.Lerp(value1.x, value2.x, amount),
TSMath.Lerp(value1.y, value2.y, amount)));
}
public static VoltAABB CreateMerged(VoltAABB aabb1, VoltAABB aabb2)
{
return(new VoltAABB(
TSMath.Max(aabb1.top, aabb2.top),
TSMath.Min(aabb1.bottom, aabb2.bottom),
TSMath.Min(aabb1.left, aabb2.left),
TSMath.Max(aabb1.right, aabb2.right)));
}
internal Manifold Assign(
VoltWorld world,
VoltShape shapeA,
VoltShape shapeB)
{
this.world = world;
this.ShapeA = shapeA;
this.ShapeB = shapeB;
this.Restitution = TSMath.Sqrt(shapeA.Restitution * shapeB.Restitution);
this.Friction = TSMath.Sqrt(shapeA.Friction * shapeB.Friction);
this.used = 0;
return(this);
}
public static TSQuaternion Slerp(TSQuaternion from, TSQuaternion to, FP t)
{
t = TSMath.Clamp(t, 0, 1);
FP dot = Dot(from, to);
if (dot < 0.0f)
{
to = Multiply(to, -1);
dot = -dot;
}
FP halfTheta = FP.Acos(dot);
return(Multiply(Multiply(from, FP.Sin((1 - t) * halfTheta)) + Multiply(to, FP.Sin(t * halfTheta)), 1 / FP.Sin(halfTheta)));
}
/// <summary>
/// Builds the AABB by combining all the shape AABBs.
/// </summary>
private void UpdateAABB()
{
FP top = FP.NegativeInfinity;
FP right = FP.NegativeInfinity;
FP bottom = FP.PositiveInfinity;
FP left = FP.PositiveInfinity;
for (int i = 0; i < this.shapeCount; i++)
{
VoltAABB aabb = this.shapes[i].AABB;
top = TSMath.Max(top, aabb.Top);
right = TSMath.Max(right, aabb.Right);
bottom = TSMath.Min(bottom, aabb.Bottom);
left = TSMath.Min(left, aabb.Left);
}
this.AABB = new VoltAABB(top, bottom, left, right);
}
/// <summary>
/// Checks a ray against a circle with a given origin and square radius.
/// </summary>
internal static bool CircleRayCast(
VoltShape shape,
TSVector2 shapeOrigin,
FP sqrRadius,
ref VoltRayCast ray,
ref VoltRayResult result)
{
TSVector2 toOrigin = shapeOrigin - ray.origin;
if (toOrigin.sqrMagnitude < sqrRadius)
{
result.SetContained(shape);
return(true);
}
FP slope = TSVector2.Dot(toOrigin, ray.direction);
if (slope < 0)
{
return(false);
}
FP sqrSlope = slope * slope;
FP d = sqrRadius + sqrSlope - TSVector2.Dot(toOrigin, toOrigin);
if (d < 0)
{
return(false);
}
FP dist = slope - TSMath.Sqrt(d);
if (dist < 0 || 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.
TSVector2 normal = (dist * ray.direction - toOrigin).normalized;
result.Set(shape, dist, normal);
return(true);
}
internal void Solve(Manifold manifold)
{
VoltBody bodyA = manifold.ShapeA.Body;
VoltBody bodyB = manifold.ShapeB.Body;
FP elasticity = bodyA.World.Elasticity;
// Calculate relative bias velocity
TSVector2 vb1 = bodyA.BiasVelocity + (bodyA.BiasRotation * this.toALeft);
TSVector2 vb2 = bodyB.BiasVelocity + (bodyB.BiasRotation * this.toBLeft);
FP vbn = TSVector2.Dot((vb1 - vb2), this.normal);
// Calculate and clamp the bias impulse
FP jbn = this.nMass * (vbn - this.bias);
jbn = TSMath.Max(-this.jBias, jbn);
this.jBias += jbn;
// Apply the bias impulse
this.ApplyNormalBiasImpulse(bodyA, bodyB, jbn);
// Calculate relative velocity
TSVector2 vr = this.RelativeVelocity(bodyA, bodyB);
FP vrn = TSVector2.Dot(vr, this.normal);
// Calculate and clamp the normal impulse
FP jn = nMass * (vrn + (this.restitution * elasticity));
jn = TSMath.Max(-this.cachedNormalImpulse, jn);
this.cachedNormalImpulse += jn;
// Calculate the relative tangent velocity
FP vrt = TSVector2.Dot(vr, this.normal.Left());
// Calculate and clamp the friction impulse
FP jtMax = manifold.Friction * this.cachedNormalImpulse;
FP jt = vrt * tMass;
FP result = TSMath.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);
}
private static VoltAABB ComputeBounds(
TSVector2[] vertices,
int count)
{
FP top = vertices[0].y;
FP bottom = vertices[0].y;
FP left = vertices[0].x;
FP right = vertices[0].x;
for (int i = 1; i < count; i++)
{
top = TSMath.Max(top, vertices[i].y);
bottom = TSMath.Min(bottom, vertices[i].y);
left = TSMath.Min(left, vertices[i].x);
right = TSMath.Max(right, vertices[i].x);
}
return(new VoltAABB(top, bottom, left, right));
}
public static TSVector2 SmoothStep(TSVector2 value1, TSVector2 value2, FP amount)
{
return(new TSVector2(
TSMath.SmoothStep(value1.x, value2.x, amount),
TSMath.SmoothStep(value1.y, value2.y, amount)));
}
public static void Min(ref TSVector2 value1, ref TSVector2 value2, out TSVector2 result)
{
result.x = TSMath.Min(value1.x, value2.x);
result.y = TSMath.Min(value1.y, value2.y);
}
public static TSVector2 Min(TSVector2 value1, TSVector2 value2)
{
return(new TSVector2(
TSMath.Min(value1.x, value2.x),
TSMath.Min(value1.y, value2.y)));
}
public static void LerpUnclamped(ref TSVector2 value1, ref TSVector2 value2, FP amount, out TSVector2 result)
{
result = new TSVector2(
TSMath.Lerp(value1.x, value2.x, amount),
TSMath.Lerp(value1.y, value2.y, amount));
}
public static TSVector2 LerpUnclamped(TSVector2 value1, TSVector2 value2, FP amount)
{
return(new TSVector2(
TSMath.Lerp(value1.x, value2.x, amount),
TSMath.Lerp(value1.y, value2.y, amount)));
}
public static FP Angle(this TSVector2 v)
{
return(TSMath.Atan2(v.y, v.x));
}
public static void Clamp(ref TSVector2 value1, ref TSVector2 min, ref TSVector2 max, out TSVector2 result)
{
result = new TSVector2(
TSMath.Clamp(value1.x, min.x, max.x),
TSMath.Clamp(value1.y, min.y, max.y));
}
public static TSVector2 Clamp(TSVector2 value1, TSVector2 min, TSVector2 max)
{
return(new TSVector2(
TSMath.Clamp(value1.x, min.x, max.x),
TSMath.Clamp(value1.y, min.y, max.y)));
}
public static TSVector2 Polar(FP radians)
{
return(new TSVector2(TSMath.Cos(radians), TSMath.Sin(radians)));
}
public static TSVector2 CatmullRom(TSVector2 value1, TSVector2 value2, TSVector2 value3, TSVector2 value4, FP amount)
{
return(new TSVector2(
TSMath.CatmullRom(value1.x, value2.x, value3.x, value4.x, amount),
TSMath.CatmullRom(value1.y, value2.y, value3.y, value4.y, amount)));
}
private static FP BiasDist(FP dist)
{
return(VoltConfig.ResolveRate * TSMath.Min(0, dist + VoltConfig.ResolveSlop));
}
public static TSVector2 Barycentric(TSVector2 value1, TSVector2 value2, TSVector2 value3, FP amount1, FP amount2)
{
return(new TSVector2(
TSMath.Barycentric(value1.x, value2.x, value3.x, amount1, amount2),
TSMath.Barycentric(value1.y, value2.y, value3.y, amount1, amount2)));
}
public static void SmoothStep(ref TSVector2 value1, ref TSVector2 value2, FP amount, out TSVector2 result)
{
result = new TSVector2(
TSMath.SmoothStep(value1.x, value2.x, amount),
TSMath.SmoothStep(value1.y, value2.y, amount));
}
public static void Hermite(ref TSVector2 value1, ref TSVector2 tangent1, ref TSVector2 value2, ref TSVector2 tangent2,
FP amount, out TSVector2 result)
{
result.x = TSMath.Hermite(value1.x, tangent1.x, value2.x, tangent2.x, amount);
result.y = TSMath.Hermite(value1.y, tangent1.y, value2.y, tangent2.y, amount);
}
public static TSQuaternion Lerp(TSQuaternion a, TSQuaternion b, FP t)
{
t = TSMath.Clamp(t, FP.Zero, FP.One);
return(LerpUnclamped(a, b, t));
}
