void ContactGenerator::box_halfspace(const CollisionShape &a, const RigidBody &rbA, const CollisionShape &b, const RigidBody &rbB, ContactManifold &contactManifold)
{
// Make sure we have contacts left and Check if they're the right shapes.
if (a.getShapeType() != SHAPE_BOX || b.getShapeType() != SHAPE_HALFSPACE ) { return; }
// then typecast their appropriate shapes
const ShapeBox& box = (const ShapeBox&)a;
const ShapeHalfspace& halfspace = (const ShapeHalfspace&)b;
const Vec3& boxHalfExtents = box.getHalfExtents();
// generate a array of all of the box's vertices
Vec3 boxVertex[8] =
{
Vec3(-boxHalfExtents.x, -boxHalfExtents.y, -boxHalfExtents.z),
Vec3(-boxHalfExtents.x, -boxHalfExtents.y, +boxHalfExtents.z),
Vec3(-boxHalfExtents.x, +boxHalfExtents.y, -boxHalfExtents.z),
Vec3(-boxHalfExtents.x, +boxHalfExtents.y, +boxHalfExtents.z),
Vec3(+boxHalfExtents.x, -boxHalfExtents.y, -boxHalfExtents.z),
Vec3(+boxHalfExtents.x, -boxHalfExtents.y, +boxHalfExtents.z),
Vec3(+boxHalfExtents.x, +boxHalfExtents.y, -boxHalfExtents.z),
Vec3(+boxHalfExtents.x, +boxHalfExtents.y, +boxHalfExtents.z),
};
const Transform& boxTransform = rbA.getTransform();
// Apply collision object offset
for (int i = 0; i < 8; i++)
{
boxVertex[i] = boxTransform * a.getOffset() * boxVertex[i];
}
// Calculate halfspace's position and normal
Vec3 posHalfspace = b.getOffset().getPosition() + rbB.getPosition();
Vec3 normHalfspace = rbB.getTransform() * b.getOffset() * Vec3(0.f, 1.f, 0.f, 0.f);
// Check each vertice for intersection with the halfspace
Scalar vertexDistance;
unsigned int numContacts = 0;
for (int i = 0; i < 8; i++)
{
vertexDistance = boxVertex[i].dot(normHalfspace) - posHalfspace.y;
if(vertexDistance <= 0)
{
// Create contact data
ContactPoint newContact;
newContact.normal = normHalfspace;
newContact.penetration = -vertexDistance;
newContact.position = boxVertex[i] + newContact.normal*(newContact.penetration*0.5f);
contactManifold.addContactPoint(newContact);
}
}
return;
}
void ContactGenerator::sphere_halfspace(const CollisionShape &a, const RigidBody &rbA, const CollisionShape &b, const RigidBody &rbB, ContactManifold &contactManifold)
{
// Make sure we have contacts left and Check if they're the right shapes.
if ( a.getShapeType() != SHAPE_SPHERE || b.getShapeType() != SHAPE_HALFSPACE ) { return; }
// then typecast their appropriate shapes
const ShapeSphere& sphere = (const ShapeSphere&)a;
const ShapeHalfspace& halfspace = (const ShapeHalfspace&)b;
// Get the positions and halfspace normal
Vec3 posSphere = a.getOffset().getPosition() + rbA.getTransform().getPosition();
Vec3 posHalfspace = a.getOffset().getPosition() + rbB.getTransform().getPosition();;
Vec3 normHalfspace = rbB.getTransform() * b.getOffset() * Vec3(0.f, 1.f, 0.f, 0.f);
// Find the distance from the plane to the sphere
Scalar distance = normHalfspace.dot(posSphere) - sphere.getRadius() - (posHalfspace).y;
// Check collision
if(distance < 0)
{
// Create contact data
ContactPoint newContact;
newContact.normal = normHalfspace;
newContact.position = posSphere + -newContact.normal*(sphere.getRadius() - newContact.penetration/2);
newContact.penetration = -distance;
contactManifold.addContactPoint(newContact);
}
return;
}
void ContactGenerator::sphere_sphere(const CollisionShape &a, const RigidBody &rbA, const CollisionShape &b, const RigidBody &rbB, ContactManifold &contactManifold)
{
// Make sure we have contacts left and Check if they're spheres.
if ( a.getShapeType() != SHAPE_SPHERE || b.getShapeType() != SHAPE_SPHERE ) { return; }
// then typecast their shape to a sphere
const ShapeSphere& shapeA = (const ShapeSphere&)a;
const ShapeSphere& shapeB = (const ShapeSphere&)b;
// Get the sphere positions
Vec3 posA = a.getOffset().getPosition() + rbA.getPosition();
Vec3 posB = b.getOffset().getPosition() + rbB.getPosition();
// Find the vector between the two object
Vec3 midLine = posA - posB;
Scalar distance = midLine.length();
// Check for collision
if (distance <= 0.0f || distance >= shapeA.getRadius() + shapeB.getRadius())
{
return;
}
// Create contact data
ContactPoint newContact;
newContact.normal = midLine * ( ((Scalar)1.0) / distance );
newContact.position = posA + (midLine * (Scalar)-0.5);
newContact.penetration = (shapeA.getRadius() + shapeA.getRadius() - distance);
contactManifold.addContactPoint(newContact);
}
void ContactGenerator::box_box(const CollisionShape &a, const RigidBody &rbA, const CollisionShape &b, const RigidBody &rbB, ContactManifold &contactManifold)
{
// Make sure we have contacts left and Check if they're boxes.
if ( a.getShapeType() != SHAPE_BOX || b.getShapeType() != SHAPE_BOX ) { return; }
// then typecast their appropriate shapes
const ShapeBox& boxA = (const ShapeBox&)a;
const ShapeBox& boxB = (const ShapeBox&)b;
const Vec3& boxAHalfExtents = boxA.getHalfExtents();
const Vec3& boxBHalfExtents = boxB.getHalfExtents();
Transform boxATransform = rbA.getTransform() * a.getOffset();
Transform boxBTransform = rbB.getTransform() * b.getOffset();
// Vector between box centres.
Vec3 separation = boxBTransform.getPosition() - boxATransform.getPosition();
Scalar smallestPen = SCALAR_MAX;
unsigned int bestPen = 0xffffffff;
// Check each axis, keeping track of the axis with the smallest penetration.
// Stops when if finds an axis without penetration.
if (!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(0), separation, 0, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(1), separation, 1, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(2), separation, 2, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxBTransform.getAxisVector(0), separation, 3, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxBTransform.getAxisVector(1), separation, 4, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxBTransform.getAxisVector(2), separation, 5, smallestPen, bestPen) )
{
return;
}
unsigned bestSingleAxis = bestPen;
if (!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(0).cross(boxBTransform.getAxisVector(0)), separation, 6, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(0).cross(boxBTransform.getAxisVector(1)), separation, 7, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(0).cross(boxBTransform.getAxisVector(2)), separation, 8, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(1).cross(boxBTransform.getAxisVector(0)), separation, 9, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(1).cross(boxBTransform.getAxisVector(1)), separation, 10, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(1).cross(boxBTransform.getAxisVector(2)), separation, 11, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(2).cross(boxBTransform.getAxisVector(0)), separation, 12, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(2).cross(boxBTransform.getAxisVector(1)), separation, 13, smallestPen, bestPen) ||
!tryAxis(boxA, boxATransform, boxB, boxBTransform, boxATransform.getAxisVector(2).cross(boxBTransform.getAxisVector(2)), separation, 14, smallestPen, bestPen) )
{
return;
}
// We've found a collision, and we know which of the axes gave the smallest penetration.
if (bestPen < 3)
{
// Vertex of boxB in face of boxA
// TODO: Do stuff
//std::cout << bestPen << " Vertex of boxB in face of boxA\n";
fillPointFaceBoxBox(boxA, boxATransform, boxB, boxBTransform, separation, contactManifold, bestPen, smallestPen, false);
return;
}
else if (bestPen < 6)
{
// Vertex of boxA in face of boxB
// TODO: Do stuff
//std::cout << bestPen << " Vertex of boxA in face of boxB\n";
fillPointFaceBoxBox(boxA, boxATransform, boxB, boxBTransform, -separation, contactManifold, bestPen-3, smallestPen, true);
return;
}
else
{
// Edge Edge contact.
//std::cout << "Colliding >= 6";
//std::cout << bestPen << " Edge-Edge\n";
// Find which axis.
bestPen -= 6;
unsigned int axisIndexA = bestPen / 3;
unsigned int axisIndexB = bestPen % 3;
Vec3 axisA = boxATransform.getAxisVector(axisIndexA);
Vec3 axisB = boxBTransform.getAxisVector(axisIndexB);
Vec3 axis = axisA.cross(axisB);
axis.normalize();
// Axis should point from box one to box two.
if ( axis.dot(separation) > 0 )
{
axis = -axis;
}
Vec3 ptOnEdgeA = boxAHalfExtents;
Vec3 ptOnEdgeB = -boxBHalfExtents;
for (unsigned int i = 0; i < 3; i++)
{
if (i == axisIndexA)
{
ptOnEdgeA[i] = 0;
}
else if (boxATransform.getAxisVector(i).dot(axis) > 0)
{
ptOnEdgeA[i] = -ptOnEdgeA[i];
}
if (i == axisIndexB)
{
ptOnEdgeB[i] = 0;
}
else if (boxBTransform.getAxisVector(i).dot(axis) > 0)
{
ptOnEdgeB[i] = -ptOnEdgeB[i];
}
}
// Transform the points into world coordinates.
ptOnEdgeA = boxATransform * ptOnEdgeA;
ptOnEdgeB = boxBTransform * ptOnEdgeB;
// Find the point of closest approach of the two
// line-segments.
Vec3 vertex = contactPoint(ptOnEdgeA, axisA, boxAHalfExtents.get(axisIndexA), ptOnEdgeB, axisB, boxBHalfExtents.get(axisIndexB), bestSingleAxis > 2);
// Create contact data
ContactPoint newContact;
newContact.normal = axis;
newContact.penetration = smallestPen;
newContact.position = vertex;
contactManifold.addContactPoint(newContact);
}
return;
}
