1945 lines
70 KiB
C#
1945 lines
70 KiB
C#
/*
|
|
* Farseer Physics Engine based on Box2D.XNA port:
|
|
* Copyright (c) 2010 Ian Qvist
|
|
*
|
|
* Box2D.XNA port of Box2D:
|
|
* Copyright (c) 2009 Brandon Furtwangler, Nathan Furtwangler
|
|
*
|
|
* Original source Box2D:
|
|
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
|
|
*
|
|
* This software is provided 'as-is', without any express or implied
|
|
* warranty. In no event will the authors be held liable for any damages
|
|
* arising from the use of this software.
|
|
* Permission is granted to anyone to use this software for any purpose,
|
|
* including commercial applications, and to alter it and redistribute it
|
|
* freely, subject to the following restrictions:
|
|
* 1. The origin of this software must not be misrepresented; you must not
|
|
* claim that you wrote the original software. If you use this software
|
|
* in a product, an acknowledgment in the product documentation would be
|
|
* appreciated but is not required.
|
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
|
* misrepresented as being the original software.
|
|
* 3. This notice may not be removed or altered from any source distribution.
|
|
*/
|
|
|
|
using System;
|
|
using System.Diagnostics;
|
|
using System.Runtime.InteropServices;
|
|
using FarseerPhysics.Collision.Shapes;
|
|
using FarseerPhysics.Common;
|
|
using Microsoft.Xna.Framework;
|
|
|
|
namespace FarseerPhysics.Collision
|
|
{
|
|
internal enum ContactFeatureType : byte
|
|
{
|
|
Vertex = 0,
|
|
Face = 1,
|
|
}
|
|
|
|
/// <summary>
|
|
/// The features that intersect to form the contact point
|
|
/// This must be 4 bytes or less.
|
|
/// </summary>
|
|
public struct ContactFeature
|
|
{
|
|
/// <summary>
|
|
/// Feature index on ShapeA
|
|
/// </summary>
|
|
public byte IndexA;
|
|
|
|
/// <summary>
|
|
/// Feature index on ShapeB
|
|
/// </summary>
|
|
public byte IndexB;
|
|
|
|
/// <summary>
|
|
/// The feature type on ShapeA
|
|
/// </summary>
|
|
public byte TypeA;
|
|
|
|
/// <summary>
|
|
/// The feature type on ShapeB
|
|
/// </summary>
|
|
public byte TypeB;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Contact ids to facilitate warm starting.
|
|
/// </summary>
|
|
[StructLayout(LayoutKind.Explicit)]
|
|
public struct ContactID
|
|
{
|
|
/// <summary>
|
|
/// The features that intersect to form the contact point
|
|
/// </summary>
|
|
[FieldOffset(0)]
|
|
public ContactFeature Features;
|
|
|
|
/// <summary>
|
|
/// Used to quickly compare contact ids.
|
|
/// </summary>
|
|
[FieldOffset(0)]
|
|
public uint Key;
|
|
}
|
|
|
|
/// <summary>
|
|
/// A manifold point is a contact point belonging to a contact
|
|
/// manifold. It holds details related to the geometry and dynamics
|
|
/// of the contact points.
|
|
/// The local point usage depends on the manifold type:
|
|
/// -ShapeType.Circles: the local center of circleB
|
|
/// -SeparationFunction.FaceA: the local center of cirlceB or the clip point of polygonB
|
|
/// -SeparationFunction.FaceB: the clip point of polygonA
|
|
/// This structure is stored across time steps, so we keep it small.
|
|
/// Note: the impulses are used for internal caching and may not
|
|
/// provide reliable contact forces, especially for high speed collisions.
|
|
/// </summary>
|
|
public struct ManifoldPoint
|
|
{
|
|
/// <summary>
|
|
/// Uniquely identifies a contact point between two Shapes
|
|
/// </summary>
|
|
public ContactID Id;
|
|
|
|
public Vector2 LocalPoint;
|
|
|
|
public float NormalImpulse;
|
|
|
|
public float TangentImpulse;
|
|
}
|
|
|
|
public enum ManifoldType
|
|
{
|
|
Circles,
|
|
FaceA,
|
|
FaceB
|
|
}
|
|
|
|
/// <summary>
|
|
/// A manifold for two touching convex Shapes.
|
|
/// Box2D supports multiple types of contact:
|
|
/// - clip point versus plane with radius
|
|
/// - point versus point with radius (circles)
|
|
/// The local point usage depends on the manifold type:
|
|
/// -ShapeType.Circles: the local center of circleA
|
|
/// -SeparationFunction.FaceA: the center of faceA
|
|
/// -SeparationFunction.FaceB: the center of faceB
|
|
/// Similarly the local normal usage:
|
|
/// -ShapeType.Circles: not used
|
|
/// -SeparationFunction.FaceA: the normal on polygonA
|
|
/// -SeparationFunction.FaceB: the normal on polygonB
|
|
/// We store contacts in this way so that position correction can
|
|
/// account for movement, which is critical for continuous physics.
|
|
/// All contact scenarios must be expressed in one of these types.
|
|
/// This structure is stored across time steps, so we keep it small.
|
|
/// </summary>
|
|
public struct Manifold
|
|
{
|
|
/// <summary>
|
|
/// Not use for Type.SeparationFunction.Points
|
|
/// </summary>
|
|
public Vector2 LocalNormal;
|
|
|
|
/// <summary>
|
|
/// Usage depends on manifold type
|
|
/// </summary>
|
|
public Vector2 LocalPoint;
|
|
|
|
/// <summary>
|
|
/// The number of manifold points
|
|
/// </summary>
|
|
public int PointCount;
|
|
|
|
/// <summary>
|
|
/// The points of contact
|
|
/// </summary>
|
|
public FixedArray2<ManifoldPoint> Points;
|
|
|
|
public ManifoldType Type;
|
|
}
|
|
|
|
/// <summary>
|
|
/// This is used for determining the state of contact points.
|
|
/// </summary>
|
|
public enum PointState
|
|
{
|
|
/// <summary>
|
|
/// Point does not exist
|
|
/// </summary>
|
|
Null,
|
|
|
|
/// <summary>
|
|
/// Point was added in the update
|
|
/// </summary>
|
|
Add,
|
|
|
|
/// <summary>
|
|
/// Point persisted across the update
|
|
/// </summary>
|
|
Persist,
|
|
|
|
/// <summary>
|
|
/// Point was removed in the update
|
|
/// </summary>
|
|
Remove,
|
|
}
|
|
|
|
/// <summary>
|
|
/// Used for computing contact manifolds.
|
|
/// </summary>
|
|
public struct ClipVertex
|
|
{
|
|
public ContactID ID;
|
|
public Vector2 V;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
|
|
/// </summary>
|
|
public struct RayCastInput
|
|
{
|
|
public float MaxFraction;
|
|
public Vector2 Point1, Point2;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2
|
|
/// come from RayCastInput.
|
|
/// </summary>
|
|
public struct RayCastOutput
|
|
{
|
|
public float Fraction;
|
|
public Vector2 Normal;
|
|
}
|
|
|
|
/// <summary>
|
|
/// An axis aligned bounding box.
|
|
/// </summary>
|
|
public struct AABB
|
|
{
|
|
private static DistanceInput _input = new DistanceInput();
|
|
|
|
/// <summary>
|
|
/// The lower vertex
|
|
/// </summary>
|
|
public Vector2 LowerBound;
|
|
|
|
/// <summary>
|
|
/// The upper vertex
|
|
/// </summary>
|
|
public Vector2 UpperBound;
|
|
|
|
public AABB(Vector2 min, Vector2 max)
|
|
: this(ref min, ref max)
|
|
{
|
|
}
|
|
|
|
public AABB(ref Vector2 min, ref Vector2 max)
|
|
{
|
|
LowerBound = min;
|
|
UpperBound = max;
|
|
}
|
|
|
|
public AABB(Vector2 center, float width, float height)
|
|
{
|
|
LowerBound = center - new Vector2(width / 2, height / 2);
|
|
UpperBound = center + new Vector2(width / 2, height / 2);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the center of the AABB.
|
|
/// </summary>
|
|
/// <value></value>
|
|
public Vector2 Center
|
|
{
|
|
get { return 0.5f * (LowerBound + UpperBound); }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the extents of the AABB (half-widths).
|
|
/// </summary>
|
|
/// <value></value>
|
|
public Vector2 Extents
|
|
{
|
|
get { return 0.5f * (UpperBound - LowerBound); }
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the perimeter length
|
|
/// </summary>
|
|
/// <value></value>
|
|
public float Perimeter
|
|
{
|
|
get
|
|
{
|
|
float wx = UpperBound.X - LowerBound.X;
|
|
float wy = UpperBound.Y - LowerBound.Y;
|
|
return 2.0f * (wx + wy);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the vertices of the AABB.
|
|
/// </summary>
|
|
/// <value>The corners of the AABB</value>
|
|
public Vertices Vertices
|
|
{
|
|
get
|
|
{
|
|
Vertices vertices = new Vertices();
|
|
vertices.Add(LowerBound);
|
|
vertices.Add(new Vector2(LowerBound.X, UpperBound.Y));
|
|
vertices.Add(UpperBound);
|
|
vertices.Add(new Vector2(UpperBound.X, LowerBound.Y));
|
|
return vertices;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// first quadrant
|
|
/// </summary>
|
|
public AABB Q1
|
|
{
|
|
get { return new AABB(Center, UpperBound); }
|
|
}
|
|
|
|
public AABB Q2
|
|
{
|
|
get
|
|
{
|
|
return new AABB(new Vector2(LowerBound.X, Center.Y), new Vector2(Center.X, UpperBound.Y));
|
|
;
|
|
}
|
|
}
|
|
|
|
public AABB Q3
|
|
{
|
|
get { return new AABB(LowerBound, Center); }
|
|
}
|
|
|
|
public AABB Q4
|
|
{
|
|
get { return new AABB(new Vector2(Center.X, LowerBound.Y), new Vector2(UpperBound.X, Center.Y)); }
|
|
}
|
|
|
|
public Vector2[] GetVertices()
|
|
{
|
|
Vector2 p1 = UpperBound;
|
|
Vector2 p2 = new Vector2(UpperBound.X, LowerBound.Y);
|
|
Vector2 p3 = LowerBound;
|
|
Vector2 p4 = new Vector2(LowerBound.X, UpperBound.Y);
|
|
return new[] { p1, p2, p3, p4 };
|
|
}
|
|
|
|
/// <summary>
|
|
/// Verify that the bounds are sorted.
|
|
/// </summary>
|
|
/// <returns>
|
|
/// <c>true</c> if this instance is valid; otherwise, <c>false</c>.
|
|
/// </returns>
|
|
public bool IsValid()
|
|
{
|
|
Vector2 d = UpperBound - LowerBound;
|
|
bool valid = d.X >= 0.0f && d.Y >= 0.0f;
|
|
valid = valid && LowerBound.IsValid() && UpperBound.IsValid();
|
|
return valid;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Combine an AABB into this one.
|
|
/// </summary>
|
|
/// <param name="aabb">The aabb.</param>
|
|
public void Combine(ref AABB aabb)
|
|
{
|
|
LowerBound = Vector2.Min(LowerBound, aabb.LowerBound);
|
|
UpperBound = Vector2.Max(UpperBound, aabb.UpperBound);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Combine two AABBs into this one.
|
|
/// </summary>
|
|
/// <param name="aabb1">The aabb1.</param>
|
|
/// <param name="aabb2">The aabb2.</param>
|
|
public void Combine(ref AABB aabb1, ref AABB aabb2)
|
|
{
|
|
LowerBound = Vector2.Min(aabb1.LowerBound, aabb2.LowerBound);
|
|
UpperBound = Vector2.Max(aabb1.UpperBound, aabb2.UpperBound);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Does this aabb contain the provided AABB.
|
|
/// </summary>
|
|
/// <param name="aabb">The aabb.</param>
|
|
/// <returns>
|
|
/// <c>true</c> if it contains the specified aabb; otherwise, <c>false</c>.
|
|
/// </returns>
|
|
public bool Contains(ref AABB aabb)
|
|
{
|
|
bool result = true;
|
|
result = result && LowerBound.X <= aabb.LowerBound.X;
|
|
result = result && LowerBound.Y <= aabb.LowerBound.Y;
|
|
result = result && aabb.UpperBound.X <= UpperBound.X;
|
|
result = result && aabb.UpperBound.Y <= UpperBound.Y;
|
|
return result;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Determines whether the AAABB contains the specified point.
|
|
/// </summary>
|
|
/// <param name="point">The point.</param>
|
|
/// <returns>
|
|
/// <c>true</c> if it contains the specified point; otherwise, <c>false</c>.
|
|
/// </returns>
|
|
public bool Contains(ref Vector2 point)
|
|
{
|
|
//using epsilon to try and gaurd against float rounding errors.
|
|
if ((point.X > (LowerBound.X + Settings.Epsilon) && point.X < (UpperBound.X - Settings.Epsilon) &&
|
|
(point.Y > (LowerBound.Y + Settings.Epsilon) && point.Y < (UpperBound.Y - Settings.Epsilon))))
|
|
{
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
public static bool TestOverlap(AABB a, AABB b)
|
|
{
|
|
return TestOverlap(ref a, ref b);
|
|
}
|
|
|
|
public static bool TestOverlap(ref AABB a, ref AABB b)
|
|
{
|
|
Vector2 d1 = b.LowerBound - a.UpperBound;
|
|
Vector2 d2 = a.LowerBound - b.UpperBound;
|
|
|
|
if (d1.X > 0.0f || d1.Y > 0.0f)
|
|
return false;
|
|
|
|
if (d2.X > 0.0f || d2.Y > 0.0f)
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
public static bool TestOverlap(Shape shapeA, int indexA,
|
|
Shape shapeB, int indexB,
|
|
ref Transform xfA, ref Transform xfB)
|
|
{
|
|
_input.ProxyA.Set(shapeA, indexA);
|
|
_input.ProxyB.Set(shapeB, indexB);
|
|
_input.TransformA = xfA;
|
|
_input.TransformB = xfB;
|
|
_input.UseRadii = true;
|
|
|
|
SimplexCache cache;
|
|
DistanceOutput output;
|
|
Distance.ComputeDistance(out output, out cache, _input);
|
|
|
|
return output.Distance < 10.0f * Settings.Epsilon;
|
|
}
|
|
|
|
|
|
// From Real-time Collision Detection, p179.
|
|
public bool RayCast(out RayCastOutput output, ref RayCastInput input)
|
|
{
|
|
output = new RayCastOutput();
|
|
|
|
float tmin = -Settings.MaxFloat;
|
|
float tmax = Settings.MaxFloat;
|
|
|
|
Vector2 p = input.Point1;
|
|
Vector2 d = input.Point2 - input.Point1;
|
|
Vector2 absD = MathUtils.Abs(d);
|
|
|
|
Vector2 normal = Vector2.Zero;
|
|
|
|
for (int i = 0; i < 2; ++i)
|
|
{
|
|
float absD_i = i == 0 ? absD.X : absD.Y;
|
|
float lowerBound_i = i == 0 ? LowerBound.X : LowerBound.Y;
|
|
float upperBound_i = i == 0 ? UpperBound.X : UpperBound.Y;
|
|
float p_i = i == 0 ? p.X : p.Y;
|
|
|
|
if (absD_i < Settings.Epsilon)
|
|
{
|
|
// Parallel.
|
|
if (p_i < lowerBound_i || upperBound_i < p_i)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
float d_i = i == 0 ? d.X : d.Y;
|
|
|
|
float inv_d = 1.0f / d_i;
|
|
float t1 = (lowerBound_i - p_i) * inv_d;
|
|
float t2 = (upperBound_i - p_i) * inv_d;
|
|
|
|
// Sign of the normal vector.
|
|
float s = -1.0f;
|
|
|
|
if (t1 > t2)
|
|
{
|
|
MathUtils.Swap(ref t1, ref t2);
|
|
s = 1.0f;
|
|
}
|
|
|
|
// Push the min up
|
|
if (t1 > tmin)
|
|
{
|
|
if (i == 0)
|
|
{
|
|
normal.X = s;
|
|
}
|
|
else
|
|
{
|
|
normal.Y = s;
|
|
}
|
|
|
|
tmin = t1;
|
|
}
|
|
|
|
// Pull the max down
|
|
tmax = Math.Min(tmax, t2);
|
|
|
|
if (tmin > tmax)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Does the ray start inside the box?
|
|
// Does the ray intersect beyond the max fraction?
|
|
if (tmin < 0.0f || input.MaxFraction < tmin)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// Intersection.
|
|
output.Fraction = tmin;
|
|
output.Normal = normal;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Edge shape plus more stuff.
|
|
/// </summary>
|
|
public struct FatEdge
|
|
{
|
|
public bool HasVertex0, HasVertex3;
|
|
public Vector2 Normal;
|
|
public Vector2 V0, V1, V2, V3;
|
|
}
|
|
|
|
/// <summary>
|
|
/// This lets us treate and edge shape and a polygon in the same
|
|
/// way in the SAT collider.
|
|
/// </summary>
|
|
public class EPProxy
|
|
{
|
|
public Vector2 Centroid;
|
|
public int Count;
|
|
public Vector2[] Normals = new Vector2[Settings.MaxPolygonVertices];
|
|
public Vector2[] Vertices = new Vector2[Settings.MaxPolygonVertices];
|
|
}
|
|
|
|
public struct EPAxis
|
|
{
|
|
public int Index;
|
|
public float Separation;
|
|
public EPAxisType Type;
|
|
}
|
|
|
|
public enum EPAxisType
|
|
{
|
|
Unknown,
|
|
EdgeA,
|
|
EdgeB,
|
|
}
|
|
|
|
public static class Collision
|
|
{
|
|
private static FatEdge _edgeA;
|
|
|
|
private static EPProxy _proxyA = new EPProxy();
|
|
private static EPProxy _proxyB = new EPProxy();
|
|
|
|
private static Transform _xf;
|
|
private static Vector2 _limit11, _limit12;
|
|
private static Vector2 _limit21, _limit22;
|
|
private static float _radius;
|
|
private static Vector2[] _tmpNormals = new Vector2[2];
|
|
|
|
/// <summary>
|
|
/// Evaluate the manifold with supplied transforms. This assumes
|
|
/// modest motion from the original state. This does not change the
|
|
/// point count, impulses, etc. The radii must come from the Shapes
|
|
/// that generated the manifold.
|
|
/// </summary>
|
|
/// <param name="manifold">The manifold.</param>
|
|
/// <param name="transformA">The transform for A.</param>
|
|
/// <param name="radiusA">The radius for A.</param>
|
|
/// <param name="transformB">The transform for B.</param>
|
|
/// <param name="radiusB">The radius for B.</param>
|
|
/// <param name="normal">World vector pointing from A to B</param>
|
|
/// <param name="points">Torld contact point (point of intersection).</param>
|
|
public static void GetWorldManifold(ref Manifold manifold,
|
|
ref Transform transformA, float radiusA,
|
|
ref Transform transformB, float radiusB, out Vector2 normal,
|
|
out FixedArray2<Vector2> points)
|
|
{
|
|
points = new FixedArray2<Vector2>();
|
|
normal = Vector2.Zero;
|
|
|
|
if (manifold.PointCount == 0)
|
|
{
|
|
normal = Vector2.UnitY;
|
|
return;
|
|
}
|
|
|
|
switch (manifold.Type)
|
|
{
|
|
case ManifoldType.Circles:
|
|
{
|
|
Vector2 tmp = manifold.Points[0].LocalPoint;
|
|
float pointAx = transformA.Position.X + transformA.R.Col1.X * manifold.LocalPoint.X +
|
|
transformA.R.Col2.X * manifold.LocalPoint.Y;
|
|
|
|
float pointAy = transformA.Position.Y + transformA.R.Col1.Y * manifold.LocalPoint.X +
|
|
transformA.R.Col2.Y * manifold.LocalPoint.Y;
|
|
|
|
float pointBx = transformB.Position.X + transformB.R.Col1.X * tmp.X +
|
|
transformB.R.Col2.X * tmp.Y;
|
|
|
|
float pointBy = transformB.Position.Y + transformB.R.Col1.Y * tmp.X +
|
|
transformB.R.Col2.Y * tmp.Y;
|
|
|
|
normal.X = 1;
|
|
normal.Y = 0;
|
|
|
|
float result = (pointAx - pointBx) * (pointAx - pointBx) +
|
|
(pointAy - pointBy) * (pointAy - pointBy);
|
|
if (result > Settings.Epsilon * Settings.Epsilon)
|
|
{
|
|
float tmpNormalx = pointBx - pointAx;
|
|
float tmpNormaly = pointBy - pointAy;
|
|
float factor = 1f / (float)Math.Sqrt(tmpNormalx * tmpNormalx + tmpNormaly * tmpNormaly);
|
|
normal.X = tmpNormalx * factor;
|
|
normal.Y = tmpNormaly * factor;
|
|
}
|
|
|
|
Vector2 c = Vector2.Zero;
|
|
c.X = (pointAx + radiusA * normal.X) + (pointBx - radiusB * normal.X);
|
|
c.Y = (pointAy + radiusA * normal.Y) + (pointBy - radiusB * normal.Y);
|
|
|
|
points[0] = 0.5f * c;
|
|
}
|
|
break;
|
|
|
|
case ManifoldType.FaceA:
|
|
{
|
|
normal.X = transformA.R.Col1.X * manifold.LocalNormal.X +
|
|
transformA.R.Col2.X * manifold.LocalNormal.Y;
|
|
normal.Y = transformA.R.Col1.Y * manifold.LocalNormal.X +
|
|
transformA.R.Col2.Y * manifold.LocalNormal.Y;
|
|
|
|
float planePointx = transformA.Position.X + transformA.R.Col1.X * manifold.LocalPoint.X +
|
|
transformA.R.Col2.X * manifold.LocalPoint.Y;
|
|
|
|
float planePointy = transformA.Position.Y + transformA.R.Col1.Y * manifold.LocalPoint.X +
|
|
transformA.R.Col2.Y * manifold.LocalPoint.Y;
|
|
|
|
for (int i = 0; i < manifold.PointCount; ++i)
|
|
{
|
|
Vector2 tmp = manifold.Points[i].LocalPoint;
|
|
|
|
float clipPointx = transformB.Position.X + transformB.R.Col1.X * tmp.X +
|
|
transformB.R.Col2.X * tmp.Y;
|
|
|
|
float clipPointy = transformB.Position.Y + transformB.R.Col1.Y * tmp.X +
|
|
transformB.R.Col2.Y * tmp.Y;
|
|
|
|
float value = (clipPointx - planePointx) * normal.X + (clipPointy - planePointy) * normal.Y;
|
|
|
|
Vector2 c = Vector2.Zero;
|
|
c.X = (clipPointx + (radiusA - value) * normal.X) + (clipPointx - radiusB * normal.X);
|
|
c.Y = (clipPointy + (radiusA - value) * normal.Y) + (clipPointy - radiusB * normal.Y);
|
|
|
|
points[i] = 0.5f * c;
|
|
}
|
|
}
|
|
break;
|
|
|
|
case ManifoldType.FaceB:
|
|
{
|
|
normal.X = transformB.R.Col1.X * manifold.LocalNormal.X +
|
|
transformB.R.Col2.X * manifold.LocalNormal.Y;
|
|
normal.Y = transformB.R.Col1.Y * manifold.LocalNormal.X +
|
|
transformB.R.Col2.Y * manifold.LocalNormal.Y;
|
|
|
|
float planePointx = transformB.Position.X + transformB.R.Col1.X * manifold.LocalPoint.X +
|
|
transformB.R.Col2.X * manifold.LocalPoint.Y;
|
|
|
|
float planePointy = transformB.Position.Y + transformB.R.Col1.Y * manifold.LocalPoint.X +
|
|
transformB.R.Col2.Y * manifold.LocalPoint.Y;
|
|
|
|
for (int i = 0; i < manifold.PointCount; ++i)
|
|
{
|
|
Vector2 tmp = manifold.Points[i].LocalPoint;
|
|
|
|
float clipPointx = transformA.Position.X + transformA.R.Col1.X * tmp.X +
|
|
transformA.R.Col2.X * tmp.Y;
|
|
|
|
float clipPointy = transformA.Position.Y + transformA.R.Col1.Y * tmp.X +
|
|
transformA.R.Col2.Y * tmp.Y;
|
|
|
|
float value = (clipPointx - planePointx) * normal.X + (clipPointy - planePointy) * normal.Y;
|
|
|
|
Vector2 c = Vector2.Zero;
|
|
c.X = (clipPointx - radiusA * normal.X) + (clipPointx + (radiusB - value) * normal.X);
|
|
c.Y = (clipPointy - radiusA * normal.Y) + (clipPointy + (radiusB - value) * normal.Y);
|
|
|
|
points[i] = 0.5f * c;
|
|
}
|
|
// Ensure normal points from A to B.
|
|
normal *= -1;
|
|
}
|
|
break;
|
|
default:
|
|
normal = Vector2.UnitY;
|
|
break;
|
|
}
|
|
}
|
|
|
|
public static void GetPointStates(out FixedArray2<PointState> state1, out FixedArray2<PointState> state2,
|
|
ref Manifold manifold1, ref Manifold manifold2)
|
|
{
|
|
state1 = new FixedArray2<PointState>();
|
|
state2 = new FixedArray2<PointState>();
|
|
|
|
// Detect persists and removes.
|
|
for (int i = 0; i < manifold1.PointCount; ++i)
|
|
{
|
|
ContactID id = manifold1.Points[i].Id;
|
|
|
|
state1[i] = PointState.Remove;
|
|
|
|
for (int j = 0; j < manifold2.PointCount; ++j)
|
|
{
|
|
if (manifold2.Points[j].Id.Key == id.Key)
|
|
{
|
|
state1[i] = PointState.Persist;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// Detect persists and adds.
|
|
for (int i = 0; i < manifold2.PointCount; ++i)
|
|
{
|
|
ContactID id = manifold2.Points[i].Id;
|
|
|
|
state2[i] = PointState.Add;
|
|
|
|
for (int j = 0; j < manifold1.PointCount; ++j)
|
|
{
|
|
if (manifold1.Points[j].Id.Key == id.Key)
|
|
{
|
|
state2[i] = PointState.Persist;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/// Compute the collision manifold between two circles.
|
|
public static void CollideCircles(ref Manifold manifold,
|
|
CircleShape circleA, ref Transform xfA,
|
|
CircleShape circleB, ref Transform xfB)
|
|
{
|
|
manifold.PointCount = 0;
|
|
|
|
float pAx = xfA.Position.X + xfA.R.Col1.X * circleA.Position.X + xfA.R.Col2.X * circleA.Position.Y;
|
|
float pAy = xfA.Position.Y + xfA.R.Col1.Y * circleA.Position.X + xfA.R.Col2.Y * circleA.Position.Y;
|
|
float pBx = xfB.Position.X + xfB.R.Col1.X * circleB.Position.X + xfB.R.Col2.X * circleB.Position.Y;
|
|
float pBy = xfB.Position.Y + xfB.R.Col1.Y * circleB.Position.X + xfB.R.Col2.Y * circleB.Position.Y;
|
|
|
|
float distSqr = (pBx - pAx) * (pBx - pAx) + (pBy - pAy) * (pBy - pAy);
|
|
float radius = circleA.Radius + circleB.Radius;
|
|
if (distSqr > radius * radius)
|
|
{
|
|
return;
|
|
}
|
|
|
|
manifold.Type = ManifoldType.Circles;
|
|
manifold.LocalPoint = circleA.Position;
|
|
manifold.LocalNormal = Vector2.Zero;
|
|
manifold.PointCount = 1;
|
|
|
|
ManifoldPoint p0 = manifold.Points[0];
|
|
|
|
p0.LocalPoint = circleB.Position;
|
|
p0.Id.Key = 0;
|
|
|
|
manifold.Points[0] = p0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Compute the collision manifold between a polygon and a circle.
|
|
/// </summary>
|
|
/// <param name="manifold">The manifold.</param>
|
|
/// <param name="polygonA">The polygon A.</param>
|
|
/// <param name="transformA">The transform of A.</param>
|
|
/// <param name="circleB">The circle B.</param>
|
|
/// <param name="transformB">The transform of B.</param>
|
|
public static void CollidePolygonAndCircle(ref Manifold manifold,
|
|
PolygonShape polygonA, ref Transform transformA,
|
|
CircleShape circleB, ref Transform transformB)
|
|
{
|
|
manifold.PointCount = 0;
|
|
|
|
// Compute circle position in the frame of the polygon.
|
|
Vector2 c =
|
|
new Vector2(
|
|
transformB.Position.X + transformB.R.Col1.X * circleB.Position.X +
|
|
transformB.R.Col2.X * circleB.Position.Y,
|
|
transformB.Position.Y + transformB.R.Col1.Y * circleB.Position.X +
|
|
transformB.R.Col2.Y * circleB.Position.Y);
|
|
Vector2 cLocal =
|
|
new Vector2(
|
|
(c.X - transformA.Position.X) * transformA.R.Col1.X +
|
|
(c.Y - transformA.Position.Y) * transformA.R.Col1.Y,
|
|
(c.X - transformA.Position.X) * transformA.R.Col2.X +
|
|
(c.Y - transformA.Position.Y) * transformA.R.Col2.Y);
|
|
|
|
// Find the min separating edge.
|
|
int normalIndex = 0;
|
|
float separation = -Settings.MaxFloat;
|
|
float radius = polygonA.Radius + circleB.Radius;
|
|
int vertexCount = polygonA.Vertices.Count;
|
|
|
|
for (int i = 0; i < vertexCount; ++i)
|
|
{
|
|
Vector2 value1 = polygonA.Normals[i];
|
|
Vector2 value2 = cLocal - polygonA.Vertices[i];
|
|
float s = value1.X * value2.X + value1.Y * value2.Y;
|
|
|
|
if (s > radius)
|
|
{
|
|
// Early out.
|
|
return;
|
|
}
|
|
|
|
if (s > separation)
|
|
{
|
|
separation = s;
|
|
normalIndex = i;
|
|
}
|
|
}
|
|
|
|
// Vertices that subtend the incident face.
|
|
int vertIndex1 = normalIndex;
|
|
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
|
|
Vector2 v1 = polygonA.Vertices[vertIndex1];
|
|
Vector2 v2 = polygonA.Vertices[vertIndex2];
|
|
|
|
// If the center is inside the polygon ...
|
|
if (separation < Settings.Epsilon)
|
|
{
|
|
manifold.PointCount = 1;
|
|
manifold.Type = ManifoldType.FaceA;
|
|
manifold.LocalNormal = polygonA.Normals[normalIndex];
|
|
manifold.LocalPoint = 0.5f * (v1 + v2);
|
|
|
|
ManifoldPoint p0 = manifold.Points[0];
|
|
|
|
p0.LocalPoint = circleB.Position;
|
|
p0.Id.Key = 0;
|
|
|
|
manifold.Points[0] = p0;
|
|
|
|
return;
|
|
}
|
|
|
|
// Compute barycentric coordinates
|
|
float u1 = (cLocal.X - v1.X) * (v2.X - v1.X) + (cLocal.Y - v1.Y) * (v2.Y - v1.Y);
|
|
float u2 = (cLocal.X - v2.X) * (v1.X - v2.X) + (cLocal.Y - v2.Y) * (v1.Y - v2.Y);
|
|
|
|
if (u1 <= 0.0f)
|
|
{
|
|
float r = (cLocal.X - v1.X) * (cLocal.X - v1.X) + (cLocal.Y - v1.Y) * (cLocal.Y - v1.Y);
|
|
if (r > radius * radius)
|
|
{
|
|
return;
|
|
}
|
|
|
|
manifold.PointCount = 1;
|
|
manifold.Type = ManifoldType.FaceA;
|
|
manifold.LocalNormal = cLocal - v1;
|
|
float factor = 1f /
|
|
(float)
|
|
Math.Sqrt(manifold.LocalNormal.X * manifold.LocalNormal.X +
|
|
manifold.LocalNormal.Y * manifold.LocalNormal.Y);
|
|
manifold.LocalNormal.X = manifold.LocalNormal.X * factor;
|
|
manifold.LocalNormal.Y = manifold.LocalNormal.Y * factor;
|
|
manifold.LocalPoint = v1;
|
|
|
|
ManifoldPoint p0b = manifold.Points[0];
|
|
|
|
p0b.LocalPoint = circleB.Position;
|
|
p0b.Id.Key = 0;
|
|
|
|
manifold.Points[0] = p0b;
|
|
}
|
|
else if (u2 <= 0.0f)
|
|
{
|
|
float r = (cLocal.X - v2.X) * (cLocal.X - v2.X) + (cLocal.Y - v2.Y) * (cLocal.Y - v2.Y);
|
|
if (r > radius * radius)
|
|
{
|
|
return;
|
|
}
|
|
|
|
manifold.PointCount = 1;
|
|
manifold.Type = ManifoldType.FaceA;
|
|
manifold.LocalNormal = cLocal - v2;
|
|
float factor = 1f /
|
|
(float)
|
|
Math.Sqrt(manifold.LocalNormal.X * manifold.LocalNormal.X +
|
|
manifold.LocalNormal.Y * manifold.LocalNormal.Y);
|
|
manifold.LocalNormal.X = manifold.LocalNormal.X * factor;
|
|
manifold.LocalNormal.Y = manifold.LocalNormal.Y * factor;
|
|
manifold.LocalPoint = v2;
|
|
|
|
ManifoldPoint p0c = manifold.Points[0];
|
|
|
|
p0c.LocalPoint = circleB.Position;
|
|
p0c.Id.Key = 0;
|
|
|
|
manifold.Points[0] = p0c;
|
|
}
|
|
else
|
|
{
|
|
Vector2 faceCenter = 0.5f * (v1 + v2);
|
|
Vector2 value1 = cLocal - faceCenter;
|
|
Vector2 value2 = polygonA.Normals[vertIndex1];
|
|
float separation2 = value1.X * value2.X + value1.Y * value2.Y;
|
|
if (separation2 > radius)
|
|
{
|
|
return;
|
|
}
|
|
|
|
manifold.PointCount = 1;
|
|
manifold.Type = ManifoldType.FaceA;
|
|
manifold.LocalNormal = polygonA.Normals[vertIndex1];
|
|
manifold.LocalPoint = faceCenter;
|
|
|
|
ManifoldPoint p0d = manifold.Points[0];
|
|
|
|
p0d.LocalPoint = circleB.Position;
|
|
p0d.Id.Key = 0;
|
|
|
|
manifold.Points[0] = p0d;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Compute the collision manifold between two polygons.
|
|
/// </summary>
|
|
/// <param name="manifold">The manifold.</param>
|
|
/// <param name="polyA">The poly A.</param>
|
|
/// <param name="transformA">The transform A.</param>
|
|
/// <param name="polyB">The poly B.</param>
|
|
/// <param name="transformB">The transform B.</param>
|
|
public static void CollidePolygons(ref Manifold manifold,
|
|
PolygonShape polyA, ref Transform transformA,
|
|
PolygonShape polyB, ref Transform transformB)
|
|
{
|
|
manifold.PointCount = 0;
|
|
float totalRadius = polyA.Radius + polyB.Radius;
|
|
|
|
int edgeA = 0;
|
|
float separationA = FindMaxSeparation(out edgeA, polyA, ref transformA, polyB, ref transformB);
|
|
if (separationA > totalRadius)
|
|
return;
|
|
|
|
int edgeB = 0;
|
|
float separationB = FindMaxSeparation(out edgeB, polyB, ref transformB, polyA, ref transformA);
|
|
if (separationB > totalRadius)
|
|
return;
|
|
|
|
PolygonShape poly1; // reference polygon
|
|
PolygonShape poly2; // incident polygon
|
|
Transform xf1, xf2;
|
|
int edge1; // reference edge
|
|
bool flip;
|
|
const float k_relativeTol = 0.98f;
|
|
const float k_absoluteTol = 0.001f;
|
|
|
|
if (separationB > k_relativeTol * separationA + k_absoluteTol)
|
|
{
|
|
poly1 = polyB;
|
|
poly2 = polyA;
|
|
xf1 = transformB;
|
|
xf2 = transformA;
|
|
edge1 = edgeB;
|
|
manifold.Type = ManifoldType.FaceB;
|
|
flip = true;
|
|
}
|
|
else
|
|
{
|
|
poly1 = polyA;
|
|
poly2 = polyB;
|
|
xf1 = transformA;
|
|
xf2 = transformB;
|
|
edge1 = edgeA;
|
|
manifold.Type = ManifoldType.FaceA;
|
|
flip = false;
|
|
}
|
|
|
|
FixedArray2<ClipVertex> incidentEdge;
|
|
FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
|
|
|
|
int count1 = poly1.Vertices.Count;
|
|
|
|
int iv1 = edge1;
|
|
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
|
|
|
|
Vector2 v11 = poly1.Vertices[iv1];
|
|
Vector2 v12 = poly1.Vertices[iv2];
|
|
|
|
float localTangentX = v12.X - v11.X;
|
|
float localTangentY = v12.Y - v11.Y;
|
|
|
|
float factor = 1f / (float)Math.Sqrt(localTangentX * localTangentX + localTangentY * localTangentY);
|
|
localTangentX = localTangentX * factor;
|
|
localTangentY = localTangentY * factor;
|
|
|
|
Vector2 localNormal = new Vector2(localTangentY, -localTangentX);
|
|
Vector2 planePoint = 0.5f * (v11 + v12);
|
|
|
|
Vector2 tangent = new Vector2(xf1.R.Col1.X * localTangentX + xf1.R.Col2.X * localTangentY,
|
|
xf1.R.Col1.Y * localTangentX + xf1.R.Col2.Y * localTangentY);
|
|
float normalx = tangent.Y;
|
|
float normaly = -tangent.X;
|
|
|
|
v11 = new Vector2(xf1.Position.X + xf1.R.Col1.X * v11.X + xf1.R.Col2.X * v11.Y,
|
|
xf1.Position.Y + xf1.R.Col1.Y * v11.X + xf1.R.Col2.Y * v11.Y);
|
|
v12 = new Vector2(xf1.Position.X + xf1.R.Col1.X * v12.X + xf1.R.Col2.X * v12.Y,
|
|
xf1.Position.Y + xf1.R.Col1.Y * v12.X + xf1.R.Col2.Y * v12.Y);
|
|
|
|
// Face offset.
|
|
float frontOffset = normalx * v11.X + normaly * v11.Y;
|
|
|
|
// Side offsets, extended by polytope skin thickness.
|
|
float sideOffset1 = -(tangent.X * v11.X + tangent.Y * v11.Y) + totalRadius;
|
|
float sideOffset2 = tangent.X * v12.X + tangent.Y * v12.Y + totalRadius;
|
|
|
|
// Clip incident edge against extruded edge1 side edges.
|
|
FixedArray2<ClipVertex> clipPoints1;
|
|
FixedArray2<ClipVertex> clipPoints2;
|
|
|
|
// Clip to box side 1
|
|
int np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1);
|
|
|
|
if (np < 2)
|
|
return;
|
|
|
|
// Clip to negative box side 1
|
|
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2);
|
|
|
|
if (np < 2)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Now clipPoints2 contains the clipped points.
|
|
manifold.LocalNormal = localNormal;
|
|
manifold.LocalPoint = planePoint;
|
|
|
|
int pointCount = 0;
|
|
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
|
|
{
|
|
Vector2 value = clipPoints2[i].V;
|
|
float separation = normalx * value.X + normaly * value.Y - frontOffset;
|
|
|
|
if (separation <= totalRadius)
|
|
{
|
|
ManifoldPoint cp = manifold.Points[pointCount];
|
|
Vector2 tmp = clipPoints2[i].V;
|
|
float tmp1X = tmp.X - xf2.Position.X;
|
|
float tmp1Y = tmp.Y - xf2.Position.Y;
|
|
cp.LocalPoint.X = tmp1X * xf2.R.Col1.X + tmp1Y * xf2.R.Col1.Y;
|
|
cp.LocalPoint.Y = tmp1X * xf2.R.Col2.X + tmp1Y * xf2.R.Col2.Y;
|
|
cp.Id = clipPoints2[i].ID;
|
|
|
|
if (flip)
|
|
{
|
|
// Swap features
|
|
ContactFeature cf = cp.Id.Features;
|
|
cp.Id.Features.IndexA = cf.IndexB;
|
|
cp.Id.Features.IndexB = cf.IndexA;
|
|
cp.Id.Features.TypeA = cf.TypeB;
|
|
cp.Id.Features.TypeB = cf.TypeA;
|
|
}
|
|
|
|
manifold.Points[pointCount] = cp;
|
|
|
|
++pointCount;
|
|
}
|
|
}
|
|
|
|
manifold.PointCount = pointCount;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Compute contact points for edge versus circle.
|
|
/// This accounts for edge connectivity.
|
|
/// </summary>
|
|
/// <param name="manifold">The manifold.</param>
|
|
/// <param name="edgeA">The edge A.</param>
|
|
/// <param name="transformA">The transform A.</param>
|
|
/// <param name="circleB">The circle B.</param>
|
|
/// <param name="transformB">The transform B.</param>
|
|
public static void CollideEdgeAndCircle(ref Manifold manifold,
|
|
EdgeShape edgeA, ref Transform transformA,
|
|
CircleShape circleB, ref Transform transformB)
|
|
{
|
|
manifold.PointCount = 0;
|
|
|
|
// Compute circle in frame of edge
|
|
Vector2 Q = MathUtils.MultiplyT(ref transformA, MathUtils.Multiply(ref transformB, ref circleB._position));
|
|
|
|
Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2;
|
|
Vector2 e = B - A;
|
|
|
|
// Barycentric coordinates
|
|
float u = Vector2.Dot(e, B - Q);
|
|
float v = Vector2.Dot(e, Q - A);
|
|
|
|
float radius = edgeA.Radius + circleB.Radius;
|
|
|
|
ContactFeature cf;
|
|
cf.IndexB = 0;
|
|
cf.TypeB = (byte)ContactFeatureType.Vertex;
|
|
|
|
Vector2 P, d;
|
|
|
|
// Region A
|
|
if (v <= 0.0f)
|
|
{
|
|
P = A;
|
|
d = Q - P;
|
|
float dd;
|
|
Vector2.Dot(ref d, ref d, out dd);
|
|
if (dd > radius * radius)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Is there an edge connected to A?
|
|
if (edgeA.HasVertex0)
|
|
{
|
|
Vector2 A1 = edgeA.Vertex0;
|
|
Vector2 B1 = A;
|
|
Vector2 e1 = B1 - A1;
|
|
float u1 = Vector2.Dot(e1, B1 - Q);
|
|
|
|
// Is the circle in Region AB of the previous edge?
|
|
if (u1 > 0.0f)
|
|
{
|
|
return;
|
|
}
|
|
}
|
|
|
|
cf.IndexA = 0;
|
|
cf.TypeA = (byte)ContactFeatureType.Vertex;
|
|
manifold.PointCount = 1;
|
|
manifold.Type = ManifoldType.Circles;
|
|
manifold.LocalNormal = Vector2.Zero;
|
|
manifold.LocalPoint = P;
|
|
ManifoldPoint mp = new ManifoldPoint();
|
|
mp.Id.Key = 0;
|
|
mp.Id.Features = cf;
|
|
mp.LocalPoint = circleB.Position;
|
|
manifold.Points[0] = mp;
|
|
return;
|
|
}
|
|
|
|
// Region B
|
|
if (u <= 0.0f)
|
|
{
|
|
P = B;
|
|
d = Q - P;
|
|
float dd;
|
|
Vector2.Dot(ref d, ref d, out dd);
|
|
if (dd > radius * radius)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Is there an edge connected to B?
|
|
if (edgeA.HasVertex3)
|
|
{
|
|
Vector2 B2 = edgeA.Vertex3;
|
|
Vector2 A2 = B;
|
|
Vector2 e2 = B2 - A2;
|
|
float v2 = Vector2.Dot(e2, Q - A2);
|
|
|
|
// Is the circle in Region AB of the next edge?
|
|
if (v2 > 0.0f)
|
|
{
|
|
return;
|
|
}
|
|
}
|
|
|
|
cf.IndexA = 1;
|
|
cf.TypeA = (byte)ContactFeatureType.Vertex;
|
|
manifold.PointCount = 1;
|
|
manifold.Type = ManifoldType.Circles;
|
|
manifold.LocalNormal = Vector2.Zero;
|
|
manifold.LocalPoint = P;
|
|
ManifoldPoint mp = new ManifoldPoint();
|
|
mp.Id.Key = 0;
|
|
mp.Id.Features = cf;
|
|
mp.LocalPoint = circleB.Position;
|
|
manifold.Points[0] = mp;
|
|
return;
|
|
}
|
|
|
|
// Region AB
|
|
float den;
|
|
Vector2.Dot(ref e, ref e, out den);
|
|
Debug.Assert(den > 0.0f);
|
|
P = (1.0f / den) * (u * A + v * B);
|
|
d = Q - P;
|
|
float dd2;
|
|
Vector2.Dot(ref d, ref d, out dd2);
|
|
if (dd2 > radius * radius)
|
|
{
|
|
return;
|
|
}
|
|
|
|
Vector2 n = new Vector2(-e.Y, e.X);
|
|
if (Vector2.Dot(n, Q - A) < 0.0f)
|
|
{
|
|
n = new Vector2(-n.X, -n.Y);
|
|
}
|
|
n.Normalize();
|
|
|
|
cf.IndexA = 0;
|
|
cf.TypeA = (byte)ContactFeatureType.Face;
|
|
manifold.PointCount = 1;
|
|
manifold.Type = ManifoldType.FaceA;
|
|
manifold.LocalNormal = n;
|
|
manifold.LocalPoint = A;
|
|
ManifoldPoint mp2 = new ManifoldPoint();
|
|
mp2.Id.Key = 0;
|
|
mp2.Id.Features = cf;
|
|
mp2.LocalPoint = circleB.Position;
|
|
manifold.Points[0] = mp2;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Collides and edge and a polygon, taking into account edge adjacency.
|
|
/// </summary>
|
|
/// <param name="manifold">The manifold.</param>
|
|
/// <param name="edgeA">The edge A.</param>
|
|
/// <param name="xfA">The xf A.</param>
|
|
/// <param name="polygonB">The polygon B.</param>
|
|
/// <param name="xfB">The xf B.</param>
|
|
public static void CollideEdgeAndPolygon(ref Manifold manifold,
|
|
EdgeShape edgeA, ref Transform xfA,
|
|
PolygonShape polygonB, ref Transform xfB)
|
|
{
|
|
MathUtils.MultiplyT(ref xfA, ref xfB, out _xf);
|
|
|
|
// Edge geometry
|
|
_edgeA.V0 = edgeA.Vertex0;
|
|
_edgeA.V1 = edgeA.Vertex1;
|
|
_edgeA.V2 = edgeA.Vertex2;
|
|
_edgeA.V3 = edgeA.Vertex3;
|
|
Vector2 e = _edgeA.V2 - _edgeA.V1;
|
|
|
|
// Normal points outwards in CCW order.
|
|
_edgeA.Normal = new Vector2(e.Y, -e.X);
|
|
_edgeA.Normal.Normalize();
|
|
_edgeA.HasVertex0 = edgeA.HasVertex0;
|
|
_edgeA.HasVertex3 = edgeA.HasVertex3;
|
|
|
|
// Proxy for edge
|
|
_proxyA.Vertices[0] = _edgeA.V1;
|
|
_proxyA.Vertices[1] = _edgeA.V2;
|
|
_proxyA.Normals[0] = _edgeA.Normal;
|
|
_proxyA.Normals[1] = -_edgeA.Normal;
|
|
_proxyA.Centroid = 0.5f * (_edgeA.V1 + _edgeA.V2);
|
|
_proxyA.Count = 2;
|
|
|
|
// Proxy for polygon
|
|
_proxyB.Count = polygonB.Vertices.Count;
|
|
_proxyB.Centroid = MathUtils.Multiply(ref _xf, ref polygonB.MassData.Centroid);
|
|
for (int i = 0; i < polygonB.Vertices.Count; ++i)
|
|
{
|
|
_proxyB.Vertices[i] = MathUtils.Multiply(ref _xf, polygonB.Vertices[i]);
|
|
_proxyB.Normals[i] = MathUtils.Multiply(ref _xf.R, polygonB.Normals[i]);
|
|
}
|
|
|
|
_radius = 2.0f * Settings.PolygonRadius;
|
|
|
|
_limit11 = Vector2.Zero;
|
|
_limit12 = Vector2.Zero;
|
|
_limit21 = Vector2.Zero;
|
|
_limit22 = Vector2.Zero;
|
|
|
|
//Collide(ref manifold); inline start
|
|
manifold.PointCount = 0;
|
|
|
|
//ComputeAdjacency(); inline start
|
|
Vector2 v0 = _edgeA.V0;
|
|
Vector2 v1 = _edgeA.V1;
|
|
Vector2 v2 = _edgeA.V2;
|
|
Vector2 v3 = _edgeA.V3;
|
|
|
|
// Determine allowable the normal regions based on adjacency.
|
|
// Note: it may be possible that no normal is admissable.
|
|
Vector2 centerB = _proxyB.Centroid;
|
|
if (_edgeA.HasVertex0)
|
|
{
|
|
Vector2 e0 = v1 - v0;
|
|
Vector2 e1 = v2 - v1;
|
|
Vector2 n0 = new Vector2(e0.Y, -e0.X);
|
|
Vector2 n1 = new Vector2(e1.Y, -e1.X);
|
|
n0.Normalize();
|
|
n1.Normalize();
|
|
|
|
bool convex = MathUtils.Cross(n0, n1) >= 0.0f;
|
|
bool front0 = Vector2.Dot(n0, centerB - v0) >= 0.0f;
|
|
bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f;
|
|
|
|
if (convex)
|
|
{
|
|
if (front0 || front1)
|
|
{
|
|
_limit11 = n1;
|
|
_limit12 = n0;
|
|
}
|
|
else
|
|
{
|
|
_limit11 = -n1;
|
|
_limit12 = -n0;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (front0 && front1)
|
|
{
|
|
_limit11 = n0;
|
|
_limit12 = n1;
|
|
}
|
|
else
|
|
{
|
|
_limit11 = -n0;
|
|
_limit12 = -n1;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_limit11 = Vector2.Zero;
|
|
_limit12 = Vector2.Zero;
|
|
}
|
|
|
|
if (_edgeA.HasVertex3)
|
|
{
|
|
Vector2 e1 = v2 - v1;
|
|
Vector2 e2 = v3 - v2;
|
|
Vector2 n1 = new Vector2(e1.Y, -e1.X);
|
|
Vector2 n2 = new Vector2(e2.Y, -e2.X);
|
|
n1.Normalize();
|
|
n2.Normalize();
|
|
|
|
bool convex = MathUtils.Cross(n1, n2) >= 0.0f;
|
|
bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f;
|
|
bool front2 = Vector2.Dot(n2, centerB - v2) >= 0.0f;
|
|
|
|
if (convex)
|
|
{
|
|
if (front1 || front2)
|
|
{
|
|
_limit21 = n2;
|
|
_limit22 = n1;
|
|
}
|
|
else
|
|
{
|
|
_limit21 = -n2;
|
|
_limit22 = -n1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (front1 && front2)
|
|
{
|
|
_limit21 = n1;
|
|
_limit22 = n2;
|
|
}
|
|
else
|
|
{
|
|
_limit21 = -n1;
|
|
_limit22 = -n2;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_limit21 = Vector2.Zero;
|
|
_limit22 = Vector2.Zero;
|
|
}
|
|
|
|
//ComputeAdjacency(); inline end
|
|
|
|
//EPAxis edgeAxis = ComputeEdgeSeparation(); inline start
|
|
EPAxis edgeAxis = ComputeEdgeSeparation();
|
|
|
|
// If no valid normal can be found than this edge should not collide.
|
|
// This can happen on the middle edge of a 3-edge zig-zag chain.
|
|
if (edgeAxis.Type == EPAxisType.Unknown)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (edgeAxis.Separation > _radius)
|
|
{
|
|
return;
|
|
}
|
|
|
|
EPAxis polygonAxis = ComputePolygonSeparation();
|
|
if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > _radius)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Use hysteresis for jitter reduction.
|
|
const float k_relativeTol = 0.98f;
|
|
const float k_absoluteTol = 0.001f;
|
|
|
|
EPAxis primaryAxis;
|
|
if (polygonAxis.Type == EPAxisType.Unknown)
|
|
{
|
|
primaryAxis = edgeAxis;
|
|
}
|
|
else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol)
|
|
{
|
|
primaryAxis = polygonAxis;
|
|
}
|
|
else
|
|
{
|
|
primaryAxis = edgeAxis;
|
|
}
|
|
|
|
EPProxy proxy1;
|
|
EPProxy proxy2;
|
|
FixedArray2<ClipVertex> incidentEdge = new FixedArray2<ClipVertex>();
|
|
if (primaryAxis.Type == EPAxisType.EdgeA)
|
|
{
|
|
proxy1 = _proxyA;
|
|
proxy2 = _proxyB;
|
|
manifold.Type = ManifoldType.FaceA;
|
|
}
|
|
else
|
|
{
|
|
proxy1 = _proxyB;
|
|
proxy2 = _proxyA;
|
|
manifold.Type = ManifoldType.FaceB;
|
|
}
|
|
|
|
int edge1 = primaryAxis.Index;
|
|
|
|
FindIncidentEdge(ref incidentEdge, proxy1, primaryAxis.Index, proxy2);
|
|
int count1 = proxy1.Count;
|
|
|
|
int iv1 = edge1;
|
|
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
|
|
|
|
Vector2 v11 = proxy1.Vertices[iv1];
|
|
Vector2 v12 = proxy1.Vertices[iv2];
|
|
|
|
Vector2 tangent = v12 - v11;
|
|
tangent.Normalize();
|
|
|
|
Vector2 normal = MathUtils.Cross(tangent, 1.0f);
|
|
Vector2 planePoint = 0.5f * (v11 + v12);
|
|
|
|
// Face offset.
|
|
float frontOffset = Vector2.Dot(normal, v11);
|
|
|
|
// Side offsets, extended by polytope skin thickness.
|
|
float sideOffset1 = -Vector2.Dot(tangent, v11) + _radius;
|
|
float sideOffset2 = Vector2.Dot(tangent, v12) + _radius;
|
|
|
|
// Clip incident edge against extruded edge1 side edges.
|
|
FixedArray2<ClipVertex> clipPoints1;
|
|
FixedArray2<ClipVertex> clipPoints2;
|
|
int np;
|
|
|
|
// Clip to box side 1
|
|
np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1);
|
|
|
|
if (np < Settings.MaxManifoldPoints)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Clip to negative box side 1
|
|
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2);
|
|
|
|
if (np < Settings.MaxManifoldPoints)
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Now clipPoints2 contains the clipped points.
|
|
if (primaryAxis.Type == EPAxisType.EdgeA)
|
|
{
|
|
manifold.LocalNormal = normal;
|
|
manifold.LocalPoint = planePoint;
|
|
}
|
|
else
|
|
{
|
|
manifold.LocalNormal = MathUtils.MultiplyT(ref _xf.R, ref normal);
|
|
manifold.LocalPoint = MathUtils.MultiplyT(ref _xf, ref planePoint);
|
|
}
|
|
|
|
int pointCount = 0;
|
|
for (int i1 = 0; i1 < Settings.MaxManifoldPoints; ++i1)
|
|
{
|
|
float separation = Vector2.Dot(normal, clipPoints2[i1].V) - frontOffset;
|
|
|
|
if (separation <= _radius)
|
|
{
|
|
ManifoldPoint cp = manifold.Points[pointCount];
|
|
|
|
if (primaryAxis.Type == EPAxisType.EdgeA)
|
|
{
|
|
cp.LocalPoint = MathUtils.MultiplyT(ref _xf, clipPoints2[i1].V);
|
|
cp.Id = clipPoints2[i1].ID;
|
|
}
|
|
else
|
|
{
|
|
cp.LocalPoint = clipPoints2[i1].V;
|
|
cp.Id.Features.TypeA = clipPoints2[i1].ID.Features.TypeB;
|
|
cp.Id.Features.TypeB = clipPoints2[i1].ID.Features.TypeA;
|
|
cp.Id.Features.IndexA = clipPoints2[i1].ID.Features.IndexB;
|
|
cp.Id.Features.IndexB = clipPoints2[i1].ID.Features.IndexA;
|
|
}
|
|
|
|
manifold.Points[pointCount] = cp;
|
|
|
|
++pointCount;
|
|
}
|
|
}
|
|
|
|
manifold.PointCount = pointCount;
|
|
|
|
//Collide(ref manifold); inline end
|
|
}
|
|
|
|
private static EPAxis ComputeEdgeSeparation()
|
|
{
|
|
// EdgeA separation
|
|
EPAxis bestAxis;
|
|
bestAxis.Type = EPAxisType.Unknown;
|
|
bestAxis.Index = -1;
|
|
bestAxis.Separation = -Settings.MaxFloat;
|
|
_tmpNormals[0] = _edgeA.Normal;
|
|
_tmpNormals[1] = -_edgeA.Normal;
|
|
|
|
for (int i = 0; i < 2; ++i)
|
|
{
|
|
Vector2 n = _tmpNormals[i];
|
|
|
|
// Adjacency
|
|
bool valid1 = MathUtils.Cross(n, _limit11) >= -Settings.AngularSlop &&
|
|
MathUtils.Cross(_limit12, n) >= -Settings.AngularSlop;
|
|
bool valid2 = MathUtils.Cross(n, _limit21) >= -Settings.AngularSlop &&
|
|
MathUtils.Cross(_limit22, n) >= -Settings.AngularSlop;
|
|
|
|
if (valid1 == false || valid2 == false)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
EPAxis axis;
|
|
axis.Type = EPAxisType.EdgeA;
|
|
axis.Index = i;
|
|
axis.Separation = Settings.MaxFloat;
|
|
|
|
for (int j = 0; j < _proxyB.Count; ++j)
|
|
{
|
|
float s = Vector2.Dot(n, _proxyB.Vertices[j] - _edgeA.V1);
|
|
if (s < axis.Separation)
|
|
{
|
|
axis.Separation = s;
|
|
}
|
|
}
|
|
|
|
if (axis.Separation > _radius)
|
|
{
|
|
return axis;
|
|
}
|
|
|
|
if (axis.Separation > bestAxis.Separation)
|
|
{
|
|
bestAxis = axis;
|
|
}
|
|
}
|
|
|
|
return bestAxis;
|
|
}
|
|
|
|
private static EPAxis ComputePolygonSeparation()
|
|
{
|
|
EPAxis axis;
|
|
axis.Type = EPAxisType.Unknown;
|
|
axis.Index = -1;
|
|
axis.Separation = -Settings.MaxFloat;
|
|
for (int i = 0; i < _proxyB.Count; ++i)
|
|
{
|
|
Vector2 n = -_proxyB.Normals[i];
|
|
|
|
// Adjacency
|
|
bool valid1 = MathUtils.Cross(n, _limit11) >= -Settings.AngularSlop &&
|
|
MathUtils.Cross(_limit12, n) >= -Settings.AngularSlop;
|
|
bool valid2 = MathUtils.Cross(n, _limit21) >= -Settings.AngularSlop &&
|
|
MathUtils.Cross(_limit22, n) >= -Settings.AngularSlop;
|
|
|
|
if (valid1 == false && valid2 == false)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
float s1 = Vector2.Dot(n, _proxyB.Vertices[i] - _edgeA.V1);
|
|
float s2 = Vector2.Dot(n, _proxyB.Vertices[i] - _edgeA.V2);
|
|
float s = Math.Min(s1, s2);
|
|
|
|
if (s > _radius)
|
|
{
|
|
axis.Type = EPAxisType.EdgeB;
|
|
axis.Index = i;
|
|
axis.Separation = s;
|
|
}
|
|
|
|
if (s > axis.Separation)
|
|
{
|
|
axis.Type = EPAxisType.EdgeB;
|
|
axis.Index = i;
|
|
axis.Separation = s;
|
|
}
|
|
}
|
|
|
|
return axis;
|
|
}
|
|
|
|
private static void FindIncidentEdge(ref FixedArray2<ClipVertex> c, EPProxy proxy1, int edge1, EPProxy proxy2)
|
|
{
|
|
int count2 = proxy2.Count;
|
|
|
|
Debug.Assert(0 <= edge1 && edge1 < proxy1.Count);
|
|
|
|
// Get the normal of the reference edge in proxy2's frame.
|
|
Vector2 normal1 = proxy1.Normals[edge1];
|
|
|
|
// Find the incident edge on proxy2.
|
|
int index = 0;
|
|
float minDot = float.MaxValue;
|
|
for (int i = 0; i < count2; ++i)
|
|
{
|
|
float dot = Vector2.Dot(normal1, proxy2.Normals[i]);
|
|
if (dot < minDot)
|
|
{
|
|
minDot = dot;
|
|
index = i;
|
|
}
|
|
}
|
|
|
|
// Build the clip vertices for the incident edge.
|
|
int i1 = index;
|
|
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
|
|
|
|
ClipVertex cTemp = new ClipVertex();
|
|
cTemp.V = proxy2.Vertices[i1];
|
|
cTemp.ID.Features.IndexA = (byte)edge1;
|
|
cTemp.ID.Features.IndexB = (byte)i1;
|
|
cTemp.ID.Features.TypeA = (byte)ContactFeatureType.Face;
|
|
cTemp.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
|
|
c[0] = cTemp;
|
|
|
|
cTemp.V = proxy2.Vertices[i2];
|
|
cTemp.ID.Features.IndexA = (byte)edge1;
|
|
cTemp.ID.Features.IndexB = (byte)i2;
|
|
cTemp.ID.Features.TypeA = (byte)ContactFeatureType.Face;
|
|
cTemp.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
|
|
c[1] = cTemp;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Clipping for contact manifolds.
|
|
/// </summary>
|
|
/// <param name="vOut">The v out.</param>
|
|
/// <param name="vIn">The v in.</param>
|
|
/// <param name="normal">The normal.</param>
|
|
/// <param name="offset">The offset.</param>
|
|
/// <param name="vertexIndexA">The vertex index A.</param>
|
|
/// <returns></returns>
|
|
private static int ClipSegmentToLine(out FixedArray2<ClipVertex> vOut, ref FixedArray2<ClipVertex> vIn,
|
|
Vector2 normal, float offset, int vertexIndexA)
|
|
{
|
|
vOut = new FixedArray2<ClipVertex>();
|
|
|
|
ClipVertex v0 = vIn[0];
|
|
ClipVertex v1 = vIn[1];
|
|
|
|
// Start with no output points
|
|
int numOut = 0;
|
|
|
|
// Calculate the distance of end points to the line
|
|
float distance0 = normal.X * v0.V.X + normal.Y * v0.V.Y - offset;
|
|
float distance1 = normal.X * v1.V.X + normal.Y * v1.V.Y - offset;
|
|
|
|
// If the points are behind the plane
|
|
if (distance0 <= 0.0f) vOut[numOut++] = v0;
|
|
if (distance1 <= 0.0f) vOut[numOut++] = v1;
|
|
|
|
// If the points are on different sides of the plane
|
|
if (distance0 * distance1 < 0.0f)
|
|
{
|
|
// Find intersection point of edge and plane
|
|
float interp = distance0 / (distance0 - distance1);
|
|
|
|
ClipVertex cv = vOut[numOut];
|
|
|
|
cv.V.X = v0.V.X + interp * (v1.V.X - v0.V.X);
|
|
cv.V.Y = v0.V.Y + interp * (v1.V.Y - v0.V.Y);
|
|
|
|
// VertexA is hitting edgeB.
|
|
cv.ID.Features.IndexA = (byte)vertexIndexA;
|
|
cv.ID.Features.IndexB = v0.ID.Features.IndexB;
|
|
cv.ID.Features.TypeA = (byte)ContactFeatureType.Vertex;
|
|
cv.ID.Features.TypeB = (byte)ContactFeatureType.Face;
|
|
|
|
vOut[numOut] = cv;
|
|
|
|
++numOut;
|
|
}
|
|
|
|
return numOut;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Find the separation between poly1 and poly2 for a give edge normal on poly1.
|
|
/// </summary>
|
|
/// <param name="poly1">The poly1.</param>
|
|
/// <param name="xf1">The XF1.</param>
|
|
/// <param name="edge1">The edge1.</param>
|
|
/// <param name="poly2">The poly2.</param>
|
|
/// <param name="xf2">The XF2.</param>
|
|
/// <returns></returns>
|
|
private static float EdgeSeparation(PolygonShape poly1, ref Transform xf1, int edge1,
|
|
PolygonShape poly2, ref Transform xf2)
|
|
{
|
|
int count2 = poly2.Vertices.Count;
|
|
|
|
Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count);
|
|
|
|
// Convert normal from poly1's frame into poly2's frame.
|
|
Vector2 p1n = poly1.Normals[edge1];
|
|
|
|
float normalWorldx = xf1.R.Col1.X * p1n.X + xf1.R.Col2.X * p1n.Y;
|
|
float normalWorldy = xf1.R.Col1.Y * p1n.X + xf1.R.Col2.Y * p1n.Y;
|
|
|
|
Vector2 normal = new Vector2(normalWorldx * xf2.R.Col1.X + normalWorldy * xf2.R.Col1.Y,
|
|
normalWorldx * xf2.R.Col2.X + normalWorldy * xf2.R.Col2.Y);
|
|
|
|
// Find support vertex on poly2 for -normal.
|
|
int index = 0;
|
|
float minDot = Settings.MaxFloat;
|
|
|
|
for (int i = 0; i < count2; ++i)
|
|
{
|
|
float dot = Vector2.Dot(poly2.Vertices[i], normal);
|
|
|
|
if (dot < minDot)
|
|
{
|
|
minDot = dot;
|
|
index = i;
|
|
}
|
|
}
|
|
|
|
Vector2 p1ve = poly1.Vertices[edge1];
|
|
Vector2 p2vi = poly2.Vertices[index];
|
|
|
|
return ((xf2.Position.X + xf2.R.Col1.X * p2vi.X + xf2.R.Col2.X * p2vi.Y) -
|
|
(xf1.Position.X + xf1.R.Col1.X * p1ve.X + xf1.R.Col2.X * p1ve.Y)) * normalWorldx +
|
|
((xf2.Position.Y + xf2.R.Col1.Y * p2vi.X + xf2.R.Col2.Y * p2vi.Y) -
|
|
(xf1.Position.Y + xf1.R.Col1.Y * p1ve.X + xf1.R.Col2.Y * p1ve.Y)) * normalWorldy;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Find the max separation between poly1 and poly2 using edge normals from poly1.
|
|
/// </summary>
|
|
/// <param name="edgeIndex">Index of the edge.</param>
|
|
/// <param name="poly1">The poly1.</param>
|
|
/// <param name="xf1">The XF1.</param>
|
|
/// <param name="poly2">The poly2.</param>
|
|
/// <param name="xf2">The XF2.</param>
|
|
/// <returns></returns>
|
|
private static float FindMaxSeparation(out int edgeIndex,
|
|
PolygonShape poly1, ref Transform xf1,
|
|
PolygonShape poly2, ref Transform xf2)
|
|
{
|
|
int count1 = poly1.Vertices.Count;
|
|
|
|
// Vector pointing from the centroid of poly1 to the centroid of poly2.
|
|
float dx = (xf2.Position.X + xf2.R.Col1.X * poly2.MassData.Centroid.X +
|
|
xf2.R.Col2.X * poly2.MassData.Centroid.Y) -
|
|
(xf1.Position.X + xf1.R.Col1.X * poly1.MassData.Centroid.X +
|
|
xf1.R.Col2.X * poly1.MassData.Centroid.Y);
|
|
float dy = (xf2.Position.Y + xf2.R.Col1.Y * poly2.MassData.Centroid.X +
|
|
xf2.R.Col2.Y * poly2.MassData.Centroid.Y) -
|
|
(xf1.Position.Y + xf1.R.Col1.Y * poly1.MassData.Centroid.X +
|
|
xf1.R.Col2.Y * poly1.MassData.Centroid.Y);
|
|
Vector2 dLocal1 = new Vector2(dx * xf1.R.Col1.X + dy * xf1.R.Col1.Y, dx * xf1.R.Col2.X + dy * xf1.R.Col2.Y);
|
|
|
|
// Find edge normal on poly1 that has the largest projection onto d.
|
|
int edge = 0;
|
|
float maxDot = -Settings.MaxFloat;
|
|
for (int i = 0; i < count1; ++i)
|
|
{
|
|
float dot = Vector2.Dot(poly1.Normals[i], dLocal1);
|
|
if (dot > maxDot)
|
|
{
|
|
maxDot = dot;
|
|
edge = i;
|
|
}
|
|
}
|
|
|
|
// Get the separation for the edge normal.
|
|
float s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);
|
|
|
|
// Check the separation for the previous edge normal.
|
|
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
|
|
float sPrev = EdgeSeparation(poly1, ref xf1, prevEdge, poly2, ref xf2);
|
|
|
|
// Check the separation for the next edge normal.
|
|
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
|
|
float sNext = EdgeSeparation(poly1, ref xf1, nextEdge, poly2, ref xf2);
|
|
|
|
// Find the best edge and the search direction.
|
|
int bestEdge;
|
|
float bestSeparation;
|
|
int increment;
|
|
if (sPrev > s && sPrev > sNext)
|
|
{
|
|
increment = -1;
|
|
bestEdge = prevEdge;
|
|
bestSeparation = sPrev;
|
|
}
|
|
else if (sNext > s)
|
|
{
|
|
increment = 1;
|
|
bestEdge = nextEdge;
|
|
bestSeparation = sNext;
|
|
}
|
|
else
|
|
{
|
|
edgeIndex = edge;
|
|
return s;
|
|
}
|
|
|
|
// Perform a local search for the best edge normal.
|
|
for (; ; )
|
|
{
|
|
if (increment == -1)
|
|
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
|
|
else
|
|
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
|
|
|
|
s = EdgeSeparation(poly1, ref xf1, edge, poly2, ref xf2);
|
|
|
|
if (s > bestSeparation)
|
|
{
|
|
bestEdge = edge;
|
|
bestSeparation = s;
|
|
}
|
|
else
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
|
|
edgeIndex = bestEdge;
|
|
return bestSeparation;
|
|
}
|
|
|
|
private static void FindIncidentEdge(out FixedArray2<ClipVertex> c,
|
|
PolygonShape poly1, ref Transform xf1, int edge1,
|
|
PolygonShape poly2, ref Transform xf2)
|
|
{
|
|
c = new FixedArray2<ClipVertex>();
|
|
|
|
int count2 = poly2.Vertices.Count;
|
|
|
|
Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count);
|
|
|
|
// Get the normal of the reference edge in poly2's frame.
|
|
Vector2 v = poly1.Normals[edge1];
|
|
float tmpx = xf1.R.Col1.X * v.X + xf1.R.Col2.X * v.Y;
|
|
float tmpy = xf1.R.Col1.Y * v.X + xf1.R.Col2.Y * v.Y;
|
|
Vector2 normal1 = new Vector2(tmpx * xf2.R.Col1.X + tmpy * xf2.R.Col1.Y,
|
|
tmpx * xf2.R.Col2.X + tmpy * xf2.R.Col2.Y);
|
|
|
|
// Find the incident edge on poly2.
|
|
int index = 0;
|
|
float minDot = Settings.MaxFloat;
|
|
for (int i = 0; i < count2; ++i)
|
|
{
|
|
float dot = Vector2.Dot(normal1, poly2.Normals[i]);
|
|
if (dot < minDot)
|
|
{
|
|
minDot = dot;
|
|
index = i;
|
|
}
|
|
}
|
|
|
|
// Build the clip vertices for the incident edge.
|
|
int i1 = index;
|
|
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
|
|
|
|
ClipVertex cv0 = c[0];
|
|
|
|
Vector2 v1 = poly2.Vertices[i1];
|
|
cv0.V.X = xf2.Position.X + xf2.R.Col1.X * v1.X + xf2.R.Col2.X * v1.Y;
|
|
cv0.V.Y = xf2.Position.Y + xf2.R.Col1.Y * v1.X + xf2.R.Col2.Y * v1.Y;
|
|
cv0.ID.Features.IndexA = (byte)edge1;
|
|
cv0.ID.Features.IndexB = (byte)i1;
|
|
cv0.ID.Features.TypeA = (byte)ContactFeatureType.Face;
|
|
cv0.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
|
|
|
|
c[0] = cv0;
|
|
|
|
ClipVertex cv1 = c[1];
|
|
Vector2 v2 = poly2.Vertices[i2];
|
|
cv1.V.X = xf2.Position.X + xf2.R.Col1.X * v2.X + xf2.R.Col2.X * v2.Y;
|
|
cv1.V.Y = xf2.Position.Y + xf2.R.Col1.Y * v2.X + xf2.R.Col2.Y * v2.Y;
|
|
cv1.ID.Features.IndexA = (byte)edge1;
|
|
cv1.ID.Features.IndexB = (byte)i2;
|
|
cv1.ID.Features.TypeA = (byte)ContactFeatureType.Face;
|
|
cv1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
|
|
|
|
c[1] = cv1;
|
|
}
|
|
}
|
|
} |