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 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 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);
}
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 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;
}
// 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>
/// 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));
// Transform vertices and normals.
for (int i = 0; i < _vertexCount; ++i)
{
_vertices[i] = xf.TransformPoint(_vertices[i]);
_normals[i] = xf.TransformDirection(_normals[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);
}
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;
}
}
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);
}
/// 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>
/// Get the world coordinates of a vector given the local coordinates.
/// </summary>
/// <param name="localVector">A vector fixed in the body.</param>
/// <returns>Return the same vector expressed in world coordinates.</returns>
public Vector2 GetWorldVector(Vector2 localVector)
{
return(_xf.TransformDirection(localVector));
}
