public static int Orthographic(double sign, double e, int axis, byte clipEdge, ClipVertex[] input, int inCount, ClipVertex[] result)
{
int resultCount = 0;
ClipVertex a = input[inCount - 1];
for (int i = 0; i < inCount; ++i)
{
ClipVertex b = input[i];
double da = sign * a.v[axis] - e;
double db = sign * b.v[axis] - e;
ClipVertex cv = new ClipVertex();
// B
if (((InFront(da) && InFront(db)) || On(da) || On(db)))
{
Assert(resultCount < 8);
result[resultCount++] = b;
}
// I
else if (InFront(da) && Behind(db))
{
cv.f = b.f;
cv.v = a.v + (b.v - a.v) * (da / (da - db));
cv.f.outR = clipEdge;
cv.f.outI = 0;
Assert(resultCount < 8);
result[resultCount++] = cv;
}
// I, B
else if (Behind(da) && InFront(db))
{
cv.f = a.f;
cv.v = a.v + (b.v - a.v) * (da / (da - db));
cv.f.inR = clipEdge;
cv.f.inI = 0;
Assert(resultCount < 8);
result[resultCount++] = cv;
Assert(resultCount < 8);
result[resultCount++] = b;
}
a = b;
}
return(resultCount);
}
public static void FindIncidentEdge(out ClipVertex[] c, PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2)
{
int count1 = poly1.VertexCount;
Vector2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
Vector2[] vertices2 = poly2.GetVertices();
Vector2[] normals2 = poly2.Normals;
//Box2DXDebug.Assert(0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Vector2 normal1 = CommonMath.MulT(xf2.R, CommonMath.Mul(xf1.R, normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = Settings.FLT_MAX;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(normal1, normals2[i]);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c = new ClipVertex[2];
c[0].V = CommonMath.Mul(xf2, vertices2[i1]);
c[0].ID.Features.ReferenceEdge = (byte)edge1;
c[0].ID.Features.IncidentEdge = (byte)i1;
c[0].ID.Features.IncidentVertex = 0;
c[1].V = CommonMath.Mul(xf2, vertices2[i2]);
c[1].ID.Features.ReferenceEdge = (byte)edge1;
c[1].ID.Features.IncidentEdge = (byte)i2;
c[1].ID.Features.IncidentVertex = 1;
}
public static int ClipSegmentToLine(out ClipVertex[] vOut, ClipVertex[] vIn, Vector2 normal, float offset)
{
if (vIn.Length != 2)
{
//Box2DXDebug.ThrowBox2DXException("vIn should contain 2 element, but contains " + vIn.Length.ToString());
}
vOut = new ClipVertex[2];
// Start with no output points
int numOut = 0;
// Calculate the distance of end points to the line
float distance0 = Vector2.Dot(normal, vIn[0].V) - offset;
float distance1 = Vector2.Dot(normal, vIn[1].V) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f)
{
vOut[numOut++] = vIn[0];
}
if (distance1 <= 0.0f)
{
vOut[numOut++] = vIn[1];
}
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
// Find intersection point of edge and plane
float interp = distance0 / (distance0 - distance1);
vOut[numOut].V = vIn[0].V + interp * (vIn[1].V - vIn[0].V);
if (distance0 > 0.0f)
{
vOut[numOut].ID = vIn[0].ID;
}
else
{
vOut[numOut].ID = vIn[1].ID;
}
++numOut;
}
return(numOut);
}
public Collision(IWorldPool argPool)
{
incidentEdge[0] = new ClipVertex();
incidentEdge[1] = new ClipVertex();
clipPoints1[0] = new ClipVertex();
clipPoints1[1] = new ClipVertex();
clipPoints2[0] = new ClipVertex();
clipPoints2[1] = new ClipVertex();
pool = argPool;
}
// 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;
}
/// <summary>
/// Clipping for contact manifolds. Sutherland-Hodgman clipping.
/// </summary>
/// <param name="vOut"></param>
/// <param name="vIn"></param>
/// <param name="normal"></param>
/// <param name="offset"></param>
/// <returns></returns>
public static int clipSegmentToLine(ClipVertex[] vOut, ClipVertex[] vIn, Vec2 normal, float offset, int vertexIndexA)
{
// Start with no output points
int numOut = 0;
// Calculate the distance of end points to the line
float distance0 = Vec2.dot(normal, vIn[0].v) - offset;
float distance1 = Vec2.dot(normal, vIn[1].v) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f)
{
vOut[numOut++].set_Renamed(vIn[0]);
}
if (distance1 <= 0.0f)
{
vOut[numOut++].set_Renamed(vIn[1]);
}
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
// Find intersection point of edge and plane
float interp = distance0 / (distance0 - distance1);
// vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
vOut[numOut].v.set_Renamed(vIn[1].v).subLocal(vIn[0].v).mulLocal(interp).addLocal(vIn[0].v);
// VertexA is hitting edgeB.
vOut[numOut].id.indexA = (sbyte)vertexIndexA;
vOut[numOut].id.indexB = vIn[0].id.indexB;
vOut[numOut].id.typeA = (sbyte)ContactID.Type.VERTEX;
vOut[numOut].id.typeB = (sbyte)ContactID.Type.FACE;
++numOut;
}
return numOut;
}
public EPCollider()
{
for (int i = 0; i < 2; i++)
{
ie[i] = new ClipVertex();
clipPoints1[i] = new ClipVertex();
clipPoints2[i] = new ClipVertex();
}
}
public virtual void set_Renamed(ClipVertex cv)
{
v.set_Renamed(cv.v);
id.set_Renamed(cv.id);
}
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);
ClipVertex c0 = c[0];
ClipVertex c1 = c[1];
Rot xf1q = xf1.q;
Rot xf2q = xf2.q;
// Get the normal of the reference edge in poly2's frame.
// Vec2 normal1 = MulT(xf2.R, Mul(xf1.R, normals1[edge1]));
// before inline:
// Rot.mulToOutUnsafe(xf1.q, normals1[edge1], normal1); // temporary
// Rot.mulTrans(xf2.q, normal1, normal1);
// after inline:
Vec2 v = normals1[edge1];
float tempx = xf1q.c*v.x - xf1q.s*v.y;
float tempy = xf1q.s*v.x + xf1q.c*v.y;
float normal1x = xf2q.c*tempx + xf2q.s*tempy;
float normal1y = -xf2q.s*tempx + xf2q.c*tempy;
// end inline
// Find the incident edge on poly2.
int index = 0;
float minDot = float.MaxValue;
for (int i = 0; i < count2; ++i)
{
Vec2 b = normals2[i];
float dot = normal1x*b.x + normal1y*b.y;
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;
// c0.v = Mul(xf2, vertices2[i1]);
Vec2 v1 = vertices2[i1];
Vec2 @ref = c0.v;
@ref.x = (xf2q.c*v1.x - xf2q.s*v1.y) + xf2.p.x;
@ref.y = (xf2q.s*v1.x + xf2q.c*v1.y) + xf2.p.y;
c0.v = @ref;
c0.id.indexA = (byte) edge1;
c0.id.indexB = (byte) i1;
c0.id.typeA = (byte) ContactID.Type.FACE;
c0.id.typeB = (byte) ContactID.Type.VERTEX;
// c1.v = Mul(xf2, vertices2[i2]);
Vec2 v2 = vertices2[i2];
Vec2 out1 = c1.v;
out1.x = (xf2q.c*v2.x - xf2q.s*v2.y) + xf2.p.x;
out1.y = (xf2q.s*v2.x + xf2q.c*v2.y) + xf2.p.y;
c1.id.indexA = (byte) edge1;
c1.id.indexB = (byte) i2;
c1.id.typeA = (byte) ContactID.Type.FACE;
c1.id.typeB = (byte) ContactID.Type.VERTEX;
}
/**
* Clipping for contact manifolds. Sutherland-Hodgman clipping.
*
* @param vOut
* @param vIn
* @param normal
* @param offset
* @return
*/
public static int clipSegmentToLine(ClipVertex[] vOut, ClipVertex[] vIn,
Vec2 normal, float offset, int vertexIndexA)
{
// Start with no output points
int numOut = 0;
ClipVertex vIn0 = vIn[0];
ClipVertex vIn1 = vIn[1];
Vec2 vIn0v = vIn0.v;
Vec2 vIn1v = vIn1.v;
// Calculate the distance of end points to the line
float distance0 = Vec2.dot(normal, vIn0v) - offset;
float distance1 = Vec2.dot(normal, vIn1v) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f)
{
vOut[numOut++].set(vIn0);
}
if (distance1 <= 0.0f)
{
vOut[numOut++].set(vIn1);
}
// If the points are on different sides of the plane
if (distance0*distance1 < 0.0f)
{
// Find intersection point of edge and plane
float interp = distance0/(distance0 - distance1);
ClipVertex vOutNO = vOut[numOut];
// vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
vOutNO.v.x = vIn0v.x + interp*(vIn1v.x - vIn0v.x);
vOutNO.v.y = vIn0v.y + interp*(vIn1v.y - vIn0v.y);
// VertexA is hitting edgeB.
vOutNO.id.indexA = (byte) vertexIndexA;
vOutNO.id.indexB = vIn0.id.indexB;
vOutNO.id.typeA = (byte) ContactID.Type.VERTEX;
vOutNO.id.typeB = (byte) ContactID.Type.FACE;
++numOut;
}
return numOut;
}
public void set(ClipVertex cv)
{
Vec2 v1 = cv.v;
v.x = v1.x;
v.y = v1.y;
ContactID c = cv.id;
id.indexA = c.indexA;
id.indexB = c.indexB;
id.typeA = c.typeA;
id.typeB = c.typeB;
}
/// <summary>
///
/// </summary>
/// <param name="contacts"> A 和 B 的接触点</param>
/// <param name="bodyA"></param>
/// <param name="bodyB"></param>
/// <returns></returns>
public static int Collide(ref Contact[] contacts, Body bodyA, Body bodyB)
{
// 初始化
Vector2f hA = 0.5f * bodyA.width_height;
Vector2f hB = 0.5f * bodyB.width_height;
Vector2f posA = bodyA.position;
Vector2f posB = bodyB.position;
Mat22 RotA = new Mat22(bodyA.rotation);
Mat22 RotB = new Mat22(bodyB.rotation);
Mat22 RotAT = RotA.Transpose();
Mat22 RotBT = RotB.Transpose();
Vector2f dp = posB - posA;
Vector2f dA = RotAT * dp;
Vector2f dB = RotBT * dp;
Mat22 C = RotAT * RotB;
Mat22 absC = MyMath.Abs(C);
Mat22 absCT = absC.Transpose();
//dA是两个矩形终点连线的向量，做了Abs运算后，dA表示的只是数值意义。
//实际上，下面的代码是Abs(dA) - (hA + absC * hB)
//hA就是矩形A右上角的点，absC * hB就是求出B点在A矩形的局部坐标系下的四个顶点中
//x,y方向上的最大值，因为dA，hA,hB都是从(0,0)点出发的向量，所以向量中的数值仅仅
//表示数值意义
//Abs(dA) - (hA + absC * hB)对于这个式子，如果只看x轴
//实际上就是dA的x坐标表示两个矩形的中心连线在x轴投影长度为s，
//hA是矩形A的x方向在中心连线的最大投影长度a,absC*hB就是矩形B的x方向在中心连线的最大投影长度b
//如果s-a-b大于0，这说明投影不相交。
// Box A faces
Vector2f faceA = MyMath.Abs(dA) - hA - absC * hB;
if (faceA.x > 0.0f || faceA.y > 0.0f)
{
return(0);
}
// Box B facesa
Vector2f faceB = MyMath.Abs(dB) - absCT * hA - hB;
if (faceB.x > 0.0f || faceB.y > 0.0f)
{
return(0);
}
// 找到最佳碰撞轴
Axis axis;
float separation;
// Box A faces
axis = Axis.FACE_A_X;
separation = faceA.x;
Vector2f normal = dA.x > 0.0f ? RotA.ex : -RotA.ex;
const float relativeTol = 0.95f;
const float absoluteTol = 0.05f;
if (faceA.y > relativeTol * separation + absoluteTol * hA.y)
{
axis = Axis.FACE_A_Y;
separation = faceA.y;
normal = dA.y > 0.0f ? RotA.ey : -RotA.ey;
}
// Box B faces
if (faceB.x > relativeTol * separation + absoluteTol * hB.x)
{
axis = Axis.FACE_B_X;
separation = faceB.x;
normal = dB.x > 0.0f ? RotB.ex : -RotB.ex;
}
if (faceB.y > relativeTol * separation + absoluteTol * hB.y)
{
axis = Axis.FACE_B_Y;
separation = faceB.y;
normal = dB.y > 0.0f ? RotB.ey : -RotB.ey;
}
// 根据分离轴初始化分离平面
Vector2f frontNormal = new Vector2f();
Vector2f sideNormal = new Vector2f();
ClipVertex[] incidentEdge = new ClipVertex[2];
incidentEdge[0] = new ClipVertex();
incidentEdge[1] = new ClipVertex();
float front = 0.0f, negSide = 0.0f, posSide = 0.0f;
char negEdge = (char)EdgeNumbers.NO_EDGE, posEdge = (char)EdgeNumbers.NO_EDGE;
// 计算分离线和要分离的线段
switch (axis)
{
case Axis.FACE_A_X:
{
frontNormal = normal;
front = MyMath.Dot(posA, frontNormal) + hA.x;
sideNormal = RotA.ey;
float side = MyMath.Dot(posA, sideNormal);
negSide = -side + hA.y;
posSide = side + hA.y;
negEdge = (char)EdgeNumbers.EDGE3;
posEdge = (char)EdgeNumbers.EDGE1;
ComputeIncidentEdge(ref incidentEdge, hB, posB, RotB, frontNormal);
}
break;
case Axis.FACE_A_Y:
{
frontNormal = normal;
front = MyMath.Dot(posA, frontNormal) + hA.y;
sideNormal = RotA.ex;
float side = MyMath.Dot(posA, sideNormal);
negSide = -side + hA.x;
posSide = side + hA.x;
negEdge = (char)EdgeNumbers.EDGE2;
posEdge = (char)EdgeNumbers.EDGE4;
ComputeIncidentEdge(ref incidentEdge, hB, posB, RotB, frontNormal);
}
break;
case Axis.FACE_B_X:
{
frontNormal = -normal;
front = MyMath.Dot(posB, frontNormal) + hB.x;
sideNormal = RotB.ey;
float side = MyMath.Dot(posB, sideNormal);
negSide = -side + hB.y;
posSide = side + hB.y;
negEdge = (char)EdgeNumbers.EDGE3;
posEdge = (char)EdgeNumbers.EDGE1;
ComputeIncidentEdge(ref incidentEdge, hA, posA, RotA, frontNormal);
}
break;
case Axis.FACE_B_Y:
{
frontNormal = -normal;
front = MyMath.Dot(posB, frontNormal) + hB.y;
sideNormal = RotB.ex;
float side = MyMath.Dot(posB, sideNormal);
negSide = -side + hB.x;
posSide = side + hB.x;
negEdge = (char)EdgeNumbers.EDGE2;
posEdge = (char)EdgeNumbers.EDGE4;
ComputeIncidentEdge(ref incidentEdge, hA, posA, RotA, frontNormal);
}
break;
}
// 分离其他面
ClipVertex[] clipPoints1 = new ClipVertex[2];
clipPoints1[0] = new ClipVertex();
clipPoints1[1] = new ClipVertex();
ClipVertex[] clipPoints2 = new ClipVertex[2];
clipPoints2[0] = new ClipVertex();
clipPoints2[1] = new ClipVertex();
int np;
// Clip to box side 1
np = ClipSegmentToLine(ref clipPoints1, ref incidentEdge, -sideNormal, negSide, negEdge);
if (np < 2)
{
return(0);
}
// Clip to negative box side 1
np = ClipSegmentToLine(ref clipPoints2, ref clipPoints1, sideNormal, posSide, posEdge);
if (np < 2)
{
return(0);
}
// Now clipPoints2 contains the clipping points.
// 由于舍入，分离可能删去所有点
int numContacts = 0;
for (int i = 0; i < 2; ++i)
{
separation = MyMath.Dot(frontNormal, clipPoints2[i].v) - front;
if (separation <= 0)
{
contacts[numContacts].separation = separation;
contacts[numContacts].normal = normal;
// slide contact point onto reference face (easy to cull)
contacts[numContacts].position = clipPoints2[i].v - separation * frontNormal;
contacts[numContacts].feature = clipPoints2[i].fp;
if (axis == Axis.FACE_B_X || axis == Axis.FACE_B_Y)
{
Flip(contacts[numContacts].feature);
}
++numContacts;
}
}
return(numContacts);
}
public void Set(ClipVertex cv)
{
V.Set(cv.V);
Id.Set(cv.Id);
}
