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 + 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);
}
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);
}
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;
}
}
}
internal bool SynchronizeFixtures()
{
Transform xf1 = new Transform();
xf1.rotation = Box2DNet.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);
}
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)
{
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));
}
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 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;
Box2DNetDebug.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;
}
/// <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]);
}
}
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);
}
/// <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 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);
}
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;
}
}
/// <summary> /// Draw a Transform. Choose your own length scale. /// </summary> /// <param name="xf">A Transform.</param> public abstract void DrawTransform(Transform xf);
/// <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);
/// <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);
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);
}
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);
}
}
if (index >= 0)
{
lambda = lower;
normal = xf.TransformDirection(_normals[index]);
return(SegmentCollide.HitCollide);
}
lambda = 0f;
return(SegmentCollide.StartInsideCollide);
}
internal void ComputeTransform(ref Transform xf, Vector2 center, Vector2 localCenter, float angle)
{
xf.rotation = Box2DNet.Common.Math.AngleToRotation(angle);
//xf.R = new Mat22(angle);
xf.position = center - xf.TransformDirection(localCenter);
}
private static void CollidePolyAndEdgeContact(ref Manifold manifold, Shape shape1, Transform xf1, Shape shape2, Transform xf2)
{
Collision.Collision.CollidePolyAndEdge(ref manifold, (PolygonShape)shape1, xf1, (EdgeShape)shape2, xf2);
}
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;
_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 (Box2DNet.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;
}
}
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);
}
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;
}
}
}
public void Create(BroadPhase broadPhase, Body body, Transform xf, FixtureDef def)
{
UserData = def.UserData;
Friction = def.Friction;
Restitution = def.Restitution;
Density = def.Density;
_body = body;
_next = null;
Filter = def.Filter;
_isSensor = def.IsSensor;
_type = def.Type;
// Allocate and initialize the child shape.
switch (_type)
{
case ShapeType.CircleShape:
{
CircleShape circle = new CircleShape();
CircleDef circleDef = (CircleDef)def;
circle._position = circleDef.LocalPosition;
circle._radius = circleDef.Radius;
_shape = circle;
}
break;
case ShapeType.PolygonShape:
{
PolygonShape polygon = new PolygonShape();
PolygonDef polygonDef = (PolygonDef)def;
polygon.Set(polygonDef.Vertices, polygonDef.VertexCount);
_shape = polygon;
}
break;
case ShapeType.EdgeShape:
{
EdgeShape edge = new EdgeShape();
EdgeDef edgeDef = (EdgeDef)def;
edge.Set(edgeDef.Vertex1, edgeDef.Vertex2);
_shape = edge;
}
break;
default:
Box2DNetDebug.Assert(false);
break;
}
// Create proxy in the broad-phase.
AABB aabb;
_shape.ComputeAABB(out aabb, xf);
bool inRange = broadPhase.InRange(aabb);
// You are creating a shape outside the world box.
Box2DNetDebug.Assert(inRange);
if (inRange)
{
_proxyId = broadPhase.CreateProxy(aabb, this);
}
else
{
_proxyId = PairManager.NullProxy;
}
}