public static b2Vec2 b2Mul(b2Transform T, b2Vec2 v)
{
float x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
float y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
return(new b2Vec2(x, y));
}
public static b2Transform b2MulT(b2Transform A, b2Transform B)
{
b2Transform C = b2Transform.Create();
C.q = b2MulT(A.q, B.q);
C.p = b2MulT(A.q, B.p - A.p);
return C;
}
public static b2Transform Create()
{
var transform = new b2Transform();
transform.p = b2Vec2.Zero;
transform.q = b2Rot.Create();
return transform;
}
// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
public static b2Transform b2Mul(b2Transform A, b2Transform B)
{
b2Transform C = b2Transform.Create();
C.q = b2Mul(A.q, B.q);
C.p = b2Mul(A.q, B.p) + A.p;
return(C);
}
public override b2AABB ComputeAABB(b2Transform transform, int childIndex)
{
b2Vec2 p = transform.p + b2Math.b2Mul(transform.q, m_p);
b2AABB aabb = new b2AABB();
aabb.lowerBound.Set(p.x - m_radius, p.y - m_radius);
aabb.upperBound.Set(p.x + m_radius, p.y + m_radius);
return (aabb);
}
public static b2Vec2 b2Mul(ref b2Transform T, ref b2Vec2 v)
{
b2Vec2 b;
b.x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
b.y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
return(b);
}
public static b2Transform b2MulT(b2Transform A, b2Transform B)
{
b2Transform C;
C.q = b2MulT(A.q, B.q);
C.p = b2MulT(A.q, B.p - A.p);
return(C);
}
public static b2Transform Create()
{
var transform = new b2Transform();
transform.p = b2Vec2.Zero;
transform.q = b2Rot.Create();
return(transform);
}
public static b2Vec2 b2Mul(b2Transform T, b2Vec2 v)
{
float x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
float y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
b2Vec2 b = b2Vec2.Zero;
b.Set(x, y);
return b;
}
public override void Evaluate(ref b2Manifold manifold, ref b2Transform xfA, ref b2Transform xfB)
{
b2ChainShape chain = (b2ChainShape)m_fixtureA.Shape;
b2EdgeShape edge;
edge = chain.GetChildEdge(m_indexA);
b2Collision.b2CollideEdgeAndCircle(ref manifold, edge, ref xfA,
(b2CircleShape)m_fixtureB.Shape, ref xfB);
}
/*public b2Sweep()
{
localCenter = new b2Vec2();
c0 = new b2Vec2();
c = new b2Vec2();
a0 = 0f;
a = 0f;
alpha0 = 0f;
}*/
/// Get the interpolated transform at a specific time.
/// @param beta is a factor in [0,1], where 0 indicates alpha0.
public void GetTransform(b2Transform xfb, float beta)
{
xfb.p = (1.0f - beta) * c0 + beta * c;
float angle = (1.0f - beta) * a0 + beta * a;
xfb.q.Set(angle);
// Shift to origin
xfb.p -= b2Math.b2Mul(xfb.q, localCenter);
}
public static b2Vec2 b2MulT(b2Transform T, b2Vec2 v)
{
float px = v.x - T.p.x;
float py = v.y - T.p.y;
float x = (T.q.c * px + T.q.s * py);
float y = (-T.q.s * px + T.q.c * py);
return(new b2Vec2(x, y));
}
public static b2Vec2 b2MulT(b2Transform T, b2Vec2 v)
{
float px = v.x - T.p.x;
float py = v.y - T.p.y;
float x = (T.q.c * px + T.q.s * py);
float y = (-T.q.s * px + T.q.c * py);
b2Vec2 b = b2Vec2.Zero;
b.Set(x, y);
return b;
}
public static b2Vec2 b2MulT(ref b2Transform T, ref b2Vec2 v)
{
b2Vec2 b;
float px = v.x - T.p.x;
float py = v.y - T.p.y;
b.x = (T.q.c * px + T.q.s * py);
b.y = (-T.q.s * px + T.q.c * py);
return(b);
}
/*public b2Sweep()
* {
* localCenter = new b2Vec2();
* c0 = new b2Vec2();
* c = new b2Vec2();
* a0 = 0f;
* a = 0f;
* alpha0 = 0f;
* }*/
/// Get the interpolated transform at a specific time.
/// @param beta is a factor in [0,1], where 0 indicates alpha0.
public void GetTransform(b2Transform xfb, float beta)
{
xfb.p = (1.0f - beta) * c0 + beta * c;
float angle = (1.0f - beta) * a0 + beta * a;
xfb.q.Set(angle);
// Shift to origin
xfb.p -= b2Math.b2Mul(xfb.q, localCenter);
}
public override bool TestPoint(ref b2Transform transform, b2Vec2 p)
{
b2Vec2 tx = b2Math.b2Mul(ref transform.q, ref Position);
b2Vec2 center;
center.x = transform.p.x + tx.x;
center.y = transform.p.y + tx.y;
// b2Vec2 center = transform.p + tx;
b2Vec2 d; // = p - center;
d.x = p.x - center.x;
d.y = p.y - center.y;
return d.LengthSquared <= Radius * Radius;
}
public override bool TestPoint(b2Transform transform, b2Vec2 p)
{
b2Vec2 tx = b2Math.b2Mul(transform.q, m_p);
b2Vec2 center;
center.x = transform.p.x + tx.x;
center.y = transform.p.y + tx.y;
// b2Vec2 center = transform.p + tx;
b2Vec2 d; // = p - center;
d.x = p.x - center.x;
d.y = p.y - center.y;
return d.LengthSquared <= m_radius * m_radius;
}
public override void DrawTransform(b2Transform xf)
{
float axisScale = 0.4f;
b2Vec2 p1 = xf.p;
b2Vec2 col1 = new b2Vec2(xf.q.c, xf.q.s);
b2Vec2 col2 = new b2Vec2(-xf.q.s, xf.q.c);
b2Vec2 p2 = p1 + axisScale * col1;
DrawSegment(p1, p2, new b2Color(1f, 0f, 0f));
p2 = p1 + axisScale * col2;
DrawSegment(p1, p2, new b2Color(0f, 0f, 1f));
}
// 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 bool RayCast(out b2RayCastOutput output, b2RayCastInput input,
b2Transform transform, int childIndex)
{
output = b2RayCastOutput.Zero;
b2Vec2 tx = b2Math.b2Mul(transform.q, m_p);
// b2Vec2 position = transform.p + tx;
b2Vec2 position;
position.x = transform.p.x + tx.x;
position.y = transform.p.y + tx.y;
// b2Vec2 s = input.p1 - position;
b2Vec2 s;
s.x = input.p1.x - position.x;
s.y = input.p1.y - position.y;
float b = s.LengthSquared - m_radius * m_radius;
// Solve quadratic equation.
b2Vec2 r;
r.x = input.p2.x - input.p1.x;
r.y = input.p2.y - input.p1.y;
// b2Vec2 r = input.p2 - input.p1;
float c = s.x * r.x + s.y * r.y; // b2Math.b2Dot(ref s, ref r);
float rr = r.LengthSquared; // b2Math.b2Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < b2Settings.b2_epsilon)
{
return false;
}
// Find the point of intersection of the line with the circle.
float a = -(c + b2Math.b2Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
output.fraction = a;
output.normal.x = s.x + a * r.x;
output.normal.y = s.y + a * r.y;
// output.normal = s + a * r;
output.normal.Normalize();
return true;
}
return false;
}
/// Get the interpolated transform at a specific time.
/// @param beta is a factor in [0,1], where 0 indicates alpha0.
public void GetTransform(out b2Transform xfb, float beta)
{
float x = (1.0f - beta) * c0.x + beta * c.x;
float y = (1.0f - beta) * c0.y + beta * c.y;
float angle = (1.0f - beta) * a0 + beta * a;
float sin = (float)Math.Sin(angle);
float cos = (float)Math.Cos(angle);
// Shift to origin
xfb.p.x = x - (cos * localCenter.x - sin * localCenter.y);
xfb.p.y = y - (sin * localCenter.x + cos * localCenter.y);
xfb.q.s = sin;
xfb.q.c = cos;
}
/// Get the interpolated transform at a specific time.
/// @param beta is a factor in [0,1], where 0 indicates alpha0.
public void GetTransform(out b2Transform xfb, float beta)
{
float x = (1.0f - beta) * c0.x + beta * c.x;
float y = (1.0f - beta) * c0.y + beta * c.y;
float angle = (1.0f - beta) * a0 + beta * a;
float sin = (float)Math.Sin(angle);
float cos = (float)Math.Cos(angle);
// Shift to origin
xfb.p.x = x - (cos * localCenter.x - sin * localCenter.y);
xfb.p.y = y - (sin * localCenter.x + cos * localCenter.y);
xfb.q.s = sin;
xfb.q.c = cos;
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
public virtual bool RayCast(out b2RayCastOutput output, b2RayCastInput input,
b2Transform transform, int childIndex)
{
output = b2RayCastOutput.Zero;
b2Vec2 position = transform.p + b2Math.b2Mul(transform.q, m_p);
b2Vec2 s = input.p1 - position;
float b = b2Math.b2Dot(s, s) - m_radius * m_radius;
// Solve quadratic equation.
b2Vec2 r = input.p2 - input.p1;
float c = b2Math.b2Dot(s, r);
float rr = b2Math.b2Dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < b2Settings.b2_epsilon)
{
return false;
}
// Find the point of intersection of the line with the circle.
float a = -(c + b2Math.b2Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
output.fraction = a;
output.normal = s + a * r;
output.normal.Normalize();
return true;
}
return false;
}
/// Determine if two generic shapes overlap.
public static bool b2TestOverlap(b2Shape shapeA, int indexA,
b2Shape shapeB, int indexB,
ref b2Transform xfA, ref b2Transform xfB)
{
b2DistanceInput input = b2DistanceInput.Create();
input.proxyA = b2DistanceProxy.Create(shapeA, indexA);
input.proxyB = b2DistanceProxy.Create(shapeB, indexB);
input.transformA = xfA;
input.transformB = xfB;
input.useRadii = true;
b2SimplexCache cache = b2SimplexCache.Create();
b2DistanceOutput output = new b2DistanceOutput();
b2Simplex.b2Distance(ref output, ref cache, ref input);
// Console.WriteLine("{2} vs {3}: distance={0} after {1} iters", output.distance, output.iterations, shapeA.ShapeType, shapeB.ShapeType);
return output.distance < 10.0f * b2Settings.b2_epsilon;
}
/// Compute the collision manifold between an edge and a circle.
public static void b2CollideEdgeAndPolygon(ref b2Manifold manifold,
b2EdgeShape edgeA, ref b2Transform xfA,
b2PolygonShape polygonB, ref b2Transform xfB)
{
b2EPCollider b = new b2EPCollider();
b.Collide(ref manifold, edgeA, ref xfA, polygonB, ref xfB);
}
/// 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;
}
public static float b2EdgeSeparation(b2PolygonShape poly1, b2Transform xf1, int edge1,
b2PolygonShape poly2, b2Transform xf2)
{
b2Vec2[] vertices1 = poly1.Vertices;
b2Vec2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
b2Vec2[] vertices2 = poly2.Vertices;
// Convert normal from poly1's frame into poly2's frame.
b2Vec2 normal1World = b2Math.b2Mul(xf1.q, normals1[edge1]);
b2Vec2 normal1 = b2Math.b2MulT(xf2.q, normal1World);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = b2Settings.b2_maxFloat;
for (int i = 0; i < count2; ++i)
{
float dot = b2Math.b2Dot(ref vertices2[i], ref normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
b2Vec2 v1 = b2Math.b2Mul(xf1, vertices1[edge1]);
b2Vec2 v2 = b2Math.b2Mul(xf2, vertices2[index]);
float separation = b2Math.b2Dot(v2 - v1, normal1World);
return separation;
}
/// Test a point for containment in this shape. This only works for convex shapes. /// @param xf the shape world transform. /// @param p a point in world coordinates. public abstract bool TestPoint(b2Transform xf, b2Vec2 p);
public override bool TestPoint(b2Transform xf, b2Vec2 p)
{
return false;
}
public override b2AABB ComputeAABB(b2Transform xf, int childIndex)
{
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == m_count)
{
i2 = 0;
}
b2Vec2 v1 = b2Math.b2Mul(xf, m_vertices[i1]);
b2Vec2 v2 = b2Math.b2Mul(xf, m_vertices[i2]);
b2AABB aabb = b2AABB.Default;
aabb.Set(b2Math.b2Min(v1, v2),b2Math.b2Max(v1, v2));
return (aabb);
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
public static float b2FindMaxSeparation(out int edgeIndex,
b2PolygonShape poly1, ref b2Transform xf1,
b2PolygonShape poly2, ref b2Transform xf2)
{
int count1 = poly1.VertexCount;
b2Vec2[] normals1 = poly1.Normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
b2Vec2 d = b2Math.b2Mul(xf2, poly2.Centroid) - b2Math.b2Mul(xf1, poly1.Centroid);
b2Vec2 dLocal1 = b2Math.b2MulT(xf1.q, d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -b2Settings.b2_maxFloat;
for (int i = 0; i < count1; ++i)
{
float dot = b2Math.b2Dot(normals1[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = b2EdgeSeparation(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 = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return bestSeparation;
}
/// Draw a transform. Choose your own length scale. /// @param xf a transform. public abstract void DrawTransform(b2Transform xf);
public static void b2FindIncidentEdge(b2ClipVertex[] c,
b2PolygonShape poly1, b2Transform xf1, int edge1,
b2PolygonShape poly2, b2Transform xf2)
{
b2Vec2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
b2Vec2[] vertices2 = poly2.Vertices;
b2Vec2[] normals2 = poly2.Normals;
// Get the normal of the reference edge in poly2's frame.
b2Vec2 normal1 = b2Math.b2MulT(xf2.q, b2Math.b2Mul(xf1.q, normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = b2Settings.b2_maxFloat;
for (int i = 0; i < count2; ++i)
{
float dot = b2Math.b2Dot(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[0].v = b2Math.b2Mul(xf2, vertices2[i1]);
c[0].id.indexA = (byte)edge1;
c[0].id.indexB = (byte)i1;
c[0].id.typeA = b2ContactFeatureType.e_face;
c[0].id.typeB = b2ContactFeatureType.e_vertex;
c[1].v = b2Math.b2Mul(xf2, vertices2[i2]);
c[1].id.indexA = (byte)edge1;
c[1].id.indexB = (byte)i2;
c[1].id.typeA = b2ContactFeatureType.e_face;
c[1].id.typeB = b2ContactFeatureType.e_vertex;
}
public override b2AABB ComputeAABB(b2Transform xf, int childIndex)
{
b2Vec2 v1 = b2Math.b2Mul(xf, m_vertex1);
b2Vec2 v2 = b2Math.b2Mul(xf, m_vertex2);
b2Vec2 lower = b2Math.b2Min(v1, v2);
b2Vec2 upper = b2Math.b2Max(v1, v2);
b2Vec2 r = new b2Vec2(m_radius, m_radius);
b2AABB aabb = new b2AABB();
aabb.lowerBound = lower - r;
aabb.upperBound = upper + r;
return(aabb);
}
public override bool RayCast(out b2RayCastOutput output, b2RayCastInput input,
b2Transform xf, int childIndex)
{
b2EdgeShape edgeShape = new b2EdgeShape();
output = b2RayCastOutput.Zero;
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == m_count)
{
i2 = 0;
}
edgeShape.Vertex1 = m_vertices[i1];
edgeShape.Vertex2 = m_vertices[i2];
b2RayCastOutput co = b2RayCastOutput.Zero;
bool b = edgeShape.RayCast(out co, input, xf, 0);
output = co;
return (b);
}
// 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 = 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 = b2Math.b2Dot(normal, v1 - p1);
float denominator = b2Math.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 = b2Math.b2Dot(r, r);
if (rr == 0.0f)
{
return false;
}
float s = b2Math.b2Dot(q - v1, 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;
}
public override b2AABB ComputeAABB(b2Transform xf, int childIndex)
{
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == m_count)
{
i2 = 0;
}
b2Vec2 v1 = b2Math.b2Mul(xf, m_vertices[i1]);
b2Vec2 v2 = b2Math.b2Mul(xf, m_vertices[i2]);
b2AABB aabb = new b2AABB();
aabb.lowerBound = b2Math.b2Min(v1, v2);
aabb.upperBound = b2Math.b2Max(v1, v2);
return (aabb);
}
/// Compute the collision manifold between two circles.
public static void b2CollideCircles(ref b2Manifold manifold,
b2CircleShape circleA, ref b2Transform xfA,
b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
b2Vec2 pA = b2Math.b2Mul(xfA, circleA.Position);
b2Vec2 pB = b2Math.b2Mul(xfB, circleB.Position);
b2Vec2 d = pB - pA;
float distSqr = b2Math.b2Dot(d, d);
float rA = circleA.Radius, rB = circleB.Radius;
float radius = rA + rB;
if (distSqr > radius * radius)
{
return;
}
manifold.type = b2ManifoldType.e_circles;
manifold.localPoint = circleA.Position;
manifold.localNormal.SetZero();
manifold.pointCount = 1;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
/// Cast a ray against a child shape.
/// @param output the ray-cast results.
/// @param input the ray-cast input parameters.
/// @param transform the transform to be applied to the shape.
/// @param childIndex the child shape index
public abstract bool RayCast(out b2RayCastOutput output, b2RayCastInput input,
b2Transform transform, int childIndex);
/// Compute the collision manifold between a polygon and a circle.
public static void b2CollidePolygonAndCircle(ref b2Manifold manifold,
b2PolygonShape polygonA, ref b2Transform xfA,
b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
// Compute circle position in the frame of the polygon.
b2Vec2 c = b2Math.b2Mul(xfB, circleB.Position);
b2Vec2 cLocal = b2Math.b2MulT(xfA, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -b2Settings.b2_maxFloat;
float radius = polygonA.Radius + circleB.Radius;
int vertexCount = polygonA.VertexCount;
b2Vec2[] vertices = polygonA.Vertices;
b2Vec2[] normals = polygonA.Normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = b2Math.b2Dot(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;
b2Vec2 v1 = vertices[vertIndex1];
b2Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < b2Settings.b2_epsilon)
{
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[normalIndex];
manifold.localPoint = 0.5f * (v1 + v2);
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
return;
}
// Compute barycentric coordinates
float u1 = b2Math.b2Dot(cLocal - v1, v2 - v1);
float u2 = b2Math.b2Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (b2Math.b2DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v1;
manifold.localNormal.Normalize();
manifold.localPoint = v1;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
else if (u2 <= 0.0f)
{
if (b2Math.b2DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v2;
manifold.localNormal.Normalize();
manifold.localPoint = v2;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
else
{
b2Vec2 faceCenter = 0.5f * (v1 + v2);
separation = b2Math.b2Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation > radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[vertIndex1];
manifold.localPoint = faceCenter;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
}
/// Given a transform, compute the associated axis aligned bounding box for a child shape. /// @param aabb returns the axis aligned box. /// @param xf the world transform of the shape. /// @param childIndex the child shape public abstract b2AABB ComputeAABB(b2Transform xf, int childIndex);
/// 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;
}
public b2PositionSolverManifold(ref b2ContactPositionConstraint pc, ref b2Transform xfA, ref b2Transform xfB, int index)
{
Debug.Assert(pc.pointCount > 0);
switch (pc.type)
{
case b2ManifoldType.e_circles:
{
b2Vec2 pointA = b2Math.b2Mul(xfA, pc.localPoint);
b2Vec2 pointB = b2Math.b2Mul(xfB, pc.localPoints[0]);
normal = pointB - pointA;
normal.Normalize();
point = 0.5f * (pointA + pointB);
separation = b2Math.b2Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB;
}
break;
case b2ManifoldType.e_faceA:
{
normal = b2Math.b2Mul(xfA.q, pc.localNormal);
b2Vec2 planePoint = b2Math.b2Mul(xfA, pc.localPoint);
b2Vec2 clipPoint = b2Math.b2Mul(xfB, pc.localPoints[index]);
separation = b2Math.b2Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
point = clipPoint;
}
break;
case b2ManifoldType.e_faceB:
{
normal = b2Math.b2Mul(xfB.q, pc.localNormal);
b2Vec2 planePoint = b2Math.b2Mul(xfB, pc.localPoint);
b2Vec2 clipPoint = b2Math.b2Mul(xfA, pc.localPoints[index]);
separation = b2Math.b2Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB;
point = clipPoint;
// Ensure normal points from A to B
normal = -normal;
}
break;
}
}
/// Draw a transform. Choose your own length scale. /// @param xf a transform. public abstract void DrawTransform(b2Transform xf);
