void CollisionSphere::genContactsS(const CollisionSphere& s, std::vector<std::shared_ptr<IContact>>& o) const { // Get distance between two objects, with this one at the origin... Vect3 distance = s.getPos() - this->getPos(); // Spheres are touching if the magnitude of the distance is less than // the sum of the radii // Early out if this is not the case. if (distance.squareMagnitude() > ((this->getRadius() + s.getRadius()) * (this->getRadius() + s.getRadius()))) { return; } else { // Generate contacts! // Contact is midpoint in world space of intersection, // normal is direction of intersection. Vect3Normal directionNormal = (s.getPos() - this->getPos()); float contactMagnitude = this->getRadius() + s.getRadius() - distance.magnitude(); // From the other object, go to the edge of the sphere. // Subtract that from the edge of this sphere. // now multiply the whole thing by 1/2, and add the edge of this sphere. // That's the midpoint of the intersected space of the two spheres. Vect3 contactPoint = ((s.getPos() - directionNormal * s.getRadius()) - (this->getPos() + directionNormal * this->getRadius())) * 0.5f + (this->getPos() + directionNormal * this->getRadius()); o.push_back(summonDemons(contactPoint, directionNormal * -1.f, contactMagnitude, s.getAttachedObjectPtr(), _attachedObject)); o.push_back(summonDemons(contactPoint, directionNormal, contactMagnitude, this->getAttachedObjectPtr(), s.getAttachedObjectPtr())); } }
// 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())); } }
bool CollisionSphere::isTouchingB(const CollisionBox& b) 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 false; // 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 points on the box. // If it's closer than the radius, we have a contact. return ((closestBoxPoint - transformedSpherePosition).squareMagnitude() <= this->getRadius() * this->getRadius()); }