// TODO: Test!
// REALLY TODO: TEST. There definitely is a bug in here.
void CollisionSphere::genContactsB(const CollisionBox& b, std::vector<std::shared_ptr<IContact>>& o) const
{
// Get the sphere position in local coordinates of the box.
Vect3 transformedSpherePosition = b.getTransformMatrix().getInverse() * this->getPos();
// If the sphere is further away than the magnitude of the furthest distance
// a corner can be, early out:
if (transformedSpherePosition.squareMagnitude() > b.getSize().squareMagnitude())
return;
// Otherwise, find the closest point on the cube.
// For any axis, it will be the cube half length on that side if
// the sphere's position is further than the half length. If not, it'll
// just be the component on that side.
Vect3 closestBoxPoint = transformedSpherePosition;
closestBoxPoint._x = CLAMP(closestBoxPoint._x, -b.getSize()._x, b.getSize()._x);
closestBoxPoint._y = CLAMP(closestBoxPoint._y, -b.getSize()._y, b.getSize()._y);
closestBoxPoint._z = CLAMP(closestBoxPoint._z, -b.getSize()._z, b.getSize()._z);
// Now we have the closest point on the box.
// If it's closer than the radius, we have a contact
if ((closestBoxPoint - transformedSpherePosition).squareMagnitude() < this->getRadius() * this->getRadius()
&& ((closestBoxPoint - transformedSpherePosition).squareMagnitude() > 0.f))
{
// Box contact data: Contact is under the surface of the sphere, pointing directly out.
Vect3Normal d = (closestBoxPoint - transformedSpherePosition);
Vect3 collisionPoint_l = transformedSpherePosition + d * _radius;
Vect3 penetration_l = closestBoxPoint - collisionPoint_l;
Vect3 collisionPoint_w = b.getTransformMatrix() * collisionPoint_l;
Vect3 penetration_w = b.getTransformMatrix().transformDirn(penetration_l);
/*Vect3 collisionPoint_l = (Vect3Normal(closestBoxPoint - this->GetPos()) * this->GetRadius()) + this->GetPos();
Vect3 collisionPoint_w = b.GetTransformMatrix() * (closestBoxPoint - transformedSpherePosition);
Vect3 penetration_w = (Vect3Normal(collisionPoint_w - this->GetPos()) * this->GetRadius()) - collisionPoint_w;*/
o.push_back(this->summonDemons(
collisionPoint_w,
penetration_w,
penetration_w.magnitude(),
b.getAttachedObjectPtr(), _attachedObject));
// Sphere contact data: Exact opposite of the box contact.
penetration_w *= -1.f;
o.push_back(this->summonDemons(
collisionPoint_w,
penetration_w,
penetration_w.magnitude(),
this->getAttachedObjectPtr(), b.getAttachedObjectPtr()));
}
}
