private Fix64 KScalar(
VoltBody bodyA,
VoltBody bodyB,
VoltVector2 normal)
{
Fix64 massSum = bodyA.InvMass + bodyB.InvMass;
Fix64 r1cnSqr = VoltMath.Square(VoltMath.Cross(this.toA, normal));
Fix64 r2cnSqr = VoltMath.Square(VoltMath.Cross(this.toB, normal));
return
(massSum +
bodyA.InvInertia * r1cnSqr +
bodyB.InvInertia * r2cnSqr);
}
private float KScalar(
VoltBody bodyA,
VoltBody bodyB,
Vector2 normal)
{
float massSum = bodyA.InvMass + bodyB.InvMass;
float r1cnSqr = VoltMath.Square(VoltMath.Cross(this.toA, normal));
float r2cnSqr = VoltMath.Square(VoltMath.Cross(this.toB, normal));
return
(massSum +
bodyA.InvInertia * r1cnSqr +
bodyB.InvInertia * r2cnSqr);
}
private static Manifold Circle_Polygon(
VoltWorld world,
VoltCircle circ,
VoltPolygon poly)
{
// Get the axis on the polygon closest to the circle's origin
float penetration;
int index =
Collision.FindAxisMaxPenetration(
circ.worldSpaceOrigin,
circ.radius,
poly,
out penetration);
if (index < 0)
{
return(null);
}
Vector2 a, b;
poly.GetEdge(index, out a, out b);
Axis axis = poly.GetWorldAxis(index);
// If the circle is past one of the two vertices, check it like
// a circle-circle intersection where the vertex has radius 0
float d = VoltMath.Cross(axis.Normal, circ.worldSpaceOrigin);
if (d > VoltMath.Cross(axis.Normal, a))
{
return(Collision.TestCircles(world, circ, poly, a, 0.0f));
}
if (d < VoltMath.Cross(axis.Normal, b))
{
return(Collision.TestCircles(world, circ, poly, b, 0.0f));
}
// Build the collision Manifold
Manifold manifold = world.AllocateManifold().Assign(world, circ, poly);
Vector2 pos =
circ.worldSpaceOrigin - (circ.radius + penetration / 2) * axis.Normal;
manifold.AddContact(pos, -axis.Normal, penetration);
return(manifold);
}
private float ComputeInertia()
{
float s1 = 0.0f;
float s2 = 0.0f;
for (int i = 0; i < this.countBody; i++)
{
Vector2 v = this.bodyVertices[i];
Vector2 u = this.bodyVertices[(i + 1) % this.countBody];
float a = VoltMath.Cross(u, v);
float b = v.sqrMagnitude + u.sqrMagnitude + Vector2.Dot(v, u);
s1 += a * b;
s2 += a;
}
return(s1 / (6.0f * s2));
}
private FP ComputeInertia()
{
FP s1 = 0.0f;
FP s2 = 0.0f;
for (int i = 0; i < this.countBody; i++)
{
TSVector2 v = this.bodyVertices[i];
TSVector2 u = this.bodyVertices[(i + 1) % this.countBody];
FP a = VoltMath.Cross(u, v);
FP b = v.sqrMagnitude + u.sqrMagnitude + TSVector2.Dot(v, u);
s1 += a * b;
s2 += a;
}
return(s1 / (6.0f * s2));
}
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));
}
protected override bool ShapeQueryCircle(
Vector2 bodySpaceOrigin,
float radius)
{
// Get the axis on the polygon closest to the circle's origin
float penetration;
int foundIndex =
Collision.FindAxisMaxPenetration(
bodySpaceOrigin,
radius,
this,
out penetration);
if (foundIndex < 0)
{
return(false);
}
int numVertices = this.countBody;
Vector2 a = this.bodyVertices[foundIndex];
Vector2 b = this.bodyVertices[(foundIndex + 1) % numVertices];
Axis axis = this.bodyAxes[foundIndex];
// If the circle is past one of the two vertices, check it like
// a circle-circle intersection where the vertex has radius 0
float d = VoltMath.Cross(axis.Normal, bodySpaceOrigin);
if (d > VoltMath.Cross(axis.Normal, a))
{
return(Collision.TestPointCircleSimple(a, bodySpaceOrigin, radius));
}
if (d < VoltMath.Cross(axis.Normal, b))
{
return(Collision.TestPointCircleSimple(b, bodySpaceOrigin, radius));
}
return(true);
}
private bool CircleCastEdges(
ref VoltRayCast bodySpaceRay,
float radius,
ref VoltRayResult result)
{
int foundIndex = -1;
bool couldBeContained = true;
// Pre-compute and initialize values
float shortestDist = float.MaxValue;
Vector2 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
Vector2 extension = curAxis.Normal * radius;
Vector2 a = this.bodyVertices[i] + extension;
Vector2 b = this.bodyVertices[(i + 1) % this.countBody] + extension;
// Update the check for containment
if (couldBeContained == true)
{
float proj =
Vector2.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 > 0.0f)
{
// See if the point is within the center Vornoi region of the edge
float 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 (Vector2.Dot(curAxis.Normal, bodySpaceRay.direction) >= 0.0f)
{
continue;
}
// See:
// https://rootllama.wordpress.com/2014/06/20/ray-line-segment-intersection-test-in-2d/
Vector2 v1 = bodySpaceRay.origin - a;
Vector2 v2 = b - a;
float denominator = Vector2.Dot(v2, v3);
float t1 = VoltMath.Cross(v2, v1) / denominator;
float t2 = Vector2.Dot(v1, v3) / denominator;
if ((t2 >= 0.0f) && (t2 <= 1.0f) && (t1 > 0.0f) && (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);
}
internal void ApplyBias(TSVector2 j, TSVector2 r)
{
this.BiasVelocity += this.InvMass * j;
this.BiasRotation -= this.InvInertia * VoltMath.Cross(j, r);
}
internal void ApplyImpulse(TSVector2 j, TSVector2 r)
{
this.LinearVelocity += this.InvMass * j;
this.AngularVelocity -= this.InvInertia * VoltMath.Cross(j, r);
}
public void AddForce(TSVector2 force, TSVector2 point)
{
this.Force += force;
this.Torque += VoltMath.Cross(this.Position - point, force);
}
