/// <summary>
/// tests for an overlap of shapeA and shapeB. Returns false if both Shapes are chains or if they are not overlapping.
/// </summary>
/// <returns><c>true</c>, if overlap was tested, <c>false</c> otherwise.</returns>
/// <param name="shapeA">Shape a.</param>
/// <param name="shapeB">Shape b.</param>
/// <param name="transformA">Transform a.</param>
/// <param name="transformB">Transform b.</param>
public static bool testOverlap( Shape shapeA, Shape shapeB, ref FSTransform transformA, ref FSTransform transformB )
{
if( shapeA.childCount == 1 && shapeB.childCount == 1 )
return Collision.testOverlap( shapeA, 0, shapeB, 0, ref transformA, ref transformB );
if( shapeA.childCount > 1 )
{
for( var i = 0; i < shapeA.childCount; i++ )
{
if( Collision.testOverlap( shapeA, i, shapeB, 0, ref transformA, ref transformB ) )
return true;
}
}
if( shapeB.childCount > 1 )
{
for( var i = 0; i < shapeB.childCount; i++ )
{
if( Collision.testOverlap( shapeA, 0, shapeB, i, ref transformA, ref transformB ) )
return true;
}
}
return false;
}
/// <summary>
/// tests for an overlap of shapeA and shapeB. Returns false if both Shapes are chains or if they are not overlapping.
/// </summary>
/// <returns><c>true</c>, if overlap was tested, <c>false</c> otherwise.</returns>
/// <param name="shapeA">Shape a.</param>
/// <param name="shapeB">Shape b.</param>
/// <param name="transformA">Transform a.</param>
/// <param name="transformB">Transform b.</param>
public static bool testOverlap(Shape shapeA, Shape shapeB, ref FSTransform transformA, ref FSTransform transformB)
{
if (shapeA.childCount == 1 && shapeB.childCount == 1)
{
return(Collision.testOverlap(shapeA, 0, shapeB, 0, ref transformA, ref transformB));
}
if (shapeA.childCount > 1)
{
for (var i = 0; i < shapeA.childCount; i++)
{
if (Collision.testOverlap(shapeA, i, shapeB, 0, ref transformA, ref transformB))
{
return(true);
}
}
}
if (shapeB.childCount > 1)
{
for (var i = 0; i < shapeB.childCount; i++)
{
if (Collision.testOverlap(shapeA, 0, shapeB, i, ref transformA, ref transformB))
{
return(true);
}
}
}
return(false);
}
private void DrawShape(Fixture fixture, Transform xf, Color color)
{
switch (fixture.Shape.ShapeType)
{
case ShapeType.Circle:
{
var circle = (FarseerCircleShape)fixture.Shape;
Vector2 center = MathUtils.Mul(ref xf, circle.Position);
float radius = circle.Radius;
Vector2 axis = xf.q.GetXAxis();
DrawSolidCircle(center, radius, axis, color.R, color.G, color.B);
}
break;
case ShapeType.Polygon:
{
var poly = (PolygonShape)fixture.Shape;
int vertexCount = poly.Vertices.Count;
var vertices = new Vector2[vertexCount];
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = MathUtils.Mul(ref xf, poly.Vertices[i]);
}
DrawSolidPolygon(vertices, vertexCount, color.R, color.G, color.B);
}
break;
default:
throw new NotImplementedException();
}
}
/// <summary>
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
/// </summary>
/// <param name="aabb">The aabb results.</param>
/// <param name="transform">The world transform of the shape.</param>
/// <param name="childIndex">The child shape index.</param>
public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
{
Vector2 p = transform.Position + MathUtils.Multiply(ref transform.R, Position);
aabb.LowerBound = new Vector2(p.x - Radius, p.y - Radius);
aabb.UpperBound = new Vector2(p.x + Radius, p.y + Radius);
}
static bool collideCircles(CircleShape circleA, ref FSTransform firstTransform, CircleShape circleB, ref FSTransform secondTransform, out FSCollisionResult result)
{
result = new FSCollisionResult();
Collision.collideCircles(ref _manifold, circleA, ref firstTransform, circleB, ref secondTransform);
if (_manifold.pointCount > 0)
{
// this is essentically directly from ContactSolver.WorldManifold.Initialize. To avoid doing the extra math twice we duplicate this code
// here because it doesnt return some values we need to calculate separation
var pointA = MathUtils.mul(ref firstTransform, _manifold.localPoint);
var pointB = MathUtils.mul(ref secondTransform, _manifold.points[0].localPoint);
result.normal = pointA - pointB;
result.normal.Normalize();
var cA = pointA - circleA.radius * result.normal;
var cB = pointB + circleB.radius * result.normal;
result.point = 0.5f * (cA + cB);
result.point *= FSConvert.simToDisplay;
var separation = Vector2.Dot(pointA - pointB, result.normal) - circleA.radius - circleB.radius;
result.minimumTranslationVector = result.normal * Math.Abs(separation);
#if DEBUG_FSCOLLISIONS
Debug.drawPixel(result.point, 5, Color.Red, 0.2f);
Debug.drawLine(result.point, result.point + result.normal * 20, Color.Yellow, 0.2f);
#endif
return(true);
}
return(false);
}
/// <summary>
/// Test a point for containment in this shape. This only works for convex shapes.
/// </summary>
/// <param name="transform">The shape world transform.</param>
/// <param name="point">a point in world coordinates.</param>
/// <returns>True if the point is inside the shape</returns>
public override bool TestPoint(ref Transform transform, ref Vector2 point)
{
Vector2 center = transform.Position + MathUtils.Multiply(ref transform.R, Position);
Vector2 d = point - center;
return(Vector2.Dot(d, d) <= Radius * Radius);
}
public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc)
{
sc = Vector2.zero;
Vector2 p = MathUtils.Multiply(ref xf, Position);
float l = -(Vector2.Dot(normal, p) - offset);
if (l < -Radius + Settings.Epsilon)
{
//Completely dry
return(0);
}
if (l > Radius)
{
//Completely wet
sc = p;
return(Settings.Pi * Radius * Radius);
}
//Magic
float r2 = Radius * Radius;
float l2 = l * l;
float area = r2 * (float)((Mathf.Asin(l / Radius) + Settings.Pi / 2) + l * Mathf.Sqrt(r2 - l2));
float com = -2.0f / 3.0f * Mathf.Pow(r2 - l2, 1.5f) / area;
sc.x = p.x + normal.x * com;
sc.y = p.y + normal.y * com;
return(area);
}
/// <summary>
/// Performs a global physical picking operation and returns the <see cref="ShapeInfo">shapes</see> that
/// intersect the specified world coordinate area.
/// </summary>
/// <param name="worldCoord"></param>
/// <param name="size"></param>
/// <param name="pickedShapes">
/// A list that will be filled with all shapes that were found.
/// The list will not be cleared before adding items.
/// </param>
/// <returns>Returns whether any new shape was found.</returns>
public bool PickShapes(Vector2 worldCoord, Vector2 size, List <ShapeInfo> pickedShapes)
{
List <RigidBody> potentialBodies = new List <RigidBody>();
this.QueryRect(worldCoord, size, potentialBodies);
if (potentialBodies.Count == 0)
{
return(false);
}
PolygonShape boxShape = new PolygonShape(new Vertices(new List <Vector2>
{
PhysicsUnit.LengthToPhysical *worldCoord,
PhysicsUnit.LengthToPhysical *new Vector2(worldCoord.X + size.X, worldCoord.Y),
PhysicsUnit.LengthToPhysical * (worldCoord + size),
PhysicsUnit.LengthToPhysical * new Vector2(worldCoord.X, worldCoord.Y + size.Y)
}), 1);
FarseerPhysics.Common.Transform boxTransform = new FarseerPhysics.Common.Transform();
boxTransform.SetIdentity();
int oldResultCount = pickedShapes.Count;
foreach (RigidBody body in potentialBodies)
{
body.PickShapes(boxShape, boxTransform, pickedShapes);
}
return(pickedShapes.Count > oldResultCount);
}
static bool collidePolygonCircle(PolygonShape polygon, ref FSTransform polyTransform, CircleShape circle, ref FSTransform circleTransform, out FSCollisionResult result)
{
result = new FSCollisionResult();
Collision.collidePolygonAndCircle(ref _manifold, polygon, ref polyTransform, circle, ref circleTransform);
if (_manifold.pointCount > 0)
{
FixedArray2 <Vector2> points;
ContactSolver.WorldManifold.initialize(ref _manifold, ref polyTransform, polygon.radius, ref circleTransform, circle.radius, out result.normal, out points);
//var circleInPolySpace = polygonFixture.Body.GetLocalPoint( circleFixture.Body.Position );
var circleInPolySpace = MathUtils.mulT(ref polyTransform, circleTransform.p);
var value1 = circleInPolySpace - _manifold.localPoint;
var value2 = _manifold.localNormal;
var separation = circle.radius - (value1.X * value2.X + value1.Y * value2.Y);
if (separation <= 0)
{
return(false);
}
result.point = points[0] * FSConvert.simToDisplay;
result.minimumTranslationVector = result.normal * separation;
#if DEBUG_FSCOLLISIONS
Debug.drawPixel(result.point, 2, Color.Red, 0.2f);
Debug.drawLine(result.point, result.point + result.normal * 20, Color.Yellow, 0.2f);
#endif
return(true);
}
return(false);
}
private void DrawShape(Fixture fixture, Transform xf, Color color)
{
switch (fixture.ShapeType)
{
case ShapeType.Circle:
{
CircleShape circle = (CircleShape)fixture.Shape;
Vector2 center = MathUtils.M
(ref xf, circle.Position);
float radius = circle.Radius;
Vector2 axis = MathUtils.Multiply(xf.q, new Vector2(1.0f, 0.0f));
DrawSolidCircle(center, radius, axis, color);
}
break;
case ShapeType.Polygon:
{
PolygonShape poly = (PolygonShape)fixture.Shape;
int vertexCount = poly.Vertices.Count;
Debug.Assert(vertexCount <= Settings.MaxPolygonVertices);
Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices];
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = MathUtils.Multiply(ref xf, poly.Vertices[i]);
}
DrawSolidPolygon(vertices, vertexCount, color);
}
break;
case ShapeType.Edge:
{
EdgeShape edge = (EdgeShape)fixture.Shape;
Vector2 v1 = MathUtils.Multiply(ref xf, edge.Vertex1);
Vector2 v2 = MathUtils.Multiply(ref xf, edge.Vertex2);
DrawSegment(v1, v2, color);
}
break;
case ShapeType.Chain:
{
ChainShape chain = (ChainShape)fixture.Shape;
int count = chain.Vertices.Count;
Vector2 v1 = MathUtils.Multiply(ref xf, chain.Vertices[count - 1]);
for (int i = 0; i < count; ++i)
{
Vector2 v2 = MathUtils.Multiply(ref xf, chain.Vertices[i]);
DrawSegment(v1, v2, color);
v1 = v2;
}
}
break;
}
}
public void DrawShape(Fixture fixture, FarseerPhysics.Common.Transform xf, Color color)
{
switch (fixture.Shape.ShapeType)
{
case ShapeType.Circle:
{
var circle = (CircleShape)fixture.Shape;
System.Numerics.Vector2 center = MathUtils.Mul(ref xf, circle.Position);
float radius = circle.Radius;
System.Numerics.Vector2 axis = MathUtils.Mul(xf.Q, new System.Numerics.Vector2(1.0f, 0.0f));
DrawSolidCircle(center, radius, axis, color);
}
break;
case ShapeType.Polygon:
{
var poly = (PolygonShape)fixture.Shape;
int vertexCount = poly.Vertices.Count;
System.Diagnostics.Debug.Assert(vertexCount <= Settings.MaxPolygonVertices);
if (vertexCount > _tempVertices.Length)
{
_tempVertices = new Vector2[vertexCount];
}
for (int i = 0; i < vertexCount; ++i)
{
_tempVertices[i] = MathUtils.Mul(ref xf, poly.Vertices[i]).ToXna();
}
DrawSolidPolygon(_tempVertices, vertexCount, color);
}
break;
case ShapeType.Edge:
{
var edge = (EdgeShape)fixture.Shape;
var v1 = MathUtils.Mul(ref xf, edge.Vertex1);
var v2 = MathUtils.Mul(ref xf, edge.Vertex2);
DrawSegment(v1, v2, color);
}
break;
case ShapeType.Chain:
{
var chain = (ChainShape)fixture.Shape;
for (int i = 0; i < chain.Vertices.Count - 1; ++i)
{
var v1 = MathUtils.Mul(ref xf, chain.Vertices[i]);
var v2 = MathUtils.Mul(ref xf, chain.Vertices[i + 1]);
DrawSegment(v1, v2, color);
}
}
break;
}
}
public void DrawShape(Fixture fixture, FP.Transform xf, Color color)
{
switch (fixture.Shape.ShapeType)
{
case ShapeType.Circle:
{
CircleShape circle = (CircleShape)fixture.Shape;
FP.Vector2 center = FP.MathUtils.Mul(ref xf, circle.Position);
float radius = circle.Radius;
FP.Vector2 axis = FP.MathUtils.Mul(xf.q, new FP.Vector2(1.0f, 0.0f));
DrawSolidCircle(center, radius, axis, color);
}
break;
case ShapeType.Polygon:
{
PolygonShape poly = (PolygonShape)fixture.Shape;
int vertexCount = poly.Vertices.Count;
Debug.Assert(vertexCount <= Settings.MaxPolygonVertices);
for (int i = 0; i < vertexCount; ++i)
{
_tempVertices[i] = FP.MathUtils.Mul(ref xf, poly.Vertices[i]);
}
DrawSolidPolygon(_tempVertices, vertexCount, color);
}
break;
case ShapeType.Edge:
{
EdgeShape edge = (EdgeShape)fixture.Shape;
FP.Vector2 v1 = FP.MathUtils.Mul(ref xf, edge.Vertex1);
FP.Vector2 v2 = FP.MathUtils.Mul(ref xf, edge.Vertex2);
DrawSegment(v1, v2, color);
}
break;
case ShapeType.Chain:
{
ChainShape chain = (ChainShape)fixture.Shape;
for (int i = 0; i < chain.Vertices.Count - 1; ++i)
{
FP.Vector2 v1 = FP.MathUtils.Mul(ref xf, chain.Vertices[i]);
FP.Vector2 v2 = FP.MathUtils.Mul(ref xf, chain.Vertices[i + 1]);
DrawSegment(v1, v2, color);
}
}
break;
}
}
public override void DrawTransform(ref Transform transform)
{
const float axisScale = 0.4f;
Vector2 p1 = transform.Position;
Vector2 p2 = p1 + axisScale * transform.R.Col1;
DrawSegment(p1, p2, Colors.Red);
p2 = p1 + axisScale * transform.R.Col2;
DrawSegment(p1, p2, Colors.Green);
}
public void drawTransform(ref FarseerPhysics.Common.Transform transform)
{
const float axisScale = 0.4f;
Vector2 p1 = transform.p;
Vector2 p2 = p1 + axisScale * transform.q.GetXAxis();
drawSegment(p1, p2, Color.Red);
p2 = p1 + axisScale * transform.q.GetYAxis();
drawSegment(p1, p2, Color.Green);
}
public override void DrawTransform(ref Transform transform)
{
const float axisScale = 0.4f;
Vector2 p1 = transform.p;
Vector2 p2 = p1 + axisScale * transform.q.GetXAxis();
DrawSegment(p1, p2, Colors.Red);
p2 = p1 + axisScale * transform.q.GetYAxis();
DrawSegment(p1, p2, Colors.Green);
}
public override void DrawTransform(ref FP.Transform transform)
{
const float axisScale = 0.4f;
FP.Vector2 p1 = transform.p;
FP.Vector2 p2 = p1 + axisScale * transform.q.GetXAxis();
debugRenderer.DrawSegment(p1, p2, Color.Red);
p2 = p1 + axisScale * transform.q.GetYAxis();
debugRenderer.DrawSegment(p1, p2, Color.Green);
}
public void DrawTransform(ref FarseerPhysics.Common.Transform transform)
{
const float AxisScale = 0.4f;
Vector2 p1 = transform.Position;
Vector2 p2 = p1 + AxisScale * transform.R.Col1;
this.DrawSegment(p1, p2, Color.Red);
p2 = p1 + AxisScale * transform.R.Col2;
this.DrawSegment(p1, p2, Color.Green);
}
public void DrawTransform(ref FarseerPhysics.Common.Transform transform)
{
const float axisScale = 0.4f;
System.Numerics.Vector2 p1 = transform.P;
System.Numerics.Vector2 p2 = p1 + axisScale * transform.Q.GetXAxis();
DrawSegment(p1, p2, Color.Red);
p2 = p1 + axisScale * transform.Q.GetYAxis();
DrawSegment(p1, p2, Color.Green);
}
static bool CollidePolygons(PolygonShape polygonA, ref FSTransform transformA, PolygonShape polygonB,
ref FSTransform transformB, out FSCollisionResult result)
{
result = new FSCollisionResult();
Collision.CollidePolygons(ref _manifold, polygonA as PolygonShape, ref transformA, polygonB as PolygonShape,
ref transformB);
if (_manifold.PointCount > 0)
{
FixedArray2 <Vector2> points;
ContactSolver.WorldManifold.Initialize(ref _manifold, ref transformA, polygonA.Radius, ref transformB,
polygonB.Radius, out result.Normal, out points);
// code adapted from PositionSolverManifold.Initialize
if (_manifold.Type == ManifoldType.FaceA)
{
result.Normal = MathUtils.Mul(transformA.Q, _manifold.LocalNormal);
var planePoint = MathUtils.Mul(ref transformA, _manifold.LocalPoint);
var clipPoint = MathUtils.Mul(ref transformB, _manifold.Points[0].LocalPoint);
var separation = Vector2.Dot(clipPoint - planePoint, result.Normal) - polygonA.Radius -
polygonB.Radius;
result.Point = clipPoint * FSConvert.SimToDisplay;
// Ensure normal points from A to B
Vector2.Negate(ref result.Normal, out result.Normal);
result.MinimumTranslationVector = result.Normal * -separation;
}
else
{
result.Normal = MathUtils.Mul(transformB.Q, _manifold.LocalNormal);
var planePoint = MathUtils.Mul(ref transformB, _manifold.LocalPoint);
var clipPoint = MathUtils.Mul(ref transformA, _manifold.Points[0].LocalPoint);
var separation = Vector2.Dot(clipPoint - planePoint, result.Normal) - polygonA.Radius -
polygonB.Radius;
result.Point = clipPoint * FSConvert.SimToDisplay;
result.MinimumTranslationVector = result.Normal * -separation;
}
#if DEBUG_FSCOLLISIONS
Debug.drawPixel(result.point, 2, Color.Red, 0.2f);
Debug.drawLine(result.point, result.point + result.normal * 20, Color.Yellow, 0.2f);
#endif
return(true);
}
return(false);
}
/// <summary>
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
/// </summary>
/// <param name="aabb">The aabb results.</param>
/// <param name="transform">The world transform of the shape.</param>
/// <param name="childIndex">The child shape index.</param>
public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
{
Vector2 v1 = MathUtils.Multiply(ref transform, _vertex1);
Vector2 v2 = MathUtils.Multiply(ref transform, _vertex2);
Vector2 lower = Vector2.Min(v1, v2);
Vector2 upper = Vector2.Max(v1, v2);
Vector2 r = new Vector2(Radius, Radius);
aabb.LowerBound = lower - r;
aabb.UpperBound = upper + r;
}
static bool CollideEdgeAndCircle(EdgeShape edge, ref FSTransform edgeTransform, CircleShape circle,
ref FSTransform circleTransform, out FSCollisionResult result)
{
result = new FSCollisionResult();
Collision.CollideEdgeAndCircle(ref _manifold, edge, ref edgeTransform, circle, ref circleTransform);
if (_manifold.PointCount > 0)
{
// code adapted from PositionSolverManifold.Initialize
if (_manifold.Type == ManifoldType.Circles)
{
// this is essentically directly from ContactSolver.WorldManifold.Initialize. To avoid doing the extra math twice we duplicate this code
// here because it doesnt return some values we need to calculate separation
var pointA = MathUtils.Mul(ref edgeTransform, _manifold.LocalPoint);
var pointB = MathUtils.Mul(ref circleTransform, _manifold.Points[0].LocalPoint);
result.Normal = pointA - pointB;
Vector2Ext.Normalize(ref result.Normal);
var cA = pointA - edge.Radius * result.Normal;
var cB = pointB + circle.Radius * result.Normal;
result.Point = 0.5f * (cA + cB);
result.Point *= FSConvert.SimToDisplay;
var separation = Vector2.Dot(pointA - pointB, result.Normal) - edge.Radius - circle.Radius;
// Ensure normal points from A to B
Vector2.Negate(ref result.Normal, out result.Normal);
result.MinimumTranslationVector = result.Normal * Math.Abs(separation);
}
else // FaceA
{
result.Normal = MathUtils.Mul(edgeTransform.Q, _manifold.LocalNormal);
var planePoint = MathUtils.Mul(ref edgeTransform, _manifold.LocalPoint);
var clipPoint = MathUtils.Mul(ref circleTransform, _manifold.Points[0].LocalPoint);
var separation = Vector2.Dot(clipPoint - planePoint, result.Normal) - edge.Radius - circle.Radius;
result.Point = (clipPoint - result.Normal * circle.Radius) * FSConvert.SimToDisplay;
result.MinimumTranslationVector = result.Normal * -separation;
}
#if DEBUG_FSCOLLISIONS
Debug.drawPixel(result.point, 5, Color.Red, 0.2f);
Debug.drawLine(result.point, result.point + result.normal * 20, Color.Yellow, 0.2f);
#endif
return(true);
}
return(false);
}
internal void ReadCache(ref SimplexCache cache,
DistanceProxy proxyA, ref Transform transformA,
DistanceProxy proxyB, ref Transform transformB)
{
//Debug.Assert(cache.Count <= 3);
// Copy data from cache.
Count = cache.Count;
for (int i = 0; i < Count; ++i)
{
SimplexVertex v = V[i];
v.IndexA = cache.IndexA[i];
v.IndexB = cache.IndexB[i];
Vector2 wALocal = proxyA.Vertices[v.IndexA];
Vector2 wBLocal = proxyB.Vertices[v.IndexB];
v.WA = MathUtils.Multiply(ref transformA, wALocal);
v.WB = MathUtils.Multiply(ref transformB, wBLocal);
v.W = v.WB - v.WA;
v.A = 0.0f;
V[i] = v;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (Count > 1)
{
float metric1 = cache.Metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.Epsilon)
{
// Reset the simplex.
Count = 0;
}
}
// If the cache is empty or invalid ...
if (Count == 0)
{
SimplexVertex v = V[0];
v.IndexA = 0;
v.IndexB = 0;
Vector2 wALocal = proxyA.Vertices[0];
Vector2 wBLocal = proxyB.Vertices[0];
v.WA = MathUtils.Multiply(ref transformA, wALocal);
v.WB = MathUtils.Multiply(ref transformB, wBLocal);
v.W = v.WB - v.WA;
V[0] = v;
Count = 1;
}
}
/// <summary>
/// Evaluate this contact with your own manifold and transforms.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="transformA">The first transform.</param>
/// <param name="transformB">The second transform.</param>
private void Evaluate(ref Manifold manifold, ref Transform transformA, ref Transform transformB)
{
switch (_type)
{
case ContactType.Polygon:
Collision.Collision.CollidePolygons(ref manifold,
(PolygonShape)FixtureA.Shape, ref transformA,
(PolygonShape)FixtureB.Shape, ref transformB);
break;
case ContactType.PolygonAndCircle:
Collision.Collision.CollidePolygonAndCircle(ref manifold,
(PolygonShape)FixtureA.Shape, ref transformA,
(CircleShape)FixtureB.Shape, ref transformB);
break;
case ContactType.EdgeAndCircle:
Collision.Collision.CollideEdgeAndCircle(ref manifold,
(EdgeShape)FixtureA.Shape, ref transformA,
(CircleShape)FixtureB.Shape, ref transformB);
break;
case ContactType.EdgeAndPolygon:
Collision.Collision.CollideEdgeAndPolygon(ref manifold,
(EdgeShape)FixtureA.Shape, ref transformA,
(PolygonShape)FixtureB.Shape, ref transformB);
break;
case ContactType.LoopAndCircle:
LoopShape loop = (LoopShape)FixtureA.Shape;
loop.GetChildEdge(ref _edge, ChildIndexA);
Collision.Collision.CollideEdgeAndCircle(ref manifold, _edge, ref transformA,
(CircleShape)FixtureB.Shape, ref transformB);
break;
case ContactType.LoopAndPolygon:
LoopShape loop2 = (LoopShape)FixtureA.Shape;
loop2.GetChildEdge(ref _edge, ChildIndexA);
Collision.Collision.CollideEdgeAndPolygon(ref manifold, _edge, ref transformA,
(PolygonShape)FixtureB.Shape, ref transformB);
break;
case ContactType.Circle:
Collision.Collision.CollideCircles(ref manifold,
(CircleShape)FixtureA.Shape, ref transformA,
(CircleShape)FixtureB.Shape, ref transformB);
break;
}
}
/// <summary>
/// Test a point for containment in this shape. This only works for convex shapes.
/// </summary>
/// <param name="transform">The shape world transform.</param>
/// <param name="point">a point in world coordinates.</param>
/// <returns>True if the point is inside the shape</returns>
public override bool TestPoint(ref Transform transform, ref Vector2 point)
{
Vector2 pLocal = MathUtils.MultiplyT(ref transform.R, point - transform.Position);
for (int i = 0; i < Vertices.Count; ++i)
{
float dot = Vector2.Dot(Normals[i], pLocal - Vertices[i]);
if (dot > 0.0f)
{
return(false);
}
}
return(true);
}
private static bool CollideEdgeAndPolygon(EdgeShape edge, ref FSTransform edgeTransform, PolygonShape polygon,
ref FSTransform polygonTransform, out FSCollisionResult result)
{
result = new FSCollisionResult();
Collision.CollideEdgeAndPolygon(ref _manifold, edge, ref edgeTransform, polygon, ref polygonTransform);
if (_manifold.PointCount > 0)
{
FixedArray2 <Vector2> points;
ContactSolver.WorldManifold.Initialize(ref _manifold, ref edgeTransform, edge.Radius,
ref polygonTransform, polygon.Radius, out result.Normal, out points);
// code adapted from PositionSolverManifold.Initialize
if (_manifold.Type == ManifoldType.FaceA)
{
result.Normal = MathUtils.Mul(edgeTransform.Q, _manifold.LocalNormal);
Vector2 planePoint = MathUtils.Mul(ref edgeTransform, _manifold.LocalPoint);
Vector2 clipPoint = MathUtils.Mul(ref polygonTransform, _manifold.Points[0].LocalPoint);
float separation = Vector2.Dot(clipPoint - planePoint, result.Normal) - edge.Radius - polygon.Radius;
result.Point = clipPoint * FSConvert.SimToDisplay;
result.MinimumTranslationVector = result.Normal * -separation;
}
else
{
result.Normal = MathUtils.Mul(polygonTransform.Q, _manifold.LocalNormal);
Vector2 planePoint = MathUtils.Mul(ref polygonTransform, _manifold.LocalPoint);
Vector2 clipPoint = MathUtils.Mul(ref edgeTransform, _manifold.Points[0].LocalPoint);
float separation = Vector2.Dot(clipPoint - planePoint, result.Normal) - edge.Radius - polygon.Radius;
result.Point = clipPoint * FSConvert.SimToDisplay;
// Ensure normal points from A to B
Vector2.Negate(ref result.Normal, out result.Normal);
result.MinimumTranslationVector = result.Normal * -separation;
}
#if DEBUG_FSCOLLISIONS
Debug.drawPixel(result.point, 5, Color.Red, 0.2f);
Debug.drawLine(result.point, result.point + result.normal * 20, Color.Yellow, 0.2f);
#endif
return(true);
}
return(false);
}
private bool collideWithHole(Fixture fixtureA, ref FSTransform transformA, Fixture fixtureB, ref FSTransform transformB, out FSCollisionResult result)
{
result = new FSCollisionResult();
result.fixture = fixtureB;
if (!ContactManager.shouldCollide(fixtureA, fixtureB))
{
return(false);
}
if (fixtureA.body.world.contactManager.onContactFilter != null && !fixtureA.body.world.contactManager.onContactFilter(fixtureA, fixtureB))
{
return(false);
}
return(collidePolygonCircle(fixtureB.shape as PolygonShape, ref transformB, fixtureA.shape as CircleShape, ref transformA, out result));
}
/// <summary>
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
/// </summary>
/// <param name="aabb">The aabb results.</param>
/// <param name="transform">The world transform of the shape.</param>
/// <param name="childIndex">The child shape index.</param>
public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
{
Vector2 lower = MathUtils.Multiply(ref transform, Vertices[0]);
Vector2 upper = lower;
for (int i = 1; i < Vertices.Count; ++i)
{
Vector2 v = MathUtils.Multiply(ref transform, Vertices[i]);
lower = Vector2.Min(lower, v);
upper = Vector2.Max(upper, v);
}
Vector2 r = new Vector2(Radius, Radius);
aabb.LowerBound = lower - r;
aabb.UpperBound = upper + r;
}
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
//Debug.Assert(childIndex < Vertices.Count);
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == Vertices.Count)
{
i2 = 0;
}
_edgeShape.Vertex1 = Vertices[i1];
_edgeShape.Vertex2 = Vertices[i2];
return(_edgeShape.RayCast(out output, ref input, ref transform, 0));
}
/// <summary>
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
/// </summary>
/// <param name="aabb">The aabb results.</param>
/// <param name="transform">The world transform of the shape.</param>
/// <param name="childIndex">The child shape index.</param>
public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
{
//Debug.Assert(childIndex < Vertices.Count);
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == Vertices.Count)
{
i2 = 0;
}
Vector2 v1 = MathUtils.Multiply(ref transform, Vertices[i1]);
Vector2 v2 = MathUtils.Multiply(ref transform, Vertices[i2]);
aabb.LowerBound = Vector2.Min(v1, v2);
aabb.UpperBound = Vector2.Max(v1, v2);
}
public static bool collideEdgeAndPolygon(EdgeShape edge, ref FSTransform edgeTransform, PolygonShape polygon, ref FSTransform polygonTransform, out CollisionResult result)
{
result = new CollisionResult();
Collision.collideEdgeAndPolygon(ref _manifold, edge, ref edgeTransform, polygon, ref polygonTransform);
if (_manifold.pointCount > 0)
{
FixedArray2 <Vector2> points;
ContactSolver.WorldManifold.initialize(ref _manifold, ref edgeTransform, edge.radius, ref polygonTransform, polygon.radius, out result.normal, out points);
// code adapted from PositionSolverManifold.Initialize
if (_manifold.type == ManifoldType.FaceA)
{
result.normal = MathUtils.mul(edgeTransform.q, _manifold.localNormal);
var planePoint = MathUtils.mul(ref edgeTransform, _manifold.localPoint);
var clipPoint = MathUtils.mul(ref polygonTransform, _manifold.points[0].localPoint);
var separation = Vector2.Dot(clipPoint - planePoint, result.normal) - edge.radius - polygon.radius;
result.point = clipPoint * FSConvert.simToDisplay;
result.minimumTranslationVector = result.normal * -separation;
}
else
{
result.normal = MathUtils.mul(polygonTransform.q, _manifold.localNormal);
var planePoint = MathUtils.mul(ref polygonTransform, _manifold.localPoint);
var clipPoint = MathUtils.mul(ref edgeTransform, _manifold.points[0].localPoint);
var separation = Vector2.Dot(clipPoint - planePoint, result.normal) - edge.radius - polygon.radius;
result.point = clipPoint * FSConvert.simToDisplay;
// Ensure normal points from A to B
Vector2.Negate(ref result.normal, out result.normal);
result.minimumTranslationVector = result.normal * -separation;
}
Debug.drawPixel(result.point, 5, Color.Red, 0.2f);
Debug.drawLine(result.point, result.point + result.normal * 20, Color.Yellow, 0.2f);
return(true);
}
return(false);
}
// These support body activation/deactivation.
internal void CreateProxies(IBroadPhase broadPhase, ref Transform xf)
{
Debug.Assert(ProxyCount == 0);
// Create proxies in the broad-phase.
ProxyCount = Shape.ChildCount;
for (int i = 0; i < ProxyCount; ++i)
{
FixtureProxy proxy = new FixtureProxy();
Shape.ComputeAABB(out proxy.AABB, ref xf, i);
proxy.Fixture = this;
proxy.ChildIndex = i;
proxy.ProxyId = broadPhase.AddProxy(ref proxy);
Proxies[i] = proxy;
}
}
/// <summary>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform,
int childIndex)
{
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
output = new RayCastOutput();
Vector2 position = transform.Position + MathUtils.Multiply(ref transform.R, Position);
Vector2 s = input.Point1 - position;
float b = Vector2.Dot(s, s) - Radius * Radius;
// Solve quadratic equation.
Vector2 r = input.Point2 - input.Point1;
float c = Vector2.Dot(s, r);
float rr = Vector2.Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.Epsilon)
{
return false;
}
// Find the point of intersection of the line with the circle.
float a = -(c + (float)Math.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.MaxFraction * rr)
{
a /= rr;
output.Fraction = a;
Vector2 norm = (s + a * r);
norm.Normalize();
output.Normal = norm;
return true;
}
return false;
}
/// <summary>
/// Test a point for containment in this shape. This only works for convex shapes.
/// </summary>
/// <param name="transform">The shape world transform.</param>
/// <param name="point">a point in world coordinates.</param>
/// <returns>True if the point is inside the shape</returns>
public override bool TestPoint(ref Transform transform, ref Vector2 point)
{
Vector2 center = transform.Position + MathUtils.Multiply(ref transform.R, Position);
Vector2 d = point - center;
return Vector2.Dot(d, d) <= Radius * Radius;
}
/// <summary>
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
/// </summary>
/// <param name="aabb">The aabb results.</param>
/// <param name="transform">The world transform of the shape.</param>
/// <param name="childIndex">The child shape index.</param>
public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
{
Vector2 p = transform.Position + MathUtils.Multiply(ref transform.R, Position);
aabb.LowerBound = new Vector2(p.x - Radius, p.y - Radius);
aabb.UpperBound = new Vector2(p.x + Radius, p.y + Radius);
}
public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc)
{
sc = Vector2.zero;
Vector2 p = MathUtils.Multiply(ref xf, Position);
float l = -(Vector2.Dot(normal, p) - offset);
if (l < -Radius + Settings.Epsilon)
{
//Completely dry
return 0;
}
if (l > Radius)
{
//Completely wet
sc = p;
return Settings.Pi * Radius * Radius;
}
//Magic
float r2 = Radius * Radius;
float l2 = l * l;
float area = r2 * (float)((Math.Asin(l / Radius) + Settings.Pi / 2) + l * Math.Sqrt(r2 - l2));
float com = -2.0f / 3.0f * (float)Math.Pow(r2 - l2, 1.5f) / area;
sc.x = p.x + normal.x * com;
sc.y = p.y + normal.y * com;
return area;
}
/// <summary>
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
/// </summary>
/// <param name="aabb">The aabb results.</param>
/// <param name="transform">The world transform of the shape.</param>
/// <param name="childIndex">The child shape index.</param>
public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
{
Vector2 v1 = MathUtils.Multiply(ref transform, _vertex1);
Vector2 v2 = MathUtils.Multiply(ref transform, _vertex2);
Vector2 lower = Vector2.Min(v1, v2);
Vector2 upper = Vector2.Max(v1, v2);
Vector2 r = new Vector2(Radius, Radius);
aabb.LowerBound = lower - r;
aabb.UpperBound = upper + r;
}
/// <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
}
/// <summary>
/// Get the body transform for the body's origin.
/// </summary>
/// <param name="transform">The transform of the body's origin.</param>
public void GetTransform(out Transform transform)
{
transform = Xf;
}
private void DrawShape(Fixture fixture, Transform xf, Color color)
{
switch (fixture.Shape.ShapeType)
{
case ShapeType.Circle:
{
var circle = (FarseerCircleShape)fixture.Shape;
Vector2 center = MathUtils.Mul(ref xf, circle.Position);
float radius = circle.Radius;
Vector2 axis = xf.q.GetXAxis();
DrawSolidCircle(center, radius, axis, color.R, color.G, color.B);
}
break;
case ShapeType.Polygon:
{
var poly = (PolygonShape)fixture.Shape;
int vertexCount = poly.Vertices.Count;
var vertices = new Vector2[vertexCount];
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = MathUtils.Mul(ref xf, poly.Vertices[i]);
}
DrawSolidPolygon(vertices, vertexCount, color.R, color.G, color.B);
}
break;
default:
throw new NotImplementedException();
}
}
/// <summary>
/// Test a point for containment in this shape. This only works for convex shapes.
/// </summary>
/// <param name="transform">The shape world transform.</param>
/// <param name="point">a point in world coordinates.</param>
/// <returns>True if the point is inside the shape</returns>
public override bool TestPoint(ref Transform transform, ref Vector2 point)
{
return false;
}
internal void SynchronizeFixtures()
{
Transform xf1 = new Transform();
float c = (float)Math.Cos(Sweep.A0), s = (float)Math.Sin(Sweep.A0);
xf1.R.Col1.x = c;
xf1.R.Col2.x = -s;
xf1.R.Col1.y = s;
xf1.R.Col2.y = c;
xf1.Position.x = Sweep.C0.x - (xf1.R.Col1.x * Sweep.LocalCenter.x + xf1.R.Col2.x * Sweep.LocalCenter.y);
xf1.Position.y = Sweep.C0.y - (xf1.R.Col1.y * Sweep.LocalCenter.x + xf1.R.Col2.y * Sweep.LocalCenter.y);
IBroadPhase broadPhase = World.ContactManager.BroadPhase;
for (int i = 0; i < FixtureList.Count; i++)
{
FixtureList[i].Synchronize(broadPhase, ref xf1, ref Xf);
}
}
public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 sc)
{
sc = Vector2.zero;
return 0;
}
private void DrawShape(Fixture fixture, Transform xf, Color color)
{
switch (fixture.Shape.ShapeType)
{
case ShapeType.Circle:
{
CircleShape circle = (CircleShape)fixture.Shape;
Vector2 center = MathUtils.Mul(ref xf, circle.Position);
float radius = circle.Radius;
Vector2 axis = MathUtils.Mul(xf.q, new Vector2(1.0f, 0.0f));
DrawSolidCircle(center, radius, axis, color);
}
break;
case ShapeType.Polygon:
{
PolygonShape poly = (PolygonShape)fixture.Shape;
int vertexCount = poly.Vertices.Count;
Debug.Assert(vertexCount <= Settings.MaxPolygonVertices);
for (int i = 0; i < vertexCount; ++i)
{
_tempVertices[i] = MathUtils.Mul(ref xf, poly.Vertices[i]);
}
DrawSolidPolygon(_tempVertices, vertexCount, color);
}
break;
case ShapeType.Edge:
{
EdgeShape edge = (EdgeShape)fixture.Shape;
Vector2 v1 = MathUtils.Mul(ref xf, edge.Vertex1);
Vector2 v2 = MathUtils.Mul(ref xf, edge.Vertex2);
DrawSegment(v1, v2, color);
}
break;
case ShapeType.Chain:
{
ChainShape chain = (ChainShape)fixture.Shape;
for (int i = 0; i < chain.Vertices.Count - 1; ++i)
{
Vector2 v1 = MathUtils.Mul(ref xf, chain.Vertices[i]);
Vector2 v2 = MathUtils.Mul(ref xf, chain.Vertices[i + 1]);
DrawSegment(v1, v2, color);
}
}
break;
}
}
public override void DrawTransform(ref Transform transform)
{
throw new NotImplementedException();
}
/// <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)
Mathf.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)
Mathf.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;
}
}
internal void Synchronize(IBroadPhase broadPhase, ref Transform transform1, ref Transform transform2)
{
if (ProxyCount == 0)
{
return;
}
for (int i = 0; i < ProxyCount; ++i)
{
FixtureProxy proxy = Proxies[i];
// Compute an AABB that covers the swept Shape (may miss some rotation effect).
AABB aabb1, aabb2;
Shape.ComputeAABB(out aabb1, ref transform1, proxy.ChildIndex);
Shape.ComputeAABB(out aabb2, ref transform2, proxy.ChildIndex);
proxy.AABB.Combine(ref aabb1, ref aabb2);
Vector2 displacement = transform2.Position - transform1.Position;
broadPhase.MoveProxy(proxy.ProxyId, ref proxy.AABB, displacement);
}
}
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;
}
/// 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;
}
public override void DrawTransform(ref Transform transform)
{
const float axisScale = 0.4f;
Vector2 p1 = transform.Position;
Vector2 p2 = p1 + axisScale * transform.R.Col1;
DrawSegment(p1, p2, Colors.Red);
p2 = p1 + axisScale * transform.R.Col2;
DrawSegment(p1, p2, Colors.Green);
}
/// <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(d, d);
//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(d, d);
//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(e, e);
//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(d, d);
//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>
/// 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 / Mathf.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>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
output = new RayCastOutput();
// Put the ray into the edge's frame of reference.
Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.Point1 - transform.Position);
Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.Point2 - transform.Position);
Vector2 d = p2 - p1;
Vector2 v1 = _vertex1;
Vector2 v2 = _vertex2;
Vector2 e = v2 - v1;
Vector2 normal = new Vector2(e.y, -e.x);
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float numerator = Vector2.Dot(normal, v1 - p1);
float denominator = Vector2.Dot(normal, d);
if (denominator == 0.0f)
{
return false;
}
float t = numerator / denominator;
if (t < 0.0f || 1.0f < t)
{
return false;
}
Vector2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
Vector2 r = v2 - v1;
float rr = Vector2.Dot(r, r);
if (rr == 0.0f)
{
return false;
}
float s = Vector2.Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return false;
}
output.Fraction = t;
if (numerator > 0.0f)
{
output.Normal = -normal;
}
else
{
output.Normal = normal;
}
return true;
}
public override void DrawTransform(ref Transform transform)
{
const float axisScale = 0.4f;
Vector2 p1 = transform.p;
Vector2 p2 = p1 + axisScale * transform.q.GetXAxis();
DrawSegment(p1, p2, Colors.Red);
p2 = p1 + axisScale * transform.q.GetYAxis();
DrawSegment(p1, p2, Colors.Green);
}
/// <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.up;
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 / Mathf.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.up;
break;
}
}
/// <summary>
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
/// </summary>
/// <param name="aabb">The aabb results.</param>
/// <param name="transform">The world transform of the shape.</param>
/// <param name="childIndex">The child shape index.</param>
public override void ComputeAABB(out AABB aabb, ref Transform transform, int childIndex)
{
//Debug.Assert(childIndex < Vertices.Count);
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == Vertices.Count)
{
i2 = 0;
}
Vector2 v1 = MathUtils.Multiply(ref transform, Vertices[i1]);
Vector2 v2 = MathUtils.Multiply(ref transform, Vertices[i2]);
aabb.LowerBound = Vector2.Min(v1, v2);
aabb.UpperBound = Vector2.Max(v1, v2);
}
/// <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>
/// Cast a ray against a child shape.
/// </summary>
/// <param name="output">The ray-cast results.</param>
/// <param name="input">The ray-cast input parameters.</param>
/// <param name="transform">The transform to be applied to the shape.</param>
/// <param name="childIndex">The child shape index.</param>
/// <returns>True if the ray-cast hits the shape</returns>
public override bool RayCast(out RayCastOutput output, ref RayCastInput input,
ref Transform transform, int childIndex)
{
//Debug.Assert(childIndex < Vertices.Count);
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == Vertices.Count)
{
i2 = 0;
}
_edgeShape.Vertex1 = Vertices[i1];
_edgeShape.Vertex2 = Vertices[i2];
return _edgeShape.RayCast(out output, ref input, ref transform, 0);
}
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;
}
// These support body activation/deactivation.
internal void CreateProxies(IBroadPhase broadPhase, ref Transform xf)
{
//Debug.Assert(ProxyCount == 0);
// Create proxies in the broad-phase.
ProxyCount = Shape.ChildCount;
for (int i = 0; i < ProxyCount; ++i)
{
FixtureProxy proxy = new FixtureProxy();
Shape.ComputeAABB(out proxy.AABB, ref xf, i);
proxy.Fixture = this;
proxy.ChildIndex = i;
proxy.ProxyId = broadPhase.AddProxy(ref proxy);
Proxies[i] = proxy;
}
}
/// <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;
}
