public override void computeAABB(AABB aabb, Transform xf, int childIndex)
{
Vec2 v1 = pool1;
Vec2 v2 = pool2;
Transform.mulToOutUnsafe(xf, m_vertex1, v1);
Transform.mulToOutUnsafe(xf, m_vertex2, v2);
Vec2.minToOut(v1, v2, aabb.lowerBound);
Vec2.maxToOut(v1, v2, aabb.upperBound);
aabb.lowerBound.x -= m_radius;
aabb.lowerBound.y -= m_radius;
aabb.upperBound.x += m_radius;
aabb.upperBound.y += m_radius;
}
public override bool TestPoint(Transform xf, Vec2 p)
{
return false;
}
private void drawShape(Fixture fixture, Transform xf, Color3f color)
{
switch (fixture.Type)
{
case ShapeType.CIRCLE:
{
CircleShape circle = (CircleShape)fixture.Shape;
// Vec2 center = Mul(xf, circle.m_p);
Transform.mulToOutUnsafe(xf, circle.m_p, center);
float radius = circle.m_radius;
xf.q.getXAxis(axis);
if (fixture.UserData != null && fixture.UserData.Equals(LIQUID_INT))
{
Body b = fixture.Body;
liquidOffset.set_Renamed(b.m_linearVelocity);
float linVelLength = b.m_linearVelocity.length();
if (averageLinearVel == -1)
{
averageLinearVel = linVelLength;
}
else
{
averageLinearVel = .98f * averageLinearVel + .02f * linVelLength;
}
liquidOffset.mulLocal(liquidLength / averageLinearVel / 2);
circCenterMoved.set_Renamed(center).addLocal(liquidOffset);
center.subLocal(liquidOffset);
m_debugDraw.drawSegment(center, circCenterMoved, liquidColor);
return;
}
m_debugDraw.drawSolidCircle(center, radius, axis, color);
}
break;
case ShapeType.POLYGON:
{
PolygonShape poly = (PolygonShape)fixture.Shape;
int vertexCount = poly.m_count;
Debug.Assert(vertexCount <= Settings.maxPolygonVertices);
Vec2[] vertices = tlvertices.get_Renamed(Settings.maxPolygonVertices);
for (int i = 0; i < vertexCount; ++i)
{
// vertices[i] = Mul(xf, poly.m_vertices[i]);
Transform.mulToOutUnsafe(xf, poly.m_vertices[i], vertices[i]);
}
m_debugDraw.drawSolidPolygon(vertices, vertexCount, color);
}
break;
case ShapeType.EDGE:
{
EdgeShape edge = (EdgeShape)fixture.Shape;
Transform.mulToOutUnsafe(xf, edge.m_vertex1, v1);
Transform.mulToOutUnsafe(xf, edge.m_vertex2, v2);
m_debugDraw.drawSegment(v1, v2, color);
}
break;
case ShapeType.CHAIN:
{
ChainShape chain = (ChainShape)fixture.Shape;
int count = chain.m_count;
Vec2[] vertices = chain.m_vertices;
Transform.mulToOutUnsafe(xf, vertices[0], v1);
for (int i = 1; i < count; ++i)
{
Transform.mulToOutUnsafe(xf, vertices[i], v2);
m_debugDraw.drawSegment(v1, v2, color);
m_debugDraw.drawCircle(v1, 0.05f, color);
v1.set_Renamed(v2);
}
}
break;
default:
break;
}
}
/// <summary>
/// Find the max separation between poly1 and poly2 using edge normals from poly1.
/// </summary>
/// <param name="edgeIndex"></param>
/// <param name="poly1"></param>
/// <param name="xf1"></param>
/// <param name="poly2"></param>
/// <param name="xf2"></param>
/// <returns></returns>
public void findMaxSeparation(EdgeResults results, PolygonShape poly1, Transform xf1, PolygonShape poly2, Transform xf2)
{
int count1 = poly1.m_count;
Vec2[] normals1 = poly1.m_normals;
//Vec2 v = poly2.m_centroid;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
// before inline:
Transform.mulToOutUnsafe(xf2, poly2.m_centroid, d);
Transform.mulToOutUnsafe(xf1, poly1.m_centroid, temp);
d.subLocal(temp);
Rot.mulTransUnsafe(xf1.q, d, dLocal1);
float dLocal1x = dLocal1.x;
float dLocal1y = dLocal1.y;
// after inline:
// final float predy = xf2.p.y + xf2.q.ex.y * v.x + xf2.q.ey.y * v.y;
// final float predx = xf2.p.x + xf2.q.ex.x * v.x + xf2.q.ey.x * v.y;
// final Vec2 v1 = poly1.m_centroid;
// final float tempy = xf1.p.y + xf1.q.ex.y * v1.x + xf1.q.ey.y * v1.y;
// final float tempx = xf1.p.x + xf1.q.ex.x * v1.x + xf1.q.ey.x * v1.y;
// final float dx = predx - tempx;
// final float dy = predy - tempy;
//
// final Mat22 R = xf1.q;
// final float dLocal1x = dx * R.ex.x + dy * R.ex.y;
// final float dLocal1y = dx * R.ey.x + dy * R.ey.y;
// end inline
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float dot;
//UPGRADE_TODO: The equivalent in .NET for field 'java.lang.Float.MIN_VALUE' may return a different value. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1043'"
float maxDot = Single.Epsilon;
for (int i = 0; i < count1; i++)
{
Vec2 normal = normals1[i];
dot = normal.x * dLocal1x + normal.y * dLocal1y;
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = edgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = edgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = edgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
// Find the best edge and the search direction.
int bestEdge;
float bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
results.edgeIndex = edge;
results.separation = s;
return;
}
// 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 = edgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
results.edgeIndex = bestEdge;
results.separation = bestSeparation;
}
public override void ComputeAABB(AABB aabb, Transform xf, int childIndex)
{
Debug.Assert(childIndex < Count);
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == Count)
{
i2 = 0;
}
Vec2 v1 = pool1;
Vec2 v2 = pool2;
Transform.MulToOutUnsafe(xf, Vertices[i1], v1);
Transform.MulToOutUnsafe(xf, Vertices[i2], v2);
Vec2.MinToOut(v1, v2, aabb.LowerBound);
Vec2.MaxToOut(v1, v2, aabb.UpperBound);
}
/// <summary>
/// Compute the collision manifold between a polygon and a circle.
/// </summary>
/// <param name="manifold"></param>
/// <param name="polygon"></param>
/// <param name="xfA"></param>
/// <param name="circle"></param>
/// <param name="xfB"></param>
public void collidePolygonAndCircle(Manifold manifold, PolygonShape polygon, Transform xfA, CircleShape circle, Transform xfB)
{
manifold.pointCount = 0;
//Vec2 v = circle.m_p;
// Compute circle position in the frame of the polygon.
// before inline:
Transform.mulToOut(xfB, circle.m_p, c);
Transform.mulTransToOut(xfA, c, cLocal);
float cLocalx = cLocal.x;
float cLocaly = cLocal.y;
// after inline:
// final float cy = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.y * v.y;
// final float cx = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y;
// final float v1x = cx - xfA.p.x;
// final float v1y = cy - xfA.p.y;
// final Vec2 b = xfA.q.ex;
// final Vec2 b1 = xfA.q.ey;
// final float cLocaly = v1x * b1.x + v1y * b1.y;
// final float cLocalx = v1x * b.x + v1y * b.y;
// end inline
// Find the min separating edge.
int normalIndex = 0;
//UPGRADE_TODO: The equivalent in .NET for field 'java.lang.Float.MIN_VALUE' may return a different value. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1043'"
float separation = Single.Epsilon;
float radius = polygon.m_radius + circle.m_radius;
int vertexCount = polygon.m_count;
Vec2[] vertices = polygon.m_vertices;
Vec2[] normals = polygon.m_normals;
for (int i = 0; i < vertexCount; i++)
{
// before inline
// temp.set(cLocal).subLocal(vertices[i]);
// float s = Vec2.dot(normals[i], temp);
// after inline
Vec2 vertex = vertices[i];
float tempx = cLocalx - vertex.x;
float tempy = cLocaly - vertex.y;
Vec2 normal = normals[i];
float s = normal.x * tempx + normal.y * tempy;
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;
Vec2 v1 = vertices[vertIndex1];
Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < Settings.EPSILON)
{
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.FACE_A;
// before inline:
// manifold.localNormal.set(normals[normalIndex]);
// manifold.localPoint.set(v1).addLocal(v2).mulLocal(.5f);
// manifold.points[0].localPoint.set(circle.m_p);
// after inline:
Vec2 normal = normals[normalIndex];
manifold.localNormal.x = normal.x;
manifold.localNormal.y = normal.y;
manifold.localPoint.x = (v1.x + v2.x) * .5f;
manifold.localPoint.y = (v1.y + v2.y) * .5f;
ManifoldPoint mpoint = manifold.points[0];
mpoint.localPoint.x = circle.m_p.x;
mpoint.localPoint.y = circle.m_p.y;
mpoint.id.zero();
// end inline
return;
}
// Compute barycentric coordinates
// before inline:
// temp.set(cLocal).subLocal(v1);
// temp2.set(v2).subLocal(v1);
// float u1 = Vec2.dot(temp, temp2);
// temp.set(cLocal).subLocal(v2);
// temp2.set(v1).subLocal(v2);
// float u2 = Vec2.dot(temp, temp2);
// after inline:
float tempX = cLocalx - v1.x;
float tempY = cLocaly - v1.y;
float temp2X = v2.x - v1.x;
float temp2Y = v2.y - v1.y;
float u1 = tempX * temp2X + tempY * temp2Y;
float temp3X = cLocalx - v2.x;
float temp3Y = cLocaly - v2.y;
float temp4X = v1.x - v2.x;
float temp4Y = v1.y - v2.y;
float u2 = temp3X * temp4X + temp3Y * temp4Y;
// end inline
if (u1 <= 0f)
{
// inlined
float dx = cLocalx - v1.x;
float dy = cLocaly - v1.y;
if (dx * dx + dy * dy > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.FACE_A;
// before inline:
// manifold.localNormal.set(cLocal).subLocal(v1);
// after inline:
manifold.localNormal.x = cLocalx - v1.x;
manifold.localNormal.y = cLocaly - v1.y;
// end inline
manifold.localNormal.normalize();
manifold.localPoint.set_Renamed(v1);
manifold.points[0].localPoint.set_Renamed(circle.m_p);
manifold.points[0].id.zero();
}
else if (u2 <= 0.0f)
{
// inlined
float dx = cLocalx - v2.x;
float dy = cLocaly - v2.y;
if (dx * dx + dy * dy > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.FACE_A;
// before inline:
// manifold.localNormal.set(cLocal).subLocal(v2);
// after inline:
manifold.localNormal.x = cLocalx - v2.x;
manifold.localNormal.y = cLocaly - v2.y;
// end inline
manifold.localNormal.normalize();
manifold.localPoint.set_Renamed(v2);
manifold.points[0].localPoint.set_Renamed(circle.m_p);
manifold.points[0].id.zero();
}
else
{
// Vec2 faceCenter = 0.5f * (v1 + v2);
// (temp is faceCenter)
// before inline:
// temp.set(v1).addLocal(v2).mulLocal(.5f);
//
// temp2.set(cLocal).subLocal(temp);
// separation = Vec2.dot(temp2, normals[vertIndex1]);
// if (separation > radius) {
// return;
// }
// after inline:
float fcx = (v1.x + v2.x) * .5f;
float fcy = (v1.y + v2.y) * .5f;
float tx = cLocalx - fcx;
float ty = cLocaly - fcy;
Vec2 normal = normals[vertIndex1];
separation = tx * normal.x + ty * normal.y;
if (separation > radius)
{
return;
}
// end inline
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.FACE_A;
manifold.localNormal.set_Renamed(normals[vertIndex1]);
manifold.localPoint.x = fcx; // (faceCenter)
manifold.localPoint.y = fcy;
manifold.points[0].localPoint.set_Renamed(circle.m_p);
manifold.points[0].id.zero();
}
}
/// <summary> Find the separation between poly1 and poly2 for a given edge normal on poly1.
///
/// </summary>
/// <param name="poly1">
/// </param>
/// <param name="xf1">
/// </param>
/// <param name="edge1">
/// </param>
/// <param name="poly2">
/// </param>
/// <param name="xf2">
/// </param>
public float edgeSeparation(PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2)
{
int count1 = poly1.m_count;
Vec2[] vertices1 = poly1.m_vertices;
Vec2[] normals1 = poly1.m_normals;
int count2 = poly2.m_count;
Vec2[] vertices2 = poly2.m_vertices;
Debug.Assert(0 <= edge1 && edge1 < count1);
// Convert normal from poly1's frame into poly2's frame.
// before inline:
// Vec2 normal1World = Mul(xf1.R, normals1[edge1]);
Rot.mulToOutUnsafe(xf1.q, normals1[edge1], normal1World);
// Vec2 normal1 = MulT(xf2.R, normal1World);
Rot.mulTransUnsafe(xf2.q, normal1World, normal1);
float normal1x = normal1.x;
float normal1y = normal1.y;
float normal1Worldx = normal1World.x;
float normal1Worldy = normal1World.y;
// after inline:
// R.mulToOut(v,out);
// final Mat22 R = xf1.q;
// final Vec2 v = normals1[edge1];
// final float normal1Worldy = R.ex.y * v.x + R.ey.y * v.y;
// final float normal1Worldx = R.ex.x * v.x + R.ey.x * v.y;
// final Mat22 R1 = xf2.q;
// final float normal1x = normal1Worldx * R1.ex.x + normal1Worldy * R1.ex.y;
// final float normal1y = normal1Worldx * R1.ey.x + normal1Worldy * R1.ey.y;
// end inline
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = Single.MaxValue;
for (int i = 0; i < count2; ++i)
{
Vec2 a = vertices2[i];
float dot = a.x * normal1x + a.y * normal1y;
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Vec2 v1 = Mul(xf1, vertices1[edge1]);
// Vec2 v2 = Mul(xf2, vertices2[index]);
// before inline:
Transform.mulToOut(xf1, vertices1[edge1], v1);
Transform.mulToOut(xf2, vertices2[index], v2);
float separation = Vec2.dot(v2.subLocal(v1), normal1World);
return separation;
// after inline:
// final Vec2 v3 = vertices1[edge1];
// final float v1y = xf1.p.y + R.ex.y * v3.x + R.ey.y * v3.y;
// final float v1x = xf1.p.x + R.ex.x * v3.x + R.ey.x * v3.y;
// final Vec2 v4 = vertices2[index];
// final float v2y = xf2.p.y + R1.ex.y * v4.x + R1.ey.y * v4.y - v1y;
// final float v2x = xf2.p.x + R1.ex.x * v4.x + R1.ey.x * v4.y - v1x;
//
// return v2x * normal1Worldx + v2y * normal1Worldy;
// end inline
}
/// <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> public abstract bool testPoint(Transform xf, Vec2 p);
public override void computeAABB(AABB aabb, Transform transform, int childIndex)
{
Vec2 p = pool1;
Rot.mulToOutUnsafe(transform.q, m_p, p);
p.addLocal(transform.p);
aabb.lowerBound.x = p.x - m_radius;
aabb.lowerBound.y = p.y - m_radius;
aabb.upperBound.x = p.x + m_radius;
aabb.upperBound.y = p.y + m_radius;
}
/// <summary> /// Given a transform, compute the associated axis aligned bounding box for a child shape. /// </summary> /// <param name="argAabb">returns the axis aligned box.</param> /// <param name="argXf">the world transform of the shape.</param> public abstract void computeAABB(AABB aabb, Transform xf, int childIndex);
/// <summary> /// Cast a ray against a child shape. /// </summary> /// <param name="argOutput">the ray-cast results.</param> /// <param name="argInput">the ray-cast input parameters.</param> /// <param name="argTransform">the transform to be applied to the shape.</param> /// <param name="argChildIndex">the child shape index</param> /// <returns>if hit</returns> public abstract bool raycast(RayCastOutput output, RayCastInput input, Transform transform, int childIndex);
public override bool Raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
{
// Put the ray into the edge's frame of reference.
Vec2 p1 = pool0.Set(input.P1).SubLocal(xf.P);
Rot.MulTrans(xf.Q, p1, p1);
Vec2 p2 = pool1.Set(input.P2).SubLocal(xf.P);
Rot.MulTrans(xf.Q, p1, p1);
Vec2 d = p2.SubLocal(p1); // we don't use p2 later
Vec2 v1 = Vertex1;
Vec2 v2 = Vertex2;
Vec2 normal = pool2.Set(v2).SubLocal(v1);
normal.Set(normal.Y, -normal.X);
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
pool3.Set(v1).SubLocal(p1);
float numerator = Vec2.Dot(normal, pool3);
float denominator = Vec2.Dot(normal, d);
if (denominator == 0.0f)
{
return false;
}
float t = numerator / denominator;
if (t < 0.0f || 1.0f < t)
{
return false;
}
Vec2 q = pool3;
Vec2 r = pool4;
// Vec2 q = p1 + t * d;
q.Set(d).MulLocal(t).AddLocal(p1);
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
// Vec2 r = v2 - v1;
r.Set(v2).SubLocal(v1);
float rr = Vec2.Dot(r, r);
if (rr == 0.0f)
{
return false;
}
pool5.Set(q).SubLocal(v1);
float s = Vec2.Dot(pool5, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return false;
}
output.Fraction = t;
if (numerator > 0.0f)
{
// argOutput.normal = -normal;
output.Normal.Set(normal).NegateLocal();
}
else
{
// output.normal = normal;
output.Normal.Set(normal);
}
return true;
}
public override void ComputeAABB(AABB aabb, Transform xf, int childIndex)
{
Vec2 v1 = pool1;
Vec2 v2 = pool2;
Transform.MulToOutUnsafe(xf, Vertex1, v1);
Transform.MulToOutUnsafe(xf, Vertex2, v2);
Vec2.MinToOut(v1, v2, aabb.LowerBound);
Vec2.MaxToOut(v1, v2, aabb.UpperBound);
aabb.LowerBound.X -= Radius;
aabb.LowerBound.Y -= Radius;
aabb.UpperBound.X += Radius;
aabb.UpperBound.Y += Radius;
}
public override bool raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
{
// Put the ray into the edge's frame of reference.
Vec2 p1 = pool0.set_Renamed(input.p1).subLocal(xf.p);
Rot.mulTrans(xf.q, p1, p1);
Vec2 p2 = pool1.set_Renamed(input.p2).subLocal(xf.p);
Rot.mulTrans(xf.q, p1, p1);
Vec2 d = p2.subLocal(p1); // we don't use p2 later
Vec2 v1 = m_vertex1;
Vec2 v2 = m_vertex2;
Vec2 normal = pool2.set_Renamed(v2).subLocal(v1);
normal.set_Renamed(normal.y, -normal.x);
normal.normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
pool3.set_Renamed(v1).subLocal(p1);
float numerator = Vec2.dot(normal, pool3);
float denominator = Vec2.dot(normal, d);
if (denominator == 0.0f)
{
return false;
}
float t = numerator / denominator;
if (t < 0.0f || 1.0f < t)
{
return false;
}
Vec2 q = pool3;
Vec2 r = pool4;
// Vec2 q = p1 + t * d;
q.set_Renamed(d).mulLocal(t).addLocal(p1);
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
// Vec2 r = v2 - v1;
r.set_Renamed(v2).subLocal(v1);
float rr = Vec2.dot(r, r);
if (rr == 0.0f)
{
return false;
}
pool5.set_Renamed(q).subLocal(v1);
float s = Vec2.dot(pool5, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return false;
}
output.fraction = t;
if (numerator > 0.0f)
{
// argOutput.normal = -normal;
output.normal.set_Renamed(normal).negateLocal();
}
else
{
// output.normal = normal;
output.normal.set_Renamed(normal);
}
return true;
}
// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
public virtual void collideEdgeAndCircle(Manifold manifold, EdgeShape edgeA, Transform xfA, CircleShape circleB, Transform xfB)
{
manifold.pointCount = 0;
// Compute circle in frame of edge
// Vec2 Q = MulT(xfA, Mul(xfB, circleB.m_p));
Transform.mulToOutUnsafe(xfB, circleB.m_p, temp);
Transform.mulTransToOutUnsafe(xfA, temp, Q);
Vec2 A = edgeA.m_vertex1;
Vec2 B = edgeA.m_vertex2;
e.set_Renamed(B).subLocal(A);
// Barycentric coordinates
float u = Vec2.dot(e, temp.set_Renamed(B).subLocal(Q));
float v = Vec2.dot(e, temp.set_Renamed(Q).subLocal(A));
float radius = edgeA.m_radius + circleB.m_radius;
// ContactFeature cf;
cf.indexB = 0;
cf.typeB = (sbyte)ContactID.Type.VERTEX;
// Region A
if (v <= 0.0f)
{
Vec2 _P = A;
d.set_Renamed(Q).subLocal(_P);
float dd = Vec2.dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.m_hasVertex0)
{
Vec2 A1 = edgeA.m_vertex0;
Vec2 B1 = A;
e1.set_Renamed(B1).subLocal(A1);
float u1 = Vec2.dot(e1, temp.set_Renamed(B1).subLocal(Q));
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = (sbyte)ContactID.Type.VERTEX;
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.CIRCLES;
manifold.localNormal.setZero();
manifold.localPoint.set_Renamed(_P);
// manifold.points[0].id.key = 0;
manifold.points[0].id.set_Renamed(cf);
manifold.points[0].localPoint.set_Renamed(circleB.m_p);
return;
}
// Region B
if (u <= 0.0f)
{
Vec2 _P = B;
d.set_Renamed(Q).subLocal(_P);
float dd = Vec2.dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.m_hasVertex3)
{
Vec2 B2 = edgeA.m_vertex3;
Vec2 A2 = B;
Vec2 e2 = e1;
e2.set_Renamed(B2).subLocal(A2);
float v2 = Vec2.dot(e2, temp.set_Renamed(Q).subLocal(A2));
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = (sbyte)ContactID.Type.VERTEX;
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.CIRCLES;
manifold.localNormal.setZero();
manifold.localPoint.set_Renamed(_P);
// manifold.points[0].id.key = 0;
manifold.points[0].id.set_Renamed(cf);
manifold.points[0].localPoint.set_Renamed(circleB.m_p);
return;
}
// Region AB
float den = Vec2.dot(e, e);
Debug.Assert(den > 0.0f);
// Vec2 P = (1.0f / den) * (u * A + v * B);
P.set_Renamed(A).mulLocal(u).addLocal(temp.set_Renamed(B).mulLocal(v));
P.mulLocal(1.0f / den);
d.set_Renamed(Q).subLocal(P);
float dd2 = Vec2.dot(d, d);
if (dd2 > radius * radius)
{
return;
}
n.x = -e.y;
n.y = e.x;
if (Vec2.dot(n, temp.set_Renamed(Q).subLocal(A)) < 0.0f)
{
n.set_Renamed(-n.x, -n.y);
}
n.normalize();
cf.indexA = 0;
cf.typeA = (sbyte)ContactID.Type.FACE;
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.FACE_A;
manifold.localNormal.set_Renamed(n);
manifold.localPoint.set_Renamed(A);
// manifold.points[0].id.key = 0;
manifold.points[0].id.set_Renamed(cf);
manifold.points[0].localPoint.set_Renamed(circleB.m_p);
}
// 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(RayCastOutput output, RayCastInput input, Transform transform, int childIndex)
{
Vec2 position = pool1;
Vec2 s = pool2;
Vec2 r = pool3;
Rot.mulToOutUnsafe(transform.q, m_p, position);
position.addLocal(transform.p);
s.set_Renamed(input.p1).subLocal(position);
float b = Vec2.dot(s, s) - m_radius * m_radius;
// Solve quadratic equation.
r.set_Renamed(input.p2).subLocal(input.p1);
float c = Vec2.dot(s, r);
float rr = Vec2.dot(r, r);
float sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < Settings.EPSILON)
{
return false;
}
// Find the point of intersection of the line with the circle.
float a = -(c + MathUtils.sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
output.fraction = a;
output.normal.set_Renamed(r).mulLocal(a);
output.normal.addLocal(s);
output.normal.normalize();
return true;
}
return false;
}
public virtual void collideEdgeAndPolygon(Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB)
{
collider.collide(manifold, edgeA, xfA, polygonB, xfB);
}
public override bool testPoint(Transform transform, Vec2 p)
{
Vec2 center = pool1;
Rot.mulToOutUnsafe(transform.q, m_p, center);
center.addLocal(transform.p);
Vec2 d = center.subLocal(p).negateLocal();
return Vec2.dot(d, d) <= m_radius * m_radius;
}
/// <summary>
/// Compute the collision manifold between two polygons.
/// </summary>
/// <param name="manifold"></param>
/// <param name="polygon1"></param>
/// <param name="xf1"></param>
/// <param name="polygon2"></param>
/// <param name="xf2"></param>
public void collidePolygons(Manifold manifold, PolygonShape polyA, Transform xfA, PolygonShape polyB, Transform xfB)
{
// 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
manifold.pointCount = 0;
float totalRadius = polyA.m_radius + polyB.m_radius;
findMaxSeparation(results1, polyA, xfA, polyB, xfB);
if (results1.separation > totalRadius)
{
return;
}
findMaxSeparation(results2, polyB, xfB, polyA, xfA);
if (results2.separation > totalRadius)
{
return;
}
PolygonShape poly1; // reference polygon
PolygonShape poly2; // incident polygon
Transform xf1, xf2;
int edge1; // reference edge
bool flip;
float k_relativeTol = 0.98f;
float k_absoluteTol = 0.001f;
if (results2.separation > k_relativeTol * results1.separation + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = results2.edgeIndex;
manifold.type = Manifold.ManifoldType.FACE_B;
flip = true;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = results1.edgeIndex;
manifold.type = Manifold.ManifoldType.FACE_A;
flip = false;
}
findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.m_count;
Vec2[] vertices1 = poly1.m_vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
v11.set_Renamed(vertices1[iv1]);
v12.set_Renamed(vertices1[iv2]);
localTangent.set_Renamed(v12).subLocal(v11);
localTangent.normalize();
Vec2.crossToOutUnsafe(localTangent, 1f, localNormal); // Vec2 localNormal = Vec2.cross(dv,
// 1.0f);
planePoint.set_Renamed(v11).addLocal(v12).mulLocal(.5f); // Vec2 planePoint = 0.5f * (v11
// + v12);
Rot.mulToOutUnsafe(xf1.q, localTangent, tangent); // Vec2 sideNormal = Mul(xf1.R, v12
// - v11);
Vec2.crossToOutUnsafe(tangent, 1f, normal); // Vec2 frontNormal = Vec2.cross(sideNormal,
// 1.0f);
Transform.mulToOut(xf1, v11, v11);
Transform.mulToOut(xf1, v12, v12);
// v11 = Mul(xf1, v11);
// v12 = Mul(xf1, v12);
// Face offset
float frontOffset = Vec2.dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -Vec2.dot(tangent, v11) + totalRadius;
float sideOffset2 = Vec2.dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
// ClipVertex clipPoints1[2];
// ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
// np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
tangent.negateLocal();
np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1, iv1);
tangent.negateLocal();
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal.set_Renamed(localNormal);
manifold.localPoint.set_Renamed(planePoint);
int pointCount = 0;
for (int i = 0; i < Settings.maxManifoldPoints; ++i)
{
float separation = Vec2.dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = manifold.points[pointCount];
Transform.mulTransToOut(xf2, clipPoints2[i].v, cp.localPoint);
// cp.m_localPoint = MulT(xf2, clipPoints2[i].v);
cp.id.set_Renamed(clipPoints2[i].id);
if (flip)
{
// Swap features
cp.id.flip();
}
++pointCount;
}
}
manifold.pointCount = pointCount;
}
public abstract void evaluate(Manifold manifold, Transform xfA, Transform xfB);
// djm pooling from above
public void findIncidentEdge(ClipVertex[] c, PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2)
{
int count1 = poly1.m_count;
Vec2[] normals1 = poly1.m_normals;
int count2 = poly2.m_count;
Vec2[] vertices2 = poly2.m_vertices;
Vec2[] normals2 = poly2.m_normals;
Debug.Assert(0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Rot.mulToOutUnsafe(xf1.q, normals1[edge1], normal1); // temporary
// Vec2 normal1 = MulT(xf2.R, Mul(xf1.R, normals1[edge1]));
Rot.mulTrans(xf2.q, normal1, normal1);
// Find the incident edge on poly2.
int index = 0;
float minDot = Single.MaxValue;
for (int i = 0; i < count2; ++i)
{
float dot = Vec2.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;
Transform.mulToOutUnsafe(xf2, vertices2[i1], c[0].v); // = Mul(xf2, vertices2[i1]);
c[0].id.indexA = (sbyte)edge1;
c[0].id.indexB = (sbyte)i1;
c[0].id.typeA = (sbyte)ContactID.Type.FACE;
c[0].id.typeB = (sbyte)ContactID.Type.VERTEX;
Transform.mulToOutUnsafe(xf2, vertices2[i2], c[1].v); // = Mul(xf2, vertices2[i2]);
c[1].id.indexA = (sbyte)edge1;
c[1].id.indexB = (sbyte)i2;
c[1].id.typeA = (sbyte)ContactID.Type.FACE;
c[1].id.typeB = (sbyte)ContactID.Type.VERTEX;
}
public void Initialize(ContactPositionConstraint pc, Transform xfA, Transform xfB, int index)
{
Debug.Assert(pc.PointCount > 0);
switch (pc.Type)
{
case Manifold.ManifoldType.Circles:
{
Transform.MulToOutUnsafe(xfA, pc.LocalPoint, pointA);
Transform.MulToOutUnsafe(xfB, pc.LocalPoints[0], pointB);
Normal.Set(pointB).SubLocal(pointA);
Normal.Normalize();
Point.Set(pointA).AddLocal(pointB).MulLocal(.5f);
temp.Set(pointB).SubLocal(pointA);
Separation = Vec2.Dot(temp, Normal) - pc.RadiusA - pc.RadiusB;
break;
}
case Manifold.ManifoldType.FaceA:
{
Rot.MulToOutUnsafe(xfA.Q, pc.LocalNormal, Normal);
Transform.MulToOutUnsafe(xfA, pc.LocalPoint, planePoint);
Transform.MulToOutUnsafe(xfB, pc.LocalPoints[index], clipPoint);
temp.Set(clipPoint).SubLocal(planePoint);
Separation = Vec2.Dot(temp, Normal) - pc.RadiusA - pc.RadiusB;
Point.Set(clipPoint);
break;
}
case Manifold.ManifoldType.FaceB:
{
Rot.MulToOutUnsafe(xfB.Q, pc.LocalNormal, Normal);
Transform.MulToOutUnsafe(xfB, pc.LocalPoint, planePoint);
Transform.MulToOutUnsafe(xfA, pc.LocalPoints[index], clipPoint);
temp.Set(clipPoint).SubLocal(planePoint);
Separation = Vec2.Dot(temp, Normal) - pc.RadiusA - pc.RadiusB;
Point.Set(clipPoint);
// Ensure normal points from A to B
Normal.NegateLocal();
}
break;
}
}
private void DrawShape(Fixture fixture, Transform xf, Color3f color)
{
switch (fixture.Type)
{
case ShapeType.Circle:
{
CircleShape circle = (CircleShape)fixture.Shape;
// Vec2 center = Mul(xf, circle.m_p);
Transform.MulToOutUnsafe(xf, circle.P, center);
float radius = circle.Radius;
xf.Q.GetXAxis(axis);
if (fixture.UserData != null && fixture.UserData.Equals(LIQUID_INT))
{
Body b = fixture.Body;
liquidOffset.Set(b.LinearVelocity);
float linVelLength = b.LinearVelocity.Length();
if (averageLinearVel == -1)
{
averageLinearVel = linVelLength;
}
else
{
averageLinearVel = .98f * averageLinearVel + .02f * linVelLength;
}
liquidOffset.MulLocal(LIQUID_LENGTH / averageLinearVel / 2);
circCenterMoved.Set(center).AddLocal(liquidOffset);
center.SubLocal(liquidOffset);
DebugDraw.DrawSegment(center, circCenterMoved, liquidColor);
return;
}
DebugDraw.DrawSolidCircle(center, radius, axis, color);
}
break;
case ShapeType.Polygon:
{
PolygonShape poly = (PolygonShape)fixture.Shape;
int vertexCount = poly.VertexCount;
Debug.Assert(vertexCount <= Settings.MAX_POLYGON_VERTICES);
Vec2[] vertices = tlvertices.Get(Settings.MAX_POLYGON_VERTICES);
for (int i = 0; i < vertexCount; ++i)
{
// vertices[i] = Mul(xf, poly.m_vertices[i]);
Transform.MulToOutUnsafe(xf, poly.Vertices[i], vertices[i]);
}
DebugDraw.DrawSolidPolygon(vertices, vertexCount, color);
}
break;
case ShapeType.Edge:
{
EdgeShape edge = (EdgeShape)fixture.Shape;
Transform.MulToOutUnsafe(xf, edge.Vertex1, v1);
Transform.MulToOutUnsafe(xf, edge.Vertex2, v2);
DebugDraw.DrawSegment(v1, v2, color);
}
break;
case ShapeType.Chain:
{
ChainShape chain = (ChainShape)fixture.Shape;
int count = chain.Count;
Vec2[] vertices = chain.Vertices;
Transform.MulToOutUnsafe(xf, vertices[0], v1);
for (int i = 1; i < count; ++i)
{
Transform.MulToOutUnsafe(xf, vertices[i], v2);
DebugDraw.DrawSegment(v1, v2, color);
DebugDraw.DrawCircle(v1, 0.05f, color);
v1.Set(v2);
}
}
break;
}
}
/// <summary>
/// Determine if two generic shapes overlap.
/// </summary>
/// <param name="shapeA"></param>
/// <param name="shapeB"></param>
/// <param name="xfA"></param>
/// <param name="xfB"></param>
/// <returns></returns>
public bool testOverlap(Shape shapeA, int indexA, Shape shapeB, int indexB, Transform xfA, Transform xfB)
{
input.proxyA.set_Renamed(shapeA, indexA);
input.proxyB.set_Renamed(shapeB, indexB);
input.transformA.set_Renamed(xfA);
input.transformB.set_Renamed(xfB);
input.useRadii = true;
cache.count = 0;
pool.getDistance().distance(output, cache, input);
// djm note: anything significant about 10.0f?
return output.distance < 10.0f * Settings.EPSILON;
}
public override bool Raycast(RayCastOutput output, RayCastInput input, Transform xf, int childIndex)
{
Debug.Assert(childIndex < Count);
EdgeShape edgeShape = pool0;
int i1 = childIndex;
int i2 = childIndex + 1;
if (i2 == Count)
{
i2 = 0;
}
edgeShape.Vertex1.Set(Vertices[i1]);
edgeShape.Vertex2.Set(Vertices[i2]);
return edgeShape.Raycast(output, input, xf, 0);
}
public virtual void collide(Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB)
{
Transform.mulTransToOutUnsafe(xfA, xfB, m_xf);
Transform.mulToOutUnsafe(m_xf, polygonB.m_centroid, m_centroidB);
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;
edge1.set_Renamed(m_v2).subLocal(m_v1);
edge1.normalize();
m_normal1.set_Renamed(edge1.y, -edge1.x);
float offset1 = Vec2.dot(m_normal1, temp.set_Renamed(m_centroidB).subLocal(m_v1));
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
edge0.set_Renamed(m_v1).subLocal(m_v0);
edge0.normalize();
m_normal0.set_Renamed(edge0.y, -edge0.x);
convex1 = Vec2.cross(edge0, edge1) >= 0.0f;
offset0 = Vec2.dot(m_normal0, temp.set_Renamed(m_centroidB).subLocal(m_v0));
}
// Is there a following edge?
if (hasVertex3)
{
edge2.set_Renamed(m_v3).subLocal(m_v2);
edge2.normalize();
m_normal2.set_Renamed(edge2.y, -edge2.x);
convex2 = Vec2.cross(edge1, edge2) > 0.0f;
offset2 = Vec2.dot(m_normal2, temp.set_Renamed(m_centroidB).subLocal(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.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal0);
m_upperLimit.set_Renamed(m_normal2);
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal1).negateLocal();
m_upperLimit.set_Renamed(m_normal1).negateLocal();
}
}
else if (convex1)
{
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal0);
m_upperLimit.set_Renamed(m_normal1);
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal2).negateLocal();
m_upperLimit.set_Renamed(m_normal1).negateLocal();
}
}
else if (convex2)
{
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal1);
m_upperLimit.set_Renamed(m_normal2);
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal1).negateLocal();
m_upperLimit.set_Renamed(m_normal0).negateLocal();
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal1);
m_upperLimit.set_Renamed(m_normal1);
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal2).negateLocal();
m_upperLimit.set_Renamed(m_normal0).negateLocal();
}
}
}
else if (hasVertex0)
{
if (convex1)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal0);
m_upperLimit.set_Renamed(m_normal1).negateLocal();
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal1);
m_upperLimit.set_Renamed(m_normal1).negateLocal();
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal1);
m_upperLimit.set_Renamed(m_normal1).negateLocal();
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal1);
m_upperLimit.set_Renamed(m_normal0).negateLocal();
}
}
}
else if (hasVertex3)
{
if (convex2)
{
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal1).negateLocal();
m_upperLimit.set_Renamed(m_normal2);
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal1).negateLocal();
m_upperLimit.set_Renamed(m_normal1);
}
}
else
{
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal1).negateLocal();
m_upperLimit.set_Renamed(m_normal1);
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal2).negateLocal();
m_upperLimit.set_Renamed(m_normal1);
}
}
}
else
{
m_front = offset1 >= 0.0f;
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal1).negateLocal();
m_upperLimit.set_Renamed(m_normal1).negateLocal();
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal1);
m_upperLimit.set_Renamed(m_normal1);
}
}
// Get polygonB in frameA
m_polygonB.count = polygonB.m_count;
for (int i = 0; i < polygonB.m_count; ++i)
{
Transform.mulToOutUnsafe(m_xf, polygonB.m_vertices[i], m_polygonB.vertices[i]);
Rot.mulToOutUnsafe(m_xf.q, polygonB.m_normals[i], m_polygonB.normals[i]);
}
m_radius = 2.0f * Settings.polygonRadius;
manifold.pointCount = 0;
computeEdgeSeparation(edgeAxis);
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.type == EPAxis.Type.UNKNOWN)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
computePolygonSeparation(polygonAxis);
if (polygonAxis.type != EPAxis.Type.UNKNOWN && polygonAxis.separation > m_radius)
{
return;
}
// Use hysteresis for jitter reduction.
float k_relativeTol = 0.98f;
float k_absoluteTol = 0.001f;
EPAxis primaryAxis;
if (polygonAxis.type == EPAxis.Type.UNKNOWN)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
// ClipVertex[] ie = new ClipVertex[2];
if (primaryAxis.type == EPAxis.Type.EDGE_A)
{
manifold.type = Manifold.ManifoldType.FACE_A;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Vec2.dot(m_normal, m_polygonB.normals[0]);
for (int i = 1; i < m_polygonB.count; ++i)
{
float value_Renamed = Vec2.dot(m_normal, m_polygonB.normals[i]);
if (value_Renamed < bestValue)
{
bestValue = value_Renamed;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie[0].v.set_Renamed(m_polygonB.vertices[i1]);
ie[0].id.indexA = 0;
ie[0].id.indexB = (sbyte)i1;
ie[0].id.typeA = (sbyte)ContactID.Type.FACE;
ie[0].id.typeB = (sbyte)ContactID.Type.VERTEX;
ie[1].v.set_Renamed(m_polygonB.vertices[i2]);
ie[1].id.indexA = 0;
ie[1].id.indexB = (sbyte)i2;
ie[1].id.typeA = (sbyte)ContactID.Type.FACE;
ie[1].id.typeB = (sbyte)ContactID.Type.VERTEX;
if (m_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1.set_Renamed(m_v1);
rf.v2.set_Renamed(m_v2);
rf.normal.set_Renamed(m_normal1);
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1.set_Renamed(m_v2);
rf.v2.set_Renamed(m_v1);
rf.normal.set_Renamed(m_normal1).negateLocal();
}
}
else
{
manifold.type = Manifold.ManifoldType.FACE_B;
ie[0].v.set_Renamed(m_v1);
ie[0].id.indexA = 0;
ie[0].id.indexB = (sbyte)primaryAxis.index;
ie[0].id.typeA = (sbyte)ContactID.Type.VERTEX;
ie[0].id.typeB = (sbyte)ContactID.Type.FACE;
ie[1].v.set_Renamed(m_v2);
ie[1].id.indexA = 0;
ie[1].id.indexB = (sbyte)primaryAxis.index;
ie[1].id.typeA = (sbyte)ContactID.Type.VERTEX;
ie[1].id.typeB = (sbyte)ContactID.Type.FACE;
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1.set_Renamed(m_polygonB.vertices[rf.i1]);
rf.v2.set_Renamed(m_polygonB.vertices[rf.i2]);
rf.normal.set_Renamed(m_polygonB.normals[rf.i1]);
}
rf.sideNormal1.set_Renamed(rf.normal.y, -rf.normal.x);
rf.sideNormal2.set_Renamed(rf.sideNormal1).negateLocal();
rf.sideOffset1 = Vec2.dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = Vec2.dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
int np;
// Clip to box side 1
np = clipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
if (np < Settings.maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = clipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
if (np < Settings.maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == EPAxis.Type.EDGE_A)
{
manifold.localNormal.set_Renamed(rf.normal);
manifold.localPoint.set_Renamed(rf.v1);
}
else
{
manifold.localNormal.set_Renamed(polygonB.m_normals[rf.i1]);
manifold.localPoint.set_Renamed(polygonB.m_vertices[rf.i1]);
}
int pointCount = 0;
for (int i = 0; i < Settings.maxManifoldPoints; ++i)
{
float separation;
separation = Vec2.dot(rf.normal, temp.set_Renamed(clipPoints2[i].v).subLocal(rf.v1));
if (separation <= m_radius)
{
ManifoldPoint cp = manifold.points[pointCount];
if (primaryAxis.type == EPAxis.Type.EDGE_A)
{
// cp.localPoint = MulT(m_xf, clipPoints2[i].v);
Transform.mulTransToOutUnsafe(m_xf, clipPoints2[i].v, cp.localPoint);
cp.id.set_Renamed(clipPoints2[i].id);
}
else
{
cp.localPoint.set_Renamed(clipPoints2[i].v);
cp.id.typeA = clipPoints2[i].id.typeB;
cp.id.typeB = clipPoints2[i].id.typeA;
cp.id.indexA = clipPoints2[i].id.indexB;
cp.id.indexB = clipPoints2[i].id.indexA;
}
++pointCount;
}
}
manifold.pointCount = pointCount;
}
public override void evaluate(Manifold manifold, Transform xfA, Transform xfB)
{
pool.getCollision().collideCircles(manifold, (CircleShape)m_fixtureA.Shape, xfA, (CircleShape)m_fixtureB.Shape, xfB);
}
/// <summary>
/// Compute the collision manifold between two circles.
/// </summary>
/// <param name="manifold"></param>
/// <param name="circle1"></param>
/// <param name="xfA"></param>
/// <param name="circle2"></param>
/// <param name="xfB"></param>
public void collideCircles(Manifold manifold, CircleShape circle1, Transform xfA, CircleShape circle2, Transform xfB)
{
manifold.pointCount = 0;
// before inline:
Transform.mulToOut(xfA, circle1.m_p, pA);
Transform.mulToOut(xfB, circle2.m_p, pB);
d.set_Renamed(pB).subLocal(pA);
float distSqr = d.x * d.x + d.y * d.y;
// after inline:
// final Vec2 v = circle1.m_p;
// final float pAy = xfA.p.y + xfA.q.ex.y * v.x + xfA.q.ey.y * v.y;
// final float pAx = xfA.p.x + xfA.q.ex.x * v.x + xfA.q.ey.x * v.y;
//
// final Vec2 v1 = circle2.m_p;
// final float pBy = xfB.p.y + xfB.q.ex.y * v1.x + xfB.q.ey.y * v1.y;
// final float pBx = xfB.p.x + xfB.q.ex.x * v1.x + xfB.q.ey.x * v1.y;
//
// final float dx = pBx - pAx;
// final float dy = pBy - pAy;
//
// final float distSqr = dx * dx + dy * dy;
// end inline
float radius = circle1.m_radius + circle2.m_radius;
if (distSqr > radius * radius)
{
return;
}
manifold.type = Manifold.ManifoldType.CIRCLES;
manifold.localPoint.set_Renamed(circle1.m_p);
manifold.localNormal.setZero();
manifold.pointCount = 1;
manifold.points[0].localPoint.set_Renamed(circle2.m_p);
manifold.points[0].id.zero();
}
public override void Evaluate(Manifold manifold, Transform xfA, Transform xfB)
{
Pool.GetCollision().CollideEdgeAndPolygon(manifold, (EdgeShape)FixtureA.Shape, xfA, (PolygonShape)FixtureB.Shape, xfB);
}
/// <summary>
/// Get the interpolated transform at a specific time.
/// </summary>
/// <param name="xf">the result is placed here - must not be null</param>
/// <param name="beta">the normalized time in [0,1].</param>
public void GetTransform(Transform xf, float beta)
{
Debug.Assert(xf != null);
// if (xf == null)
// xf = new XForm();
// center = p + R * localCenter
/*
* if (1.0f - t0 > Settings.EPSILON) { float alpha = (t - t0) / (1.0f - t0); xf.position.x =
* (1.0f - alpha) * c0.x + alpha * c.x; xf.position.y = (1.0f - alpha) * c0.y + alpha * c.y;
* float angle = (1.0f - alpha) * a0 + alpha * a; xf.R.set(angle); } else { xf.position.set(c);
* xf.R.set(a); }
*/
xf.P.X = (1.0f - beta) * C0.X + beta * C.X;
xf.P.Y = (1.0f - beta) * C0.Y + beta * C.Y;
// float angle = (1.0f - alpha) * a0 + alpha * a;
// xf.R.set(angle);
xf.Q.Set((1.0f - beta) * A0 + beta * A);
// Shift to origin
//xf->p -= b2Mul(xf->q, localCenter);
Rot q = xf.Q;
xf.P.X -= (q.Cos * LocalCenter.X - q.Sin * LocalCenter.Y);
xf.P.Y -= (q.Sin * LocalCenter.X + q.Cos * LocalCenter.Y);
}
