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);
}
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);
}
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>
/// checks for collisions between two Fixtures. Note that the first Fixture must have a Circle/PolygonShape and one of the Fixtures must be
/// static.
/// </summary>
/// <returns><c>true</c>, if shapes was collided, <c>false</c> otherwise.</returns>
/// <param name="fixtureA">Fixture a.</param>
/// <param name="fixtureB">Fixture b.</param>
/// <param name="result">Result.</param>
public static bool collideFixtures(Fixture fixtureA, Fixture fixtureB, out FSCollisionResult result)
{
// gather our transforms
FSTransform transformA;
fixtureA.body.getTransform(out transformA);
FSTransform transformB;
fixtureB.body.getTransform(out transformB);
return(collideFixtures(fixtureA, ref transformA, fixtureB, ref transformB, out result));
}
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);
}
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);
}
/// <summary>
/// checks to see if fixtureA with motion applied (delta movement vector) collides with any collider. If it does, true will be
/// returned and result will be populated with collision data. Motion will be set to the maximum distance the Body can travel
/// before colliding.
/// </summary>
/// <returns><c>true</c>, if fixtures was collided, <c>false</c> otherwise.</returns>
/// <param name="fixtureA">Fixture a.</param>
/// <param name="motion">delta movement in simulation space</param>
/// <param name="fixtureB">Fixture b.</param>
/// <param name="result">Result.</param>
public static bool CollideFixtures(Fixture fixtureA, ref Vector2 motion, Fixture fixtureB,
out FSCollisionResult result)
{
// gather our transforms and adjust fixtureA's transform to account for the motion so we check for the collision at its new location
FSTransform transformA;
fixtureA.Body.GetTransform(out transformA);
transformA.P += motion;
FSTransform transformB;
fixtureB.Body.GetTransform(out transformB);
if (CollideFixtures(fixtureA, ref transformA, fixtureB, ref transformB, out result))
{
motion += result.MinimumTranslationVector;
return(true);
}
return(false);
}
/// <summary>
/// checks to see if the Fixture with motion applied (delta movement vector) collides with any collider. If it does, true will be
/// returned and result will be populated with collision data. motion will be set to the maximum distance the Body can travel
/// before colliding.
/// </summary>
/// <returns><c>true</c>, if with any was collidesed, <c>false</c> otherwise.</returns>
/// <param name="self">Fixture a.</param>
/// <param name="motion">the delta movement in Nez pixel coordinates</param>
/// <param name="result">Result.</param>
public static bool CollidesWithAnyFixtures(this Fixture self, ref Vector2 motion, out FSCollisionResult result)
{
result = new FSCollisionResult();
motion *= FSConvert.DisplayToSim;
AABB aabb;
FSTransform xf;
var didCollide = false;
self.Body.GetTransform(out xf);
xf.P += motion;
self.Shape.ComputeAABB(out aabb, ref xf, 0);
var neighbors = ListPool <Fixture> .Obtain();
self.Body.World.QueryAABB(ref aabb, neighbors);
if (neighbors.Count > 1)
{
// handle collisions with all but ourself
for (var i = 0; i < neighbors.Count; i++)
{
if (neighbors[i].FixtureId == self.FixtureId)
{
continue;
}
if (FSCollisions.CollideFixtures(self, ref motion, neighbors[i], out result))
{
// if we have a collision, adjust the transform to account for it
xf.P += result.MinimumTranslationVector;
}
}
}
ListPool <Fixture> .Free(neighbors);
motion *= FSConvert.SimToDisplay;
return(didCollide);
}
/// <summary>
/// checks for collisions between two Fixtures. Note that the first Fixture must have a Circle/PolygonShape and one of the Fixtures must be
/// static for a collision to occur.
/// </summary>
/// <returns><c>true</c>, if fixtures was collided, <c>false</c> otherwise.</returns>
/// <param name="self">Self.</param>
/// <param name="fixtureB">Fixture b.</param>
/// <param name="result">Result.</param>
public static bool CollideFixtures(this Fixture self, Fixture fixtureB, out FSCollisionResult result)
{
return(FSCollisions.CollideFixtures(self, fixtureB, out result));
}
/// <summary>
/// checks for collisions between two Fixtures. Note that the first Fixture must have a Circle/PolygonShape and one of the Fixtures must be
/// static.
/// </summary>
/// <returns><c>true</c>, if fixtures was collided, <c>false</c> otherwise.</returns>
/// <param name="fixtureA">Fixture a.</param>
/// <param name="transformA">Transform a.</param>
/// <param name="fixtureB">Fixture b.</param>
/// <param name="transformB">Transform b.</param>
/// <param name="result">Result.</param>
public static bool collideFixtures(Fixture fixtureA, ref FSTransform transformA, Fixture fixtureB, ref FSTransform transformB, out FSCollisionResult result)
{
result = new FSCollisionResult();
result.fixture = fixtureB;
// we need at least one static fixture
if (!fixtureA.body.isStatic && !fixtureB.body.isStatic)
{
// if the body is dyanmic and asleep wake it up
if (fixtureB.body.isDynamic && !fixtureB.body.isAwake)
{
fixtureB.body.isAwake = true;
}
return(false);
}
// check normal collision filtering
if (!ContactManager.shouldCollide(fixtureA, fixtureB))
{
return(false);
}
// check user filtering
if (fixtureA.body.world.contactManager.onContactFilter != null && !fixtureA.body.world.contactManager.onContactFilter(fixtureA, fixtureB))
{
return(false);
}
// we only handle Circle or Polygon collisions
if (fixtureA.shape is CircleShape)
{
if (fixtureB.shape is CircleShape)
{
return(collideCircles(fixtureA.shape as CircleShape, ref transformA, fixtureB.shape as CircleShape, ref transformB, out result));
}
if (fixtureB.shape is PolygonShape)
{
return(collidePolygonCircle(fixtureB.shape as PolygonShape, ref transformB, fixtureA.shape as CircleShape, ref transformA, out result));
}
if (fixtureB.shape is EdgeShape)
{
return(collideEdgeAndCircle(fixtureB.shape as EdgeShape, ref transformB, fixtureA.shape as CircleShape, ref transformA, out result));
}
if (fixtureB.shape is ChainShape)
{
var chain = fixtureB.shape as ChainShape;
for (var i = 0; i < chain.childCount; i++)
{
var edge = chain.getChildEdge(i);
if (collideEdgeAndCircle(edge, ref transformB, fixtureA.shape as CircleShape, ref transformA, out result))
{
return(true);
}
}
}
}
if (fixtureA.shape is PolygonShape)
{
if (fixtureB.shape is CircleShape)
{
var res = collidePolygonCircle(fixtureA.shape as PolygonShape, ref transformA, fixtureB.shape as CircleShape, ref transformB, out result);
result.invertResult();
return(res);
}
if (fixtureB.shape is PolygonShape)
{
return(collidePolygons(fixtureA.shape as PolygonShape, ref transformA, fixtureB.shape as PolygonShape, ref transformB, out result));
}
if (fixtureB.shape is EdgeShape)
{
return(collideEdgeAndPolygon(fixtureB.shape as EdgeShape, ref transformB, fixtureA.shape as PolygonShape, ref transformA, out result));
}
if (fixtureB.shape is ChainShape)
{
var chain = fixtureB.shape as ChainShape;
for (var i = 0; i < chain.childCount; i++)
{
var edge = chain.getChildEdge(i);
if (collideEdgeAndPolygon(edge, ref transformB, fixtureA.shape as PolygonShape, ref transformA, out result))
{
return(true);
}
}
}
}
return(false);
}
/// <summary>
/// checks for collisions between two Fixtures. Note that the first Fixture must have a Circle/PolygonShape and one of the Fixtures must be
/// static.
/// </summary>
/// <returns><c>true</c>, if fixtures was collided, <c>false</c> otherwise.</returns>
/// <param name="fixtureA">Fixture a.</param>
/// <param name="transformA">Transform a.</param>
/// <param name="fixtureB">Fixture b.</param>
/// <param name="transformB">Transform b.</param>
/// <param name="result">Result.</param>
public static bool CollideFixtures(Fixture fixtureA, ref FSTransform transformA, Fixture fixtureB,
ref FSTransform transformB, out FSCollisionResult result)
{
result = new FSCollisionResult {
Fixture = fixtureB
};
// we need at least one static fixture
if (!fixtureA.Body.IsStatic && !fixtureB.Body.IsStatic)
{
// if the body is dyanmic and asleep wake it up
if (fixtureB.Body.IsDynamic && !fixtureB.Body.IsAwake)
{
fixtureB.Body.IsAwake = true;
}
return(false);
}
// check normal collision filtering
if (!ContactManager.ShouldCollide(fixtureA, fixtureB))
{
return(false);
}
// check user filtering
if (fixtureA.Body.World.ContactManager.OnContactFilter != null &&
!fixtureA.Body.World.ContactManager.OnContactFilter(fixtureA, fixtureB))
{
return(false);
}
// we only handle Circle or Polygon collisions
if (fixtureA.Shape is CircleShape)
{
if (fixtureB.Shape is CircleShape)
{
return(CollideCircles(fixtureA.Shape as CircleShape, ref transformA, fixtureB.Shape as CircleShape,
ref transformB, out result));
}
if (fixtureB.Shape is PolygonShape)
{
return(CollidePolygonCircle(fixtureB.Shape as PolygonShape, ref transformB,
fixtureA.Shape as CircleShape, ref transformA, out result));
}
if (fixtureB.Shape is EdgeShape)
{
return(CollideEdgeAndCircle(fixtureB.Shape as EdgeShape, ref transformB,
fixtureA.Shape as CircleShape, ref transformA, out result));
}
if (fixtureB.Shape is ChainShape)
{
ChainShape chain = fixtureB.Shape as ChainShape;
for (int i = 0; i < chain.ChildCount; i++)
{
EdgeShape edge = chain.GetChildEdge(i);
if (CollideEdgeAndCircle(edge, ref transformB, fixtureA.Shape as CircleShape, ref transformA,
out result))
{
return(true);
}
}
}
}
if (fixtureA.Shape is PolygonShape)
{
if (fixtureB.Shape is CircleShape)
{
bool res = CollidePolygonCircle(fixtureA.Shape as PolygonShape, ref transformA,
fixtureB.Shape as CircleShape, ref transformB, out result);
result.InvertResult();
return(res);
}
if (fixtureB.Shape is PolygonShape)
{
return(CollidePolygons(fixtureA.Shape as PolygonShape, ref transformA,
fixtureB.Shape as PolygonShape, ref transformB, out result));
}
if (fixtureB.Shape is EdgeShape)
{
return(CollideEdgeAndPolygon(fixtureB.Shape as EdgeShape, ref transformB,
fixtureA.Shape as PolygonShape, ref transformA, out result));
}
if (fixtureB.Shape is ChainShape)
{
ChainShape chain = fixtureB.Shape as ChainShape;
for (int i = 0; i < chain.ChildCount; i++)
{
EdgeShape edge = chain.GetChildEdge(i);
if (CollideEdgeAndPolygon(edge, ref transformB, fixtureA.Shape as PolygonShape, ref transformA,
out result))
{
return(true);
}
}
}
}
return(false);
}
