public override void Step(Settings settings)
{
base.Step(settings);
// Traverse the contact results. Apply a force on shapes
// that overlap the sensor.
for (int i = 0; i < e_count; ++i)
{
if (m_touching[i] == false)
{
continue;
}
b2Body body = m_bodies[i];
b2Body ground = m_sensor.Body;
b2CircleShape circle = (b2CircleShape)m_sensor.Shape;
b2Vec2 center = ground.GetWorldPoint(circle.Position);
b2Vec2 position = body.Position;
b2Vec2 d = center - position;
if (d.LengthSquared < float.Epsilon * float.Epsilon)
{
continue;
}
d.Normalize();
b2Vec2 F = 100.0f * d;
body.ApplyForce(F, position);
}
}
public override bool SolvePositionConstraints(b2SolverData data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 u = cB + rB - cA - rA;
float length = u.Normalize();
float C = length - m_maxLength;
C = b2Math.b2Clamp(C, 0.0f, b2Settings.b2_maxLinearCorrection);
float impulse = -m_mass * C;
b2Vec2 P = impulse * u;
cA -= m_invMassA * P;
aA -= m_invIA * b2Math.b2Cross(rA, P);
cB += m_invMassB * P;
aB += m_invIB * b2Math.b2Cross(rB, P);
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return(length - m_maxLength < b2Settings.b2_linearSlop);
}
private void SolveC2()
{
int count2 = m_count - 1;
for (int i = 0; i < count2; ++i)
{
b2Vec2 p1 = m_ps[i];
b2Vec2 p2 = m_ps[i + 1];
b2Vec2 d = p2 - p1;
float L = d.Normalize();
float im1 = m_ims[i];
float im2 = m_ims[i + 1];
if (im1 + im2 == 0.0f)
{
continue;
}
float s1 = im1 / (im1 + im2);
float s2 = im2 / (im1 + im2);
p1 -= m_k2 * s1 * (m_Ls[i] - L) * d;
p2 += m_k2 * s2 * (m_Ls[i] - L) * d;
m_ps[i] = p1;
m_ps[i + 1] = p2;
}
}
public b2PrismaticJoint(b2PrismaticJointDef def)
: base(def)
{
m_localAnchorA = def.localAnchorA;
m_localAnchorB = def.localAnchorB;
m_localXAxisA = def.localAxisA;
m_localXAxisA.Normalize();
m_localYAxisA = m_localXAxisA.NegUnitCross(); // b2Math.b2Cross(1.0f, m_localXAxisA);
m_referenceAngle = def.referenceAngle;
m_impulse.SetZero();
m_motorMass = 0.0f;
m_motorImpulse = 0.0f;
m_lowerTranslation = def.lowerTranslation;
m_upperTranslation = def.upperTranslation;
m_maxMotorForce = def.maxMotorForce;
m_motorSpeed = def.motorSpeed;
m_enableLimit = def.enableLimit;
m_enableMotor = def.enableMotor;
m_limitState = b2LimitState.e_inactiveLimit;
m_axis.SetZero();
m_perp.SetZero();
}
public override void SolveVelocityConstraints(b2SolverData data)
{
b2Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
b2Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
float h = data.step.dt;
// Solve angular friction
{
float Cdot = wB - wA;
float impulse = -m_angularMass * Cdot;
float oldImpulse = m_angularImpulse;
float maxImpulse = h * m_maxTorque;
m_angularImpulse = b2Math.b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
b2Vec2 Cdot = vB + b2Math.b2Cross(wB, m_rB) - vA - b2Math.b2Cross(wA, m_rA);
b2Vec2 impulse = -b2Math.b2Mul(m_linearMass, Cdot);
b2Vec2 oldImpulse = m_linearImpulse;
m_linearImpulse += impulse;
float maxImpulse = h * m_maxForce;
if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
m_linearImpulse.Normalize();
m_linearImpulse *= maxImpulse;
}
impulse = m_linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * b2Math.b2Cross(m_rA, impulse);
vB += mB * impulse;
wB += iB * b2Math.b2Cross(m_rB, impulse);
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
internal override bool SolvePositionConstraints(b2SolverData data)
{
if (m_frequencyHz > 0.0f)
{
// There is no position correction for soft distance constraints.
return(true);
}
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
b2Vec2 rA = Utils.b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = Utils.b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 u = cB + rB - cA - rA;
float length = u.Normalize();
float C = length - m_length;
C = Utils.b2Clamp(C, -Settings.b2_maxLinearCorrection, Settings.b2_maxLinearCorrection);
float impulse = -m_mass * C;
b2Vec2 P = impulse * u;
cA -= m_invMassA * P;
aA -= m_invIA * Utils.b2Cross(rA, P);
cB += m_invMassB * P;
aB += m_invIB * Utils.b2Cross(rB, P);
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return(Utils.b2Abs(C) < Settings.b2_linearSlop);
}
public Prismatic()
{
b2Body ground = null;
{
b2BodyDef bd = new b2BodyDef();
ground = m_world.CreateBody(bd);
b2EdgeShape shape = new b2EdgeShape();
shape.Set(new b2Vec2(-40.0f, 0.0f), new b2Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
{
b2PolygonShape shape = new b2PolygonShape();
shape.SetAsBox(2.0f, 0.5f);
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position.Set(-10.0f, 10.0f);
bd.angle = 0.5f * b2Settings.b2_pi;
bd.allowSleep = false;
b2Body body = m_world.CreateBody(bd);
body.CreateFixture(shape, 5.0f);
b2PrismaticJointDef pjd = new b2PrismaticJointDef();
// Bouncy limit
b2Vec2 axis = new b2Vec2(2.0f, 1.0f);
axis.Normalize();
pjd.Initialize(ground, body, new b2Vec2(0.0f, 0.0f), axis);
// Non-bouncy limit
//pjd.Initialize(ground, body, b2Vec2(-10.0f, 10.0f), b2Vec2(1.0f, 0.0f));
pjd.motorSpeed = 10.0f;
pjd.maxMotorForce = 10000.0f;
pjd.enableMotor = true;
pjd.lowerTranslation = 0.0f;
pjd.upperTranslation = 20.0f;
pjd.enableLimit = true;
m_joint = (b2PrismaticJoint)m_world.CreateJoint(pjd);
}
}
public override bool SolvePositionConstraints(b2SolverData data)
{
if (m_frequencyHz > 0.0f)
{
// There is no position correction for soft distance constraints.
return(true);
}
b2Vec2 cA = m_bodyA.InternalPosition.c;
float aA = m_bodyA.InternalPosition.a;
b2Vec2 cB = m_bodyB.InternalPosition.c;
float aB = m_bodyB.InternalPosition.a;
b2Rot qA = new b2Rot(aA);
b2Rot qB = new b2Rot(aB);
b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 u = cB + rB - cA - rA;
float length = u.Normalize();
float C = length - m_length;
C = b2Math.b2Clamp(C, -b2Settings.b2_maxLinearCorrection, b2Settings.b2_maxLinearCorrection);
float impulse = -m_mass * C;
b2Vec2 P = impulse * u;
cA -= m_invMassA * P;
aA -= m_invIA * b2Math.b2Cross(ref rA, ref P);
cB += m_invMassB * P;
aB += m_invIB * b2Math.b2Cross(ref rB, ref P);
m_bodyA.InternalPosition.c = cA;
m_bodyA.InternalPosition.a = aA;
m_bodyB.InternalPosition.c = cB;
m_bodyB.InternalPosition.a = aB;
return(b2Math.b2Abs(C) < b2Settings.b2_linearSlop);
}
/// Implement b2Shape.
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
public override bool RayCast(b2RayCastOutput output, b2RayCastInput input, b2Transform xf, int childIndex)
{
// Put the ray into the edge's frame of reference.
b2Vec2 p1 = Utils.b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = Utils.b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
b2Vec2 v1 = new b2Vec2(m_vertex1);
b2Vec2 v2 = new b2Vec2(m_vertex2);
b2Vec2 e = v2 - v1;
b2Vec2 normal = new b2Vec2(e.y, -e.x);
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float numerator = Utils.b2Dot(normal, v1 - p1);
float denominator = Utils.b2Dot(normal, d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || input.maxFraction < t)
{
return(false);
}
b2Vec2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
b2Vec2 r = v2 - v1;
float rr = Utils.b2Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
float s = Utils.b2Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return(false);
}
output.fraction = t;
if (numerator > 0.0f)
{
output.normal = -Utils.b2Mul(xf.q, normal);
}
else
{
output.normal = Utils.b2Mul(xf.q, normal);
}
return(true);
}
public static void Distance(b2DistanceOutput output, b2SimplexCache cache, b2DistanceInput input)
{
++b2_gjkCalls;
b2DistanceProxy proxyA = input.proxyA;
b2DistanceProxy proxyB = input.proxyB;
b2Transform transformA = input.transformA;
b2Transform transformB = input.transformB;
// Initialize the simplex
b2Simplex simplex = s_simplex;
simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an vector.
b2SimplexVertex[] vertices = simplex.m_vertices;
const int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and preven cycling
int[] saveA = s_saveA;
int[] saveB = s_saveB;
int saveCount = 0;
b2Vec2 closestPoint = simplex.GetClosestPoint();
float distanceSqr1 = closestPoint.LengthSquared();
float distanceSqr2 = distanceSqr1;
int i;
b2Vec2 p;
// Main iteration loop
int iter = 0;
while (iter < k_maxIters)
{
// Copy the simplex so that we can identify duplicates
saveCount = simplex.m_count;
for (i = 0; i < saveCount; i++)
{
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
}
/*switch(simplex.m_count)
* {
* case 1:
* break;
* case 2:
* simplex.Solve2();
* break;
* case 3:
* simplex.Solve3();
* break;
* default:
* b2Settings.b2Assert(false);
* }*/
if (simplex.m_count == 1)
{
}
else if (simplex.m_count == 2)
{
simplex.Solve2();
}
else if (simplex.m_count == 3)
{
simplex.Solve3();
}
else
{
b2Settings.b2Assert(false);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.m_count == 3)
{
break;
}
// Compute the closest point.
p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 > distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
b2Vec2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < 0.0f /*Number.MinValue * Number.MinValue*/)
{
// 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 very 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
b2SimplexVertex vertex = vertices[simplex.m_count];
vertex.indexA = (int)proxyA.GetSupport(b2Math.MulTMV(transformA.R, d.GetNegative()));
vertex.wA = b2Math.MulX(transformA, proxyA.GetVertex(vertex.indexA));
vertex.indexB = (int)proxyB.GetSupport(b2Math.MulTMV(transformB.R, d));
vertex.wB = b2Math.MulX(transformB, proxyB.GetVertex(vertex.indexB));
vertex.w = b2Math.SubtractVV(vertex.wB, vertex.wA);
// Iteration count is equated to the number of support point calls.
++iter;
++b2_gjkIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (i = 0; i < saveCount; i++)
{
if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exist to avoid cycling
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.m_count;
}
b2_gjkMaxIters = (int)b2Math.Max(b2_gjkMaxIters, iter);
// Prepare output
simplex.GetWitnessPoints(output.pointA, output.pointB);
output.distance = b2Math.SubtractVV(output.pointA, output.pointB).Length();
output.iterations = iter;
// Cache the simplex
simplex.WriteCache(cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = proxyA.m_radius;
float rB = proxyB.m_radius;
if (output.distance > rA + rB && output.distance > float.MinValue)
{
// Shapes are still not overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
b2Vec2 normal = b2Math.SubtractVV(output.pointB, output.pointA);
normal.Normalize();
output.pointA.x += rA * normal.x;
output.pointA.y += rA * normal.y;
output.pointB.x -= rB * normal.x;
output.pointB.y -= rB * normal.y;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
p = new b2Vec2();
p.x = 0.5f * (output.pointA.x + output.pointB.x);
p.y = 0.5f * (output.pointA.y + output.pointB.y);
output.pointA.x = output.pointB.x = p.x;
output.pointA.y = output.pointB.y = p.y;
output.distance = 0.0f;
}
}
}
/// Compute the collision manifold between an edge and a circle.
public static void b2CollideEdgeAndCircle(ref b2Manifold manifold,
b2EdgeShape edgeA, ref b2Transform xfA,
b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
// Compute circle in frame of edge
b2Vec2 Q = b2Math.b2MulT(xfA, b2Math.b2Mul(xfB, circleB.Position));
b2Vec2 A = edgeA.Vertex1, B = edgeA.Vertex2;
b2Vec2 e = B - A;
b2Vec2 diff;
// Barycentric coordinates
diff = B - Q;
float u = b2Math.b2Dot(ref e, ref diff); // B - Q);
diff = Q - A;
float v = b2Math.b2Dot(ref e, ref diff); // Q - A);
float radius = edgeA.Radius + circleB.Radius;
b2ContactFeature cf = b2ContactFeature.Zero;
cf.indexB = 0;
cf.typeB = b2ContactFeatureType.e_vertex;
// Region A
if (v <= 0.0f)
{
b2Vec2 P = A;
b2Vec2 d = Q - P;
float dd = d.LengthSquared; // b2Math.b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.HasVertex0)
{
b2Vec2 A1 = edgeA.Vertex0;
b2Vec2 B1 = A;
b2Vec2 e1 = B1 - A1;
diff = B1 - Q;
float u1 = b2Math.b2Dot(ref e1, ref diff);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = b2ContactFeatureType.e_vertex;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
return;
}
// Region B
if (u <= 0.0f)
{
b2Vec2 P = B;
b2Vec2 d = Q - P;
float dd = d.LengthSquared; // b2Math.b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.HasVertex3)
{
b2Vec2 B2 = edgeA.Vertex3;
b2Vec2 A2 = B;
b2Vec2 e2 = B2 - A2;
diff = Q - A2;
float v2 = b2Math.b2Dot(ref e2, ref diff);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = b2ContactFeatureType.e_vertex;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
return;
}
// Region AB
float den = e.Length; // b2Math.b2Dot(e, e);
System.Diagnostics.Debug.Assert(den > 0.0f);
b2Vec2 xP = (1.0f / den) * (u * A + v * B);
b2Vec2 xd = Q - xP;
float xdd = xd.LengthSquared; // b2Math.b2Dot(xd, xd);
if (xdd > radius * radius)
{
return;
}
b2Vec2 n = b2Vec2.Zero; // new b2Vec2(-e.y, e.x);
n.m_x = -e.y;
n.m_y = e.x;
diff = Q - A;
if (b2Math.b2Dot(ref n, ref diff) < 0.0f)
{
// n.Set(-n.x, -n.y);
n.Set(-n.m_x, -n.m_y);
}
n.Normalize();
cf.indexA = 0;
cf.typeA = b2ContactFeatureType.e_face;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = n;
manifold.localPoint = A;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
}
/// Compute the collision manifold between two polygons.
// 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 b2CollidePolygons(ref b2Manifold manifold,
b2PolygonShape polyA, ref b2Transform xfA,
b2PolygonShape polyB, ref b2Transform xfB)
{
manifold.pointCount = 0;
float totalRadius = polyA.Radius + polyB.Radius;
int edgeA = 0;
float separationA = b2FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
return;
int edgeB = 0;
float separationB = b2FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
return;
b2PolygonShape poly1; // reference polygon
b2PolygonShape poly2; // incident polygon
b2Transform 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 = b2ManifoldType.e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.type = b2ManifoldType.e_faceA;
flip = 0;
}
b2ClipVertex[] incidentEdge = new b2ClipVertex[2];
b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
b2Vec2[] vertices1 = poly1.Vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 localTangent = v12 - v11;
localTangent.Normalize();
b2Vec2 localNormal = localTangent.UnitCross(); // b2Math.b2Cross(localTangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
b2Vec2 tangent = b2Math.b2Mul(xf1.q, localTangent);
b2Vec2 normal = tangent.UnitCross(); // b2Math.b2Cross(tangent, 1.0f);
v11 = b2Math.b2Mul(xf1, v11);
v12 = b2Math.b2Mul(xf1, v12);
// Face offset.
float frontOffset = b2Math.b2Dot(ref normal, ref v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -b2Math.b2Dot(ref tangent, ref v11) + totalRadius;
float sideOffset2 = b2Math.b2Dot(ref tangent, ref v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = new b2ClipVertex[2];
b2ClipVertex[] clipPoints2 = new b2ClipVertex[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, (byte)iv1);
if (np < 2)
return;
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, (byte)iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal = localNormal;
manifold.localPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
{
float separation = b2Math.b2Dot(ref normal, ref clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
cp.localPoint = b2Math.b2MulT(xf2, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
if (flip != 0)
{
// Swap features
b2ContactFeature cf = cp.id;
cp.id.indexA = cf.indexB;
cp.id.indexB = cf.indexA;
cp.id.typeA = cf.typeB;
cp.id.typeB = cf.typeA;
}
manifold.points[pointCount] = cp;
++pointCount;
}
}
manifold.pointCount = pointCount;
}
// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
public void Collide(b2Manifold manifold, b2EdgeShape edgeA, b2Transform xfA, b2PolygonShape polygonB, b2Transform xfB)
{
m_xf = Utils.b2MulT(xfA, xfB);
m_centroidB = Utils.b2Mul(m_xf, polygonB.m_centroid);
m_v0 = edgeA.m_vertex0;
m_v1 = edgeA.m_vertex1;
m_v2 = edgeA.m_vertex2;
m_v3 = edgeA.m_vertex3;
bool hasVertex0 = edgeA.m_hasVertex0;
bool hasVertex3 = edgeA.m_hasVertex3;
b2Vec2 edge1 = m_v2 - m_v1;
edge1.Normalize();
m_normal1.Set(edge1.y, -edge1.x);
float offset1 = Utils.b2Dot(m_normal1, m_centroidB - m_v1);
float offset0 = 0.0f;
float offset2 = 0.0f;
bool convex1 = false;
bool convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
b2Vec2 edge0 = m_v1 - m_v0;
edge0.Normalize();
m_normal0.Set(edge0.y, -edge0.x);
convex1 = Utils.b2Cross(edge0, edge1) >= 0.0f;
offset0 = Utils.b2Dot(m_normal0, m_centroidB - m_v0);
}
// Is there a following edge?
if (hasVertex3)
{
b2Vec2 edge2 = m_v3 - m_v2;
edge2.Normalize();
m_normal2.Set(edge2.y, -edge2.x);
convex2 = Utils.b2Cross(edge1, edge2) > 0.0f;
offset2 = Utils.b2Dot(m_normal2, m_centroidB - m_v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
}
else if (convex1)
{
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal1;
}
}
else if (convex2)
{
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal0;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
}
else
{
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = m_normal1;
}
}
}
else
{
m_front = offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
}
// Get polygonB in frameA
m_polygonB.count = polygonB.m_count;
for (int i = 0; i < polygonB.m_count; ++i)
{
m_polygonB.vertices[i] = Utils.b2Mul(m_xf, polygonB.m_vertices[i]);
m_polygonB.normals[i] = Utils.b2Mul(m_xf.q, polygonB.m_normals[i]);
}
m_radius = polygonB.m_radius + edgeA.m_radius;
manifold.pointCount = 0;
b2EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.type == b2EPAxis.Type.e_unknown)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
b2EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.type != b2EPAxis.Type.e_unknown && polygonAxis.separation > m_radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
b2EPAxis primaryAxis = new b2EPAxis();
if (polygonAxis.type == b2EPAxis.Type.e_unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
b2ClipVertex[] ie = Arrays.InitializeWithDefaultInstances <b2ClipVertex>(2);
b2ReferenceFace rf = new b2ReferenceFace();
if (primaryAxis.type == b2EPAxis.Type.e_edgeA)
{
manifold.type = b2Manifold.Type.e_faceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Utils.b2Dot(m_normal, m_polygonB.normals[0]);
for (int i = 1; i < m_polygonB.count; ++i)
{
float value = Utils.b2Dot(m_normal, m_polygonB.normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie[0].v = m_polygonB.vertices[i1];
ie[0].id.cf.indexA = 0;
ie[0].id.cf.indexB = (byte)i1;
ie[0].id.cf.typeA = (int)b2ContactFeature.Type.e_face;
ie[0].id.cf.typeB = (int)b2ContactFeature.Type.e_vertex;
ie[1].v = m_polygonB.vertices[i2];
ie[1].id.cf.indexA = 0;
ie[1].id.cf.indexB = (byte)i2;
ie[1].id.cf.typeA = (int)b2ContactFeature.Type.e_face;
ie[1].id.cf.typeB = (int)b2ContactFeature.Type.e_vertex;
if (m_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = m_v1;
rf.v2 = m_v2;
rf.normal = m_normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = m_v2;
rf.v2 = m_v1;
rf.normal = -m_normal1;
}
}
else
{
manifold.type = b2Manifold.Type.e_faceB;
ie[0].v = m_v1;
ie[0].id.cf.indexA = 0;
ie[0].id.cf.indexB = (byte)primaryAxis.index;
ie[0].id.cf.typeA = (int)b2ContactFeature.Type.e_vertex;
ie[0].id.cf.typeB = (int)b2ContactFeature.Type.e_face;
ie[1].v = m_v2;
ie[1].id.cf.indexA = 0;
ie[1].id.cf.indexB = (byte)primaryAxis.index;
ie[1].id.cf.typeA = (int)b2ContactFeature.Type.e_vertex;
ie[1].id.cf.typeB = (int)b2ContactFeature.Type.e_face;
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1 = m_polygonB.vertices[rf.i1];
rf.v2 = m_polygonB.vertices[rf.i2];
rf.normal = m_polygonB.normals[rf.i1];
}
rf.sideNormal1.Set(rf.normal.y, -rf.normal.x);
rf.sideNormal2 = -rf.sideNormal1;
rf.sideOffset1 = Utils.b2Dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = Utils.b2Dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = Arrays.InitializeWithDefaultInstances <b2ClipVertex>(2);
b2ClipVertex[] clipPoints2 = Arrays.InitializeWithDefaultInstances <b2ClipVertex>(2);
int np;
// Clip to box side 1
np = Utils.b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
if (np < Settings.b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = Utils.b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
if (np < Settings.b2_maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == b2EPAxis.Type.e_edgeA)
{
manifold.localNormal = rf.normal;
manifold.localPoint = rf.v1;
}
else
{
manifold.localNormal = polygonB.m_normals[rf.i1];
manifold.localPoint = polygonB.m_vertices[rf.i1];
}
int pointCount = 0;
for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i)
{
float separation;
separation = Utils.b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
if (separation <= m_radius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
if (primaryAxis.type == b2EPAxis.Type.e_edgeA)
{
cp.localPoint = Utils.b2MulT(m_xf, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
}
else
{
cp.localPoint = clipPoints2[i].v;
cp.id.cf.typeA = clipPoints2[i].id.cf.typeB;
cp.id.cf.typeB = clipPoints2[i].id.cf.typeA;
cp.id.cf.indexA = clipPoints2[i].id.cf.indexB;
cp.id.cf.indexB = clipPoints2[i].id.cf.indexA;
}
++pointCount;
}
}
manifold.pointCount = pointCount;
}
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
public override bool RayCast(out b2RayCastOutput output, b2RayCastInput input,
b2Transform xf, int childIndex)
{
output = b2RayCastOutput.Zero;
// Put the ray into the edge's frame of reference.
b2Vec2 p1 = b2Math.b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = b2Math.b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
b2Vec2 v1 = m_vertex1;
b2Vec2 v2 = m_vertex2;
b2Vec2 e = v2 - v1;
b2Vec2 normal = b2Vec2.Zero; // new b2Vec2(e.y, -e.x);
normal.m_x = e.m_y;
normal.m_y = -e.m_x;
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
b2Vec2 diff = v1 - p1;
float numerator = b2Math.b2Dot(ref normal, ref diff);
float denominator = b2Math.b2Dot(ref normal, ref d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || input.maxFraction < t)
{
return(false);
}
b2Vec2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
b2Vec2 r = v2 - v1;
float rr = r.LengthSquared; // b2Math.b2Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
diff = q - v1;
float s = b2Math.b2Dot(ref diff, ref r) / rr;
if (s < 0.0f || 1.0f < s)
{
return(false);
}
output.fraction = t;
if (numerator > 0.0f)
{
output.normal = -normal;
}
else
{
output.normal = normal;
}
return(true);
}
// The normal points from 1 to 2
static public void CollidePolygons(b2Manifold manifold,
b2PolygonShape polyA, b2Transform xfA,
b2PolygonShape polyB, b2Transform xfB)
{
ClipVertex cv;
manifold.m_pointCount = 0;
float totalRadius = polyA.m_radius + polyB.m_radius;
int edgeA = 0;
s_edgeAO[0] = edgeA;
float separationA = FindMaxSeparation(s_edgeAO, polyA, xfA, polyB, xfB);
edgeA = s_edgeAO[0];
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
s_edgeBO[0] = edgeB;
float separationB = FindMaxSeparation(s_edgeBO, polyB, xfB, polyA, xfA);
edgeB = s_edgeBO[0];
if (separationB > totalRadius)
{
return;
}
b2PolygonShape poly1; // reference poly
b2PolygonShape poly2; // incident poly
b2Transform xf1;
b2Transform xf2;
int edge1; // reference edge
uint flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
b2Mat22 tMat;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.m_type = b2Manifold.e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.m_type = b2Manifold.e_faceA;
flip = 0;
}
ClipVertex[] incidentEdge = s_incidentEdge;
FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.m_vertexCount;
List <b2Vec2> vertices1 = poly1.m_vertices;
b2Vec2 local_v11 = vertices1[edge1];
b2Vec2 local_v12;
if (edge1 + 1 < count1)
{
local_v12 = vertices1[(int)(edge1 + 1)];
}
else
{
local_v12 = vertices1[0];
}
b2Vec2 localTangent = s_localTangent;
localTangent.Set(local_v12.x - local_v11.x, local_v12.y - local_v11.y);
localTangent.Normalize();
b2Vec2 localNormal = s_localNormal;
localNormal.x = localTangent.y;
localNormal.y = -localTangent.x;
b2Vec2 planePoint = s_planePoint;
planePoint.Set(0.5f * (local_v11.x + local_v12.x), 0.5f * (local_v11.y + local_v12.y));
b2Vec2 tangent = s_tangent;
//tangent = b2Math.b2MulMV(xf1.R, localTangent);
tMat = xf1.R;
tangent.x = (tMat.col1.x * localTangent.x + tMat.col2.x * localTangent.y);
tangent.y = (tMat.col1.y * localTangent.x + tMat.col2.y * localTangent.y);
b2Vec2 tangent2 = s_tangent2;
tangent2.x = -tangent.x;
tangent2.y = -tangent.y;
b2Vec2 normal = s_normal;
normal.x = tangent.y;
normal.y = -tangent.x;
//v11 = b2Math.MulX(xf1, local_v11);
//v12 = b2Math.MulX(xf1, local_v12);
b2Vec2 v11 = s_v11;
b2Vec2 v12 = s_v12;
v11.x = xf1.position.x + (tMat.col1.x * local_v11.x + tMat.col2.x * local_v11.y);
v11.y = xf1.position.y + (tMat.col1.y * local_v11.x + tMat.col2.y * local_v11.y);
v12.x = xf1.position.x + (tMat.col1.x * local_v12.x + tMat.col2.x * local_v12.y);
v12.y = xf1.position.y + (tMat.col1.y * local_v12.x + tMat.col2.y * local_v12.y);
// Face offset
float frontOffset = normal.x * v11.x + normal.y * v11.y;
// Side offsets, extended by polytope skin thickness
float sideOffset1 = -tangent.x * v11.x - tangent.y * v11.y + totalRadius;
float sideOffset2 = tangent.x * v12.x + tangent.y * v12.y + totalRadius;
// Clip incident edge against extruded edge1 side edges.
ClipVertex[] clipPoints1 = s_clipPoints1;
ClipVertex[] clipPoints2 = s_clipPoints2;
int np;
// Clip to box side 1
//np = ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1);
np = ClipSegmentToLine(clipPoints1, incidentEdge, tangent2, sideOffset1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.m_localPlaneNormal.SetV(localNormal);
manifold.m_localPoint.SetV(planePoint);
int pointCount = 0;
for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
{
cv = clipPoints2[i];
float separation = normal.x * cv.v.x + normal.y * cv.v.y - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint cp = manifold.m_points[pointCount];
//cp.m_localPoint = b2Math.b2MulXT(xf2, cv.v);
tMat = xf2.R;
float tX = cv.v.x - xf2.position.x;
float tY = cv.v.y - xf2.position.y;
cp.m_localPoint.x = (tX * tMat.col1.x + tY * tMat.col1.y);
cp.m_localPoint.y = (tX * tMat.col2.x + tY * tMat.col2.y);
cp.m_id.Set(cv.id);
cp.m_id.features.flip = (int)flip;
++pointCount;
}
}
manifold.m_pointCount = pointCount;
}
public static void b2Distance(ref b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input)
{
++b2DistanceProxy.b2_gjkCalls;
b2DistanceProxy proxyA = input.proxyA.Copy();
b2DistanceProxy proxyB = input.proxyB.Copy();
b2Transform transformA = input.transformA;
b2Transform transformB = input.transformB;
// Initialize the simplex.
b2Simplex simplex = new b2Simplex();
simplex.ReadCache(ref cache, ref proxyA, ref transformA, ref proxyB, ref transformB);
// Get simplex vertices as an array.
b2SimplexVertex[] vertices = new b2SimplexVertex[] { simplex.m_vertices[0], simplex.m_vertices[1], simplex.m_vertices[2] };
int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int[] saveA = new int[3];
int[] saveB = new int[3];
int saveCount = 0;
b2Vec2 closestPoint = simplex.GetClosestPoint();
float distanceSqr1 = closestPoint.LengthSquared();
float distanceSqr2 = distanceSqr1;
// Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1);
// Main iteration loop.
#region Main Iteration Loop
int iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int i = 0; i < saveCount; ++i)
{
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
}
switch (simplex.m_count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
Debug.Assert(false);
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.m_count == 3)
{
break;
}
// Compute closest point.
b2Vec2 p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
b2Vec2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < b2Settings.b2_epsilon * b2Settings.b2_epsilon)
{
// 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.
b2SimplexVertex vertex = vertices[simplex.m_count];
vertex.indexA = proxyA.GetSupport(b2Math.b2MulT(transformA.q, -d));
vertex.wA = b2Math.b2Mul(transformA, proxyA.GetVertex(vertex.indexA));
// b2Vec2 wBLocal = new b2Vec2();
vertex.indexB = proxyB.GetSupport(b2Math.b2MulT(transformB.q, d));
vertex.wB = b2Math.b2Mul(transformB, proxyB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
vertices[simplex.m_count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
++b2DistanceProxy.b2_gjkIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int i = 0; i < saveCount; ++i)
{
if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i])
{
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.m_count;
}
#endregion
b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(ref output.pointA, ref output.pointB);
output.distance = b2Math.b2Distance(output.pointA, output.pointB);
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = proxyA.Radius;
float rB = proxyB.Radius;
if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon)
{
// Shapes are still not overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
b2Vec2 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.
b2Vec2 p = 0.5f * (output.pointA + output.pointB);
output.pointA = p;
output.pointB = p;
output.distance = 0.0f;
}
}
// Copy back the vertex changes because they are structs, but in C++ land they are reference values
simplex.m_vertices[0] = vertices[0];
simplex.m_vertices[1] = vertices[1];
simplex.m_vertices[2] = vertices[2];
}
public void RayCast(b2WorldRayCastWrapper callback, b2RayCastInput input)
{
b2Vec2 p1 = input.p1;
b2Vec2 p2 = input.p2;
b2Vec2 r = p2 - p1;
Debug.Assert(r.LengthSquared > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
b2Vec2 v = r.NegUnitCross(); // b2Math.b2Cross(1.0f, r);
b2Vec2 abs_v = b2Math.b2Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float maxFraction = input.maxFraction;
// Build a bounding box for the segment.
b2AABB segmentAABB = b2AABB.Default;
{
b2Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.Set(b2Math.b2Min(p1, t), b2Math.b2Max(p1, t));
}
Stack <int> stack = new Stack <int>();
stack.Push(m_root);
while (stack.Count > 0)
{
int nodeId = stack.Pop();
if (nodeId == b2TreeNode.b2_nullNode)
{
continue;
}
b2TreeNode node = m_nodes[nodeId];
if (b2Collision.b2TestOverlap(ref node.aabb, ref segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
b2Vec2 c = node.aabb.Center;
b2Vec2 h = node.aabb.Extents;
float separation = b2Math.b2Abs(b2Math.b2Dot(v, p1 - c)) - b2Math.b2Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
b2RayCastInput subInput = new b2RayCastInput();
subInput.p1 = input.p1;
subInput.p2 = input.p2;
subInput.maxFraction = maxFraction;
float value = callback.RayCastCallback(subInput, nodeId);
if (value == 0.0f)
{
// The client has terminated the ray cast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
b2Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.Set(b2Math.b2Min(p1, t), b2Math.b2Max(p1, t));
}
}
else
{
stack.Push(node.child1);
stack.Push(node.child2);
}
}
}
/**
* Ray-cast against the proxies in the tree. This relies on the callback
* to perform a exact ray-cast in the case were the proxy contains a shape.
* The callback also performs the any collision filtering. This has performance
* roughly equal to k * log(n), where k is the number of collisions and n is the
* number of proxies in the tree.
* @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
* @param callback a callback class that is called for each proxy that is hit by the ray.
* It should be of signature:
* ----- <code>function callback(input:b2RayCastInput, proxy:*):void</code>
* <code>function callback(input:b2RayCastInput, proxy:*):Number</code>
*/
public void RayCast(BroadPhaseRayCastCallback callback, b2RayCastInput input)
{
if (m_root == null)
{
return;
}
b2Vec2 p1 = input.p1;
b2Vec2 p2 = input.p2;
b2Vec2 r = b2Math.SubtractVV(p1, p2);
//b2Settings.b2Assert(r.LengthSquared() > 0.0);
r.Normalize();
// v is perpendicular to the segment
b2Vec2 v = b2Math.CrossFV(1.0f, r);
b2Vec2 abs_v = b2Math.AbsV(v);
float maxFraction = input.maxFraction;
// Build a bounding box for the segment
b2AABB segmentAABB = new b2AABB();
float tX;
float tY;
{
tX = p1.x + maxFraction * (p2.x - p1.x);
tY = p1.y + maxFraction * (p2.y - p1.y);
segmentAABB.lowerBound.x = Mathf.Min(p1.x, tX);
segmentAABB.lowerBound.y = Mathf.Min(p1.y, tY);
segmentAABB.upperBound.x = Mathf.Max(p1.x, tX);
segmentAABB.upperBound.y = Mathf.Max(p1.y, tY);
}
Dictionary <int, b2DynamicTreeNode> stack = new Dictionary <int, b2DynamicTreeNode>();
int count = 0;
stack[count++] = m_root;
while (count > 0)
{
b2DynamicTreeNode node = stack[--count];
if (node.aabb.TestOverlap(segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80)
// |dot(v, p1 - c)| > dot(|v|,h)
b2Vec2 c = node.aabb.GetCenter();
b2Vec2 h = node.aabb.GetExtents();
float separation = Mathf.Abs(v.x * (p1.x - c.x) + v.y * (p1.y - c.y))
- abs_v.x * h.x - abs_v.y * h.y;
if (separation > 0.0f)
{
continue;
}
if (node.IsLeaf())
{
b2RayCastInput subInput = new b2RayCastInput();
subInput.p1 = input.p1;
subInput.p2 = input.p2;
//*************by kingBook 2015/10/22 16:17*************
subInput.maxFraction = maxFraction;
float value = callback(subInput, node);
if (value == 0)
{
return;
}
if (value > 0)
{
//Update the segment bounding box
maxFraction = value;
//******************************************************
tX = p1.x + maxFraction * (p2.x - p1.x);
tY = p1.y + maxFraction * (p2.y - p1.y);
segmentAABB.lowerBound.x = Mathf.Min(p1.x, tX);
segmentAABB.lowerBound.y = Mathf.Min(p1.y, tY);
segmentAABB.upperBound.x = Mathf.Max(p1.x, tX);
segmentAABB.upperBound.y = Mathf.Max(p1.y, tY);
}
}
else
{
// No stack limit, so no assert
stack[count++] = node.child1;
stack[count++] = node.child2;
}
}
}