hB.scale(0.5f);
//Vector2f hB = MathUtil.scale(((Box) bodyB.getBodyShape()).getSize(), 0.5f);
//Vector2f hA = MathUtil.scale(bodyA.getSize(), 0.5f);
//Vector2f hB = MathUtil.scale(bodyB.getSize(), 0.5f);
ROVector2f posA = bodyA.getPosition();
ROVector2f posB = bodyB.getPosition();
Matrix2f rotA = new Matrix2f(bodyA.getRotation());
Matrix2f rotB = new Matrix2f(bodyB.getRotation());
Matrix2f RotAT = rotA.transpose();
Matrix2f RotBT = rotB.transpose();
// unused?
// 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);