internal void Initialize(SimplexCache cache, Shape shapeA, Transform transformA, Shape shapeB, Transform transformB)
{
ShapeA = shapeA;
ShapeB = shapeB;
int count = cache.Count;
if (count == 1)
{
FaceType = Type.Points;
Vector2 localPointA = ShapeA.GetVertex(cache.IndexA[0]);
Vector2 localPointB = ShapeB.GetVertex(cache.IndexB[0]);
Vector2 pointA = transformA.TransformPoint(localPointA);
Vector2 pointB = transformB.TransformPoint(localPointB);
Axis = pointB - pointA;
Axis.Normalize();
}
else if (cache.IndexB[0] == cache.IndexB[1])
{
// Two points on A and one on B
FaceType = Type.FaceA;
Vector2 localPointA1 = ShapeA.GetVertex(cache.IndexA[0]);
Vector2 localPointA2 = ShapeA.GetVertex(cache.IndexA[1]);
Vector2 localPointB = ShapeB.GetVertex(cache.IndexB[0]);
LocalPoint = 0.5f * (localPointA1 + localPointA2);
Axis = (localPointA2 - localPointA1).CrossScalarPostMultiply(1.0f);
Axis.Normalize();
Vector2 normal = transformA.TransformDirection(Axis);
Vector2 pointA = transformA.TransformPoint(LocalPoint);
Vector2 pointB = transformB.TransformPoint(localPointB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
Axis = -Axis;
}
}
else
{
// Two points on B and one or two points on A.
// We ignore the second point on A.
FaceType = Type.FaceB;
Vector2 localPointA = shapeA.GetVertex(cache.IndexA[0]);
Vector2 localPointB1 = shapeB.GetVertex(cache.IndexB[0]);
Vector2 localPointB2 = shapeB.GetVertex(cache.IndexB[1]);
LocalPoint = 0.5f * (localPointB1 + localPointB2);
Axis = (localPointB2 - localPointB1).CrossScalarPostMultiply(1.0f);
Axis.Normalize();
Vector2 normal = transformB.TransformDirection(Axis);
Vector2 pointB = transformB.TransformPoint(LocalPoint);
Vector2 pointA = transformA.TransformPoint(localPointA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
Axis = -Axis;
}
}
}
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)
{
Box2DNetDebug.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:
Box2DNetDebug.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));
}
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 + Box2DNet.Common.Settings.FLT_EPSILON)
{
//Completely dry
c = new Vector2();
return(0);
}
if (l > _radius)
{
//Completely wet
c = p;
return(Box2DNet.Common.Settings.Pi * _radius * _radius);
}
//Magic
float r2 = _radius * _radius;
float l2 = l * l;
float area = r2 * ((float)System.Math.Asin(l / _radius) + Box2DNet.Common.Settings.Pi / 2) +
l * Box2DNet.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);
}
internal void ReadCache(SimplexCache cache, Shape shapeA, Transform transformA, Shape shapeB, Transform transformB)
{
Box2DNetDebug.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 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 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>
/// 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 = Box2DNet.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]);
}
}
/// <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;
//This fixed it, not sure why it was broken
if (normals1.Length - 1 <= edge1 || edge1 < 0)
{
return(0);
}
// 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);
}
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 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);
}
/// 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).LengthSquared() > (Box2DNet.Common.Math.Epsilon * Box2DNet.Common.Math.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 static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygon, Transform xf1, CircleShape circle, Transform xf2)
{
manifold.PointCount = 0;
// Compute circle position in the frame of the polygon.
Vector2 c = xf2.TransformPoint(circle._position);
Vector2 cLocal = xf1.InverseTransformPoint(c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -Settings.FLT_MAX;
float radius = polygon._radius + circle._radius;
int vertexCount = polygon._vertexCount;
Vector2[] vertices = polygon._vertices;
Vector2[] normals = polygon._normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = Vector2.Dot(normals[i], cLocal - vertices[i]);
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 = vertices[vertIndex1];
Vector2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Common.Settings.FLT_EPSILON)
{
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = normals[normalIndex];
manifold.LocalPoint = 0.5f * (v1 + v2);
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
return;
}
// Compute barycentric coordinates
float u1 = Vector2.Dot(cLocal - v1, v2 - v1);
float u2 = Vector2.Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if ((cLocal - v1).LengthSquared() > 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)
{
if ((cLocal - v2).LengthSquared() > 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
{
Vector2 faceCenter = 0.5f * (v1 + v2);
float separation_ = Vector2.Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation_ > radius)
{
return;
}
manifold.PointCount = 1;
manifold.Type = ManifoldType.FaceA;
manifold.LocalPlaneNormal = normals[vertIndex1];
manifold.LocalPoint = faceCenter;
manifold.Points[0].LocalPoint = circle._position;
manifold.Points[0].ID.Key = 0;
}
}
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:
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.LengthSquared();
// 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(ref output.PointA, ref output.PointB);
output.Distance = Box2DNet.Common.Math.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;
}
}
}
internal Body(BodyDef bd, World world)
{
Box2DNetDebug.Assert(world._lock == false);
_flags = 0;
if (bd.IsBullet)
{
_flags |= BodyFlags.Bullet;
}
if (bd.FixedRotation)
{
_flags |= BodyFlags.FixedRotation;
}
if (bd.AllowSleep)
{
_flags |= BodyFlags.AllowSleep;
}
if (bd.IsSleeping)
{
_flags |= BodyFlags.Sleep;
}
_world = world;
_xf.position = bd.Position;
_xf.rotation = Box2DNet.Common.Math.AngleToRotation(bd.Angle);
//_xf.R = new Mat22(bd.Angle);
_sweep.LocalCenter = bd.MassData.Center;
_sweep.T0 = 1.0f;
_sweep.A0 = _sweep.A = bd.Angle;
_sweep.C0 = _sweep.C = _xf.TransformPoint(_sweep.LocalCenter);
//_jointList = null;
//_contactList = null;
//_controllerList = null;
//_prev = null;
//_next = null;
_linearVelocity = bd.LinearVelocity;
_angularVelocity = bd.AngularVelocity;
_linearDamping = bd.LinearDamping;
_angularDamping = bd.AngularDamping;
//_force.Set(0.0f, 0.0f);
//_torque = 0.0f;
//_linearVelocity.SetZero();
//_angularVelocity = 0.0f;
//_sleepTime = 0.0f;
//_invMass = 0.0f;
//_I = 0.0f;
//_invI = 0.0f;
_mass = bd.MassData.Mass;
if (_mass > 0.0f)
{
_invMass = 1.0f / _mass;
}
_I = bd.MassData.I;
if (_I > 0.0f && (_flags & BodyFlags.FixedRotation) == 0)
{
_invI = 1.0f / _I;
}
if (_invMass == 0.0f && _invI == 0.0f)
{
_type = BodyType.Static;
}
else
{
_type = BodyType.Dynamic;
}
_userData = bd.UserData;
//_fixtureList = null;
//_fixtureCount = 0;
bodyID = bodyCount++;
}
