internal bool SynchronizeFixtures()
{
Transform xf1 = default(Transform);
xf1.rotation = Box2DX.Common.Math.AngleToRotation(_sweep.A0);
//xf1.R = new Mat22(_sweep.A0);
xf1.position = _sweep.C0 - xf1.TransformDirection(_sweep.LocalCenter);
bool inRange = true;
for (Fixture f = _fixtureList; f != null; f = f.Next)
{
inRange = f.Synchronize(_world._broadPhase, xf1, _xf);
if (inRange == false)
{
break;
}
}
if (inRange == false)
{
_flags |= BodyFlags.Frozen;
_linearVelocity = Vector2.zero;
_angularVelocity = 0.0f;
// Failure
return(false);
}
// Success
return(true);
}
internal bool Synchronize(BroadPhase broadPhase, Transform Transform1, Transform Transform2)
{
if (_proxyId == PairManager.NullProxy)
{
return(false);
}
// Compute an AABB that covers the swept shape (may miss some rotation effect).
AABB aabb1, aabb2;
_shape.ComputeAABB(out aabb1, Transform1);
_shape.ComputeAABB(out aabb2, Transform2);
AABB aabb = new AABB();
aabb.Combine(aabb1, aabb2);
if (broadPhase.InRange(aabb))
{
broadPhase.MoveProxy(_proxyId, aabb);
return(true);
}
else
{
return(false);
}
}
public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 c)
{
Vector2 p = xf.TransformPoint(_position);
float l = -(Vector2.Dot(normal, p) - offset);
if (l < -_radius + Box2DX.Common.Settings.FLT_EPSILON)
{
//Completely dry
c = new Vector2();
return(0);
}
if (l > _radius)
{
//Completely wet
c = p;
return(Box2DX.Common.Settings.Pi * _radius * _radius);
}
//Magic
float r2 = _radius * _radius;
float l2 = l * l;
float area = r2 * ((float)System.Math.Asin(l / _radius) + Box2DX.Common.Settings.Pi / 2) +
l * Box2DX.Common.Math.Sqrt(r2 - l2);
float com = -2.0f / 3.0f * (float)System.Math.Pow(r2 - l2, 1.5f) / area;
c.X = p.X + normal.X * com;
c.Y = p.Y + normal.Y * com;
return(area);
}
public override bool TestPoint(Transform xf, Vector2 p)
{
Vector2 center = xf.position + xf.TransformDirection(_position);
Vector2 d = p - center;
return(Vector2.Dot(d, d) <= _radius * _radius);
}
public static void CollideCircles(out Manifold manifold,
CircleShape circle1, Transform xf1,
CircleShape circle2, Transform xf2)
{
manifold = new Manifold();
manifold.PointCount = 0;
Vec2 p1 = Math.Mul(xf1, circle1._p);
Vec2 p2 = Math.Mul(xf2, circle2._p);
Vec2 d = p2 - p1;
float distSqr = Vec2.Dot(d, d);
float radius = circle1._radius + circle2._radius;
if (distSqr > radius * radius)
{
return;
}
manifold.Type = Manifold.ManifoldType.Circles;
manifold.LocalPoint = circle1._p;
manifold.LocalPlaneNormal.SetZero();
manifold.PointCount = 1;
manifold.Points[0].LocalPoint = circle2._p;
manifold.Points[0].ID.Key = 0;
}
public override void ComputeAABB(out AABB aabb, Transform xf)
{
aabb = new AABB();
Vector2 p = xf.position + xf.TransformDirection(_position);
aabb.LowerBound = new Vector2(p.x - _radius, p.y - _radius);
aabb.UpperBound = new Vector2(p.x + _radius, p.y + _radius);
}
public override void ComputeAABB(out AABB aabb, ref Transform transform)
{
aabb = new AABB();
Vec2 p = transform.Position + Math.Mul(transform.R, _p);
aabb.LowerBound.Set(p.X - _radius, p.Y - _radius);
aabb.UpperBound.Set(p.X + _radius, p.Y + _radius);
}
public override void ComputeAABB(out AABB aabb, Transform xf)
{
aabb = new AABB();
Vector2 p = xf.position + xf.TransformDirection(_position);
aabb.LowerBound = new Vector2(p.X - _radius, p.Y - _radius);
aabb.UpperBound = new Vector2(p.X + _radius, p.Y + _radius);
}
public override void ComputeAABB(out AABB aabb, Transform xf)
{
Vector2 v1 = xf.TransformPoint(_v1);
Vector2 v2 = xf.TransformPoint(_v2);
Vector2 r = new Vector2(_radius, _radius);
aabb.LowerBound = Vector2.Min(v1, v2) - r;
aabb.UpperBound = Vector2.Max(v1, v2) + r;
}
internal unsafe void ReadCache(SimplexCache *cache, Shape shapeA, Transform TransformA, Shape shapeB, Transform TransformB)
{
Box2DXDebug.Assert(0 <= cache->Count && cache->Count <= 3);
// Copy data from cache.
_count = cache->Count;
SimplexVertex **vertices = stackalloc SimplexVertex *[3];
fixed(SimplexVertex *v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3)
{
vertices[0] = v1Ptr;
vertices[1] = v2Ptr;
vertices[2] = v3Ptr;
for (int i = 0; i < _count; ++i)
{
SimplexVertex *v = vertices[i];
v->indexA = cache->IndexA[i];
v->indexB = cache->IndexB[i];
Vector2 wALocal = shapeA.GetVertex(v->indexA);
Vector2 wBLocal = shapeB.GetVertex(v->indexB);
v->wA = TransformA.TransformPoint(wALocal);
v->wB = TransformB.TransformPoint(wBLocal);
v->w = v->wB - v->wA;
v->a = 0.0f;
}
// 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 < Common.Settings.FLT_EPSILON)
{
// Reset the simplex.
_count = 0;
}
}
// If the cache is empty or invalid ...
if (_count == 0)
{
SimplexVertex *v = vertices[0];
v->indexA = 0;
v->indexB = 0;
Vector2 wALocal = shapeA.GetVertex(0);
Vector2 wBLocal = shapeB.GetVertex(0);
v->wA = TransformA.TransformPoint(wALocal);
v->wB = TransformB.TransformPoint(wBLocal);
v->w = v->wB - v->wA;
_count = 1;
}
}
}
internal float Evaluate(Transform TransformA, Transform TransformB)
{
switch (FaceType)
{
case Type.Points:
{
Vector2 axisA = TransformA.InverseTransformDirection(Axis);
Vector2 axisB = TransformB.InverseTransformDirection(-Axis);
Vector2 localPointA = ShapeA.GetSupportVertex(axisA);
Vector2 localPointB = ShapeB.GetSupportVertex(axisB);
Vector2 pointA = TransformA.TransformPoint(localPointA);
Vector2 pointB = TransformB.TransformPoint(localPointB);
float separation = Vector2.Dot(pointB - pointA, Axis);
return(separation);
}
case Type.FaceA:
{
Vector2 normal = TransformA.TransformDirection(Axis);
Vector2 pointA = TransformA.TransformPoint(LocalPoint);
Vector2 axisB = TransformB.InverseTransformDirection(-normal);
Vector2 localPointB = ShapeB.GetSupportVertex(axisB);
Vector2 pointB = TransformB.TransformPoint(localPointB);
float separation = Vector2.Dot(pointB - pointA, normal);
return(separation);
}
case Type.FaceB:
{
Vector2 normal = TransformB.TransformDirection(Axis);
Vector2 pointB = TransformB.TransformPoint(LocalPoint);
Vector2 axisA = TransformA.InverseTransformDirection(-normal);
Vector2 localPointA = ShapeA.GetSupportVertex(axisA);
Vector2 pointA = TransformA.TransformPoint(localPointA);
float separation = Vector2.Dot(pointA - pointB, normal);
return(separation);
}
default:
Box2DXDebug.Assert(false);
return(0.0f);
}
}
public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 c)
{
//Note that v0 is independent of any details of the specific edge
//We are relying on v0 being consistent between multiple edges of the same body
Vector2 v0 = offset * normal;
//b2Vec2 v0 = xf.position + (offset - b2Dot(normal, xf.position)) * normal;
Vector2 v1 = xf.TransformPoint(_v1);
Vector2 v2 = xf.TransformPoint(_v2);
float d1 = Vector2.Dot(normal, v1) - offset;
float d2 = Vector2.Dot(normal, v2) - offset;
if (d1 > 0.0f)
{
if (d2 > 0.0f)
{
c = new Vector2();
return(0.0f);
}
else
{
v1 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2;
}
}
else
{
if (d2 > 0.0f)
{
v2 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2;
}
else
{
//Nothing
}
}
// v0,v1,v2 represents a fully submerged triangle
float k_inv3 = 1.0f / 3.0f;
// Area weighted centroid
c = k_inv3 * (v0 + v1 + v2);
Vector2 e1 = v1 - v0;
Vector2 e2 = v2 - v0;
return(0.5f * e1.Cross(e2));
}
internal void ReadCache(SimplexCache cache, Shape shapeA, Transform transformA, Shape shapeB, Transform transformB)
{
Box2DXDebug.Assert(0 <= cache.Count && cache.Count <= 3);
// Copy data from cache.
_count = cache.Count;
SimplexVertex[] vertices = new SimplexVertex[] { _v1, _v2, _v3 };
for (int i = 0; i < _count; ++i)
{
SimplexVertex v = vertices[i];
v.indexA = cache.IndexA[i];
v.indexB = cache.IndexB[i];
Vector2 wALocal = shapeA.GetVertex(v.indexA);
Vector2 wBLocal = shapeB.GetVertex(v.indexB);
v.wA = transformA.TransformPoint(wALocal);
v.wB = transformB.TransformPoint(wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
}
// 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 < Common.Settings.FLT_EPSILON)
{
// Reset the simplex.
_count = 0;
}
}
// If the cache is empty or invalid ...
if (_count == 0)
{
SimplexVertex v = vertices[0];
v.indexA = 0;
v.indexB = 0;
Vector2 wALocal = shapeA.GetVertex(0);
Vector2 wBLocal = shapeB.GetVertex(0);
v.wA = transformA.TransformPoint(wALocal);
v.wB = transformB.TransformPoint(wBLocal);
v.w = v.wB - v.wA;
_count = 1;
}
}
/// <summary>
/// Build vertices to represent an oriented box.
/// </summary>
/// <param name="hx">The half-width</param>
/// <param name="hy">The half-height.</param>
/// <param name="center">The center of the box in local coordinates.</param>
/// <param name="angle">The rotation of the box in local coordinates.</param>
public void SetAsBox(float hx, float hy, Vector2 center, float angle)
{
SetAsBox(hx, hy);
Transform xf = new Transform();
xf.position = center;
xf.rotation = Box2DX.Common.Math.AngleToRotation(angle);
//xf.R = new Mat22(angle);
//Debug.Log(string.Format("xf.position = ({0},{1}) xf.rotation = ({2},{3},{4},{5})", xf.position.x, xf.position.y, xf.rotation.x, xf.rotation.y, xf.rotation.z, xf.rotation.w));
for (int i = 0; i < VertexCount; ++i)
{
Vertices[i] = xf.TransformPoint(Vertices[i]);
}
}
public static void FindIncidentEdge(out ClipVertex[] c, PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2)
{
int count1 = poly1._vertexCount;
Vector2[] normals1 = poly1._normals;
int count2 = poly2._vertexCount;
Vector2[] vertices2 = poly2._vertices;
Vector2[] normals2 = poly2._normals;
Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Vector2 normal1 = xf2.InverseTransformDirection(xf1.TransformDirection(normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = Settings.FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(normal1, normals2[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;
c = new ClipVertex[2];
c[0].V = Common.Math.Mul(xf2, vertices2[i1]);
c[0].ID.Features.ReferenceEdge = (byte)edge1;
c[0].ID.Features.IncidentEdge = (byte)i1;
c[0].ID.Features.IncidentVertex = 0;
c[1].V = Common.Math.Mul(xf2, vertices2[i2]);
c[1].ID.Features.ReferenceEdge = (byte)edge1;
c[1].ID.Features.IncidentEdge = (byte)i2;
c[1].ID.Features.IncidentVertex = 1;
}
public override void ComputeAABB(out AABB aabb, Transform xf)
{
Vector2 lower = xf.TransformPoint(_vertices[0]);
Vector2 upper = lower;
for (int i = 1; i < _vertexCount; ++i)
{
Vector2 v = xf.TransformPoint(_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;
}
public override bool TestPoint(Transform xf, Vector2 p)
{
Vector2 pLocal = xf.InverseTransformDirection(p - xf.position);
int vc = _vertexCount;
for (int i = 0; i < vc; ++i)
{
float dot = Vector2.Dot(_normals[i], pLocal - _vertices[i]);
if (dot > 0.0f)
{
return(false);
}
}
return(true);
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = Vector2.Zero;
Vector2 position = xf.position + xf.TransformDirection(_position);
Vector2 s = segment.P1 - position;
float b = Vector2.Dot(s, s) - _radius * _radius;
// Does the segment start inside the circle?
if (b < 0.0f)
{
lambda = 0f;
return(SegmentCollide.StartInsideCollide);
}
// Solve quadratic equation.
Vector2 r = segment.P2 - segment.P1;
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 < Common.Settings.FLT_EPSILON)
{
return(SegmentCollide.MissCollide);
}
// Find the point of intersection of the line with the circle.
float a = -(c + Common.Math.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= maxLambda * rr)
{
a /= rr;
lambda = a;
normal = s + a * r;
normal.Normalize();
return(SegmentCollide.HitCollide);
}
return(SegmentCollide.MissCollide);
}
public static void CollideCircles(ref Manifold manifold, CircleShape circle1, Transform xf1, CircleShape circle2, Transform xf2)
{
manifold.PointCount = 0;
Vector2 p1 = xf1.TransformPoint(circle1._position);
Vector2 p2 = xf2.TransformPoint(circle2._position);
Vector2 d = p2 - p1;
float distSqr = Vector2.Dot(d, d);
float radius = circle1._radius + circle2._radius;
if (distSqr > radius * radius)
{
return;
}
manifold.Type = ManifoldType.Circles;
manifold.LocalPoint = circle1._position;
manifold.LocalPlaneNormal = Vector2.zero;
manifold.PointCount = 1;
manifold.Points[0].LocalPoint = circle2._position;
manifold.Points[0].ID.Key = 0;
}
public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda)
{
Vector2 r = segment.P2 - segment.P1;
Vector2 v1 = xf.TransformPoint(_v1);
Vector2 d = ((Vector2)xf.TransformPoint(_v2)) - v1;
Vector2 n = d.CrossScalarPostMultiply(1.0f);
float k_slop = 100.0f * Common.Settings.FLT_EPSILON;
float denom = -Vector2.Dot(r, n);
// Cull back facing collision and ignore parallel segments.
if (denom > k_slop)
{
// Does the segment intersect the infinite line associated with this segment?
Vector2 b = segment.P1 - v1;
float a = Vector2.Dot(b, n);
if (0.0f <= a && a <= maxLambda * denom)
{
float mu2 = -r.x * b.y + r.y * b.x;
// Does the segment intersect this segment?
if (-k_slop * denom <= mu2 && mu2 <= denom * (1.0f + k_slop))
{
a /= denom;
n.Normalize();
lambda = a;
normal = n;
return(SegmentCollide.HitCollide);
}
}
}
lambda = 0;
normal = new Vector2();
return(SegmentCollide.MissCollide);
}
/// <summary>
/// Find the separation between poly1 and poly2 for a give edge normal on poly1.
/// </summary>
public static float EdgeSeparation(PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2)
{
int count1 = poly1._vertexCount;
Vector2[] vertices1 = poly1._vertices;
Vector2[] normals1 = poly1._normals;
int count2 = poly2._vertexCount;
Vector2[] vertices2 = poly2._vertices;
Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);
// Convert normal from poly1's frame into poly2's frame.
Vector2 normal1World = xf1.TransformDirection(normals1[edge1]);
Vector2 normal1 = xf2.InverseTransformDirection(normal1World);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Common.Settings.FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(vertices2[i], normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
Vector2 v1 = xf1.TransformPoint(vertices1[edge1]);
Vector2 v2 = xf2.TransformPoint(vertices2[index]);
float separation = Vector2.Dot(v2 - v1, normal1World);
return(separation);
}
internal void RefilterProxy(BroadPhase broadPhase, Transform Transform)
{
if (_proxyId == PairManager.NullProxy)
{
return;
}
broadPhase.DestroyProxy(_proxyId);
AABB aabb;
_shape.ComputeAABB(out aabb, Transform);
bool inRange = broadPhase.InRange(aabb);
if (inRange)
{
_proxyId = broadPhase.CreateProxy(aabb, this);
}
else
{
_proxyId = PairManager.NullProxy;
}
}
private void DrawFixture(Fixture fixture, Transform xf, Color color, bool core)
{
#warning "the core argument is not used, the coreColor variable is also not used"
Color coreColor = new Color(0.9f, 0.6f, 0.6f);
switch (fixture.ShapeType)
{
case ShapeType.CircleShape:
{
CircleShape circle = (CircleShape)fixture.Shape;
Vector2 center = xf.TransformPoint(circle._position);
float radius = circle._radius;
// [CHRISK] FIXME Vector2 axis = xf.R.Col1;
//_debugDraw.DrawSolidCircle(center, radius, axis, color);
}
break;
case ShapeType.PolygonShape:
{
PolygonShape poly = (PolygonShape)fixture.Shape;
int vertexCount = poly._vertexCount;
Vector2[] localVertices = poly._vertices;
Box2DXDebug.Assert(vertexCount <= Settings.MaxPolygonVertices);
Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices];
for (int i = 0; i < vertexCount; ++i)
{
vertices[i] = xf.TransformPoint(localVertices[i]);
}
_debugDraw.DrawSolidPolygon(vertices, vertexCount, color);
}
break;
case ShapeType.EdgeShape:
{
EdgeShape edge = (EdgeShape)fixture.Shape;
_debugDraw.DrawSegment(xf.TransformPoint(edge.Vertex1), xf.TransformPoint(edge.Vertex2), color);
}
break;
}
}
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
public static void CollidePolygons(ref Manifold manifold,
PolygonShape polyA, Transform xfA, PolygonShape polyB, Transform xfB)
{
manifold.PointCount = 0;
float totalRadius = polyA._radius + polyB._radius;
int edgeA = 0;
float separationA = Collision.FindMaxSeparation(ref edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
float separationB = Collision.FindMaxSeparation(ref edgeB, polyB, xfB, polyA, xfA);
if (separationB > totalRadius)
{
return;
}
PolygonShape poly1; // reference poly
PolygonShape poly2; // incident poly
Transform xf1, xf2;
int edge1; // reference edge
byte 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 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.Type = ManifoldType.FaceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.Type = ManifoldType.FaceA;
flip = 0;
}
ClipVertex[] incidentEdge;
Collision.FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1._vertexCount;
Vector2[] vertices1 = poly1._vertices;
Vector2 v11 = vertices1[edge1];
Vector2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
Vector2 dv = v12 - v11;
Vector2 localNormal = dv.CrossScalarPostMultiply(1.0f);
localNormal.Normalize();
Vector2 planePoint = 0.5f * (v11 + v12);
Vector2 sideNormal = xf1.TransformDirection(v12 - v11);
sideNormal.Normalize();
Vector2 frontNormal = sideNormal.CrossScalarPostMultiply(1.0f);
v11 = Common.Math.Mul(xf1, v11);
v12 = Common.Math.Mul(xf1, v12);
float frontOffset = Vector2.Dot(frontNormal, v11);
float sideOffset1 = -Vector2.Dot(sideNormal, v11);
float sideOffset2 = Vector2.Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1;
ClipVertex[] clipPoints2;
int np;
// Clip to box side 1
np = Collision.ClipSegmentToLine(out clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.LocalPlaneNormal = localNormal;
manifold.LocalPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vector2.Dot(frontNormal, clipPoints2[i].V) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.LocalPoint = xf2.InverseTransformPoint(clipPoints2[i].V);
cp.ID = clipPoints2[i].ID;
cp.ID.Features.Flip = flip;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
/// Cast a ray against this shape. /// @param output the ray-cast results. /// @param input the ray-cast input parameters. /// @param transform the transform to be applied to the shape. public abstract void RayCast(out RayCastOutput output, ref RayCastInput input, Transform transform);
/// <summary> /// Draw a Transform. Choose your own length scale. /// </summary> /// <param name="xf">A Transform.</param> public abstract void DrawTransform(Transform xf);
// This implements 2-sided edge vs circle collision.
public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, Transform transformA, CircleShape circle, Transform transformB)
{
manifold.PointCount = 0;
Vector2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position));
Vector2 normal = edge._normal;
Vector2 v1 = edge._v1;
Vector2 v2 = edge._v2;
float radius = edge._radius + circle._radius;
// Barycentric coordinates
float u1 = Vector2.Dot(cLocal - v1, v2 - v1);
float u2 = Vector2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
// Behind v1
if ((cLocal- v1).sqrMagnitude > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = cLocal - v1;
manifold.LocalPlaneNormal.Normalize();
manifold.LocalPoint = v1;
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
else if (u2 <= 0.0f)
{
// Ahead of v2
if ((cLocal- v2).sqrMagnitude > radius * radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = cLocal - v2;
manifold.LocalPlaneNormal.Normalize();
manifold.LocalPoint = v2;
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
else
{
float separation = Vector2.Dot(cLocal - v1, normal);
if (separation < -radius || radius < separation)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = separation < 0.0f ? -normal : normal;
manifold.LocalPoint = 0.5f * (v1 + v2);
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
}
public override bool TestPoint(Transform xf, Vector2 p)
{
return(false);
}
public void Synchronize(BroadPhase broadPhase, Transform transform1, Transform transform2)
{
if (ProxyId == BroadPhase.NullProxy)
{
return;
}
// Compute an AABB that covers the swept shape (may miss some rotation effect).
AABB aabb1, aabb2;
Shape.ComputeAABB(out aabb1, ref transform1);
Shape.ComputeAABB(out aabb2, ref transform2);
Aabb.Combine(aabb1, aabb2);
Vec2 displacement = transform2.Position - transform1.Position;
broadPhase.MoveProxy(ProxyId, Aabb, displacement);
}
public void SetAsBox(float hx, float hy, Vec2 center, float angle)
{
VertexCount = 4;
Vertices[0].Set(-hx, -hy);
Vertices[1].Set(hx, -hy);
Vertices[2].Set(hx, hy);
Vertices[3].Set(-hx, hy);
Normals[0].Set(0.0f, -1.0f);
Normals[1].Set(1.0f, 0.0f);
Normals[2].Set(0.0f, 1.0f);
Normals[3].Set(-1.0f, 0.0f);
Centroid = center;
Transform xf = new Transform();
xf.Position = center;
xf.R.Set(angle);
// Transform vertices and normals.
for (int i = 0; i < VertexCount; ++i)
{
Vertices[i] = Math.Mul(xf, Vertices[i]);
Normals[i] = Math.Mul(xf.R, Normals[i]);
}
}
public override void RayCast(out RayCastOutput output, ref RayCastInput input, Transform xf)
{
output = new RayCastOutput();
float lower = 0.0f, upper = input.MaxFraction;
// Put the ray into the polygon's frame of reference.
Vec2 p1 = Math.MulT(xf.R, input.P1 - xf.Position);
Vec2 p2 = Math.MulT(xf.R, input.P2 - xf.Position);
Vec2 d = p2 - p1;
int index = -1;
output.Hit = false;
for (int i = 0; i < VertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vec2.Dot(Normals[i], Vertices[i] - p1);
float denominator = Vec2.Dot(Normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower)
{
return;
}
}
Box2DXDebug.Assert(0.0f <= lower && lower <= input.MaxFraction);
if (index >= 0)
{
output.Hit = true;
output.Fraction = lower;
output.Normal = Math.Mul(xf.R, Normals[index]);
return;
}
}
/// <summary>
/// Find the max separation between poly1 and poly2 using edge normals from poly1.
/// </summary>
/// <param name="edgeIndex"></param>
/// <param name="poly1"></param>
/// <param name="xf1"></param>
/// <param name="poly2"></param>
/// <param name="xf2"></param>
/// <returns></returns>
public static float FindMaxSeparation(out int edgeIndex,
PolygonShape poly1, Transform xf1,
PolygonShape poly2, Transform xf2)
{
int count1 = poly1.VertexCount;
Vec2[] normals1 = poly1.Normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
Vec2 d = Math.Mul(xf2, poly2.Centroid) - Math.Mul(xf1, poly1.Centroid);
Vec2 dLocal1 = Math.MulT(xf1.R, d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -Settings.FLT_MAX;
for (int i = 0; i < count1; ++i)
{
float dot = Vec2.Dot(normals1[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, 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, xf1, edge, poly2, xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return bestSeparation;
}
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
public static void CollidePolygons(out Manifold manifold,
PolygonShape polyA, Transform xfA,
PolygonShape polyB, Transform xfB)
{
manifold = new Manifold();
manifold.PointCount = 0;
float totalRadius = polyA._radius + polyB._radius;
int edgeA = 0;
float separationA = FindMaxSeparation(out edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
return;
int edgeB = 0;
float separationB = FindMaxSeparation(out edgeB, polyB, xfB, polyA, xfA);
if (separationB > totalRadius)
return;
PolygonShape poly1; // reference poly
PolygonShape poly2; // incident poly
Transform xf1, xf2;
int edge1; // reference edge
byte 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 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.Type = Manifold.ManifoldType.FaceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.Type = Manifold.ManifoldType.FaceA;
flip = 0;
}
ClipVertex[] incidentEdge;
FindIncidentEdge(out incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
Vec2[] vertices1 = poly1.Vertices;
Vec2 v11 = vertices1[edge1];
Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1 + 1] : vertices1[0];
Vec2 localTangent = v12 - v11;
localTangent.Normalize();
Vec2 localNormal = Vec2.Cross(localTangent, 1.0f);
Vec2 planePoint = 0.5f * (v11 + v12);
Vec2 tangent = Math.Mul(xf1.R, localTangent);
Vec2 normal = Vec2.Cross(tangent, 1.0f);
v11 = Math.Mul(xf1, v11);
v12 = Math.Mul(xf1, v12);
// Face offset.
float frontOffset = Vec2.Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Vec2.Dot(tangent, v11) + totalRadius;
float sideOffset2 = Vec2.Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1;
ClipVertex[] clipPoints2;
int np;
// Clip to box side 1
np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.LocalPlaneNormal = localNormal;
manifold.LocalPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vec2.Dot(normal, clipPoints2[i].V) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = manifold.Points[pointCount];
cp.LocalPoint = Math.MulT(xf2, clipPoints2[i].V);
cp.ID = clipPoints2[i].ID;
cp.ID.Features.Flip = flip;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
public static void FindIncidentEdge(out ClipVertex[] c,
PolygonShape poly1, Transform xf1, int edge1,
PolygonShape poly2, Transform xf2)
{
int count1 = poly1.VertexCount;
Vec2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
Vec2[] vertices2 = poly2.Vertices;
Vec2[] normals2 = poly2.Normals;
Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Vec2 normal1 = Math.MulT(xf2.R, Math.Mul(xf1.R, normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = Settings.FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vec2.Dot(normal1, normals2[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;
c = new ClipVertex[2];
c[0].V = Math.Mul(xf2, vertices2[i1]);
c[0].ID.Features.ReferenceEdge = (byte)edge1;
c[0].ID.Features.IncidentEdge = (byte)i1;
c[0].ID.Features.IncidentVertex = 0;
c[1].V = Math.Mul(xf2, vertices2[i2]);
c[1].ID.Features.ReferenceEdge = (byte)edge1;
c[1].ID.Features.IncidentEdge = (byte)i2;
c[1].ID.Features.IncidentVertex = 1;
}
/// <summary>
/// Find the separation between poly1 and poly2 for a give edge normal on poly1.
/// </summary>
/// <param name="poly1"></param>
/// <param name="xf1"></param>
/// <param name="edge1"></param>
/// <param name="poly2"></param>
/// <param name="xf2"></param>
/// <returns></returns>
public static float EdgeSeparation(PolygonShape poly1, Transform xf1, int edge1,
PolygonShape poly2, Transform xf2)
{
int count1 = poly1.VertexCount;
Vec2[] vertices1 = poly1.Vertices;
Vec2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
Vec2[] vertices2 = poly2.Vertices;
Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);
// Convert normal from poly1's frame into poly2's frame.
Vec2 normal1World = Math.Mul(xf1.R, normals1[edge1]);
Vec2 normal1 = Math.MulT(xf2.R, normal1World);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Settings.FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vec2.Dot(vertices2[i], normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
Vec2 v1 = Math.Mul(xf1, vertices1[edge1]);
Vec2 v2 = Math.Mul(xf2, vertices2[index]);
float separation = Vec2.Dot(v2 - v1, normal1World);
return separation;
}
public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 c)
{
//Transform plane into shape co-ordinates
Vector2 normalL = xf.InverseTransformDirection(normal);
float offsetL = offset - Vector2.Dot(normal, xf.position);
float[] depths = new float[Common.Settings.MaxPolygonVertices];
int diveCount = 0;
int intoIndex = -1;
int outoIndex = -1;
bool lastSubmerged = false;
int i;
for (i = 0; i < _vertexCount; i++)
{
depths[i] = Vector2.Dot(normalL, _vertices[i]) - offsetL;
bool isSubmerged = depths[i] < -Common.Settings.FLT_EPSILON;
if (i > 0)
{
if (isSubmerged)
{
if (!lastSubmerged)
{
intoIndex = i - 1;
diveCount++;
}
}
else
{
if (lastSubmerged)
{
outoIndex = i - 1;
diveCount++;
}
}
}
lastSubmerged = isSubmerged;
}
switch (diveCount)
{
case 0:
if (lastSubmerged)
{
//Completely submerged
MassData md;
ComputeMass(out md, 1f);
c = xf.TransformPoint(md.Center);
return(md.Mass);
}
else
{
//Completely dry
c = new Vector2();
return(0);
}
break;
case 1:
if (intoIndex == -1)
{
intoIndex = _vertexCount - 1;
}
else
{
outoIndex = _vertexCount - 1;
}
break;
}
int intoIndex2 = (intoIndex + 1) % _vertexCount;
int outoIndex2 = (outoIndex + 1) % _vertexCount;
float intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]);
float outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]);
Vector2 intoVec = new Vector2(_vertices[intoIndex].X * (1 - intoLambda) + _vertices[intoIndex2].X * intoLambda,
_vertices[intoIndex].Y * (1 - intoLambda) + _vertices[intoIndex2].Y * intoLambda);
Vector2 outoVec = new Vector2(_vertices[outoIndex].X * (1 - outoLambda) + _vertices[outoIndex2].X * outoLambda,
_vertices[outoIndex].Y * (1 - outoLambda) + _vertices[outoIndex2].Y * outoLambda);
//Initialize accumulator
float area = 0;
Vector2 center = Vector2.Zero;
Vector2 p2 = _vertices[intoIndex2];
Vector2 p3;
const float k_inv3 = 1.0f / 3.0f;
//An awkward loop from intoIndex2+1 to outIndex2
i = intoIndex2;
while (i != outoIndex2)
{
i = (i + 1) % _vertexCount;
if (i == outoIndex2)
{
p3 = outoVec;
}
else
{
p3 = _vertices[i];
}
//Add the triangle formed by intoVec,p2,p3
{
Vector2 e1 = p2 - intoVec;
Vector2 e2 = p3 - intoVec;
float D = e1.Cross(e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (intoVec + p2 + p3);
}
//
p2 = p3;
}
//Normalize and Transform centroid
center *= 1.0f / area;
c = xf.TransformPoint(center);
return(area);
}
internal override void InitVelocityConstraints(TimeStep step)
{
Body b1 = _body1;
Body b2 = _body2;
_localCenter1 = b1.GetLocalCenter();
_localCenter2 = b2.GetLocalCenter();
Transform xf1 = b1.GetTransform();
Transform xf2 = b2.GetTransform();
// Compute the effective masses.
Vector2 r1 = xf1.TransformDirection(_localAnchor1 - _localCenter1);
Vector2 r2 = xf2.TransformDirection(_localAnchor2 - _localCenter2);
Vector2 d = b2._sweep.C + r2 - b1._sweep.C - r1;
_invMass1 = b1._invMass;
_invI1 = b1._invI;
_invMass2 = b2._invMass;
_invI2 = b2._invI;
// Compute motor Jacobian and effective mass.
{
_axis = xf1.TransformDirection(_localXAxis1);
_a1 = (d + r1).Cross(_axis);
_a2 = r2.Cross(_axis);
_motorMass = _invMass1 + _invMass2 + _invI1 * _a1 * _a1 + _invI2 * _a2 * _a2;
Box2DXDebug.Assert(_motorMass > Settings.FLT_EPSILON);
_motorMass = 1.0f / _motorMass;
}
// Prismatic constraint.
{
_perp = xf1.TransformDirection(_localYAxis1);
_s1 = (d + r1).Cross(_perp);
_s2 = r2.Cross(_perp);
float m1 = _invMass1, m2 = _invMass2;
float i1 = _invI1, i2 = _invI2;
float k11 = m1 + m2 + i1 * _s1 * _s1 + i2 * _s2 * _s2;
float k12 = i1 * _s1 * _a1 + i2 * _s2 * _a2;
float k22 = m1 + m2 + i1 * _a1 * _a1 + i2 * _a2 * _a2;
_K.Col1 = new Vector2(k11, k12);
_K.Col2 = new Vector2(k12, k22);
}
// Compute motor and limit terms.
if (_enableLimit)
{
float jointTranslation = Vector2.Dot(_axis, d);
if (Box2DX.Common.Math.Abs(_upperTranslation - _lowerTranslation) < 2.0f * Settings.LinearSlop)
{
_limitState = LimitState.EqualLimits;
}
else if (jointTranslation <= _lowerTranslation)
{
if (_limitState != LimitState.AtLowerLimit)
{
_limitState = LimitState.AtLowerLimit;
_impulse.Y = 0.0f;
}
}
else if (jointTranslation >= _upperTranslation)
{
if (_limitState != LimitState.AtUpperLimit)
{
_limitState = LimitState.AtUpperLimit;
_impulse.Y = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
_impulse.Y = 0.0f;
}
}
else
{
_limitState = LimitState.InactiveLimit;
}
if (_enableMotor == false)
{
_motorImpulse = 0.0f;
}
if (step.WarmStarting)
{
// Account for variable time step.
_impulse *= step.DtRatio;
_motorImpulse *= step.DtRatio;
Vector2 P = _impulse.X * _perp + (_motorImpulse + _impulse.Y) * _axis;
float L1 = _impulse.X * _s1 + (_motorImpulse + _impulse.Y) * _a1;
float L2 = _impulse.X * _s2 + (_motorImpulse + _impulse.Y) * _a2;
b1._linearVelocity -= _invMass1 * P;
b1._angularVelocity -= _invI1 * L1;
b2._linearVelocity += _invMass2 * P;
b2._angularVelocity += _invI2 * L2;
}
else
{
_impulse = Vector2.Zero;
_motorImpulse = 0.0f;
}
}
static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB)
{
output = new DistanceOutput();
Transform transformA = input.TransformA;
Transform transformB = input.TransformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
#if ALLOWUNSAFE
fixed(SimplexCache *sPtr = &cache)
{
simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
}
#else
simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB);
#endif
// Get simplex vertices as an array.
#if ALLOWUNSAFE
SimplexVertex *vertices = &simplex._v1;
#else
SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 };
#endif
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
#if ALLOWUNSAFE
int *lastA = stackalloc int[4], lastB = stackalloc int[4];
#else
int[] lastA = new int[4];
int[] lastB = new int[4];
#endif // ALLOWUNSAFE
int lastCount;
// Main iteration loop.
int iter = 0;
const int k_maxIterationCount = 20;
while (iter < k_maxIterationCount)
{
// Copy simplex so we can identify duplicates.
lastCount = simplex._count;
int i;
for (i = 0; i < lastCount; ++i)
{
lastA[i] = vertices[i].indexA;
lastB[i] = vertices[i].indexB;
}
switch (simplex._count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
#if DEBUG
Box2DXDebug.Assert(false);
#endif
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex._count == 3)
{
break;
}
// Compute closest point.
Vector2 p = simplex.GetClosestPoint();
float distanceSqr = p.sqrMagnitude;
// Ensure the search direction is numerically fit.
if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
#if ALLOWUNSAFE
SimplexVertex *vertex = vertices + simplex._count;
vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
vertex->wA = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA));
//Vec2 wBLocal;
vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
vertex->wB = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB));
vertex->w = vertex->wB - vertex->wA;
#else
SimplexVertex vertex = vertices[simplex._count - 1];
vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
vertex.wA = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA));
//Vec2 wBLocal;
vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
vertex.wB = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
#endif // ALLOWUNSAFE
// Iteration count is equated to the number of support point calls.
++iter;
// Check for convergence.
#if ALLOWUNSAFE
float lowerBound = Vector2.Dot(p, vertex->w);
#else
float lowerBound = Vector2.Dot(p, vertex.w);
#endif
float upperBound = distanceSqr;
const float k_relativeTolSqr = 0.01f * 0.01f; // 1:100
if (upperBound - lowerBound <= k_relativeTolSqr * upperBound)
{
// Converged!
break;
}
// Check for duplicate support points.
bool duplicate = false;
for (i = 0; i < lastCount; ++i)
{
#if ALLOWUNSAFE
if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i])
#else
if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i])
#endif
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex._count;
}
#if ALLOWUNSAFE
fixed(DistanceOutput *doPtr = &output)
{
// Prepare output.
simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
doPtr->Distance = Vector2.Distance(doPtr->PointA, doPtr->PointB);
doPtr->Iterations = iter;
}
fixed(SimplexCache *sPtr = &cache)
{
// Cache the simplex.
simplex.WriteCache(sPtr);
}
#else
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = Vector2.Distance(output.PointA, output.PointB);
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
#endif
// Apply radii if requested.
if (input.UseRadii)
{
float rA = shapeA._radius;
float rB = shapeB._radius;
if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vector2 normal = output.PointB - output.PointA;
normal.Normalize();
output.PointA += rA * normal;
output.PointB -= rB * normal;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
Vector2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}
/// 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.
public void Initialize(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB)
{
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case ManifoldType.Circles:
{
Vector2 pointA = xfA.TransformPoint(manifold.LocalPoint);
Vector2 pointB = xfB.TransformPoint(manifold.Points[0].LocalPoint);
Vector2 normal = new Vector2(1.0f, 0.0f);
if ((pointA - pointB).sqrMagnitude > (Mathf.Epsilon * Mathf.Epsilon))
{
normal = pointB - pointA;
normal.Normalize();
}
Normal = normal;
Vector2 cA = pointA + radiusA * normal;
Vector2 cB = pointB - radiusB * normal;
Points[0] = 0.5f * (cA + cB);
}
break;
case ManifoldType.FaceA:
{
Vector2 normal = xfA.TransformDirection(manifold.LocalPlaneNormal);
Vector2 planePoint = xfA.TransformPoint(manifold.LocalPoint);
// Ensure normal points from A to B.
Normal = normal;
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 clipPoint = xfB.TransformPoint(manifold.Points[i].LocalPoint);
Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Vector2 cB = clipPoint - radiusB * normal;
Points[i] = 0.5f * (cA + cB);
}
}
break;
case ManifoldType.FaceB:
{
Vector2 normal = xfB.TransformDirection(manifold.LocalPlaneNormal);
Vector2 planePoint = xfB.TransformPoint(manifold.LocalPoint);
// Ensure normal points from A to B.
Normal = -normal;
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 clipPoint = xfA.TransformPoint(manifold.Points[i].LocalPoint);
Vector2 cA = clipPoint - radiusA * normal;
Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Points[i] = 0.5f * (cA + cB);
}
}
break;
}
}
/// <summary>
/// Find the max separation between poly1 and poly2 using edge normals from poly1.
/// </summary>
public static float FindMaxSeparation(ref int edgeIndex, PolygonShape poly1, Transform xf1, PolygonShape poly2, Transform xf2)
{
int count1 = poly1._vertexCount;
Vector2[] normals1 = poly1._normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
Vector2 d = xf2.TransformPoint(poly2._centroid) - xf1.TransformPoint(poly2._centroid);
Vector2 dLocal1 = xf1.InverseTransformDirection(d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -Common.Settings.FLT_MAX;
for (int i = 0; i < count1; ++i)
{
float dot = Vector2.Dot(normals1[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = Collision.EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = Collision.EdgeSeparation(poly1, xf1, nextEdge, poly2, 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 = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return(bestSeparation);
}
public override void DrawXForm(Transform xf)
{
Vec2 p1 = xf.Position, p2;
float k_axisScale = 0.4f;
Gl.glBegin(Gl.GL_LINES);
Gl.glColor3f(1.0f, 0.0f, 0.0f);
Gl.glVertex2f(p1.X, p1.Y);
p2 = p1 + k_axisScale * xf.R.Col1;
Gl.glVertex2f(p2.X, p2.Y);
Gl.glColor3f(0.0f, 1.0f, 0.0f);
Gl.glVertex2f(p1.X, p1.Y);
p2 = p1 + k_axisScale * xf.R.Col2;
Gl.glVertex2f(p2.X, p2.Y);
Gl.glEnd();
}
internal void ComputeTransform(ref Transform xf, Vector2 center, Vector2 localCenter, float angle)
{
xf.rotation = Box2DX.Common.Math.AngleToRotation(angle);
//xf.R = new Mat22(angle);
xf.position = center - xf.TransformDirection(localCenter);
}
// We need separation create/destroy functions from the constructor/destructor because
// the destructor cannot access the allocator or broad-phase (no destructor arguments allowed by C++).
public void Create(BroadPhase broadPhase, Body body, Transform xf, FixtureDef def)
{
UserData = def.UserData;
Friction = def.Friction;
Restitution = def.Restitution;
Body = body;
_next = null;
Filter = def.Filter;
IsSensor = def.IsSensor;
Shape = def.Shape.Clone();
Shape.ComputeMass(out _massData, def.Density);
// Create proxy in the broad-phase.
Shape.ComputeAABB(out Aabb, ref xf);
ProxyId = broadPhase.CreateProxy(Aabb, this);
}
internal void SynchronizeFixtures()
{
Transform xf1 = new Transform();
xf1.R.Set(_sweep.A0);
xf1.Position = _sweep.C0 - Math.Mul(xf1.R, _sweep.LocalCenter);
BroadPhase broadPhase = _world._contactManager._broadPhase;
for (Fixture f = _fixtureList; f != null; f = f._next)
{
f.Synchronize(broadPhase, xf1, _xf);
}
}
// Polygon versus 2-sided edge.
public static void CollidePolyAndEdge(ref Manifold manifold, PolygonShape polygon, Transform TransformA, EdgeShape edge, Transform TransformB)
{
PolygonShape polygonB = new PolygonShape();
polygonB.SetAsEdge(edge._v1, edge._v2);
CollidePolygons(ref manifold, polygon, TransformA, polygonB, TransformB);
}
public override bool TestPoint(Transform transform, Vec2 p)
{
Vec2 center = transform.Position + Math.Mul(transform.R, _p);
Vec2 d = p - center;
return Vec2.Dot(d, d) <= _radius * _radius;
}
/// <summary> /// Test a point for containment in this shape. This only works for convex shapes. /// </summary> /// <param name="xf">The shape world Transform.</param> /// <param name="p">A point in world coordinates.</param> /// <returns></returns> public abstract bool TestPoint(Transform xf, Vector2 p);
internal unsafe void ReadCache(SimplexCache* cache, Shape shapeA, Transform TransformA, Shape shapeB, Transform TransformB)
{
Box2DXDebug.Assert(0 <= cache->Count && cache->Count <= 3);
// Copy data from cache.
_count = cache->Count;
SimplexVertex** vertices = stackalloc SimplexVertex*[3];
fixed (SimplexVertex* v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3)
{
vertices[0] = v1Ptr;
vertices[1] = v2Ptr;
vertices[2] = v3Ptr;
for (int i = 0; i < _count; ++i)
{
SimplexVertex* v = vertices[i];
v->indexA = cache->IndexA[i];
v->indexB = cache->IndexB[i];
Vector2 wALocal = shapeA.GetVertex(v->indexA);
Vector2 wBLocal = shapeB.GetVertex(v->indexB);
v->wA = TransformA.TransformPoint(wALocal);
v->wB = TransformB.TransformPoint(wBLocal);
v->w = v->wB - v->wA;
v->a = 0.0f;
}
// 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 < Common.Settings.FLT_EPSILON)
{
// Reset the simplex.
_count = 0;
}
}
// If the cache is empty or invalid ...
if (_count == 0)
{
SimplexVertex* v = vertices[0];
v->indexA = 0;
v->indexB = 0;
Vector2 wALocal = shapeA.GetVertex(0);
Vector2 wBLocal = shapeB.GetVertex(0);
v->wA = TransformA.TransformPoint(wALocal);
v->wB = TransformB.TransformPoint(wBLocal);
v->w = v->wB - v->wA;
_count = 1;
}
}
}
/// <summary> /// Given a Transform, compute the associated axis aligned bounding box for this shape. /// </summary> /// <param name="aabb">Returns the axis aligned box.</param> /// <param name="xf">The world Transform of the shape.</param> public abstract void ComputeAABB(out AABB aabb, Transform xf);
public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda)
{
lambda = 0f;
normal = Vector2.Zero;
float lower = 0.0f, upper = maxLambda;
Vector2 p1 = xf.InverseTransformDirection(segment.P1 - xf.position);
Vector2 p2 = xf.InverseTransformDirection(segment.P2 - xf.position);
Vector2 d = p2 - p1;
int index = -1;
for (int i = 0; i < _vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float numerator = Vector2.Dot(_normals[i], _vertices[i] - p1);
float denominator = Vector2.Dot(_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return(SegmentCollide.MissCollide);
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower)
{
return(SegmentCollide.MissCollide);
}
}
Box2DXDebug.Assert(0.0f <= lower && lower <= maxLambda);
if (index >= 0)
{
lambda = lower;
normal = xf.TransformDirection(_normals[index]);
return(SegmentCollide.HitCollide);
}
lambda = 0f;
return(SegmentCollide.StartInsideCollide);
}
private static void CollidePolygonCircle(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2)
{
Collision.Collision.CollidePolygonAndCircle(ref manifold, (PolygonShape)shape1, xf1, (CircleShape)shape2, xf2);
}
/// <summary> /// Compute the volume and centroid of this shape intersected with a half plane. /// </summary> /// <param name="normal">Normal the surface normal.</param> /// <param name="offset">Offset the surface offset along normal.</param> /// <param name="xf">The shape Transform.</param> /// <param name="c">Returns the centroid.</param> /// <returns>The total volume less than offset along normal.</returns> public abstract float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 c);
/// 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.
public void Initialize(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB)
{
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case Manifold.ManifoldType.Circles:
{
Vec2 pointA = Math.Mul(xfA, manifold.LocalPoint);
Vec2 pointB = Math.Mul(xfB, manifold.Points[0].LocalPoint);
Vec2 normal = new Vec2(1.0f, 0.0f);
if (Vec2.DistanceSquared(pointA, pointB) > Settings.FLT_EPSILON * Settings.FLT_EPSILON)
{
normal = pointB - pointA;
normal.Normalize();
}
Normal = normal;
Vec2 cA = pointA + radiusA * normal;
Vec2 cB = pointB - radiusB * normal;
Points[0] = 0.5f * (cA + cB);
}
break;
case Manifold.ManifoldType.FaceA:
{
Vec2 normal = Math.Mul(xfA.R, manifold.LocalPlaneNormal);
Vec2 planePoint = Math.Mul(xfA, manifold.LocalPoint);
// Ensure normal points from A to B.
Normal = normal;
for (int i = 0; i < manifold.PointCount; ++i)
{
Vec2 clipPoint = Math.Mul(xfB, manifold.Points[i].LocalPoint);
Vec2 cA = clipPoint + (radiusA - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
Vec2 cB = clipPoint - radiusB * normal;
Points[i] = 0.5f * (cA + cB);
}
}
break;
case Manifold.ManifoldType.FaceB:
{
Vec2 normal = Math.Mul(xfB.R, manifold.LocalPlaneNormal);
Vec2 planePoint = Math.Mul(xfB, manifold.LocalPoint);
// Ensure normal points from A to B.
Normal = -normal;
for (int i = 0; i < manifold.PointCount; ++i)
{
Vec2 clipPoint = Math.Mul(xfA, manifold.Points[i].LocalPoint);
Vec2 cA = clipPoint - radiusA * normal;
Vec2 cB = clipPoint + (radiusB - Vec2.Dot(clipPoint - planePoint, normal)) * normal;
Points[i] = 0.5f * (cA + cB);
}
}
break;
}
}
/// <summary> /// Test a point for containment in this shape. This only works for convex shapes. /// </summary> /// <param name="xf">The shape world Transform.</param> /// <param name="p">A point in world coordinates.</param> /// <returns></returns> public abstract bool TestPoint(Transform xf, Vector2 p);
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public override void RayCast(out RayCastOutput output, ref RayCastInput input, Transform transform)
{
output = new RayCastOutput();
Vec2 position = transform.Position + Math.Mul(transform.R, _p);
Vec2 s = input.P1 - position;
float b = Vec2.Dot(s, s) - _radius * _radius;
// Solve quadratic equation.
Vec2 r = input.P2 - input.P1;
float c = Vec2.Dot(s, r);
float rr = Vec2.Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.FLT_EPSILON)
{
output.Hit = false;
return;
}
// Find the point of intersection of the line with the circle.
float a = -(c + Math.Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.MaxFraction * rr)
{
a /= rr;
output.Hit = true;
output.Fraction = a;
output.Normal = s + a*r;
output.Normal.Normalize();
return;
}
output.Hit = false;
return;
}
/// <summary> /// Perform a ray cast against this shape. /// </summary> /// <param name="xf">The shape world Transform.</param> /// <param name="lambda">Returns the hit fraction. You can use this to compute the contact point /// p = (1 - lambda) * segment.P1 + lambda * segment.P2.</param> /// <param name="normal"> Returns the normal at the contact point. If there is no intersection, /// the normal is not set.</param> /// <param name="segment">Defines the begin and end point of the ray cast.</param> /// <param name="maxLambda">A number typically in the range [0,1].</param> public abstract SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda);
public override void ComputeAABB(out AABB aabb, ref Transform xf)
{
Vec2 lower = Math.Mul(xf, Vertices[0]);
Vec2 upper = lower;
for (int i = 1; i < VertexCount; ++i)
{
Vec2 v = Math.Mul(xf, Vertices[i]);
lower = Math.Min(lower, v);
upper = Math.Max(upper, v);
}
Vec2 r = new Vec2(_radius, _radius);
aabb.LowerBound = lower - r;
aabb.UpperBound = upper + r;
}
/// <summary>
/// Set the position of the body's origin and rotation (radians).
/// This breaks any contacts and wakes the other bodies.
/// Note this is less efficient than the other overload - you should use that
/// if the angle is available.
/// </summary>
/// <param name="xf">The Transform of position and angle to set the body to.</param>
/// <returns>False if the movement put a shape outside the world. In this case the
/// body is automatically frozen.</returns>
public bool SetTransform(Transform xf)
{
return(SetTransform(xf.position, xf.rotation));
}
internal void ReadCache(SimplexCache cache, Shape shapeA, Transform transformA, Shape shapeB, Transform transformB)
{
Box2DXDebug.Assert(0 <= cache.Count && cache.Count <= 3);
// Copy data from cache.
_count = cache.Count;
SimplexVertex[] vertices = new SimplexVertex[] { _v1, _v2, _v3 };
for (int i = 0; i < _count; ++i)
{
SimplexVertex v = vertices[i];
v.indexA = cache.IndexA[i];
v.indexB = cache.IndexB[i];
Vector2 wALocal = shapeA.GetVertex(v.indexA);
Vector2 wBLocal = shapeB.GetVertex(v.indexB);
v.wA = transformA.TransformPoint(wALocal);
v.wB = transformB.TransformPoint(wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
}
// 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 < Common.Settings.FLT_EPSILON)
{
// Reset the simplex.
_count = 0;
}
}
// If the cache is empty or invalid ...
if (_count == 0)
{
SimplexVertex v = vertices[0];
v.indexA = 0;
v.indexB = 0;
Vector2 wALocal = shapeA.GetVertex(0);
Vector2 wBLocal = shapeB.GetVertex(0);
v.wA = transformA.TransformPoint(wALocal);
v.wB = transformB.TransformPoint(wBLocal);
v.w = v.wB - v.wA;
_count = 1;
}
}
public override bool TestPoint(Transform xf, Vec2 p)
{
Vec2 pLocal = Math.MulT(xf.R, p - xf.Position);
for (int i = 0; i < VertexCount; ++i)
{
float dot = Vec2.Dot(Normals[i], pLocal - Vertices[i]);
if (dot > 0.0f)
{
return false;
}
}
return true;
}
