private void ComputeDynamics()
{
this.Mass = 0.0f;
this.Inertia = 0.0f;
for (int i = 0; i < this.shapeCount; i++)
{
VoltShape shape = this.shapes[i];
if (shape.Density == 0.0f)
{
continue;
}
FP curMass = shape.Mass;
FP curInertia = shape.Inertia;
this.Mass += curMass;
this.Inertia += curMass * curInertia;
}
if (this.Mass < VoltConfig.MINIMUM_DYNAMIC_MASS)
{
throw new InvalidOperationException("Mass of dynamic too small");
}
else
{
this.InvMass = 1.0f / this.Mass;
this.InvInertia = 1.0f / this.Inertia;
}
this.BodyType = VoltBodyType.Dynamic;
}
private static Manifold __Polygon_Polygon(
VoltWorld world,
VoltShape sa,
VoltShape sb)
{
return(Polygon_Polygon(world, (VoltPolygon)sa, (VoltPolygon)sb));
}
private static Manifold __Circle_Circle(
VoltWorld world,
VoltShape sa,
VoltShape sb)
{
return(Circle_Circle(world, (VoltCircle)sa, (VoltCircle)sb));
}
private static Manifold __Polygon_Circle(
VoltWorld world,
VoltShape sa,
VoltShape sb)
{
return(Circle_Polygon(world, (VoltCircle)sb, (VoltPolygon)sa));
}
/// <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,
Vector2 overrideBCenter, // For testing vertices in circles
float overrideBRadius)
{
Vector2 r = overrideBCenter - shapeA.worldSpaceOrigin;
float min = shapeA.radius + overrideBRadius;
float distSq = r.sqrMagnitude;
if (distSq >= min * min)
{
return(null);
}
float dist = Mathf.Sqrt(distSq);
float distInv = 1.0f / dist;
Vector2 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);
}
/// <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 void Draw(VoltShape shape)
{
shape.GizmoDraw(
new Color(1.0f, 1.0f, 0.0f, 1.0f), // Edge Color
new Color(1.0f, 0.0f, 1.0f, 1.0f), // Normal Color
new Color(0.0f, 0.0f, 0.0f, 1.0f), // Origin Color
new Color(0.7f, 0.0f, 0.3f, 1.0f), // AABB Color
0.25f);
}
internal static Manifold Dispatch(
VoltWorld world,
VoltShape sa,
VoltShape sb)
{
Test test = Collision.tests[(int)sa.Type, (int)sb.Type];
return(test(world, sa, sb));
}
internal void Set(
VoltShape shape,
Fix64 distance,
VoltVector2 normal)
{
if (this.IsValid == false || distance < this.distance)
{
this.shape = shape;
this.distance = distance;
this.normal = normal;
}
}
internal Manifold Assign(
VoltWorld world,
VoltShape shapeA,
VoltShape shapeB)
{
this.world = world;
this.ShapeA = shapeA;
this.ShapeB = shapeB;
this.Restitution = Mathf.Sqrt(shapeA.Restitution * shapeB.Restitution);
this.Friction = Mathf.Sqrt(shapeA.Friction * shapeB.Friction);
this.used = 0;
return(this);
}
internal void FreeShape(VoltShape shape)
{
switch (shape.Type)
{
case VoltShape.ShapeType.Circle:
this.circlePool.Deallocate(shape);
break;
case VoltShape.ShapeType.Polygon:
this.polygonPool.Deallocate(shape);
break;
default:
VoltDebug.LogError("Unknown shape for deallocation");
break;
}
}
/// <summary>
/// Creates a manifold for two shapes if they collide.
/// </summary>
private void NarrowPhase(
VoltShape sa,
VoltShape sb)
{
if (sa.AABB.Intersect(sb.AABB) == false)
{
return;
}
VoltShape.OrderShapes(ref sa, ref sb);
Manifold manifold = Collision.Dispatch(this, sa, sb);
if (manifold != null)
{
this.manifolds.Add(manifold);
}
}
/// <summary>
/// Checks a ray against a circle with a given origin and square radius.
/// </summary>
internal static bool CircleRayCast(
VoltShape shape,
Vector2 shapeOrigin,
float sqrRadius,
ref VoltRayCast ray,
ref VoltRayResult result)
{
Vector2 toOrigin = shapeOrigin - ray.origin;
if (toOrigin.sqrMagnitude < sqrRadius)
{
result.SetContained(shape);
return(true);
}
float slope = Vector2.Dot(toOrigin, ray.direction);
if (slope < 0)
{
return(false);
}
float sqrSlope = slope * slope;
float d = sqrRadius + sqrSlope - Vector2.Dot(toOrigin, toOrigin);
if (d < 0)
{
return(false);
}
float dist = slope - Mathf.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.
Vector2 normal = (dist * ray.direction - toOrigin).normalized;
result.Set(shape, dist, normal);
return(true);
}
internal void SetContained(VoltShape shape)
{
this.shape = shape;
this.distance = Fix64.Zero;
this.normal = VoltVector2.zero;
}
internal void Reset()
{
this.shape = null;
}
internal void SetContained(VoltShape shape)
{
this.shape = shape;
this.distance = 0.0f;
this.normal = Vector2.zero;
}
