/// <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,
VoltVector2 overrideBCenter, // For testing vertices in circles
Fix64 overrideBRadius)
{
VoltVector2 r = overrideBCenter - shapeA.worldSpaceOrigin;
Fix64 min = shapeA.radius + overrideBRadius;
Fix64 distSq = r.sqrMagnitude;
if (distSq >= min * min)
{
return(null);
}
Fix64 dist = VoltMath.Sqrt(distSq);
// 최소값을 지정하여 divide by zero 방지
Fix64 distInv = Fix64.One / VoltMath.Max(dist, min / (Fix64)10);
VoltVector2 pos =
shapeA.worldSpaceOrigin +
(Fix64.One / (Fix64)2 + distInv * (shapeA.radius - min / (Fix64)2)) * r;
// Build the collision Manifold
Manifold manifold =
world.AllocateManifold().Assign(world, shapeA, shapeB);
manifold.AddContact(pos, distInv * r, dist - min);
return(manifold);
}
public static VoltAABB CreateMerged(VoltAABB aabb1, VoltAABB aabb2)
{
return(new VoltAABB(
VoltMath.Max(aabb1.top, aabb2.top),
VoltMath.Min(aabb1.bottom, aabb2.bottom),
VoltMath.Min(aabb1.left, aabb2.left),
VoltMath.Max(aabb1.right, aabb2.right)));
}
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);
}
private static VoltAABB ComputeBounds(
VoltVector2[] vertices,
int count)
{
Fix64 top = vertices[0].y;
Fix64 bottom = vertices[0].y;
Fix64 left = vertices[0].x;
Fix64 right = vertices[0].x;
for (int i = 1; i < count; i++)
{
top = VoltMath.Max(top, vertices[i].y);
bottom = VoltMath.Min(bottom, vertices[i].y);
left = VoltMath.Min(left, vertices[i].x);
right = VoltMath.Max(right, vertices[i].x);
}
return(new VoltAABB(top, bottom, left, right));
}
/// <summary>
/// Builds the AABB by combining all the shape AABBs.
/// </summary>
private void UpdateAABB()
{
Fix64 top = Fix64.MinValue;
Fix64 right = Fix64.MaxValue;
Fix64 bottom = Fix64.MaxValue;
Fix64 left = Fix64.MinValue;
for (int i = 0; i < this.shapeCount; i++)
{
VoltAABB aabb = this.shapes[i].AABB;
top = VoltMath.Max(top, aabb.Top);
right = VoltMath.Max(right, aabb.Right);
bottom = VoltMath.Min(bottom, aabb.Bottom);
left = VoltMath.Min(left, aabb.Left);
}
this.AABB = new VoltAABB(top, bottom, left, right);
}
