From bf8f9cf482d0a24f53ac8969a8a086438dfbe365 Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Thu, 30 Jun 2011 20:23:01 +0000 Subject: [PATCH] Add Transform and changes in code design git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@428 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- src/body/AABB.cpp | 84 +++++-------- src/body/AABB.h | 157 +++++++----------------- src/body/Body.cpp | 18 ++- src/body/Body.h | 57 +++++++-- src/body/BoundingSphere.cpp | 3 +- src/body/BoundingSphere.h | 13 +- src/body/BoundingVolume.h | 2 +- src/body/BroadBoundingVolume.cpp | 39 ------ src/body/BroadBoundingVolume.h | 52 -------- src/body/NarrowBoundingVolume.h | 3 +- src/body/OBB.cpp | 173 +++++--------------------- src/body/OBB.h | 203 ++++--------------------------- src/body/RigidBody.cpp | 18 ++- src/body/RigidBody.h | 74 ++--------- 14 files changed, 228 insertions(+), 668 deletions(-) delete mode 100644 src/body/BroadBoundingVolume.cpp delete mode 100644 src/body/BroadBoundingVolume.h diff --git a/src/body/AABB.cpp b/src/body/AABB.cpp index e52387b8..eef7a5fd 100644 --- a/src/body/AABB.cpp +++ b/src/body/AABB.cpp @@ -26,24 +26,22 @@ #include "AABB.h" #include #ifdef VISUAL_DEBUG - #include // TODO : Remove this in the final version - #include // TODO : Remove this in the final version + #include + #include #endif using namespace reactphysics3d; using namespace std; // Constructor -AABB::AABB(const Vector3D& center,double extentX, double extentY, double extentZ) { - this->center = center; +AABB::AABB(const Body* bodyPointer) : bodyPointer(bodyPointer) { + +} - this->extent[0] = extentX; - this->extent[1] = extentY; - this->extent[2] = extentZ; - - this->originalAABBExtent[0] = extentX; - this->originalAABBExtent[1] = extentY; - this->originalAABBExtent[2] = extentZ; +// Constructor +AABB::AABB(const Body* bodyPointer, const Transform& transform, const Vector3D& extents) + : bodyPointer(bodyPointer) { + update(transform, extents); } // Destructor @@ -55,64 +53,48 @@ AABB::~AABB() { // Draw the OBB (only for testing purpose) void AABB::draw() const { - Vector3D s1 = center + Vector3D(extent[0], extent[1], -extent[2]); - Vector3D s2 = center + Vector3D(extent[0], extent[1], extent[2]); - Vector3D s3 = center + Vector3D(-extent[0], extent[1], extent[2]); - Vector3D s4 = center + Vector3D(-extent[0], extent[1], -extent[2]); - Vector3D s5 = center + Vector3D(extent[0], -extent[1], -extent[2]); - Vector3D s6 = center + Vector3D(extent[0], -extent[1], extent[2]); - Vector3D s7 = center + Vector3D(-extent[0], -extent[1], extent[2]); - Vector3D s8 = center + Vector3D(-extent[0], -extent[1], -extent[2]); - // Draw in red glColor3f(1.0, 0.0, 0.0); // Draw the OBB glBegin(GL_LINES); - glVertex3f(s1.getX(), s1.getY(), s1.getZ()); - glVertex3f(s2.getX(), s2.getY(), s2.getZ()); + glVertex3f(maxCoordinates.getX(), minCoordinates.getY(), minCoordinates.getZ()); + glVertex3f(maxCoordinates.getX(), maxCoordinates.getY(), minCoordinates.getZ()); - glVertex3f(s2.getX(), s2.getY(), s2.getZ()); - glVertex3f(s3.getX(), s3.getY(), s3.getZ()); + glVertex3f(maxCoordinates.getX(), minCoordinates.getY(), minCoordinates.getZ()); + glVertex3f(maxCoordinates.getX(), minCoordinates.getY(), maxCoordinates.getZ()); - glVertex3f(s3.getX(), s3.getY(), s3.getZ()); - glVertex3f(s4.getX(), s4.getY(), s4.getZ()); + glVertex3f(maxCoordinates.getX(), minCoordinates.getY(), maxCoordinates.getZ()); + glVertex3f(maxCoordinates.getX(), maxCoordinates.getY(), maxCoordinates.getZ()); - glVertex3f(s4.getX(), s4.getY(), s4.getZ()); - glVertex3f(s1.getX(), s1.getY(), s1.getZ()); + glVertex3f(maxCoordinates.getX(), maxCoordinates.getY(), minCoordinates.getZ()); + glVertex3f(maxCoordinates.getX(), maxCoordinates.getY(), maxCoordinates.getZ()); - glVertex3f(s5.getX(), s5.getY(), s5.getZ()); - glVertex3f(s6.getX(), s6.getY(), s6.getZ()); + glVertex3f(minCoordinates.getX(), minCoordinates.getY(), minCoordinates.getZ()); + glVertex3f(minCoordinates.getX(), maxCoordinates.getY(), minCoordinates.getZ()); - glVertex3f(s6.getX(), s6.getY(), s6.getZ()); - glVertex3f(s7.getX(), s7.getY(), s7.getZ()); + glVertex3f(minCoordinates.getX(), minCoordinates.getY(), minCoordinates.getZ()); + glVertex3f(minCoordinates.getX(), minCoordinates.getY(), maxCoordinates.getZ()); - glVertex3f(s7.getX(), s7.getY(), s7.getZ()); - glVertex3f(s8.getX(), s8.getY(), s8.getZ()); + glVertex3f(minCoordinates.getX(), minCoordinates.getY(), maxCoordinates.getZ()); + glVertex3f(minCoordinates.getX(), maxCoordinates.getY(), maxCoordinates.getZ()); - glVertex3f(s8.getX(), s8.getY(), s8.getZ()); - glVertex3f(s5.getX(), s5.getY(), s5.getZ()); + glVertex3f(minCoordinates.getX(), maxCoordinates.getY(), minCoordinates.getZ()); + glVertex3f(minCoordinates.getX(), maxCoordinates.getY(), maxCoordinates.getZ()); - glVertex3f(s1.getX(), s1.getY(), s1.getZ()); - glVertex3f(s5.getX(), s5.getY(), s5.getZ()); + glVertex3f(minCoordinates.getX(), minCoordinates.getY(), minCoordinates.getZ()); + glVertex3f(maxCoordinates.getX(), minCoordinates.getY(), minCoordinates.getZ()); - glVertex3f(s4.getX(), s4.getY(), s4.getZ()); - glVertex3f(s8.getX(), s8.getY(), s8.getZ()); + glVertex3f(minCoordinates.getX(), maxCoordinates.getY(), minCoordinates.getZ()); + glVertex3f(maxCoordinates.getX(), maxCoordinates.getY(), minCoordinates.getZ()); - glVertex3f(s3.getX(), s3.getY(), s3.getZ()); - glVertex3f(s7.getX(), s7.getY(), s7.getZ()); + glVertex3f(minCoordinates.getX(), maxCoordinates.getY(), maxCoordinates.getZ()); + glVertex3f(maxCoordinates.getX(), maxCoordinates.getY(), maxCoordinates.getZ()); - glVertex3f(s2.getX(), s2.getY(), s2.getZ()); - glVertex3f(s6.getX(), s6.getY(), s6.getZ()); + glVertex3f(minCoordinates.getX(), minCoordinates.getY(), maxCoordinates.getZ()); + glVertex3f(maxCoordinates.getX(), minCoordinates.getY(), maxCoordinates.getZ()); glEnd(); } #endif -// Static method that computes an AABB from a set of vertices. The "center" argument corresponds to the center of the AABB -// This method allocates a new AABB object and return a pointer to the new allocated AABB object -AABB* AABB::computeFromVertices(const vector& vertices, const Vector3D& center) { - // TODO : Implement this method; - return NULL; -} - diff --git a/src/body/AABB.h b/src/body/AABB.h index a6b98455..898002d5 100644 --- a/src/body/AABB.h +++ b/src/body/AABB.h @@ -26,11 +26,13 @@ #define AABB_H // Libraries -#include "BroadBoundingVolume.h" #include "../mathematics/mathematics.h" // ReactPhysics3D namespace namespace reactphysics3d { + +// Declaration +class Body; /* ------------------------------------------------------------------- Class AABB : @@ -40,109 +42,46 @@ namespace reactphysics3d { point and three extent size in the x,y and z directions. ------------------------------------------------------------------- */ -class AABB : public BroadBoundingVolume { - protected : - Vector3D center; // Center point of the AABB - double extent[3]; // Three extents size in the x, y and z directions - double originalAABBExtent[3]; // Extents of the original AABB (this is used to update the AABB) +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 public : - AABB(const Vector3D& center, double extentX, double extentY, double extentZ); // Constructor - virtual ~AABB(); // Destructor + AABB(const Body* body); // Constructor + AABB(const Body* bodyPointer, const Transform& transform, const Vector3D& extents); // Constructor + virtual ~AABB(); // Destructor - Vector3D getCenter() const; // Return the center point - void setCenter(const Vector3D& center); // Set the center point - Vector3D getVertex(uint index) const throw (std::invalid_argument); // Return a vertex of the AABB - double getExtent(uint index) const throw(std::invalid_argument); // Return an extent value - void setExtent(uint index, double extent) throw(std::invalid_argument); // Set an extent value - double getMinValueOnAxis(uint axis) const throw(std::invalid_argument); // Return the minimum position value on the given axis - double getMaxValueOnAxis(uint axis) const throw(std::invalid_argument); // Return the maximum position value on the given axis - bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument - virtual void update(const Vector3D& newCenter, const Quaternion& rotationQuaternion); // 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 + 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 #ifdef VISUAL_DEBUG - virtual void draw() const; // Draw the AABB (only for testing purpose) + virtual void draw() const; // Draw the AABB (only for testing purpose) #endif - static AABB* computeFromVertices(const std::vector& vertices, const Vector3D& center); // Compute an AABB from a set of vertices }; -// Return the center point +// Return the center point of the AABB in world coordinates inline Vector3D AABB::getCenter() const { - return center; + return (minCoordinates + maxCoordinates) * 0.5; } -// Set the center point -inline void AABB::setCenter(const Vector3D& center) { - this->center = center; +// Return the minimum coordinates of the AABB +inline const Vector3D& AABB::getMinCoordinates() const { + return minCoordinates; } -// Return one of the 8 vertices of the AABB -inline Vector3D AABB::getVertex(unsigned int index) const throw (std::invalid_argument) { - // Check if the index value is valid - if (index >= 0 && index <8) { - switch(index) { - case 0 : return center + Vector3D(extent[0], extent[1], -extent[2]); - case 1 : return center + Vector3D(extent[0], extent[1], extent[2]); - case 2 : return center + Vector3D(-extent[0], extent[1], extent[2]); - case 3 : return center + Vector3D(-extent[0], extent[1], -extent[2]); - case 4 : return center + Vector3D(extent[0], -extent[1], -extent[2]); - case 5 : return center + Vector3D(extent[0], -extent[1], extent[2]); - case 6 : return center + Vector3D(-extent[0], -extent[1], extent[2]); - case 7 : return center + Vector3D(-extent[0], -extent[1], -extent[2]); - } - } - else { - // The index value is not valid, we throw an exception - throw std::invalid_argument("Exception : The index value has to be between 0 and 8"); - } +// Return the maximum coordinates of the AABB +inline const Vector3D& AABB::getMaxCoordinates() const { + return maxCoordinates; } - -// Return an extent value -inline double AABB::getExtent(unsigned int index) const throw(std::invalid_argument) { - // Check if the index value is valid - if (index >= 0 && index <3) { - return extent[index]; - } - else { - // The index value is not valid, we throw an exception - throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); - } -} - -// Set an extent value -inline void AABB::setExtent(unsigned int index, double extent) throw(std::invalid_argument) { - // Check if the index value is valid - if (index >= 0 && index <3) { - this->extent[index] = extent; - } - else { - // The index value is not valid, we throw an exception - throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); - } -} - -// Return the minimum position value on the given axis -// We consider the object margin here -inline double AABB::getMinValueOnAxis(uint axis) const throw(std::invalid_argument) { - switch (axis) { - case 0: return center.getX() - extent[0] - OBJECT_MARGIN; - case 1: return center.getY() - extent[1] - OBJECT_MARGIN; - case 2: return center.getZ() - extent[2] - OBJECT_MARGIN; - default: // The index value is not valid, we throw an exception - throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); - } -} - -// Return the maximum position value on the given axis -// We consider the object margin here -inline double AABB::getMaxValueOnAxis(uint axis) const throw(std::invalid_argument) { - switch (axis) { - case 0: return center.getX() + extent[0] + OBJECT_MARGIN; - case 1: return center.getY() + extent[1] + OBJECT_MARGIN; - case 2: return center.getZ() + extent[2] + OBJECT_MARGIN; - default: // The index value is not valid, we throw an exception - throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); - } +// Return a pointer to the owner body +inline const Body* AABB::getBodyPointer() const { + return bodyPointer; } // Return true if the current AABB is overlapping with the AABB in argument @@ -150,32 +89,24 @@ inline double AABB::getMaxValueOnAxis(uint axis) const throw(std::invalid_argume inline bool AABB::testCollision(const AABB& aabb) const { Vector3D center2 = aabb.getCenter(); double margin = 2 * OBJECT_MARGIN; - if (std::abs(center.getX() - center2.getX()) > (extent[0] + aabb.getExtent(0) + margin)) return false; - if (std::abs(center.getY() - center2.getY()) > (extent[1] + aabb.getExtent(1) + margin)) return false; - if (std::abs(center.getZ() - center2.getZ()) > (extent[2] + aabb.getExtent(2) + margin)) return false; + 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 || + aabb.maxCoordinates.getY() + OBJECT_MARGIN < minCoordinates.getY() - OBJECT_MARGIN) return false; + if (maxCoordinates.getZ() + OBJECT_MARGIN < aabb.minCoordinates.getZ() - OBJECT_MARGIN || + aabb.maxCoordinates.getZ() + OBJECT_MARGIN < minCoordinates.getZ() - OBJECT_MARGIN) return false; return true; } -// Update the orientation of the AABB according to the orientation of the rigid body -// In order to compute the new AABB we use the original AABB (represented by the originalAABBExtent -// values). The goal is to rotate the original AABB according to the current rotation (rotationQuaternion) -// and then compute the new extent values from the rotated axis of the original AABB. The three columns of -// the rotation matrix correspond to the rotated axis of the rotated original AABB. The we have to compute -// the projections of the three rotated axis onto the x, y and z axis. The projections are easy to compute -// because for instance if the size of the projection of the vector (4, 5, 6) onto the x axis is simply 4. -inline void AABB::update(const Vector3D& newCenter, const Quaternion& rotationQuaternion) { - // Update the center of the AABB - center = newCenter; - - // Recompute the new extents size from the rotated AABB - Matrix rotationMatrix = rotationQuaternion.getMatrix(); // Rotation matrix - for (int i=0; i<3; i++) { // For each x, y and z axis - extent[i] = 0.0; - for (int j=0; j<3; j++) { // For each rotated axis of the rotated original AABB - // Add the size of the projection of the current rotated axis to the extent of the current (x, y, or z) axis - extent[i] += std::abs(rotationMatrix.getValue(i, j)) * originalAABBExtent[j]; - } - } +// Update the world minimum and maximum coordinates of the AABB on the three x,y and z axis +inline void AABB::update(const Transform& newTransform, const Vector3D& extents) { + Vector3D localExtents = extents + Vector3D(OBJECT_MARGIN, OBJECT_MARGIN, OBJECT_MARGIN); + Matrix3x3 worldAxis = newTransform.getOrientation().getAbsoluteMatrix(); + Vector3D worldExtents = Vector3D(worldAxis.getColumn(0).dot(localExtents), + worldAxis.getColumn(1).dot(localExtents), + worldAxis.getColumn(2).dot(localExtents)); + minCoordinates = newTransform.getPosition() - worldExtents; + maxCoordinates = newTransform.getPosition() + worldExtents; } }; // End of the ReactPhysics3D namespace diff --git a/src/body/Body.cpp b/src/body/Body.cpp index 64079491..4fa64371 100644 --- a/src/body/Body.cpp +++ b/src/body/Body.cpp @@ -31,31 +31,45 @@ using namespace reactphysics3d; // Constructor -Body::Body(double mass) throw(std::invalid_argument) - : mass(mass), broadBoundingVolume(0), narrowBoundingVolume(0) { +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"); } + + // Initialize the old transform + oldTransform = transform; + + // Create the AABB for Broad-Phase collision detection + aabb = new AABB(this); } // 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) { diff --git a/src/body/Body.h b/src/body/Body.h index 7bc4555a..b7f62cfe 100644 --- a/src/body/Body.h +++ b/src/body/Body.h @@ -28,12 +28,12 @@ // Libraries #include #include - +#include "../mathematics/Transform.h" +#include "../body/AABB.h" // Namespace reactphysics3d namespace reactphysics3d { -class BroadBoundingVolume; class NarrowBoundingVolume; /* ------------------------------------------------------------------- @@ -44,27 +44,35 @@ class NarrowBoundingVolume; */ class Body { protected : - double mass; // Mass of the body - BroadBoundingVolume* broadBoundingVolume; // Bounding volume used for the broad-phase collision detection + 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 setBroadBoundingVolume(BroadBoundingVolume* broadBoundingVolume); // Set the broad-phase bounding volume void setNarrowBoundingVolume(NarrowBoundingVolume* narrowBoundingVolume); // Set the narrow-phase bounding volume public : - Body(double mass) throw(std::invalid_argument); // Constructor + Body(const Transform& transform, double mass) throw(std::invalid_argument); // Constructor virtual ~Body(); // Destructor 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 + void setTransform(const Transform& transform); // Set the current position and orientation + const AABB* getAABB() const; // Return the AAABB of the body + Transform getInterpolatedTransform() const; // Return the interpolated transform for rendering + void setInterpolationFactor(double factor); // Set the interpolation factor of the body bool getIsMotionEnabled() const; // Return if the rigid body can move 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 BroadBoundingVolume* getBroadBoundingVolume() const; // Return the broad-phase bounding volume const NarrowBoundingVolume* getNarrowBoundingVolume() const; // Return the narrow-phase bounding volume of the body + void updateOldTransform(); // Update the old transform with the current one }; // Method that return the mass of the body @@ -72,6 +80,17 @@ inline double Body::getMass() const { return mass; }; +// Return the interpolated transform for rendering +inline Transform Body::getInterpolatedTransform() const { + return Transform::interpolateTransforms(oldTransform, transform, interpolationFactor); +} + +// Set the interpolation factor of the body +inline void Body::setInterpolationFactor(double factor) { + // Set the factor + interpolationFactor = factor; +} + // Return if the rigid body can move inline bool Body::getIsMotionEnabled() const { return isMotionEnabled; @@ -87,6 +106,21 @@ inline void Body::setMass(double mass) { this->mass = mass; } +// Return the current position and orientation +inline const Transform& Body::getTransform() const { + return transform; +} + +// Set the current position and orientation +inline void Body::setTransform(const Transform& transform) { + this->transform = transform; +} + +// Return the AAABB of the body +inline const AABB* Body::getAABB() const { + return aabb; +} + // Return true if the body can collide with others bodies inline bool Body::getIsCollisionEnabled() const { return isCollisionEnabled; @@ -97,16 +131,25 @@ 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; +} + + } #endif diff --git a/src/body/BoundingSphere.cpp b/src/body/BoundingSphere.cpp index b2004d93..28fd66da 100644 --- a/src/body/BoundingSphere.cpp +++ b/src/body/BoundingSphere.cpp @@ -60,9 +60,10 @@ void BoundingSphere::draw() const { } #endif - +/*TODO: DELETE THIS // Return the corresponding AABB AABB* BoundingSphere::computeAABB() const { // Create and return the AABB return new AABB(center, radius, radius, radius); } +*/ \ No newline at end of file diff --git a/src/body/BoundingSphere.h b/src/body/BoundingSphere.h index 1b8ae4f1..4fb4d41b 100644 --- a/src/body/BoundingSphere.h +++ b/src/body/BoundingSphere.h @@ -52,8 +52,9 @@ class BoundingSphere : public NarrowBoundingVolume { void setRadius(double radius); // Set the radius of the sphere virtual void update(const Vector3D& newCenter, const Quaternion& rotationQuaternion); // Update the sphere orientation according to a new orientation of the rigid body - virtual AABB* computeAABB() const; // Return the corresponding AABB + // TODO : DELETE virtual AABB* computeAABB() const; // Return the corresponding AABB virtual Vector3D getSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a support point in a given direction + virtual Vector3D getLocalExtents() const; // Return the local extents in x,y and z direction #ifdef VISUAL_DEBUG virtual void draw() const; // Draw the sphere (only for testing purpose) @@ -94,12 +95,18 @@ inline Vector3D BoundingSphere::getSupportPoint(const Vector3D& direction, doubl // If the direction vector is not the zero vector if (length > 0.0) { // Return the support point of the sphere in the given direction - return center + (radius + margin) * direction.getUnit(); + return (radius + margin) * direction.getUnit(); } // If the direction vector is the zero vector we return a point on the // boundary of the sphere - return center + Vector3D(0, radius + margin, 0); + return Vector3D(0, radius + margin, 0); +} + +// Return the local extents of the shape (half-width) in x,y and z local direction +// This method is used to compute the AABB of the box +inline Vector3D BoundingSphere::getLocalExtents() const { + return Vector3D(radius, radius, radius); } }; // End of the ReactPhysics3D namespace diff --git a/src/body/BoundingVolume.h b/src/body/BoundingVolume.h index be9f80d0..a27f155a 100644 --- a/src/body/BoundingVolume.h +++ b/src/body/BoundingVolume.h @@ -52,7 +52,7 @@ class BoundingVolume { Body* getBodyPointer() const; // Return the body pointer void setBodyPointer(Body* body); // Set the body pointer - virtual void update(const Vector3D& newCenter, const Quaternion& rotationQuaternion)=0; // Update the orientation of the bounding volume according to the new orientation of the body + #ifdef VISUAL_DEBUG virtual void draw() const=0; // Display the bounding volume (only for testing purpose) #endif diff --git a/src/body/BroadBoundingVolume.cpp b/src/body/BroadBoundingVolume.cpp deleted file mode 100644 index fa7a59ef..00000000 --- a/src/body/BroadBoundingVolume.cpp +++ /dev/null @@ -1,39 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010 Daniel Chappuis * -********************************************************************************* -* * -* Permission is hereby granted, free of charge, to any person obtaining a copy * -* of this software and associated documentation files (the "Software"), to deal * -* in the Software without restriction, including without limitation the rights * -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * -* copies of the Software, and to permit persons to whom the Software is * -* furnished to do so, subject to the following conditions: * -* * -* The above copyright notice and this permission notice shall be included in * -* all copies or substantial portions of the Software. * -* * -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * -* THE SOFTWARE. * -********************************************************************************/ - -// Libraries -#include "BroadBoundingVolume.h" - -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; - -// Constructor -BroadBoundingVolume::BroadBoundingVolume() : BoundingVolume() { - -} - -// Destructor -BroadBoundingVolume::~BroadBoundingVolume() { - -} diff --git a/src/body/BroadBoundingVolume.h b/src/body/BroadBoundingVolume.h deleted file mode 100644 index dad2e361..00000000 --- a/src/body/BroadBoundingVolume.h +++ /dev/null @@ -1,52 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010 Daniel Chappuis * -********************************************************************************* -* * -* Permission is hereby granted, free of charge, to any person obtaining a copy * -* of this software and associated documentation files (the "Software"), to deal * -* in the Software without restriction, including without limitation the rights * -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell * -* copies of the Software, and to permit persons to whom the Software is * -* furnished to do so, subject to the following conditions: * -* * -* The above copyright notice and this permission notice shall be included in * -* all copies or substantial portions of the Software. * -* * -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE * -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER * -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, * -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN * -* THE SOFTWARE. * -********************************************************************************/ - -#ifndef BROAD_BOUNDING_VOLUME_H -#define BROAD_BOUNDING_VOLUME_H - -// Libraries -#include "BoundingVolume.h" - - -// ReactPhysics3D namespace -namespace reactphysics3d { - -/* ------------------------------------------------------------------- - Class BroadBoundingVolume : - This class represents the volume that contains a rigid body - This volume will be used to compute the broad-phase collision - detection. - ------------------------------------------------------------------- -*/ -class BroadBoundingVolume : public BoundingVolume { - protected : - - public : - BroadBoundingVolume(); // Constructor - virtual ~BroadBoundingVolume(); // Destructor -}; - -} - -#endif \ No newline at end of file diff --git a/src/body/NarrowBoundingVolume.h b/src/body/NarrowBoundingVolume.h index d0a19e97..b477eff0 100644 --- a/src/body/NarrowBoundingVolume.h +++ b/src/body/NarrowBoundingVolume.h @@ -48,8 +48,9 @@ class NarrowBoundingVolume : public BoundingVolume { NarrowBoundingVolume(); // Constructor virtual ~NarrowBoundingVolume(); // Destructor - virtual AABB* computeAABB() const=0; // Return the corresponding AABB + // TODO : DELETE virtual AABB* computeAABB() const=0; // Return the corresponding AABB 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 }; } diff --git a/src/body/OBB.cpp b/src/body/OBB.cpp index 426a948e..7c84845b 100644 --- a/src/body/OBB.cpp +++ b/src/body/OBB.cpp @@ -36,21 +36,8 @@ using namespace reactphysics3d; using namespace std; // Constructor -OBB::OBB(const Vector3D& center, const Vector3D& axis1, const Vector3D& axis2, - const Vector3D& axis3, double extent1, double extent2, double extent3) { - this->center = center; +OBB::OBB(const Vector3D& extent) : extent(extent) { - oldAxis[0] = axis1.getUnit(); - oldAxis[1] = axis2.getUnit(); - oldAxis[2] = axis3.getUnit(); - - this->axis[0] = oldAxis[0]; - this->axis[1] = oldAxis[1]; - this->axis[2] = oldAxis[2]; - - this->extent[0] = extent1; - this->extent[1] = extent2; - this->extent[2] = extent3; } // Destructor @@ -61,155 +48,51 @@ OBB::~OBB() { #ifdef VISUAL_DEBUG // Draw the OBB (only for testing purpose) void OBB::draw() const { - double e0 = extent[0]; - double e1 = extent[1]; - double e2 = extent[2]; - - Vector3D s1 = center + (axis[0]*e0) + (axis[1]*e1) - (axis[2]*e2); - Vector3D s2 = center + (axis[0]*e0) + (axis[1]*e1) + (axis[2]*e2); - Vector3D s3 = center - (axis[0]*e0) + (axis[1]*e1) + (axis[2]*e2); - Vector3D s4 = center - (axis[0]*e0) + (axis[1]*e1) - (axis[2]*e2); - Vector3D s5 = center + (axis[0]*e0) - (axis[1]*e1) - (axis[2]*e2); - Vector3D s6 = center + (axis[0]*e0) - (axis[1]*e1) + (axis[2]*e2); - Vector3D s7 = center - (axis[0]*e0) - (axis[1]*e1) + (axis[2]*e2); - Vector3D s8 = center - (axis[0]*e0) - (axis[1]*e1) - (axis[2]*e2); + double e1 = extent.getX(); + double e2 = extent.getY(); + double e3 = extent.getZ(); // Draw in red glColor3f(1.0, 0.0, 0.0); // Draw the OBB glBegin(GL_LINES); - glVertex3f(s1.getX(), s1.getY(), s1.getZ()); - glVertex3f(s2.getX(), s2.getY(), s2.getZ()); + glVertex3f(e1, -e2, -e3); + glVertex3f(e1, e2, -e3); - glVertex3f(s2.getX(), s2.getY(), s2.getZ()); - glVertex3f(s3.getX(), s3.getY(), s3.getZ()); + glVertex3f(e1, -e2, -e3); + glVertex3f(e1, -e2, e3); - glVertex3f(s3.getX(), s3.getY(), s3.getZ()); - glVertex3f(s4.getX(), s4.getY(), s4.getZ()); + glVertex3f(e1, -e2, e3); + glVertex3f(e1, e2, e3); - glVertex3f(s4.getX(), s4.getY(), s4.getZ()); - glVertex3f(s1.getX(), s1.getY(), s1.getZ()); + glVertex3f(e1, e2, e3); + glVertex3f(e1, e2, -e3); - glVertex3f(s5.getX(), s5.getY(), s5.getZ()); - glVertex3f(s6.getX(), s6.getY(), s6.getZ()); + glVertex3f(-e1, -e2, -e3); + glVertex3f(-e1, e2, -e3); - glVertex3f(s6.getX(), s6.getY(), s6.getZ()); - glVertex3f(s7.getX(), s7.getY(), s7.getZ()); + glVertex3f(-e1, -e2, -e3); + glVertex3f(-e1, -e2, e3); - glVertex3f(s7.getX(), s7.getY(), s7.getZ()); - glVertex3f(s8.getX(), s8.getY(), s8.getZ()); + glVertex3f(-e1, -e2, e3); + glVertex3f(-e1, e2, e3); - glVertex3f(s8.getX(), s8.getY(), s8.getZ()); - glVertex3f(s5.getX(), s5.getY(), s5.getZ()); + glVertex3f(-e1, e2, e3); + glVertex3f(-e1, e2, -e3); - glVertex3f(s1.getX(), s1.getY(), s1.getZ()); - glVertex3f(s5.getX(), s5.getY(), s5.getZ()); + glVertex3f(e1, -e2, -e3); + glVertex3f(-e1, -e2, -e3); - glVertex3f(s4.getX(), s4.getY(), s4.getZ()); - glVertex3f(s8.getX(), s8.getY(), s8.getZ()); + glVertex3f(e1, -e2, -e3); + glVertex3f(-e1, -e2, -e3); - glVertex3f(s3.getX(), s3.getY(), s3.getZ()); - glVertex3f(s7.getX(), s7.getY(), s7.getZ()); + glVertex3f(e1, -e2, e3); + glVertex3f(-e1, -e2, e3); - glVertex3f(s2.getX(), s2.getY(), s2.getZ()); - glVertex3f(s6.getX(), s6.getY(), s6.getZ()); - - glVertex3f(center.getX(), center.getY(), center.getZ()); - glVertex3f(center.getX() + 8.0 * axis[1].getX(), center.getY() + 8.0 * axis[1].getY(), center.getZ() + 8.0 * axis[1].getZ()); + glVertex3f(e1, e2, e3); + glVertex3f(-e1, e2, e3); glEnd(); } #endif - -// Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis. -// If the extreme vertices are part of a face of the OBB, the returned vertices will be ordered according to the face. -vector OBB::getExtremeVertices(const Vector3D& directionAxis) const { - assert(directionAxis.length() != 0); - - vector extremeVertices; - - // Check if the given axis is parallel to an axis on the OBB - if (axis[0].isParallelWith(directionAxis)) { - if (axis[0].dot(directionAxis) >= 0) { // If both axis are in the same direction - extremeVertices = getFace(0); // The extreme is the face 0 - } - else { - extremeVertices = getFace(1); // The extreme is the face 1 - } - } - else if(axis[1].isParallelWith(directionAxis)) { - if (axis[1].dot(directionAxis) >= 0) { // If both axis are in the same direction - extremeVertices = getFace(2); // The extreme is the face 2 - } - else { - extremeVertices = getFace(3); // The extreme is the face 3 - } - - } - else if(axis[2].isParallelWith(directionAxis)) { - if (axis[2].dot(directionAxis) >= 0) { // If both axis are in the same direction - extremeVertices = getFace(4); // The extreme is the face 4 - } - else { - extremeVertices = getFace(5); // The extreme is the face 5 - } - } - else { // The extreme is made of an unique vertex or an edge - double maxProjectionLength = 0.0; // Longest projection length of a vertex onto the projection axis - - // For each vertex of the OBB - for (unsigned int i=0; i<8; ++i) { - Vector3D vertex = getVertex(i); - - // Compute the projection length of the current vertex onto the projection axis - double projectionLength = directionAxis.dot(vertex-center) / directionAxis.length(); - - // If we found a bigger projection length - if (projectionLength > maxProjectionLength + EPSILON_TEST) { - maxProjectionLength = projectionLength; - extremeVertices.clear(); - extremeVertices.push_back(vertex); - } - else if (approxEqual(projectionLength, maxProjectionLength)) { - extremeVertices.push_back(vertex); - } - } - - assert(extremeVertices.size() == 1 || extremeVertices.size() == 2); - } - - // An extreme should be a unique vertex, an edge or a face - assert(extremeVertices.size() == 1 || extremeVertices.size() == 2 || extremeVertices.size() == 4); - - // Return the extreme vertices - return extremeVertices; -} - -// Static method that computes an OBB from a set of vertices. The "center" argument corresponds to the center of the OBB -// This method allocates a new OBB object and return a pointer to the new allocated OBB object -OBB* OBB::computeFromVertices(const vector& vertices, const Vector3D& center) { - // TODO : Implement this method; - return NULL; -} - -// Return the corresponding AABB -AABB* OBB::computeAABB() const { - double maxLength[] = {0.0, 0.0, 0.0}; // Maximum length for each of the three x,y and z axis - Vector3D vertex; - double length; - - // For each vertex of the OBB - for (int i = 0; i<8; i++) { - vertex = getVertex(i) - center; - for (int j=0; j<3; j++) { - length = std::abs(vertex.getValue(j)); - if (length > maxLength[j]) { - maxLength[j] = length; - } - } - } - - // Create and return the AABB - return new AABB(center, maxLength[0], maxLength[1], maxLength[2]); -} diff --git a/src/body/OBB.h b/src/body/OBB.h index 413daaad..331354cf 100644 --- a/src/body/OBB.h +++ b/src/body/OBB.h @@ -35,206 +35,53 @@ namespace reactphysics3d { /* ------------------------------------------------------------------- Class OBB : - This class represents a bounding volume of type "Oriented Bounding - Box". It's a box that has a given orientation is space. The three - axis of the OBB are three axis that give the orientation of the - OBB. The three axis are normal vectors to the faces of the OBB. - Those axis are unit length. The three extents are half-widths - of the box along the three axis of the OBB. + This class represents a 3D box. Those axis are unit length. + The three extents are half-widths of the box along the three + axis x, y, z local axis. The "transform" of the corresponding + rigid body given an orientation and a position to the box ------------------------------------------------------------------- */ class OBB : public NarrowBoundingVolume { - protected : - Vector3D center; // Center point of the OBB - Vector3D oldAxis[3]; // Array that contains the three unit length axis at the beginning - Vector3D axis[3]; // Array that contains the three unit length axis of the OBB - double extent[3]; // Array that contains the three extents size of the OBB + private : + Vector3D extent; // Extent sizes of the box public : - OBB(const Vector3D& center, const Vector3D& axis1, const Vector3D& axis2, - const Vector3D& axis3, double extent1, double extent2, double extent3); // Constructor - virtual ~OBB(); // Destructor + OBB(const Vector3D& extent); // Constructor + virtual ~OBB(); // Destructor - Vector3D getCenter() const; // Return the center point of the OBB - void setCenter(const Vector3D& center); // Set the center point - Vector3D getAxis(unsigned int index) const throw(std::invalid_argument); // Return an axis of the OBB - void setAxis(unsigned int index, const Vector3D& axis) throw(std::invalid_argument); // Set an axis - Vector3D getVertex(unsigned int index) const throw (std::invalid_argument); // Return a vertex of the OBB - std::vector getFace(unsigned int index) const throw(std::invalid_argument); // Return the 4 vertices the OBB's face in the direction of a given axis - double getExtent(unsigned int index) const throw(std::invalid_argument); // Return an extent value - void setExtent(unsigned int index, double extent) throw(std::invalid_argument); // Set an extent value - virtual std::vector getExtremeVertices(const Vector3D& axis) const; // Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis - virtual void update(const Vector3D& newCenter, const Quaternion& rotationQuaternion); // Update the oriented bounding box orientation according to a new orientation of the rigid body - virtual AABB* computeAABB() const; // Return the corresponding AABB - virtual Vector3D getSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a support point in a given direction + const Vector3D& getExtent() const; // Return the extents of the box + void setExtent(const Vector3D& extent); // Set the extents of the box + virtual Vector3D getLocalExtents() const; // Return the local extents in x,y and z direction + virtual Vector3D getSupportPoint(const Vector3D& direction, double margin=0.0) const; // Return a support point in a given direction #ifdef VISUAL_DEBUG virtual void draw() const; // Draw the OBB (only for testing purpose) #endif - static OBB* computeFromVertices(const std::vector& vertices, const Vector3D& center); // Compute an OBB from a set of vertices }; -// Return the center point of the OBB -inline Vector3D OBB::getCenter() const { - return center; +// Return the extents of the box +inline const Vector3D& OBB::getExtent() const { + return extent; } -// Set the center point -inline void OBB::setCenter(const Vector3D& center) { - this->center = center; + // Set the extents of the box +inline void OBB::setExtent(const Vector3D& extent) { + this->extent = extent; } -// Return an axis of the OBB -inline Vector3D OBB::getAxis(unsigned int index) const throw(std::invalid_argument) { - // Check if the index value is valid - if (index >= 0 && index <3) { - return axis[index]; - } - else { - // The index value is not valid, we throw an exception - throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); - } -} - -// Set an axis -inline void OBB::setAxis(unsigned int index, const Vector3D& axis) throw(std::invalid_argument) { - // Check if the index value is valid - if (index >= 0 && index <3) { - this->axis[index] = axis; - } - else { - // The index value is not valid, we throw an exception - throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); - } -} - -// Return a vertex of the OBB -inline Vector3D OBB::getVertex(unsigned int index) const throw (std::invalid_argument) { - // Check if the index value is valid - if (index >= 0 && index <8) { - Vector3D vertex; - - switch(index) { - case 0 : vertex = center + (axis[0]*extent[0]) + (axis[1]*extent[1]) - (axis[2]*extent[2]); - break; - case 1 : vertex = center + (axis[0]*extent[0]) + (axis[1]*extent[1]) + (axis[2]*extent[2]); - break; - case 2 : vertex = center - (axis[0]*extent[0]) + (axis[1]*extent[1]) + (axis[2]*extent[2]); - break; - case 3 : vertex = center - (axis[0]*extent[0]) + (axis[1]*extent[1]) - (axis[2]*extent[2]); - break; - case 4 : vertex = center + (axis[0]*extent[0]) - (axis[1]*extent[1]) - (axis[2]*extent[2]); - break; - case 5 : vertex = center + (axis[0]*extent[0]) - (axis[1]*extent[1]) + (axis[2]*extent[2]); - break; - case 6 : vertex = center - (axis[0]*extent[0]) - (axis[1]*extent[1]) + (axis[2]*extent[2]); - break; - case 7 : vertex = center - (axis[0]*extent[0]) - (axis[1]*extent[1]) - (axis[2]*extent[2]); - break; - } - - // Return the vertex - return vertex; - } - else { - // The index value is not valid, we throw an exception - throw std::invalid_argument("Exception : The index value has to be between 0 and 8"); - } -} - -// Return the 4 vertices of a face of the OBB. The 4 vertices will be ordered. The convention is that the index 0 corresponds to -// the face in the direction of the axis[0], 1 corresponds to the face in the opposite direction of the axis[0], 2 corresponds to -// the face in the direction of the axis[1], etc. -inline std::vector OBB::getFace(unsigned int index) const throw(std::invalid_argument) { - // Check the argument - if (index >=0 && index <6) { - std::vector vertices; - switch(index) { - case 0: vertices.push_back(center + (axis[0]*extent[0]) + (axis[1]*extent[1]) - (axis[2]*extent[2])); - vertices.push_back(center + (axis[0]*extent[0]) + (axis[1]*extent[1]) + (axis[2]*extent[2])); - vertices.push_back(center + (axis[0]*extent[0]) - (axis[1]*extent[1]) + (axis[2]*extent[2])); - vertices.push_back(center + (axis[0]*extent[0]) - (axis[1]*extent[1]) - (axis[2]*extent[2])); - break; - case 1: vertices.push_back(center - (axis[0]*extent[0]) + (axis[1]*extent[1]) - (axis[2]*extent[2])); - vertices.push_back(center - (axis[0]*extent[0]) + (axis[1]*extent[1]) + (axis[2]*extent[2])); - vertices.push_back(center - (axis[0]*extent[0]) - (axis[1]*extent[1]) + (axis[2]*extent[2])); - vertices.push_back(center - (axis[0]*extent[0]) - (axis[1]*extent[1]) - (axis[2]*extent[2])); - break; - case 2: vertices.push_back(center + (axis[1]*extent[1]) + (axis[0]*extent[0]) - (axis[2]*extent[2])); - vertices.push_back(center + (axis[1]*extent[1]) + (axis[0]*extent[0]) + (axis[2]*extent[2])); - vertices.push_back(center + (axis[1]*extent[1]) - (axis[0]*extent[0]) + (axis[2]*extent[2])); - vertices.push_back(center + (axis[1]*extent[1]) - (axis[0]*extent[0]) - (axis[2]*extent[2])); - break; - case 3: vertices.push_back(center - (axis[1]*extent[1]) + (axis[0]*extent[0]) - (axis[2]*extent[2])); - vertices.push_back(center - (axis[1]*extent[1]) + (axis[0]*extent[0]) + (axis[2]*extent[2])); - vertices.push_back(center - (axis[1]*extent[1]) - (axis[0]*extent[0]) + (axis[2]*extent[2])); - vertices.push_back(center - (axis[1]*extent[1]) - (axis[0]*extent[0]) - (axis[2]*extent[2])); - break; - case 4: vertices.push_back(center + (axis[2]*extent[2]) + (axis[0]*extent[0]) - (axis[1]*extent[1])); - vertices.push_back(center + (axis[2]*extent[2]) + (axis[0]*extent[0]) + (axis[1]*extent[1])); - vertices.push_back(center + (axis[2]*extent[2]) - (axis[0]*extent[0]) + (axis[1]*extent[1])); - vertices.push_back(center + (axis[2]*extent[2]) - (axis[0]*extent[0]) - (axis[1]*extent[1])); - break; - case 5: vertices.push_back(center - (axis[2]*extent[2]) + (axis[0]*extent[0]) - (axis[1]*extent[1])); - vertices.push_back(center - (axis[2]*extent[2]) + (axis[0]*extent[0]) + (axis[1]*extent[1])); - vertices.push_back(center - (axis[2]*extent[2]) - (axis[0]*extent[0]) + (axis[1]*extent[1])); - vertices.push_back(center - (axis[2]*extent[2]) - (axis[0]*extent[0]) - (axis[1]*extent[1])); - break; - } - - // Return the vertices - assert(vertices.size() == 4); - return vertices; - } - else { - // Throw an exception - throw std::invalid_argument("Exception: The argument must be between 0 and 5"); - } -} - -// Return an extent value -inline double OBB::getExtent(unsigned int index) const throw(std::invalid_argument) { - // Check if the index value is valid - if (index >= 0 && index <3) { - return extent[index]; - } - else { - // The index value is not valid, we throw an exception - throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); - } -} - -// Set an extent value -inline void OBB::setExtent(unsigned int index, double extent) throw(std::invalid_argument) { - // Check if the index value is valid - if (index >= 0 && index <3) { - this->extent[index] = extent; - } - else { - // The index value is not valid, we throw an exception - throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); - } -} - -// Update the orientation of the OBB according to the orientation of the rigid body -inline void OBB::update(const Vector3D& newCenter, const Quaternion& rotationQuaternion) { - // Update the center of the OBB - center = newCenter; - - // Update the three axis of the OBB by rotating and normalize then - axis[0] = rotateVectorWithQuaternion(oldAxis[0], rotationQuaternion).getUnit(); - axis[1] = rotateVectorWithQuaternion(oldAxis[1], rotationQuaternion).getUnit(); - axis[2] = rotateVectorWithQuaternion(oldAxis[2], rotationQuaternion).getUnit(); +// Return the local extents of the shape (half-width) in x,y and z local direction +// This method is used to compute the AABB of the box +inline Vector3D OBB::getLocalExtents() const { + return extent; } // Return a support point in a given direction inline Vector3D OBB::getSupportPoint(const Vector3D& direction, double margin) const { - assert(direction.length() > 0.0); assert(margin >= 0.0); - // TODO : Implement this method - assert(false); - return Vector3D(); + return Vector3D(direction.getX() < 0.0 ? -extent.getX()-margin : extent.getX()+margin, + direction.getY() < 0.0 ? -extent.getY()-margin : extent.getY()+margin, + direction.getZ() < 0.0 ? -extent.getZ()-margin : extent.getZ()+margin); } }; // End of the ReactPhysics3D namespace diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 9e375edb..53374729 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -31,10 +31,11 @@ using namespace reactphysics3d; // Constructor - RigidBody::RigidBody(const Vector3D& position, const Quaternion& orientation, double mass, const Matrix3x3& inertiaTensorLocal, + // 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(mass), position(position), orientation(orientation.getUnit()), inertiaTensorLocal(inertiaTensorLocal), - inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass), oldPosition(position), oldOrientation(orientation) { + : Body(transform, mass), inertiaTensorLocal(inertiaTensorLocal), + inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass) { restitution = 1.0; isMotionEnabled = true; @@ -44,13 +45,9 @@ // Set the bounding volume for the narrow-phase collision detection setNarrowBoundingVolume(narrowBoundingVolume); - // Compute the broad-phase bounding volume (an AABB) - setBroadBoundingVolume(narrowBoundingVolume->computeAABB()); - // Update the orientation of the OBB according to the orientation of the rigid body update(); - assert(broadBoundingVolume); assert(narrowBoundingVolume); } @@ -61,7 +58,8 @@ RigidBody::~RigidBody() { // Update the rigid body in order to reflect a change in the body state void RigidBody::update() { - // Update the orientation of the corresponding bounding volumes of the rigid body - broadBoundingVolume->update(position, orientation); - narrowBoundingVolume->update(position, orientation); + // 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 e3ecf788..6eba6636 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -42,10 +42,11 @@ namespace reactphysics3d { */ class RigidBody : public Body { protected : - 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 + // 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 @@ -53,18 +54,13 @@ class RigidBody : public Body { Matrix3x3 inertiaTensorLocal; // Local inertia tensor of the body (in body coordinates) Matrix3x3 inertiaTensorLocalInverse; // Inverse of the inertia tensor of the body (in body coordinates) double massInverse; // Inverse of the mass of the body - double interpolationFactor; // Interpolation factor used for the state interpolation double restitution; // Coefficient of restitution (between 0 and 1), 1 for a very boucing body public : - RigidBody(const Vector3D& position, const Quaternion& orientation, double mass, + RigidBody(const Transform& transform, double mass, const Matrix3x3& inertiaTensorLocal, NarrowBoundingVolume* narrowBoundingVolume); // Constructor // Copy-constructor virtual ~RigidBody(); // Destructor - Vector3D getPosition() const; // Return the position of the body - void setPosition(const Vector3D& position); // Set the position of the body - Quaternion getOrientation() const; // Return the orientation quaternion - void setOrientation(const Quaternion& orientation); // Set the orientation quaternion Vector3D getLinearVelocity() const; // Return the linear velocity void setLinearVelocity(const Vector3D& linearVelocity); // Set the linear velocity of the body Vector3D getAngularVelocity() const; // Return the angular velocity @@ -80,40 +76,12 @@ class RigidBody : public Body { Matrix3x3 getInertiaTensorLocalInverse() const; // Get the inverse of the inertia tensor Matrix3x3 getInertiaTensorWorld() const; // Return the inertia tensor in world coordinates Matrix3x3 getInertiaTensorInverseWorld() const; // Return the inverse of the inertia tensor in world coordinates - void setInterpolationFactor(double factor); // Set the interpolation factor of the body - Vector3D getInterpolatedPosition() const; // Return the interpolated position - Quaternion getInterpolatedOrientation() const; // Return the interpolated orientation + double getRestitution() const; // Get the restitution coefficient void setRestitution(double restitution) throw(std::invalid_argument); // Set the restitution coefficient - void updateOldPositionAndOrientation(); // Update the previous position and orientation of the body void update(); // Update the rigid body in order to reflect a change in the body state }; -// --- Inline functions --- // - -// Return the position of the body -inline Vector3D RigidBody::getPosition() const { - return position; -} - -// Set the position of the body -inline void RigidBody::setPosition(const Vector3D& position) { - this->position = position; -} - -// Return the orientation quaternion of the body -inline Quaternion RigidBody::getOrientation() const { - return orientation; -} - -// Set the orientation quaternion -inline void RigidBody::setOrientation(const Quaternion& orientation) { - this->orientation = orientation; - - // Normalize the orientation quaternion - orientation.getUnit(); -} - // Return the linear velocity inline Vector3D RigidBody::getLinearVelocity() const { return linearVelocity; @@ -179,7 +147,7 @@ inline void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal // where R is the rotation matrix (and R^T its transpose) of the current orientation quaternion of the body inline Matrix3x3 RigidBody::getInertiaTensorWorld() const { // Compute and return the inertia tensor in world coordinates - return orientation.getMatrix() * inertiaTensorLocal * orientation.getMatrix().getTranspose(); + return transform.getOrientation() * inertiaTensorLocal * transform.getOrientation().getTranspose(); } // Return the inverse of the inertia tensor in world coordinates @@ -188,25 +156,7 @@ inline Matrix3x3 RigidBody::getInertiaTensorWorld() const { // where R is the rotation matrix (and R^T its transpose) of the current orientation quaternion of the body inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const { // Compute and return the inertia tensor in world coordinates - return orientation.getMatrix() * inertiaTensorLocalInverse * orientation.getMatrix().getTranspose(); -} - -// Set the interpolation factor of the body -inline void RigidBody::setInterpolationFactor(double factor) { - // Set the factor - interpolationFactor = factor; -} - -// Return the interpolated position -inline Vector3D RigidBody::getInterpolatedPosition() const { - // Compute the interpolated position - return oldPosition * (1-interpolationFactor) + position * interpolationFactor; -} - - // Return the interpolated orientation -inline Quaternion RigidBody::getInterpolatedOrientation() const { - // Compute the interpolated orientation - return Quaternion::slerp(oldOrientation, orientation, interpolationFactor); + return transform.getOrientation() * inertiaTensorLocalInverse * transform.getOrientation().getTranspose(); } // Set the linear velocity of the rigid body @@ -234,12 +184,6 @@ inline void RigidBody::setRestitution(double restitution) throw(std::invalid_arg } } -// Update the previous body state of the body -// This is used to compute the interpolated position and orientation of the body -inline void RigidBody::updateOldPositionAndOrientation() { - oldPosition = position; - oldOrientation = orientation; -} } // End of the ReactPhyscis3D namespace