public static VoltVector2 WorldToBodyPoint(
VoltVector2 bodyPosition,
VoltVector2 bodyFacing,
VoltVector2 vector)
{
return((vector - bodyPosition).InvRotate(bodyFacing));
}
internal Axis BodyToWorldAxis(Axis axis)
{
VoltVector2 normal = axis.Normal.Rotate(this.facing);
Fix64 width = VoltVector2.Dot(normal, this.position) + axis.Width;
return(new Axis(normal, width));
}
public static VoltVector2 BodyToWorldPoint(
VoltVector2 bodyPosition,
VoltVector2 bodyFacing,
VoltVector2 vector)
{
return(vector.Rotate(bodyFacing) + bodyPosition);
}
public static VoltAABB CreateSwept(VoltAABB source, VoltVector2 vector)
{
Fix64 top = source.top;
Fix64 bottom = source.bottom;
Fix64 left = source.left;
Fix64 right = source.right;
if (vector.x < Fix64.Zero)
{
left += vector.x;
}
else
{
right += vector.x;
}
if (vector.y < Fix64.Zero)
{
bottom += vector.y;
}
else
{
top += vector.y;
}
return(new VoltAABB(top, bottom, left, right));
}
/// <summary>
/// Returns the index of the axis with the max circle penetration depth.
/// Breaks out if a separating axis is found between the two shapes.
/// Outputs the penetration depth of the circle in the axis (if any).
/// </summary>
internal static int FindAxisMaxPenetration(
VoltVector2 origin,
Fix64 radius,
VoltPolygon poly,
out Fix64 penetration)
{
int index = 0;
int found = 0;
penetration = Fix64.MinValue;
for (int i = 0; i < poly.countWorld; i++)
{
Axis axis = poly.worldAxes[i];
Fix64 dot = VoltVector2.Dot(axis.Normal, origin);
Fix64 dist = dot - axis.Width - radius;
if (dist > Fix64.Zero)
{
return(-1);
}
if (dist > penetration)
{
penetration = dist;
found = index;
}
index++;
}
return(found);
}
/// <summary>
/// Finds all bodies intersecting with a given circle.
///
/// Subsequent calls to other Query functions (Point, Circle, Bounds) will
/// invalidate the resulting enumeration from this function.
/// </summary>
public VoltBuffer <VoltBody> QueryCircle(
VoltVector2 origin,
Fix64 radius,
VoltBodyFilter filter = null,
int ticksBehind = 0)
{
if (ticksBehind < 0)
{
throw new ArgumentOutOfRangeException("ticksBehind");
}
this.reusableBuffer.Clear();
this.staticBroadphase.QueryCircle(origin, radius, this.reusableBuffer);
this.dynamicBroadphase.QueryCircle(origin, radius, this.reusableBuffer);
this.reusableOutput.Clear();
for (int i = 0; i < this.reusableBuffer.Count; i++)
{
VoltBody body = this.reusableBuffer[i];
if (VoltBody.Filter(body, filter))
{
if (body.QueryCircle(origin, radius, ticksBehind))
{
this.reusableOutput.Add(body);
}
}
}
return(this.reusableOutput);
}
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);
}
public void SetForce(VoltVector2 force, Fix64 torque, VoltVector2 biasVelocity, Fix64 biasRotation)
{
this.Force = force;
this.Torque = torque;
this.BiasVelocity = biasVelocity;
this.BiasRotation = biasRotation;
}
/// <summary>
/// Checks if a point is contained in this body.
/// Begins with AABB checks unless bypassed.
/// </summary>
internal bool QueryPoint(
VoltVector2 point,
int ticksBehind,
bool bypassAABB = false)
{
HistoryRecord record = this.GetState(ticksBehind);
// AABB check done in world space (because it keeps changing)
if (bypassAABB == false)
{
if (record.aabb.QueryPoint(point) == false)
{
return(false);
}
}
// Actual query on shapes done in body space
VoltVector2 bodySpacePoint = record.WorldToBodyPoint(point);
for (int i = 0; i < this.shapeCount; i++)
{
if (this.shapes[i].QueryPoint(bodySpacePoint))
{
return(true);
}
}
return(false);
}
/// <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);
}
/// <summary>
/// Returns the index of the nearest axis on the poly to a point.
/// Outputs the minimum distance between the axis and the point.
/// </summary>
internal static int FindAxisShortestDistance(
VoltVector2 point,
Axis[] axes,
out Fix64 minDistance)
{
int ix = 0;
minDistance = Fix64.MaxValue;
bool inside = true;
for (int i = 0; i < axes.Length; i++)
{
Fix64 dot = VoltVector2.Dot(axes[i].Normal, point);
Fix64 dist = axes[i].Width - dot;
if (dist < Fix64.Zero)
{
inside = false;
}
if (dist < minDistance)
{
minDistance = dist;
ix = i;
}
}
if (inside == true)
{
minDistance = Fix64.Zero;
ix = -1;
}
return(ix);
}
/// <summary>
/// Finds all dynamic bodies that overlap with the explosion AABB
/// and pass the target filter test. Does not test actual shapes.
/// </summary>
private void PopulateFiltered(
VoltVector2 origin,
Fix64 radius,
VoltBodyFilter targetFilter,
int ticksBehind,
ref VoltBuffer <VoltBody> filterBuffer)
{
if (filterBuffer == null)
{
filterBuffer = new VoltBuffer <VoltBody>();
}
filterBuffer.Clear();
this.reusableBuffer.Clear();
this.staticBroadphase.QueryCircle(origin, radius, this.reusableBuffer);
this.dynamicBroadphase.QueryCircle(origin, radius, this.reusableBuffer);
VoltAABB aabb = new VoltAABB(origin, radius);
for (int i = 0; i < this.reusableBuffer.Count; i++)
{
VoltBody body = this.reusableBuffer[i];
if ((targetFilter == null) || targetFilter.Invoke(body))
{
if (body.QueryAABBOnly(aabb, ticksBehind))
{
filterBuffer.Add(body);
}
}
}
}
/// <summary>
/// Checks if a circle overlaps with this body.
/// Begins with AABB checks.
/// </summary>
internal bool QueryCircle(
VoltVector2 origin,
Fix64 radius,
int ticksBehind,
bool bypassAABB = false)
{
HistoryRecord record = this.GetState(ticksBehind);
// AABB check done in world space (because it keeps changing)
if (bypassAABB == false)
{
if (record.aabb.QueryCircleApprox(origin, radius) == false)
{
return(false);
}
}
// Actual query on shapes done in body space
VoltVector2 bodySpaceOrigin = record.WorldToBodyPoint(origin);
for (int i = 0; i < this.shapeCount; i++)
{
if (this.shapes[i].QueryCircle(bodySpaceOrigin, radius))
{
return(true);
}
}
return(false);
}
public void QueryCircle(
VoltVector2 point,
Fix64 radius,
VoltBuffer <VoltBody> outBuffer)
{
outBuffer.Add(this.bodies, this.count);
}
private void Initialize(
VoltVector2 position,
Fix64 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
}
/// <summary>
/// Tries to get a reference frame for a given number of ticks behind
/// the current tick. Returns true if a value was found, false if a
/// value was not found (in which case we clamp to the nearest).
/// </summary>
public bool TryGetSpace(
int ticksBehind,
out VoltVector2 position,
out VoltVector2 facing)
{
if (ticksBehind < 0)
{
throw new ArgumentOutOfRangeException("ticksBehind");
}
if (ticksBehind == 0)
{
position = this.Position;
facing = this.Facing;
return(true);
}
if (this.history == null)
{
position = this.Position;
facing = this.Facing;
return(false);
}
HistoryRecord record;
bool found = this.history.TryGet(ticksBehind - 1, out record);
position = record.position;
facing = record.facing;
return(found);
}
public void Set(VoltVector2 position, Fix64 radians)
{
this.Position = position;
this.Angle = radians;
this.Facing = VoltMath.Polar(radians);
this.OnPositionUpdated();
}
/// <summary>
/// A fallback for handling degenerate "Star of David" cases.
/// </summary>
private static void FindVertsFallback(
VoltPolygon poly1,
VoltPolygon poly2,
VoltVector2 normal,
Fix64 penetration,
Manifold manifold)
{
for (int i = 0; i < poly1.countWorld; i++)
{
VoltVector2 vertex = poly1.worldVertices[i];
if (poly2.ContainsPointPartial(vertex, normal) == true)
{
if (manifold.AddContact(vertex, normal, penetration) == false)
{
return;
}
}
}
for (int i = 0; i < poly2.countWorld; i++)
{
VoltVector2 vertex = poly2.worldVertices[i];
if (poly1.ContainsPointPartial(vertex, -normal) == true)
{
if (manifold.AddContact(vertex, normal, penetration) == false)
{
return;
}
}
}
}
/// <summary>
/// Note: This doesn't take rounded edges into account.
/// </summary>
public bool QueryCircleApprox(VoltVector2 origin, Fix64 radius)
{
return
((this.left - radius) <= origin.x &&
(this.right + radius) >= origin.x &&
(this.bottom - radius) <= origin.y &&
(this.top + radius) >= origin.y);
}
/// <summary>
/// Performs a point test on the AABB.
/// </summary>
public bool QueryPoint(VoltVector2 point)
{
return
(this.left <= point.x &&
this.right >= point.x &&
this.bottom <= point.y &&
this.top >= point.y);
}
private void IntegrateForces(
VoltVector2 force,
Fix64 torque,
Fix64 mult)
{
this.LinearVelocity += this.World.DeltaTime * force * mult;
this.AngularVelocity -= this.World.DeltaTime * torque * mult;
}
/// <summary>
/// Checks if a point is contained in this shape.
/// Begins with an AABB check.
/// </summary>
internal bool QueryPoint(VoltVector2 bodySpacePoint)
{
// Queries and casts on shapes are always done in body space
if (this.bodySpaceAABB.QueryPoint(bodySpacePoint))
{
return(this.ShapeQueryPoint(bodySpacePoint));
}
return(false);
}
protected override bool ShapeQueryPoint(
VoltVector2 bodySpacePoint)
{
return
(Collision.TestPointCircleSimple(
this.bodySpaceOrigin,
bodySpacePoint,
this.radius));
}
protected override void Reset()
{
base.Reset();
this.worldSpaceOrigin = VoltVector2.zero;
this.radius = Fix64.Zero;
this.sqrRadius = Fix64.Zero;
this.bodySpaceOrigin = VoltVector2.zero;
}
internal void InitializeStatic(
VoltVector2 position,
Fix64 radians,
VoltShape[] shapesToAdd)
{
this.Initialize(position, radians, shapesToAdd);
this.OnPositionUpdated();
this.SetStatic();
}
internal void InitializeDynamic(
VoltVector2 position,
Fix64 radians,
VoltShape[] shapesToAdd)
{
this.Initialize(position, radians, shapesToAdd);
this.OnPositionUpdated();
this.ComputeDynamics();
}
/// <summary>
/// Simple check for point-circle containment.
/// </summary>
internal static bool TestPointCircleSimple(
VoltVector2 point,
VoltVector2 origin,
Fix64 radius)
{
VoltVector2 delta = origin - point;
return(delta.sqrMagnitude <= (radius * radius));
}
/// <summary>
/// Checks if a circle overlaps with this shape.
/// Begins with an AABB check.
/// </summary>
internal bool QueryCircle(VoltVector2 bodySpaceOrigin, Fix64 radius)
{
// Queries and casts on shapes are always done in body space
if (this.bodySpaceAABB.QueryCircleApprox(bodySpaceOrigin, radius))
{
return(this.ShapeQueryCircle(bodySpaceOrigin, radius));
}
return(false);
}
private void ApplyNormalBiasImpulse(
VoltBody bodyA,
VoltBody bodyB,
Fix64 normalBiasImpulse)
{
VoltVector2 impulse = normalBiasImpulse * this.normal;
bodyA.ApplyBias(-impulse, this.toA);
bodyB.ApplyBias(impulse, this.toB);
}
/// <summary>
/// Simple check for two overlapping circles.
/// </summary>
internal static bool TestCircleCircleSimple(
VoltVector2 originA,
VoltVector2 originB,
Fix64 radiusA,
Fix64 radiusB)
{
Fix64 radiusTotal = radiusA + radiusB;
return((originA - originB).sqrMagnitude <= (radiusTotal * radiusTotal));
}
