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>
/// 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 void Set(TSVector2 position, FP radians)
{
this.Position = position;
this.Angle = radians;
this.Facing = VoltMath.Polar(radians);
this.OnPositionUpdated();
}
private void Initialize(
TSVector2 position,
FP radians,
VoltShape[] shapesToAdd)
{
this.Position = position;
this.Angle = radians;
this.Facing = VoltMath.Polar(radians);
#if DEBUG
for (int i = 0; i < shapesToAdd.Length; i++)
{
VoltDebug.Assert(shapesToAdd[i].IsInitialized);
}
#endif
if ((this.shapes == null) || (this.shapes.Length < shapesToAdd.Length))
{
this.shapes = new VoltShape[shapesToAdd.Length];
}
Array.Copy(shapesToAdd, this.shapes, shapesToAdd.Length);
this.shapeCount = shapesToAdd.Length;
for (int i = 0; i < this.shapeCount; i++)
{
this.shapes[i].AssignBody(this);
}
#if DEBUG
this.IsInitialized = true;
#endif
}
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)));
}
private void IntegrateVelocity()
{
this.Position +=
this.World.DeltaTime * this.LinearVelocity + this.BiasVelocity;
this.Angle +=
this.World.DeltaTime * this.AngularVelocity + this.BiasRotation;
this.Facing = VoltMath.Polar(this.Angle);
}
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);
}
internal Manifold Assign(
VoltWorld world,
VoltShape shapeA,
VoltShape shapeB)
{
this.world = world;
this.ShapeA = shapeA;
this.ShapeB = shapeB;
this.Restitution = VoltMath.Sqrt(shapeA.Restitution * shapeB.Restitution);
this.Friction = VoltMath.Sqrt(shapeA.Friction * shapeB.Friction);
this.used = 0;
return(this);
}
public void PerformExplosion(
TSVector2 origin,
FP radius,
VoltExplosionCallback callback,
VoltBodyFilter targetFilter = null,
VoltBodyFilter occlusionFilter = null,
int ticksBehind = 0,
int rayCount = 32)
{
if (ticksBehind < 0)
{
throw new ArgumentOutOfRangeException("ticksBehind");
}
// Get all target bodies
this.PopulateFiltered(
origin,
radius,
targetFilter,
ticksBehind,
ref this.targetBodies);
// Get all occluding bodies
this.PopulateFiltered(
origin,
radius,
occlusionFilter,
ticksBehind,
ref this.occludingBodies);
VoltRayCast ray;
FP rayWeight = 1.0f / rayCount;
FP angleIncrement = (TSMath.Pi * 2.0f) * rayWeight;
for (int i = 0; i < rayCount; i++)
{
TSVector2 normal = VoltMath.Polar(angleIncrement * i);
ray = new VoltRayCast(origin, normal, radius);
FP minDistance =
this.GetOccludingDistance(ray, ticksBehind);
minDistance += VoltWorld.EXPLOSION_OCCLUDER_SLOP;
this.TestTargets(ray, callback, ticksBehind, minDistance, rayWeight);
}
}
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 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));
}
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));
}
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);
}
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);
}
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 static Fix64 BiasDist(Fix64 dist)
{
return(VoltConfig.ResolveRate * VoltMath.Min(Fix64.Zero, dist + VoltConfig.ResolveSlop));
}
internal Vector2 WorldToBodyDirection(Vector2 vector)
{
return(VoltMath.WorldToBodyDirection(this.facing, vector));
}
internal Vector2 WorldToBodyPoint(Vector2 vector)
{
return(VoltMath.WorldToBodyPoint(this.position, this.facing, vector));
}
public static VoltVector2 Polar(Fix64 radians)
{
return(new VoltVector2(VoltMath.Cos(radians), VoltMath.Sin(radians)));
}
public static Fix64 Angle(this VoltVector2 v)
{
return(VoltMath.Atan2(v.y, v.x));
}
internal Vector2 BodyToWorldDirection(Vector2 vector)
{
return(VoltMath.BodyToWorldDirection(this.facing, vector));
}
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);
}
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 Vector2 BodyToWorldPoint(Vector2 vector)
{
return(VoltMath.BodyToWorldPoint(this.position, this.facing, vector));
}
