// Vector2f a1 = rotA.col1;
// Vector2f a2 = rotA.col2;
// Vector2f b1 = rotB.col1;
// Vector2f b2 = rotB.col2;
Vector2f dp = MathUtil.sub(posB,posA);
Vector2f dA = MathUtil.mul(RotAT,dp);
Vector2f dB = MathUtil.mul(RotBT,dp);
Matrix2f C = MathUtil.mul(RotAT,rotB);
Matrix2f absC = MathUtil.abs(C);
Matrix2f absCT = absC.transpose();
// Box A faces
Vector2f faceA = MathUtil.abs(dA);
faceA.sub(hA);
faceA.sub(MathUtil.mul(absC,hB));
if (faceA.x > 0.0f || faceA.y > 0.0f) {
return 0;
}
// Box B faces
Vector2f faceB = MathUtil.abs(dB);
faceB.sub(MathUtil.mul(absCT,hA));
faceB.sub(hB);
//MathUtil.sub(MathUtil.sub(MathUtil.abs(dB),MathUtil.mul(absCT,hA)),hB);
if (faceB.x > 0.0f || faceB.y > 0.0f) {
return 0;
}
// Find best axis
int axis;
float separation;
Vector2f normal;
// Box A faces
axis = FACE_A_X;
separation = faceA.x;
normal = dA.x > 0.0f ? rotA.col1 : MathUtil.scale(rotA.col1,-1);
if (faceA.y > 1.05f * separation + 0.01f * hA.y)
{
axis = FACE_A_Y;
separation = faceA.y;
normal = dA.y > 0.0f ? rotA.col2 : MathUtil.scale(rotA.col2,-1);
}
// Box B faces
if (faceB.x > 1.05f * separation + 0.01f * hB.x)
{
axis = FACE_B_X;
separation = faceB.x;
normal = dB.x > 0.0f ? rotB.col1 : MathUtil.scale(rotB.col1,-1);
}
if (faceB.y > 1.05f * separation + 0.01f * hB.y)
{
axis = FACE_B_Y;
separation = faceB.y;
normal = dB.y > 0.0f ? rotB.col2 : MathUtil.scale(rotB.col2,-1);
}
// Setup clipping plane data based on the separating axis
Vector2f frontNormal, sideNormal;
ClipVertex[] incidentEdge = new ClipVertex[] {new ClipVertex(), new ClipVertex()};
float front, negSide, posSide;
char negEdge, posEdge;
// Compute the clipping lines and the line segment to be clipped.
switch (axis)
{
case FACE_A_X:
{
frontNormal = normal;
front = posA.dot(frontNormal) + hA.x;
sideNormal = rotA.col2;
float side = posA.dot(sideNormal);
negSide = -side + hA.y;
posSide = side + hA.y;
negEdge = EDGE3;
posEdge = EDGE1;
computeIncidentEdge(incidentEdge, hB, posB, rotB, frontNormal);
}
break;
case FACE_A_Y:
{
frontNormal = normal;
front = posA.dot(frontNormal) + hA.y;
sideNormal = rotA.col1;
float side = posA.dot(sideNormal);
negSide = -side + hA.x;
posSide = side + hA.x;
negEdge = EDGE2;
posEdge = EDGE4;
computeIncidentEdge(incidentEdge, hB, posB, rotB, frontNormal);
}
break;
case FACE_B_X:
{
frontNormal = MathUtil.scale(normal,-1);
front = posB.dot(frontNormal) + hB.x;
sideNormal = rotB.col2;
float side = posB.dot(sideNormal);
negSide = -side + hB.y;
posSide = side + hB.y;
negEdge = EDGE3;
posEdge = EDGE1;
computeIncidentEdge(incidentEdge, hA, posA, rotA, frontNormal);
}
break;
case FACE_B_Y:
{
frontNormal = MathUtil.scale(normal,-1);
front = posB.dot(frontNormal) + hB.y;
sideNormal = rotB.col1;
float side = posB.dot(sideNormal);
negSide = -side + hB.x;
posSide = side + hB.x;
negEdge = EDGE2;
posEdge = EDGE4;
computeIncidentEdge(incidentEdge, hA, posA, rotA, frontNormal);
}
break;
default:
throw new RuntimeException("Unknown face!");
}
// clip other face with 5 box planes (1 face plane, 4 edge planes)
ClipVertex[] clipPoints1 = new ClipVertex[] {new ClipVertex(), new ClipVertex()};
ClipVertex[] clipPoints2 = new ClipVertex[] {new ClipVertex(), new ClipVertex()};
int np;
// Clip to box side 1
np = clipSegmentToLine(clipPoints1, incidentEdge, MathUtil.scale(sideNormal,-1), negSide, negEdge);
if (np < 2)
return 0;
// Clip to negative box side 1
np = clipSegmentToLine(clipPoints2, clipPoints1, sideNormal, posSide, posEdge);
if (np < 2)
return 0;
// Now clipPoints2 contains the clipping points.
// Due to roundoff, it is possible that clipping removes all points.
int numContacts = 0;
for (int i = 0; i < 2; ++i)
{
float separation2 = frontNormal.dot(clipPoints2[i].v) - front;
if (separation2 <= 0)
{
contacts[numContacts].setSeparation(separation2);
contacts[numContacts].setNormal(normal);