diff --git a/src/body/AABB.cpp b/src/body/AABB.cpp index eef7a5fd..901c0e11 100644 --- a/src/body/AABB.cpp +++ b/src/body/AABB.cpp @@ -34,13 +34,12 @@ using namespace reactphysics3d; using namespace std; // Constructor -AABB::AABB(const Body* bodyPointer) : bodyPointer(bodyPointer) { +AABB::AABB() : bodyPointer(0) { } // Constructor -AABB::AABB(const Body* bodyPointer, const Transform& transform, const Vector3D& extents) - : bodyPointer(bodyPointer) { +AABB::AABB(const Transform& transform, const Vector3D& extents) : bodyPointer(0) { update(transform, extents); } diff --git a/src/body/AABB.h b/src/body/AABB.h index 898002d5..d6a1b8af 100644 --- a/src/body/AABB.h +++ b/src/body/AABB.h @@ -46,19 +46,20 @@ class AABB { private : Vector3D minCoordinates; // Minimum world coordinates of the AABB on the x,y and z axis Vector3D maxCoordinates; // Maximum world coordinates of the AABB on the x,y and z axis - const Body* bodyPointer; // Pointer to the owner body + Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable) public : - AABB(const Body* body); // Constructor - AABB(const Body* bodyPointer, const Transform& transform, const Vector3D& extents); // Constructor - virtual ~AABB(); // Destructor + AABB(); // Constructor + AABB(const Transform& transform, const Vector3D& extents); // Constructor + virtual ~AABB(); // Destructor - Vector3D getCenter() const; // Return the center point - const Vector3D& getMinCoordinates() const; // Return the minimum coordinates of the AABB - const Vector3D& getMaxCoordinates() const; // Return the maximum coordinates of the AABB - const Body* getBodyPointer() const; // Return a pointer to the owner body - bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument - virtual void update(const Transform& newTransform, const Vector3D& extents); // Update the oriented bounding box orientation according to a new orientation of the rigid body + Vector3D getCenter() const; // Return the center point + const Vector3D& getMinCoordinates() const; // Return the minimum coordinates of the AABB + const Vector3D& getMaxCoordinates() const; // Return the maximum coordinates of the AABB + Body* getBodyPointer() const; // Return a pointer to the owner body + void setBodyPointer(Body* bodyPointer); // Set the body pointer + bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument + virtual void update(const Transform& newTransform, const Vector3D& extents); // Update the oriented bounding box orientation according to a new orientation of the rigid body #ifdef VISUAL_DEBUG virtual void draw() const; // Draw the AABB (only for testing purpose) #endif @@ -80,15 +81,18 @@ inline const Vector3D& AABB::getMaxCoordinates() const { } // Return a pointer to the owner body -inline const Body* AABB::getBodyPointer() const { +inline Body* AABB::getBodyPointer() const { return bodyPointer; } +// Set the body pointer +inline void AABB::setBodyPointer(Body* bodyPointer) { + this->bodyPointer = bodyPointer; +} + // Return true if the current AABB is overlapping with the AABB in argument // Two AABB overlap if they overlap in the three x, y and z axis at the same time inline bool AABB::testCollision(const AABB& aabb) const { - Vector3D center2 = aabb.getCenter(); - double margin = 2 * OBJECT_MARGIN; if (maxCoordinates.getX() + OBJECT_MARGIN < aabb.minCoordinates.getX() - OBJECT_MARGIN || aabb.maxCoordinates.getX() + OBJECT_MARGIN < minCoordinates.getX() - OBJECT_MARGIN) return false; if (maxCoordinates.getY() + OBJECT_MARGIN < aabb.minCoordinates.getY() - OBJECT_MARGIN || diff --git a/src/body/Body.cpp b/src/body/Body.cpp index 4fa64371..8ef260c5 100644 --- a/src/body/Body.cpp +++ b/src/body/Body.cpp @@ -24,56 +24,31 @@ // Libraries #include "Body.h" -#include "BroadBoundingVolume.h" -#include "NarrowBoundingVolume.h" +#include "Shape.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; // Constructor -Body::Body(const Transform& transform, double mass) throw(std::invalid_argument) - : transform(transform), mass(mass), narrowBoundingVolume(0), aabb(0) { - // Check if the mass is not larger than zero - if (mass <= 0.0) { - // We throw an exception - throw std::invalid_argument("Exception in Body constructor : the mass has to be different larger than zero"); - } +Body::Body(const Transform& transform, Shape* shape, double mass) + : shape(shape), transform(transform), mass(mass) { + assert(mass > 0.0); + assert(shape); + + isMotionEnabled = true; + isCollisionEnabled = true; + interpolationFactor = 0.0; // Initialize the old transform oldTransform = transform; - // Create the AABB for Broad-Phase collision detection - aabb = new AABB(this); + // Create the AABB for broad-phase collision detection + aabb = new AABB(transform, shape->getLocalExtents()); } // Destructor Body::~Body() { - /* TODO : DELETE THIS - if (broadBoundingVolume) { - delete broadBoundingVolume; - } - */ - - if (narrowBoundingVolume) { - delete narrowBoundingVolume; - } // Delete the AABB delete aabb; } - -/* TODO : DELETE THIS -// Set the broad-phase bounding volume -void Body::setBroadBoundingVolume(BroadBoundingVolume* broadBoundingVolume) { - assert(broadBoundingVolume); - this->broadBoundingVolume = broadBoundingVolume; - broadBoundingVolume->setBodyPointer(this); -} -*/ - -// Set the narrow-phase bounding volume -void Body::setNarrowBoundingVolume(NarrowBoundingVolume* narrowBoundingVolume) { - assert(narrowBoundingVolume); - this->narrowBoundingVolume = narrowBoundingVolume; - narrowBoundingVolume->setBodyPointer(this); -} diff --git a/src/body/Body.h b/src/body/Body.h index b7f62cfe..6ea5dab8 100644 --- a/src/body/Body.h +++ b/src/body/Body.h @@ -30,11 +30,11 @@ #include #include "../mathematics/Transform.h" #include "../body/AABB.h" +#include "../body/Shape.h" // Namespace reactphysics3d namespace reactphysics3d { -class NarrowBoundingVolume; /* ------------------------------------------------------------------- Class Body : @@ -44,22 +44,21 @@ class NarrowBoundingVolume; */ class Body { protected : - double mass; // Mass of the body - Transform transform; // Position and orientation of the body - Transform oldTransform; // Last position and orientation of the body - double interpolationFactor; // Interpolation factor used for the state interpolation - // TODO : DELETE BroadBoundingVolume* broadBoundingVolume; // Bounding volume used for the broad-phase collision detection - NarrowBoundingVolume* narrowBoundingVolume; // Bounding volume used for the narrow-phase collision detection - bool isMotionEnabled; // True if the body is able to move - bool isCollisionEnabled; // True if the body can collide with others bodies - AABB* aabb; // Axis-Aligned Bounding Box for Broad-Phase collision detection - - void setNarrowBoundingVolume(NarrowBoundingVolume* narrowBoundingVolume); // Set the narrow-phase bounding volume + Shape* shape; // Collision shape of the body + double mass; // Mass of the body + Transform transform; // Position and orientation of the body + Transform oldTransform; // Last position and orientation of the body + double interpolationFactor; // Interpolation factor used for the state interpolation + bool isMotionEnabled; // True if the body is able to move + bool isCollisionEnabled; // True if the body can collide with others bodies + AABB* aabb; // Axis-Aligned Bounding Box for Broad-Phase collision detection public : - Body(const Transform& transform, double mass) throw(std::invalid_argument); // Constructor - virtual ~Body(); // Destructor + Body(const Transform& transform, Shape* shape, double mass); // Constructor + virtual ~Body(); // Destructor + Shape* getShape() const; // Return the collision shape + void setShape(Shape* shape); // Set the collision shape double getMass() const; // Return the mass of the body void setMass(double mass); // Set the mass of the body const Transform& getTransform() const; // Return the current position and orientation @@ -71,10 +70,22 @@ class Body { void setIsMotionEnabled(bool isMotionEnabled); // Set the value to true if the body can move bool getIsCollisionEnabled() const; // Return true if the body can collide with others bodies void setIsCollisionEnabled(bool isCollisionEnabled); // Set the isCollisionEnabled value - const NarrowBoundingVolume* getNarrowBoundingVolume() const; // Return the narrow-phase bounding volume of the body void updateOldTransform(); // Update the old transform with the current one + void updateAABB(); // Update the Axis-Aligned Bounding Box coordinates }; +// Return the collision shape +inline Shape* Body::getShape() const { + assert(shape); + return shape; +} + +// Set the collision shape +inline void Body::setShape(Shape* shape) { + assert(shape); + this->shape = shape; +} + // Method that return the mass of the body inline double Body::getMass() const { return mass; @@ -131,24 +142,18 @@ inline void Body::setIsCollisionEnabled(bool isCollisionEnabled) { this->isCollisionEnabled = isCollisionEnabled; } -/* TODO : DELETE -// Return the broad-phase bounding volume -inline const BroadBoundingVolume* Body::getBroadBoundingVolume() const { - return broadBoundingVolume; -} -*/ - -// Return the oriented bounding box of the rigid body -inline const NarrowBoundingVolume* Body::getNarrowBoundingVolume() const { - return narrowBoundingVolume; -} - // Update the old transform with the current one // This is used to compute the interpolated position and orientation of the body inline void Body::updateOldTransform() { oldTransform = transform; } +// Update the rigid body in order to reflect a change in the body state +inline void Body::updateAABB() { + // Update the AABB + aabb->update(transform, shape->getLocalExtents()); +} + } diff --git a/src/body/BoundingSphere.h b/src/body/BoundingSphere.h index 4fb4d41b..0d05b7e6 100644 --- a/src/body/BoundingSphere.h +++ b/src/body/BoundingSphere.h @@ -26,7 +26,7 @@ #define BOUNDING_SPHERE_H // Libraries -#include "NarrowBoundingVolume.h" +#include "Shape.h" #include "../mathematics/mathematics.h" // ReactPhysics3D namespace @@ -37,7 +37,7 @@ namespace reactphysics3d { This class represents a sphere bounding volume. ------------------------------------------------------------------- */ -class BoundingSphere : public NarrowBoundingVolume { +class BoundingSphere : public Shape { protected : Vector3D center; // Center point of the sphere double radius; // Radius of the sphere diff --git a/src/body/ConeShape.h b/src/body/ConeShape.h index de2a09e5..91122594 100644 --- a/src/body/ConeShape.h +++ b/src/body/ConeShape.h @@ -26,7 +26,7 @@ #define BOUNDING_CONE_H // Libraries -#include "NarrowBoundingVolume.h" +#include "Shape.h" #include "../mathematics/mathematics.h" // ReactPhysics3D namespace @@ -37,7 +37,7 @@ namespace reactphysics3d { This class represents a cone bounding volume. ------------------------------------------------------------------- */ -class ConeShape : public NarrowBoundingVolume { +class ConeShape : public Shape { protected : Vector3D center; // Center point of the sphere double radius; // Radius of the sphere diff --git a/src/body/OBB.h b/src/body/OBB.h index 331354cf..20c6ce4f 100644 --- a/src/body/OBB.h +++ b/src/body/OBB.h @@ -27,7 +27,7 @@ // Libraries #include -#include "NarrowBoundingVolume.h" +#include "Shape.h" #include "../mathematics/mathematics.h" // ReactPhysics3D namespace @@ -41,7 +41,7 @@ namespace reactphysics3d { rigid body given an orientation and a position to the box ------------------------------------------------------------------- */ -class OBB : public NarrowBoundingVolume { +class OBB : public Shape { private : Vector3D extent; // Extent sizes of the box diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 53374729..336b8c01 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -24,31 +24,24 @@ // Libraries #include "RigidBody.h" -#include "BroadBoundingVolume.h" -#include "NarrowBoundingVolume.h" +#include "Shape.h" - // We want to use the ReactPhysics3D namespace - using namespace reactphysics3d; +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; // Constructor - // TODO : Use a Transform in the constructor instead of "position" and "orientation" - RigidBody::RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, - NarrowBoundingVolume* narrowBoundingVolume) - : Body(transform, mass), inertiaTensorLocal(inertiaTensorLocal), + RigidBody::RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, Shape* shape) + : Body(transform, shape, mass), inertiaTensorLocal(inertiaTensorLocal), inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass) { restitution = 1.0; - isMotionEnabled = true; - isCollisionEnabled = true; - interpolationFactor = 0.0; - // Set the bounding volume for the narrow-phase collision detection - setNarrowBoundingVolume(narrowBoundingVolume); + // Set the body pointer of the AABB and the shape + aabb->setBodyPointer(this); + shape->setBodyPointer(this); - // Update the orientation of the OBB according to the orientation of the rigid body - update(); - - assert(narrowBoundingVolume); + assert(shape); + assert(aabb); } // Destructor @@ -56,10 +49,3 @@ RigidBody::~RigidBody() { }; -// Update the rigid body in order to reflect a change in the body state -void RigidBody::update() { - // TODO : Remove the following code when using a Transform - - // Update the AABB - aabb->update(transform, narrowBoundingVolume->getLocalExtents()); -} \ No newline at end of file diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 6eba6636..494f182b 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -28,6 +28,7 @@ // Libraries #include #include "Body.h" +#include "Shape.h" #include "../mathematics/mathematics.h" // Namespace reactphysics3d @@ -42,11 +43,6 @@ namespace reactphysics3d { */ class RigidBody : public Body { protected : - // TODO : Remove some of the following variables - //Vector3D position; // Position of the center of mass of the body - //Vector3D oldPosition; // Old position used to compute the interpolated position - //Quaternion orientation; // Orientation quaternion of the body - //Quaternion oldOrientation; // Old orientation used to compute the interpolated orientation Vector3D linearVelocity; // Linear velocity of the body Vector3D angularVelocity; // Angular velocity of the body Vector3D externalForce; // Current external force on the body @@ -58,8 +54,8 @@ class RigidBody : public Body { public : RigidBody(const Transform& transform, double mass, - const Matrix3x3& inertiaTensorLocal, NarrowBoundingVolume* narrowBoundingVolume); // Constructor // Copy-constructor - virtual ~RigidBody(); // Destructor + const Matrix3x3& inertiaTensorLocal, Shape* shape); // Constructor // Copy-constructor + virtual ~RigidBody(); // Destructor Vector3D getLinearVelocity() const; // Return the linear velocity void setLinearVelocity(const Vector3D& linearVelocity); // Set the linear velocity of the body @@ -79,7 +75,6 @@ class RigidBody : public Body { double getRestitution() const; // Get the restitution coefficient void setRestitution(double restitution) throw(std::invalid_argument); // Set the restitution coefficient - void update(); // Update the rigid body in order to reflect a change in the body state }; // Return the linear velocity diff --git a/src/body/NarrowBoundingVolume.cpp b/src/body/Shape.cpp similarity index 93% rename from src/body/NarrowBoundingVolume.cpp rename to src/body/Shape.cpp index 848a333c..ed10b6f3 100644 --- a/src/body/NarrowBoundingVolume.cpp +++ b/src/body/Shape.cpp @@ -23,17 +23,17 @@ ********************************************************************************/ // Libraries -#include "NarrowBoundingVolume.h" +#include "Shape.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; // Constructor -NarrowBoundingVolume::NarrowBoundingVolume() : BoundingVolume() { +Shape::Shape() { } // Destructor -NarrowBoundingVolume::~NarrowBoundingVolume() { +Shape::~Shape() { } diff --git a/src/body/NarrowBoundingVolume.h b/src/body/Shape.h similarity index 71% rename from src/body/NarrowBoundingVolume.h rename to src/body/Shape.h index b477eff0..be31d2ad 100644 --- a/src/body/NarrowBoundingVolume.h +++ b/src/body/Shape.h @@ -23,36 +23,50 @@ * THE SOFTWARE. * ********************************************************************************/ -#ifndef NARROW_BOUNDING_VOLUME_H -#define NARROW_BOUNDING_VOLUME_H +#ifndef SHAPE_H +#define SHAPE_H // Libraries -#include "BoundingVolume.h" -#include "AABB.h" - +#include +#include "../mathematics/Vector3D.h" // ReactPhysics3D namespace namespace reactphysics3d { +// Declarations +class Body; + /* ------------------------------------------------------------------- - Class NarrowBoundingVolume : - This class represents the volume that contains a rigid body - This volume will be used to compute the narrow-phase collision - detection. + Class Shape : + This abstract class represents the shape of a body that is used during + the narrow-phase collision detection. ------------------------------------------------------------------- */ -class NarrowBoundingVolume : public BoundingVolume { +class Shape { protected : + Body* bodyPointer; // Pointer to the owner body (not the abstract class Body but its derivative which is instanciable) public : - NarrowBoundingVolume(); // Constructor - virtual ~NarrowBoundingVolume(); // Destructor + Shape(); // Constructor + virtual ~Shape(); // Destructor - // TODO : DELETE virtual AABB* computeAABB() const=0; // Return the corresponding AABB + Body* getBodyPointer() const; // Return the body pointer + void setBodyPointer(Body* bodyPointer); // Set the body pointer virtual Vector3D getSupportPoint(const Vector3D& direction, double margin=0.0) const=0; // Return a support point in a given direction virtual Vector3D getLocalExtents() const=0; // Return the local extents in x,y and z direction }; +// Return the body pointer +inline Body* Shape::getBodyPointer() const { + assert(bodyPointer != NULL); + return bodyPointer; +} + +// Set the body pointer +inline void Shape::setBodyPointer(Body* bodyPointer) { + this->bodyPointer = bodyPointer; +} + } #endif \ No newline at end of file diff --git a/src/collision/BroadPhaseAlgorithm.h b/src/collision/BroadPhaseAlgorithm.h index ebc213f8..9d88ac2b 100644 --- a/src/collision/BroadPhaseAlgorithm.h +++ b/src/collision/BroadPhaseAlgorithm.h @@ -26,7 +26,8 @@ #define BROAD_PHASE_ALGORITHM_H // Libraries -#include "../body/BoundingVolume.h" +#include +#include "../body/Body.h" // Namespace ReactPhysics3D namespace reactphysics3d { @@ -52,7 +53,7 @@ class BroadPhaseAlgorithm { virtual ~BroadPhaseAlgorithm(); // Destructor virtual void computePossibleCollisionPairs(std::vector addedBodies, std::vector removedBodies, - std::vector >& possibleCollisionPairs)=0; // Compute the possible collision pairs of bodies + std::vector >& possibleCollisionPairs)=0; // Compute the possible collision pairs of bodies }; } // End of reactphysics3d namespace diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 5096cb7f..8aee85cf 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -90,13 +90,12 @@ void CollisionDetection::computeNarrowPhase() { for (unsigned int i=0; i(possibleCollisionPairs.at(i).first); - const RigidBody* rigidBody2 = dynamic_cast(possibleCollisionPairs.at(i).second); + Body* const body1 = possibleCollisionPairs.at(i).first; + Body* const body2 = possibleCollisionPairs.at(i).second; // Use the narrow-phase collision detection algorithm to check if there really are a contact - if (narrowPhaseAlgorithm->testCollision(rigidBody1->getNarrowBoundingVolume(), rigidBody1->getTransform(), - rigidBody2->getNarrowBoundingVolume(), rigidBody2->getTransform(), - contactInfo)) { + if (narrowPhaseAlgorithm->testCollision(body1->getShape(), body1->getTransform(), + body2->getShape(), body2->getTransform(), contactInfo)) { assert(contactInfo); // Add the contact info the current list of collision informations diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index f965ac61..1e0aeb5b 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -47,7 +47,7 @@ namespace reactphysics3d { class CollisionDetection { private : PhysicsWorld* world; // Pointer to the physics world - std::vector > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase) + std::vector > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase) std::vector contactInfos; // Contact informations (computed by narrowphase) BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm diff --git a/src/collision/ContactInfo.cpp b/src/collision/ContactInfo.cpp index 46613abf..2d3a46db 100644 --- a/src/collision/ContactInfo.cpp +++ b/src/collision/ContactInfo.cpp @@ -28,13 +28,14 @@ using namespace reactphysics3d; // Constructor for SAT -ContactInfo::ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth) +// TODO : DELETE THIS +ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3D& normal, double penetrationDepth) : body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(0.0, 0.0, 0.0), point2(0.0, 0.0, 0.0) { } // Constructor for GJK -ContactInfo::ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal, +ContactInfo::ContactInfo(Body* body1, Body* body2, const Vector3D& normal, double penetrationDepth, const Vector3D& point1, const Vector3D& point2) : body1(body1), body2(body2), normal(normal), penetrationDepth(penetrationDepth), point1(point1), point2(point2) { diff --git a/src/collision/ContactInfo.h b/src/collision/ContactInfo.h index 1b93f79e..f0b7c1cf 100644 --- a/src/collision/ContactInfo.h +++ b/src/collision/ContactInfo.h @@ -49,9 +49,9 @@ struct ContactInfo { const Vector3D normal; // Normal vector the the collision contact const double penetrationDepth; // Penetration depth of the contact - ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal, + ContactInfo(Body* body1, Body* body2, const Vector3D& normal, double penetrationDepth); // Constructor for SAT - ContactInfo(Body* const body1, Body* const body2, const Vector3D& normal, + ContactInfo(Body* body1, Body* body2, const Vector3D& normal, double penetrationDepth, const Vector3D& point1, const Vector3D& point2); // Constructor for GJK }; diff --git a/src/collision/EPA/EPAAlgorithm.cpp b/src/collision/EPA/EPAAlgorithm.cpp index c0752a25..77ba25e8 100644 --- a/src/collision/EPA/EPAAlgorithm.cpp +++ b/src/collision/EPA/EPAAlgorithm.cpp @@ -79,10 +79,8 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3D& p1, const Vector3D& p2, // intersect. An initial simplex that contains origin has been computed with // GJK algorithm. The EPA Algorithm will extend this simplex polytope to find // the correct penetration depth -bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const NarrowBoundingVolume* const boundingVolume1, - const Transform& transform1, - const NarrowBoundingVolume* const boundingVolume2, - const Transform& transform2, +bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1, + const Shape* shape2, const Transform& transform2, Vector3D& v, ContactInfo*& contactInfo) { Vector3D suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates Vector3D suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates @@ -148,18 +146,18 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons Vector3D v3 = rotationMat * v2; // Compute the support point in the direction of v1 - suppPointsA[2] = boundingVolume1->getSupportPoint(v1, OBJECT_MARGIN); - suppPointsB[2] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v1.getOpposite(), OBJECT_MARGIN); + suppPointsA[2] = shape1->getSupportPoint(v1, OBJECT_MARGIN); + suppPointsB[2] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v1.getOpposite(), OBJECT_MARGIN); points[2] = suppPointsA[2] - suppPointsB[2]; // Compute the support point in the direction of v2 - suppPointsA[3] = boundingVolume1->getSupportPoint(v2, OBJECT_MARGIN); - suppPointsB[3] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v2.getOpposite(), OBJECT_MARGIN); + suppPointsA[3] = shape1->getSupportPoint(v2, OBJECT_MARGIN); + suppPointsB[3] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v2.getOpposite(), OBJECT_MARGIN); points[3] = suppPointsA[3] - suppPointsB[3]; // Compute the support point in the direction of v3 - suppPointsA[4] = boundingVolume1->getSupportPoint(v3, OBJECT_MARGIN); - suppPointsB[4] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v3.getOpposite(), OBJECT_MARGIN); + suppPointsA[4] = shape1->getSupportPoint(v3, OBJECT_MARGIN); + suppPointsB[4] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v3.getOpposite(), OBJECT_MARGIN); points[4] = suppPointsA[4] - suppPointsB[4]; // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the @@ -253,11 +251,11 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons Vector3D n = v1.cross(v2); // Compute the two new vertices to obtain a hexahedron - suppPointsA[3] = boundingVolume1->getSupportPoint(n, OBJECT_MARGIN); - suppPointsB[3] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * n.getOpposite(), OBJECT_MARGIN); + suppPointsA[3] = shape1->getSupportPoint(n, OBJECT_MARGIN); + suppPointsB[3] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * n.getOpposite(), OBJECT_MARGIN); points[3] = suppPointsA[3] - suppPointsB[3]; - suppPointsA[4] = boundingVolume1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN); - suppPointsB[4] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * n, OBJECT_MARGIN); + suppPointsA[4] = shape1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN); + suppPointsB[4] = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * n, OBJECT_MARGIN); points[4] = suppPointsA[4] - suppPointsB[4]; // Construct the triangle faces @@ -326,8 +324,8 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons } // Compute the support point of the Minkowski difference (A-B) in the closest point direction - suppPointsA[nbVertices] = boundingVolume1->getSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN); - suppPointsB[nbVertices] = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * triangle->getClosestPoint().getOpposite(), OBJECT_MARGIN); + suppPointsA[nbVertices] = shape1->getSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN); + suppPointsB[nbVertices] = shape2ToShape1 *shape2->getSupportPoint(rotateToShape2 * triangle->getClosestPoint().getOpposite(), OBJECT_MARGIN); points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; int indexNewVertex = nbVertices; @@ -376,8 +374,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons Vector3D normal = v.getUnit(); double penetrationDepth = v.length(); assert(penetrationDepth > 0.0); - contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), - normal, penetrationDepth, pA, pB); + contactInfo = new ContactInfo(shape1->getBodyPointer(), shape2->getBodyPointer(), normal, penetrationDepth, pA, pB); return true; } diff --git a/src/collision/EPA/EPAAlgorithm.h b/src/collision/EPA/EPAAlgorithm.h index b21fefa1..8fdeddd4 100644 --- a/src/collision/EPA/EPAAlgorithm.h +++ b/src/collision/EPA/EPAAlgorithm.h @@ -27,7 +27,7 @@ // Libraries #include "../GJK/Simplex.h" -#include "../../body/NarrowBoundingVolume.h" +#include "../../body/Shape.h" #include "../ContactInfo.h" #include "../../mathematics/mathematics.h" #include "TriangleEPA.h" @@ -82,10 +82,8 @@ class EPAAlgorithm { EPAAlgorithm(); // Constructor ~EPAAlgorithm(); // Destructor - bool computePenetrationDepthAndContactPoints(Simplex simplex, const NarrowBoundingVolume* const boundingVolume1, - const Transform& transform1, - const NarrowBoundingVolume* const boundingVolume2, - const Transform& transform2, + bool computePenetrationDepthAndContactPoints(Simplex simplex, const Shape* shape1, const Transform& transform1, + const Shape* shape2, const Transform& transform2, Vector3D& v, ContactInfo*& contactInfo); // Compute the penetration depth with EPA algorithm }; diff --git a/src/collision/GJK/GJKAlgorithm.cpp b/src/collision/GJK/GJKAlgorithm.cpp index 224e06ef..146361de 100644 --- a/src/collision/GJK/GJKAlgorithm.cpp +++ b/src/collision/GJK/GJKAlgorithm.cpp @@ -59,9 +59,11 @@ GJKAlgorithm::~GJKAlgorithm() { // algorithm on the enlarged object to obtain a simplex polytope that contains the // origin, they we give that simplex polytope to the EPA algorithm which will compute // the correct penetration depth and contact points between the enlarged objects. -bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1, - const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2, - ContactInfo*& contactInfo) { +bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform1, + const Shape* shape2, const Transform& transform2, ContactInfo*& contactInfo) { + + assert(shape1 != shape2); + Vector3D suppA; // Support point of object A Vector3D suppB; // Support point of object B Vector3D w; // Support point of Minkowski difference A-B @@ -69,6 +71,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum Vector3D pB; // Closest point of object B double vDotw; double prevDistSquare; + Body* const body1 = shape1->getBodyPointer(); + Body* const body2 = shape2->getBodyPointer(); // Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1) Transform shape2ToShape1 = transform1.inverse() * transform2; @@ -76,8 +80,6 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum // Matrix that transform a direction from body space of shape 1 into body space of shape 2 Matrix3x3 rotateToShape2 = transform2.getOrientation().getTranspose() * transform1.getOrientation(); - assert(boundingVolume1 != boundingVolume2); - // Initialize the margin (sum of margins of both objects) double margin = 2 * OBJECT_MARGIN; double marginSquare = margin * margin; @@ -96,8 +98,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum do { // Compute the support points for original objects (without margins) A and B - suppA = boundingVolume1->getSupportPoint(v.getOpposite()); - suppB = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v); + suppA = shape1->getSupportPoint(v.getOpposite()); + suppB = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; @@ -125,8 +127,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum // Compute the contact info Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit(); double penetrationDepth = margin - dist; - contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), - normal, penetrationDepth, pA, pB); + contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); // There is an intersection, therefore we return true return true; @@ -150,8 +151,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum // Compute the contact info Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit(); double penetrationDepth = margin - dist; - contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), - normal, penetrationDepth, pA, pB); + contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); // There is an intersection, therefore we return true return true; @@ -173,8 +173,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum // Compute the contact info Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit(); double penetrationDepth = margin - dist; - contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), - normal, penetrationDepth, pA, pB); + contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); // There is an intersection, therefore we return true return true; @@ -204,8 +203,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum // Compute the contact info Vector3D normal = transform1.getOrientation() * v.getOpposite().getUnit(); double penetrationDepth = margin - dist; - contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), - normal, penetrationDepth, pA, pB); + contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB); // There is an intersection, therefore we return true return true; @@ -220,7 +218,7 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum // enlarged objects to compute a simplex polytope that contains the origin. Then, we give that simplex // polytope to the EPA algorithm to compute the correct penetration depth and contact points between // the enlarged objects. - return computePenetrationDepthForEnlargedObjects(boundingVolume1, transform1, boundingVolume2, transform2, contactInfo, v); + return computePenetrationDepthForEnlargedObjects(shape1, transform1, shape2, transform2, contactInfo, v); } // This method runs the GJK algorithm on the two enlarged objects (with margin) @@ -228,8 +226,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum // assumed to intersect in the original objects (without margin). Therefore such // a polytope must exist. Then, we give that polytope to the EPA algorithm to // compute the correct penetration depth and contact points of the enlarged objects. -bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1, - const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2, +bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const shape1, const Transform& transform1, + const Shape* const shape2, const Transform& transform2, ContactInfo*& contactInfo, Vector3D& v) { Simplex simplex; Vector3D suppA; @@ -247,8 +245,8 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundin do { // Compute the support points for the enlarged object A and B - suppA = boundingVolume1->getSupportPoint(v.getOpposite(), OBJECT_MARGIN); - suppB = shape2ToShape1 * boundingVolume2->getSupportPoint(rotateToShape2 * v, OBJECT_MARGIN); + suppA = shape1->getSupportPoint(v.getOpposite(), OBJECT_MARGIN); + suppB = shape2ToShape1 * shape2->getSupportPoint(rotateToShape2 * v, OBJECT_MARGIN); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; @@ -284,6 +282,5 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const NarrowBoundin // Give the simplex computed with GJK algorithm to the EPA algorithm which will compute the correct // penetration depth and contact points between the two enlarged objects - return algoEPA.computePenetrationDepthAndContactPoints(simplex, boundingVolume1, transform1, - boundingVolume2, transform2, v, contactInfo); + return algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1, transform1, shape2, transform2, v, contactInfo); } diff --git a/src/collision/GJK/GJKAlgorithm.h b/src/collision/GJK/GJKAlgorithm.h index ebe2460c..bf3b5a25 100644 --- a/src/collision/GJK/GJKAlgorithm.h +++ b/src/collision/GJK/GJKAlgorithm.h @@ -28,7 +28,7 @@ // Libraries #include "../NarrowPhaseAlgorithm.h" #include "../ContactInfo.h" -#include "../../body/NarrowBoundingVolume.h" +#include "../../body/Shape.h" #include "../EPA/EPAAlgorithm.h" @@ -61,23 +61,19 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { private : EPAAlgorithm algoEPA; // EPA Algorithm - bool computePenetrationDepthForEnlargedObjects(const NarrowBoundingVolume* const boundingVolume1, - const Transform& transform1, - const NarrowBoundingVolume* const boundingVolume2, - const Transform& transform2, + bool computePenetrationDepthForEnlargedObjects(const Shape* shape1, const Transform& transform1, + const Shape* shape2, const Transform& transform2, ContactInfo*& contactInfo, Vector3D& v); // Compute the penetration depth for enlarged objects public : GJKAlgorithm(); // Constructor ~GJKAlgorithm(); // Destructor - virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1, - const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2, - ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide + virtual bool testCollision(const Shape* shape1, const Transform& transform1, + const Shape* shape2, const Transform& transform2, + ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide }; } // End of the ReactPhysics3D namespace -// TODO : Check what would be a correct value for the OBJECT_MARGIN constant - #endif diff --git a/src/collision/NarrowPhaseAlgorithm.h b/src/collision/NarrowPhaseAlgorithm.h index 9cb05309..9d48ab18 100644 --- a/src/collision/NarrowPhaseAlgorithm.h +++ b/src/collision/NarrowPhaseAlgorithm.h @@ -26,7 +26,7 @@ #define NARROW_PHASE_ALGORITHM_H // Libraries -#include "../body/BoundingVolume.h" +#include "../body/Body.h" #include "ContactInfo.h" // Namespace ReactPhysics3D @@ -47,11 +47,9 @@ class NarrowPhaseAlgorithm { NarrowPhaseAlgorithm(); // Constructor virtual ~NarrowPhaseAlgorithm(); // Destructor - virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1, - const Transform& transform1, - const NarrowBoundingVolume* const boundingVolume2, - const Transform& transform2, - ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide + virtual bool testCollision(const Shape* shape1, const Transform& transform1, + const Shape* shape2, const Transform& transform2, + ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide }; } // End of reactphysics3d namespace diff --git a/src/collision/SAPAlgorithm.cpp b/src/collision/SAPAlgorithm.cpp index e6d376b7..f2f008cc 100644 --- a/src/collision/SAPAlgorithm.cpp +++ b/src/collision/SAPAlgorithm.cpp @@ -78,7 +78,7 @@ void SAPAlgorithm::addBodiesAABB(vector bodies) { // computation. This methods computes the current possible collision pairs of // bodies and update the "possibleCollisionPairs" argument. void SAPAlgorithm::computePossibleCollisionPairs(vector addedBodies, vector removedBodies, - vector >& possibleCollisionPairs) { + vector >& possibleCollisionPairs) { double variance[3]; // Variance of the distribution of the AABBs on the three x, y and z axis double esperance[] = {0.0, 0.0, 0.0}; // Esperance of the distribution of the AABBs on the three x, y and z axis double esperanceSquare[] = {0.0, 0.0, 0.0}; // Esperance of the square of the distribution values of the AABBs on the three x, y and z axis diff --git a/src/collision/SAPAlgorithm.h b/src/collision/SAPAlgorithm.h index 32cc5bd6..2fe7859f 100644 --- a/src/collision/SAPAlgorithm.h +++ b/src/collision/SAPAlgorithm.h @@ -62,7 +62,7 @@ class SAPAlgorithm : public BroadPhaseAlgorithm { virtual ~SAPAlgorithm(); // Destructor virtual void computePossibleCollisionPairs(std::vector addedBodies, std::vector removedBodies, - std::vector >& possibleCollisionPairs); // Compute the possible collision pairs of bodies + std::vector >& possibleCollisionPairs); // Compute the possible collision pairs of bodies }; // Static method that compare two AABBs. This method will be used to compare to AABBs diff --git a/src/collision/SATAlgorithm.cpp b/src/collision/SATAlgorithm.cpp index 4e9f2fe3..a56473ed 100644 --- a/src/collision/SATAlgorithm.cpp +++ b/src/collision/SATAlgorithm.cpp @@ -27,6 +27,7 @@ #include "../body/OBB.h" #include "../body/RigidBody.h" #include "../constraint/Contact.h" +#include "../mathematics/Transform.h" #include #include #include @@ -51,17 +52,16 @@ SATAlgorithm::~SATAlgorithm() { // Return true and compute a contact info if the two bounding volume collide. // The method returns false if there is no collision between the two bounding volumes. -bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolume1, const Transform& transform1, - const NarrowBoundingVolume* const boundingVolume2, const Transform& transform2, - ContactInfo*& contactInfo) { +bool SATAlgorithm::testCollision(const Body* body1, const Body* body2, ContactInfo*& contactInfo) { - assert(boundingVolume1 != boundingVolume2); + assert(body1 != body2); - // If the two bounding volumes are OBB - //const OBB* const obb1 = dynamic_cast(boundingVolume1); - //const OBB* const obb2 = dynamic_cast(boundingVolume2); - const OBB* const obb1 = dynamic_cast(boundingVolume1); - const OBB* const obb2 = dynamic_cast(boundingVolume2); + const Transform& transform1 = body1->getTransform(); + const Transform& transform2 = body2->getTransform(); + const RigidBody* rigidBody1 = dynamic_cast(body1); + const RigidBody* rigidBody2 = dynamic_cast(body2); + const OBB* obb1 = dynamic_cast(rigidBody1->getShape()); + const OBB* obb2 = dynamic_cast(rigidBody2->getShape()); // If the two bounding volumes are OBB if (obb1 && obb2) { @@ -81,8 +81,8 @@ bool SATAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum // OBB are the six face normals (3 for each OBB) and the nine vectors V = Ai x Bj where Ai is the ith face normal // vector of OBB 1 and Bj is the jth face normal vector of OBB 2. We will use the notation Ai for the ith face // normal of OBB 1 and Bj for the jth face normal of OBB 2. -bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& transform1, - const OBB* const obb2, const Transform& transform2, ContactInfo*& contactInfo) const { +bool SATAlgorithm::computeCollisionTest(const OBB* obb1, const Transform& transform1, + const OBB* obb2, const Transform& transform2, ContactInfo*& contactInfo) const { double center; // Center of a projection interval @@ -121,8 +121,8 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } udc1[0] = transform1.getOrientation().getColumn(0).dot(boxDistance); center = udc1[0]; - radius1 = obb1->getExtent(0); - radius2 = obb2->getExtent(0)*absC[0][0] + obb2->getExtent(1)*absC[0][1] + obb2->getExtent(2) * absC[0][2]; + radius1 = obb1->getExtent().getX(); + radius2 = obb2->getExtent().getX()*absC[0][0] + obb2->getExtent().getY()*absC[0][1] + obb2->getExtent().getZ() * absC[0][2]; min1 = -radius1; max1 = radius1; min2 = center - radius2; @@ -133,7 +133,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb1->getAxis(0), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform1.getOrientation().getColumn(0), boxDistance); // Compute the contact normal with the correct sign } // Axis A1 @@ -146,8 +146,8 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } udc1[1] = transform1.getOrientation().getColumn(1).dot(boxDistance); center = udc1[1]; - radius1 = obb1->getExtent(1); - radius2 = obb2->getExtent(0)*absC[1][0] + obb2->getExtent(1)*absC[1][1] + obb2->getExtent(2) * absC[1][2]; + radius1 = obb1->getExtent().getY(); + radius2 = obb2->getExtent().getX()*absC[1][0] + obb2->getExtent().getY()*absC[1][1] + obb2->getExtent().getZ() * absC[1][2]; min1 = -radius1; max1 = radius1; min2 = center - radius2; @@ -158,7 +158,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb1->getAxis(1), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform1.getOrientation().getColumn(1), boxDistance); // Compute the contact normal with the correct sign } // Axis A2 @@ -171,8 +171,8 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } udc1[2] = transform1.getOrientation().getColumn(2).dot(boxDistance); center = udc1[2]; - radius1 = obb1->getExtent(2); - radius2 = obb2->getExtent(0)*absC[2][0] + obb2->getExtent(1)*absC[2][1] + obb2->getExtent(2)*absC[2][2]; + radius1 = obb1->getExtent().getZ(); + radius2 = obb2->getExtent().getX()*absC[2][0] + obb2->getExtent().getY()*absC[2][1] + obb2->getExtent().getZ()*absC[2][2]; min1 = -radius1; max1 = radius1; min2 = center - radius2; @@ -183,14 +183,14 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb1->getAxis(2), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform1.getOrientation().getColumn(2), boxDistance); // Compute the contact normal with the correct sign } // Axis B0 udc2[0] = transform2.getOrientation().getColumn(0).dot(boxDistance); center = udc2[0]; - radius1 = obb1->getExtent(0)*absC[0][0] + obb1->getExtent(1)*absC[1][0] + obb1->getExtent(2) * absC[2][0]; - radius2 = obb2->getExtent(0); + radius1 = obb1->getExtent().getX()*absC[0][0] + obb1->getExtent().getY()*absC[1][0] + obb1->getExtent().getZ() * absC[2][0]; + radius2 = obb2->getExtent().getX(); min1 = -radius1; max1 = radius1; min2 = center - radius2; @@ -201,14 +201,14 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb2->getAxis(0), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform2.getOrientation().getColumn(0), boxDistance); // Compute the contact normal with the correct sign } // Axis B1 udc2[1] = transform2.getOrientation().getColumn(1).dot(boxDistance); center = udc2[1]; - radius1 = obb1->getExtent(0)*absC[0][1] + obb1->getExtent(1)*absC[1][1] + obb1->getExtent(2) * absC[2][1]; - radius2 = obb2->getExtent(1); + radius1 = obb1->getExtent().getX()*absC[0][1] + obb1->getExtent().getY()*absC[1][1] + obb1->getExtent().getZ() * absC[2][1]; + radius2 = obb2->getExtent().getY(); min1 = - radius1; max1 = radius1; min2 = center - radius2; @@ -219,14 +219,14 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb2->getAxis(1), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform2.getOrientation().getColumn(1), boxDistance); // Compute the contact normal with the correct sign } // Axis B2 udc2[2] = transform2.getOrientation().getColumn(2).dot(boxDistance); center = udc2[2]; - radius1 = obb1->getExtent(0)*absC[0][2] + obb1->getExtent(1)*absC[1][2] + obb1->getExtent(2)*absC[2][2]; - radius2 = obb2->getExtent(2); + radius1 = obb1->getExtent().getX()*absC[0][2] + obb1->getExtent().getY()*absC[1][2] + obb1->getExtent().getZ()*absC[2][2]; + radius2 = obb2->getExtent().getZ(); min1 = - radius1; max1 = radius1; min2 = center - radius2; @@ -237,7 +237,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb2->getAxis(2), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform2.getOrientation().getColumn(2), boxDistance); // Compute the contact normal with the correct sign } // If there exists a parallel pair of face normals @@ -253,8 +253,8 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& // Axis A0 x B0 center = udc1[2] * c[1][0] - udc1[1] * c[2][0]; - radius1 = obb1->getExtent(1) * absC[2][0] + obb1->getExtent(2) * absC[1][0]; - radius2 = obb2->getExtent(1) * absC[0][2] + obb2->getExtent(2) * absC[0][1]; + radius1 = obb1->getExtent().getY() * absC[2][0] + obb1->getExtent().getZ() * absC[1][0]; + radius2 = obb2->getExtent().getY() * absC[0][2] + obb2->getExtent().getZ() * absC[0][1]; min1 = -radius1; max1 = radius1; min2 = center - radius2; @@ -265,13 +265,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb1->getAxis(0).cross(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform1.getOrientation().getColumn(0).cross(transform2.getOrientation().getColumn(0)), boxDistance); // Compute the contact normal with the correct sign } // Axis A0 x B1 center = udc1[2] * c[1][1] - udc1[1] * c[2][1]; - radius1 = obb1->getExtent(1) * absC[2][1] + obb1->getExtent(2) * absC[1][1]; - radius2 = obb2->getExtent(0) * absC[0][2] + obb2->getExtent(2) * absC[0][0]; + radius1 = obb1->getExtent().getY() * absC[2][1] + obb1->getExtent().getZ() * absC[1][1]; + radius2 = obb2->getExtent().getX() * absC[0][2] + obb2->getExtent().getZ() * absC[0][0]; min1 = -radius1; max1 = radius1; min2 = center - radius2; @@ -282,13 +282,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb1->getAxis(0).cross(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform1.getOrientation().getColumn(0).cross(transform2.getOrientation().getColumn(1)), boxDistance); // Compute the contact normal with the correct sign } // Axis A0 x B2 center = udc1[2] * c[1][2] - udc1[1] * c[2][2]; - radius1 = obb1->getExtent(1) * absC[2][2] + obb1->getExtent(2) * absC[1][2]; - radius2 = obb2->getExtent(0) * absC[0][1] + obb2->getExtent(1) * absC[0][0]; + radius1 = obb1->getExtent().getY() * absC[2][2] + obb1->getExtent().getZ() * absC[1][2]; + radius2 = obb2->getExtent().getX() * absC[0][1] + obb2->getExtent().getY() * absC[0][0]; min1 = -radius1; max1 = radius1; min2 = center - radius2; @@ -299,13 +299,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb1->getAxis(0).cross(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform1.getOrientation().getColumn(0).cross(transform2.getOrientation().getColumn(2)), boxDistance); // Compute the contact normal with the correct sign } // Axis A1 x B0 center = udc1[0] * c[2][0] - udc1[2] * c[0][0]; - radius1 = obb1->getExtent(0) * absC[2][0] + obb1->getExtent(2) * absC[0][0]; - radius2 = obb2->getExtent(1) * absC[1][2] + obb2->getExtent(2) * absC[1][1]; + radius1 = obb1->getExtent().getX() * absC[2][0] + obb1->getExtent().getZ() * absC[0][0]; + radius2 = obb2->getExtent().getY() * absC[1][2] + obb2->getExtent().getZ() * absC[1][1]; min1 = -radius1; max1 = radius1; min2 = center - radius2; @@ -316,13 +316,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb1->getAxis(1).cross(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform1.getOrientation().getColumn(1).cross(transform2.getOrientation().getColumn(0)), boxDistance); // Compute the contact normal with the correct sign } // Axis A1 x B1 center = udc1[0] * c[2][1] - udc1[2] * c[0][1]; - radius1 = obb1->getExtent(0) * absC[2][1] + obb1->getExtent(2) * absC[0][1]; - radius2 = obb2->getExtent(0) * absC[1][2] + obb2->getExtent(2) * absC[1][0]; + radius1 = obb1->getExtent().getX() * absC[2][1] + obb1->getExtent().getZ() * absC[0][1]; + radius2 = obb2->getExtent().getX() * absC[1][2] + obb2->getExtent().getZ() * absC[1][0]; min1 = -radius1; max1 = radius1; min2 = center - radius2; @@ -333,13 +333,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb1->getAxis(1).cross(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform1.getOrientation().getColumn(1).cross(transform2.getOrientation().getColumn(1)), boxDistance); // Compute the contact normal with the correct sign } // Axis A1 x B2 center = udc1[0] * c[2][2] - udc1[2] * c[0][2]; - radius1 = obb1->getExtent(0) * absC[2][2] + obb1->getExtent(2) * absC[0][2]; - radius2 = obb2->getExtent(0) * absC[1][1] + obb2->getExtent(1) * absC[1][0]; + radius1 = obb1->getExtent().getX() * absC[2][2] + obb1->getExtent().getZ() * absC[0][2]; + radius2 = obb2->getExtent().getX() * absC[1][1] + obb2->getExtent().getY() * absC[1][0]; min1 = -radius1; max1 = radius1; min2 = center - radius2; @@ -350,13 +350,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb1->getAxis(1).cross(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform1.getOrientation().getColumn(1).cross(transform2.getOrientation().getColumn(2)), boxDistance); // Compute the contact normal with the correct sign } // Axis A2 x B0 center = udc1[1] * c[0][0] - udc1[0] * c[1][0]; - radius1 = obb1->getExtent(0) * absC[1][0] + obb1->getExtent(1) * absC[0][0]; - radius2 = obb2->getExtent(1) * absC[2][2] + obb2->getExtent(2) * absC[2][1]; + radius1 = obb1->getExtent().getX() * absC[1][0] + obb1->getExtent().getY() * absC[0][0]; + radius2 = obb2->getExtent().getY() * absC[2][2] + obb2->getExtent().getZ() * absC[2][1]; min1 = -radius1; max1 = radius1; min2 = center - radius2; @@ -367,13 +367,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb1->getAxis(2).cross(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform1.getOrientation().getColumn(2).cross(transform2.getOrientation().getColumn(0)), boxDistance); // Compute the contact normal with the correct sign } // Axis A2 x B1 center = udc1[1] * c[0][1] - udc1[0] * c[1][1]; - radius1 = obb1->getExtent(0) * absC[1][1] + obb1->getExtent(1) * absC[0][1]; - radius2 = obb2->getExtent(0) * absC[2][2] + obb2->getExtent(2) * absC[2][0]; + radius1 = obb1->getExtent().getX() * absC[1][1] + obb1->getExtent().getY() * absC[0][1]; + radius2 = obb2->getExtent().getX() * absC[2][2] + obb2->getExtent().getZ() * absC[2][0]; min1 = -radius1; max1 = radius1; min2 = center - radius2; @@ -384,13 +384,13 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb1->getAxis(2).cross(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform1.getOrientation().getColumn(2).cross(transform2.getOrientation().getColumn(1)), boxDistance); // Compute the contact normal with the correct sign } // Axis A2 x B2 center = udc1[1] * c[0][2] - udc1[0] * c[1][2]; - radius1 = obb1->getExtent(0) * absC[1][2] + obb1->getExtent(1) * absC[0][2]; - radius2 = obb2->getExtent(0) * absC[2][1] + obb2->getExtent(1) * absC[2][0]; + radius1 = obb1->getExtent().getX() * absC[1][2] + obb1->getExtent().getY() * absC[0][2]; + radius2 = obb2->getExtent().getX() * absC[2][1] + obb2->getExtent().getY() * absC[2][0]; min1 = -radius1; max1 = radius1; min2 = center - radius2; @@ -401,7 +401,7 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const Transform& } else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth - normal = computeContactNormal(obb1->getAxis(2).cross(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign + normal = computeContactNormal(transform1.getOrientation().getColumn(2).cross(transform2.getOrientation().getColumn(2)), boxDistance); // Compute the contact normal with the correct sign } // Compute the contact info diff --git a/src/collision/SATAlgorithm.h b/src/collision/SATAlgorithm.h index 180dd70a..3804d830 100644 --- a/src/collision/SATAlgorithm.h +++ b/src/collision/SATAlgorithm.h @@ -50,8 +50,8 @@ namespace reactphysics3d { */ class SATAlgorithm : public NarrowPhaseAlgorithm { private : - bool computeCollisionTest(const OBB* const obb1, const Transform& transform1, - const OBB* const obb2, const Transform& transform2, + bool computeCollisionTest(const OBB* obb1, const Transform& transform1, + const OBB* obb2, const Transform& transform2, ContactInfo*& contactInfo) const; // Return true and compute a contact info if the two OBB collide double computePenetrationDepth(double min1, double max1, double min2, double max2) const; // Compute the penetration depth of two projection intervals Vector3D computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const; // Compute a contact normal @@ -60,11 +60,7 @@ class SATAlgorithm : public NarrowPhaseAlgorithm { SATAlgorithm(); // Constructor ~SATAlgorithm(); // Destructor - virtual bool testCollision(const NarrowBoundingVolume* const boundingVolume1, - const Transform& transform1, - const NarrowBoundingVolume* const boundingVolume2, - const Transform& transform2, - ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide + virtual bool testCollision(const Body* body1, const Body* body2, ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide }; // Return the contact normal with the correct sign (from obb1 toward obb2). "axis" is the axis vector direction where the diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp index d4cc2cc9..704839d6 100644 --- a/src/constraint/Contact.cpp +++ b/src/constraint/Contact.cpp @@ -52,8 +52,9 @@ Contact::~Contact() { // of the contact. The argument "noConstraint", is the row were the method have // to start to fill in the J_sp matrix. void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const { - RigidBody* rigidBody1 = dynamic_cast(body1); - RigidBody* rigidBody2 = dynamic_cast(body2); + assert(body1); + assert(body2); + Vector3D r1; Vector3D r2; Vector3D r1CrossN; @@ -62,13 +63,10 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const { Vector3D r2CrossU1; Vector3D r1CrossU2; Vector3D r2CrossU2; - Vector3D body1Position = rigidBody1->getTransform().getPosition(); - Vector3D body2Position = rigidBody2->getTransform().getPosition(); + Vector3D body1Position = body1->getTransform().getPosition(); + Vector3D body2Position = body2->getTransform().getPosition(); int currentIndex = noConstraint; // Current constraint index - assert(rigidBody1); - assert(rigidBody2); - // For each point in the contact for (int i=0; i(body1); RigidBody* rigidBody2 = dynamic_cast(body2); int index = noConstraint; - assert(rigidBody1); - assert(rigidBody2); assert(noConstraint >= 0 && noConstraint + nbConstraints <= errorValues.getNbComponent()); // Compute the error value for the contact constraint diff --git a/src/engine/PhysicsEngine.cpp b/src/engine/PhysicsEngine.cpp index c321e689..60fac5b3 100644 --- a/src/engine/PhysicsEngine.cpp +++ b/src/engine/PhysicsEngine.cpp @@ -143,9 +143,8 @@ void PhysicsEngine::updateAllBodiesMotion() { // Update the position and the orientation of the body according to the new velocity updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity); - // If the body state has changed, we have to update some informations in the rigid body - // TODO : DELETE THIS - rigidBody->update(); + // Update the AABB of the rigid body + rigidBody->updateAABB(); } } } diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index c9bf8cb3..60af0a51 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -38,7 +38,7 @@ #include "body/RigidBody.h" #include "engine/PhysicsWorld.h" #include "engine/PhysicsEngine.h" -#include "body/BoundingVolume.h" +#include "body/Shape.h" #include "body/OBB.h" #include "body/BoundingSphere.h" #include "body/AABB.h"