mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Add AABB bounding volume and implementation of the Sweep And Prune broad-phase algorithm

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@373 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2010-08-05 15:06:34 +00:00
parent d3edeedd31
commit 619c70dc48
19 changed files with 697 additions and 150 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

109
sources/reactphysics3d/body/AABB.cpp Normal file
View File

@ -0,0 +1,109 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
****************************************************************************/
// Libraries
#include "AABB.h"
#include <GL/freeglut.h> // TODO : Remove this in the final version
#include <GL/gl.h> // TODO : Remove this in the final version
#include <cassert>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
AABB::AABB(const Vector3D& center,double extentX, double extentY, double extentZ) {
this->center = center;
this->extent[0] = extentX;
this->extent[1] = extentY;
this->extent[2] = extentZ;
this->originalAABBExtent[0] = extentX;
this->originalAABBExtent[1] = extentY;
this->originalAABBExtent[2] = extentZ;
}
// Destructor
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(s2.getX(), s2.getY(), s2.getZ());
glVertex3f(s3.getX(), s3.getY(), s3.getZ());
glVertex3f(s3.getX(), s3.getY(), s3.getZ());
glVertex3f(s4.getX(), s4.getY(), s4.getZ());
glVertex3f(s4.getX(), s4.getY(), s4.getZ());
glVertex3f(s1.getX(), s1.getY(), s1.getZ());
glVertex3f(s5.getX(), s5.getY(), s5.getZ());
glVertex3f(s6.getX(), s6.getY(), s6.getZ());
glVertex3f(s6.getX(), s6.getY(), s6.getZ());
glVertex3f(s7.getX(), s7.getY(), s7.getZ());
glVertex3f(s7.getX(), s7.getY(), s7.getZ());
glVertex3f(s8.getX(), s8.getY(), s8.getZ());
glVertex3f(s8.getX(), s8.getY(), s8.getZ());
glVertex3f(s5.getX(), s5.getY(), s5.getZ());
glVertex3f(s1.getX(), s1.getY(), s1.getZ());
glVertex3f(s5.getX(), s5.getY(), s5.getZ());
glVertex3f(s4.getX(), s4.getY(), s4.getZ());
glVertex3f(s8.getX(), s8.getY(), s8.getZ());
glVertex3f(s3.getX(), s3.getY(), s3.getZ());
glVertex3f(s7.getX(), s7.getY(), s7.getZ());
glVertex3f(s2.getX(), s2.getY(), s2.getZ());
glVertex3f(s6.getX(), s6.getY(), s6.getZ());
glEnd();
}
// 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 std::vector<Vector3D>& vertices, const Vector3D& center) {
// TODO : Implement this method;
return 0;
}

173
sources/reactphysics3d/body/AABB.h Normal file
View File

@ -0,0 +1,173 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef AABB_H
#define AABB_H
// Libraries
#include "BoundingVolume.h"
#include "../mathematics/mathematics.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class AABB :
This class represents a bounding volume of type "Axis Aligned
Bounding Box". It's a box where all the edges are always aligned
with the world coordinate system. The AABB is defined by a center
point and three extent size in the x,y and z directions.
-------------------------------------------------------------------
*/
class AABB : public BoundingVolume {
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)
public :
AABB(const Vector3D& center, double extentX, double extentY, double extentZ); // 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
virtual void draw() const; // Draw the AABB (only for testing purpose)
static AABB* computeFromVertices(const std::vector<Vector3D>& vertices, const Vector3D& center); // Compute an AABB from a set of vertices
};
// Return the center point
inline Vector3D AABB::getCenter() const {
return center;
}
// Set the center point
inline void AABB::setCenter(const Vector3D& center) {
this->center = center;
}
// 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 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
inline double AABB::getMinValueOnAxis(uint axis) const throw(std::invalid_argument) {
switch (axis) {
case 0: return center.getX() - extent[0];
case 1: return center.getY() - extent[1];
case 2: return center.getZ() - extent[2];
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
inline double AABB::getMaxValueOnAxis(uint axis) const throw(std::invalid_argument) {
switch (axis) {
case 0: return center.getX() + extent[0];
case 1: return center.getY() + extent[1];
case 2: return center.getZ() + extent[2];
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 true if the current AABB is overlapping is 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();
if (std::abs(center.getX() - center2.getX()) > (extent[0] + aabb.getExtent(0))) return false;
if (std::abs(center.getY() - center2.getY()) > (extent[1] + aabb.getExtent(1))) return false;
if (std::abs(center.getZ() - center2.getZ()) > (extent[2] + aabb.getExtent(2))) 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];
}
}
}
}; // End of the ReactPhysics3D namespace
#endif

14
sources/reactphysics3d/body/Body.cpp
View File

@ -18,13 +18,15 @@
***************************************************************************/ ***************************************************************************/
// Libraries // Libraries
#include "Body.h" #include "Body.h"
#include "BoundingVolume.h"
// We want to use the ReactPhysics3D namespace // We want to use the ReactPhysics3D namespace
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
Body::Body(double mass) throw(std::invalid_argument) : mass(mass) { Body::Body(double mass, BoundingVolume* broadBoundingVolume, BoundingVolume* narrowBoundingVolume) throw(std::invalid_argument)
: mass(mass), broadBoundingVolume(broadBoundingVolume), narrowBoundingVolume(narrowBoundingVolume) {
// Check if the mass is not larger than zero // Check if the mass is not larger than zero
if (mass <= 0.0) { if (mass <= 0.0) {
// We throw an exception // We throw an exception
@ -32,12 +34,8 @@ Body::Body(double mass) throw(std::invalid_argument) : mass(mass) {
} }
} }
// Copy-constructor
Body::Body(const Body& body) : mass(body.mass) {
}
// Destructor // Destructor
Body::~Body() { Body::~Body() {
delete broadBoundingVolume;
delete narrowBoundingVolume;
} }

41
sources/reactphysics3d/body/Body.h
View File

@ -23,9 +23,12 @@
// Libraries // Libraries
#include <stdexcept> #include <stdexcept>
// Namespace reactphysics3d // Namespace reactphysics3d
namespace reactphysics3d { namespace reactphysics3d {
class BoundingVolume;
/* ------------------------------------------------------------------- /* -------------------------------------------------------------------
Class Body : Class Body :
This class is an abstract class to represent body of the physics This class is an abstract class to represent body of the physics
@ -34,21 +37,25 @@ namespace reactphysics3d {
*/ */
class Body { class Body {
protected : protected :
double mass; // Mass of the body double mass; // Mass of the body
bool isMotionEnabled; // True if the body is able to move BoundingVolume* broadBoundingVolume; // Bounding volume used for the broad-phase collision detection
bool isCollisionEnabled; // True if the body can collide with others bodies BoundingVolume* 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
public : public :
Body(double mass) throw(std::invalid_argument); // Constructor Body(double mass, BoundingVolume* broadBoundingVolume,
Body(const Body& body); // Copy-constructor BoundingVolume* narrowBoundingVolume) throw(std::invalid_argument); // Constructor
virtual ~Body(); // Destructor virtual ~Body(); // Destructor
double getMass() const; // Return the mass of the body double getMass() const; // Return the mass of the body
void setMass(double mass); // Set the mass of the body void setMass(double mass); // Set the mass of the body
bool getIsMotionEnabled() const; // Return if the rigid body can move bool getIsMotionEnabled() const; // Return if the rigid body can move
void setIsMotionEnabled(bool isMotionEnabled); // Set the value to true if the 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 bool getIsCollisionEnabled() const; // Return true if the body can collide with others bodies
void setIsCollisionEnabled(bool isCollisionEnabled); // Set the isCollisionEnabled value void setIsCollisionEnabled(bool isCollisionEnabled); // Set the isCollisionEnabled value
const BoundingVolume* getBroadBoundingVolume() const; // Return the broad-phase bounding volume
const BoundingVolume* getNarrowBoundingVolume() const; // Return the narrow-phase bounding volume of the body
}; };
// --- Inlines function --- // // --- Inlines function --- //
@ -83,6 +90,16 @@ inline void Body::setIsCollisionEnabled(bool isCollisionEnabled) {
this->isCollisionEnabled = isCollisionEnabled; this->isCollisionEnabled = isCollisionEnabled;
} }
// Return the broad-phase bounding volume
inline const BoundingVolume* Body::getBroadBoundingVolume() const {
return broadBoundingVolume;
}
// Return the oriented bounding box of the rigid body
inline const BoundingVolume* Body::getNarrowBoundingVolume() const {
return narrowBoundingVolume;
}
} }
#endif #endif

8
sources/reactphysics3d/body/BoundingVolume.h
View File

@ -21,10 +21,11 @@
#define BOUNDING_VOLUME_H #define BOUNDING_VOLUME_H
// Libraries // Libraries
#include "../mathematics/mathematics.h"
#include "Body.h" #include "Body.h"
#include "../mathematics/mathematics.h"
#include <cassert> #include <cassert>
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
@ -46,9 +47,8 @@ class BoundingVolume {
Body* getBodyPointer() const; // Return the body pointer Body* getBodyPointer() const; // Return the body pointer
void setBodyPointer(Body* body); // Set the body pointer void setBodyPointer(Body* body); // Set the body pointer
virtual void updateOrientation(const Vector3D& newCenter, const Quaternion& rotationQuaternion)=0; // Update the orientation of the bounding volume according to the new orientation of the body 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
virtual std::vector<Vector3D> getExtremeVertices(const Vector3D& axis) const=0; // Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis virtual void draw() const=0; // Display the bounding volume (only for testing purpose)
virtual void draw() const=0; // Display the bounding volume (only for testing purpose)
}; };
// Return the body pointer // Return the body pointer

31
sources/reactphysics3d/body/OBB.cpp
View File

@ -50,7 +50,6 @@ OBB::~OBB() {
} }
// TODO : Remove this method in the final version
// Draw the OBB (only for testing purpose) // Draw the OBB (only for testing purpose)
void OBB::draw() const { void OBB::draw() const {
double e0 = extent[0]; double e0 = extent[0];
@ -227,29 +226,9 @@ std::vector<Vector3D> OBB::getFace(unsigned int index) const throw(std::invalid_
} }
} }
// Return the axis that correspond the better to the vector // Static method that computes an OBB from a set of vertices. The "center" argument corresponds to the center of the OBB
Vector3D OBB::getBestAxis(const Vector3D& vector) const { // This method allocates a new OBB object and return a pointer to the new allocated OBB object
double vectorLength = vector.length(); OBB* OBB::computeFromVertices(const std::vector<Vector3D>& vertices, const Vector3D& center) {
double minDifference = DBL_MAX; // TODO : Implement this method;
int bestAxis = -1; return 0;
bool opposite = false;
for (int i=0; i<3; i++) {
double scalarProd = axis[i].scalarProduct(vector);
double lengthValue = axis[i].length() * vectorLength;
if (std::abs(std::abs(scalarProd) - lengthValue) < minDifference) {
bestAxis = i;
minDifference = std::abs(std::abs(scalarProd) - lengthValue);
if (scalarProd >= 0) {
opposite = false;
}
else {
opposite = true;
}
}
}
return opposite ? axis[bestAxis].getOpposite() : axis[bestAxis];
} }

30
sources/reactphysics3d/body/OBB.h
View File

@ -47,21 +47,21 @@ class OBB : public BoundingVolume {
public : public :
OBB(const Vector3D& center, const Vector3D& axis1, const Vector3D& axis2, OBB(const Vector3D& center, const Vector3D& axis1, const Vector3D& axis2,
const Vector3D& axis3, double extent1, double extent2, double extent3); // Constructor const Vector3D& axis3, double extent1, double extent2, double extent3); // Constructor
virtual ~OBB(); // Destructor virtual ~OBB(); // Destructor
Vector3D getCenter() const; // Return the center point of the OBB Vector3D getCenter() const; // Return the center point of the OBB
void setCenter(const Vector3D& center); // Set the center point 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 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 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 Vector3D getVertex(unsigned int index) const throw (std::invalid_argument); // Return a vertex of the OBB
std::vector<Vector3D> getFace(unsigned int index) const throw(std::invalid_argument); // Return the 4 vertices the OBB's face in the direction of a given axis std::vector<Vector3D> 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 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 void setExtent(unsigned int index, double extent) throw(std::invalid_argument); // Set an extent value
virtual std::vector<Vector3D> 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 std::vector<Vector3D> 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 updateOrientation(const Vector3D& newCenter, const Quaternion& rotationQuaternion); // Update the oriented bounding box orientation according to a new orientation of the rigid body 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 void draw() const; // Draw the OBB (only for testing purpose) virtual void draw() const; // Draw the OBB (only for testing purpose)
Vector3D getBestAxis(const Vector3D& vector) const; // Return the axis that correspond the better to the vector static OBB* computeFromVertices(const std::vector<Vector3D>& vertices, const Vector3D& center); // Compute an OBB from a set of vertices
}; };
// Return the center point of the OBB // Return the center point of the OBB
@ -158,7 +158,7 @@ inline void OBB::setExtent(unsigned int index, double extent) throw(std::invalid
} }
// Update the orientation of the OBB according to the orientation of the rigid body // Update the orientation of the OBB according to the orientation of the rigid body
inline void OBB::updateOrientation(const Vector3D& newCenter, const Quaternion& rotationQuaternion) { inline void OBB::update(const Vector3D& newCenter, const Quaternion& rotationQuaternion) {
// Update the center of the OBB // Update the center of the OBB
center = newCenter; center = newCenter;

18
sources/reactphysics3d/body/RigidBody.cpp
View File

@ -24,9 +24,10 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
RigidBody::RigidBody(const Vector3D& position, const Quaternion& orientation, double mass, const Matrix3x3& inertiaTensorLocal, const OBB& obb) RigidBody::RigidBody(const Vector3D& position, const Quaternion& orientation, double mass, const Matrix3x3& inertiaTensorLocal, BoundingVolume* broadBoundingVolume,
: Body(mass), position(position), orientation(orientation.getUnit()), inertiaTensorLocal(inertiaTensorLocal), inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), BoundingVolume* narrowBoundingVolume)
massInverse(1.0/mass), oldPosition(position), oldOrientation(orientation), obb(obb) { : Body(mass, broadBoundingVolume, narrowBoundingVolume), position(position), orientation(orientation.getUnit()), inertiaTensorLocal(inertiaTensorLocal),
inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass), oldPosition(position), oldOrientation(orientation) {
restitution = 1.0; restitution = 1.0;
isMotionEnabled = true; isMotionEnabled = true;
@ -34,10 +35,15 @@
interpolationFactor = 0.0; interpolationFactor = 0.0;
// Update the orientation of the OBB according to the orientation of the rigid body // Update the orientation of the OBB according to the orientation of the rigid body
this->update(); update();
// Set the body pointer to the OBB assert(broadBoundingVolume);
this->obb.setBodyPointer(this); assert(narrowBoundingVolume);
// Set the body pointer to the bounding volumes
// TODO : Move this in the Body constructor
broadBoundingVolume->setBodyPointer(this);
narrowBoundingVolume->setBodyPointer(this);
} }
// Destructor // Destructor

35
sources/reactphysics3d/body/RigidBody.h
View File

@ -15,16 +15,16 @@
* * * *
* You should have received a copy of the GNU Lesser General Public License * * You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. * * along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/ ***************************************************************************/
#ifndef RIGIDBODY_H #ifndef RIGIDBODY_H
#define RIGIDBODY_H #define RIGIDBODY_H
// Libraries // Libraries
#include <cassert> #include <cassert>
#include "Body.h" #include "Body.h"
#include "OBB.h" #include "BoundingVolume.h"
#include "../mathematics/mathematics.h" #include "../mathematics/mathematics.h"
// Namespace reactphysics3d // Namespace reactphysics3d
namespace reactphysics3d { namespace reactphysics3d {
@ -48,15 +48,15 @@ class RigidBody : public Body {
Vector3D externalTorque; // Current external torque on the body Vector3D externalTorque; // Current external torque on the body
Matrix3x3 inertiaTensorLocal; // Local inertia tensor of the body (in body coordinates) Matrix3x3 inertiaTensorLocal; // Local inertia tensor of the body (in body coordinates)
Matrix3x3 inertiaTensorLocalInverse; // Inverse of the 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 massInverse; // Inverse of the mass of the body
double interpolationFactor; // Interpolation factor used for the state interpolation double interpolationFactor; // Interpolation factor used for the state interpolation
double restitution; // Coefficient of restitution (between 0 and 1), 1 for a very boucing body double restitution; // Coefficient of restitution (between 0 and 1), 1 for a very boucing body
OBB obb; // Oriented bounding box that contains the rigid body
public : public :
RigidBody(const Vector3D& position, const Quaternion& orientation, double mass, const Matrix3x3& inertiaTensorLocal, const OBB& obb); // Constructor // Copy-constructor RigidBody(const Vector3D& position, const Quaternion& orientation, double mass, const Matrix3x3& inertiaTensorLocal,
virtual ~RigidBody(); // Destructor BoundingVolume* broadBoundingVolume, BoundingVolume* narrowBoundingVolume); // Constructor // Copy-constructor
virtual ~RigidBody(); // Destructor
Vector3D getPosition() const; // Return the position of the body Vector3D getPosition() const; // Return the position of the body
void setPosition(const Vector3D& position); // Set the position of the body void setPosition(const Vector3D& position); // Set the position of the body
@ -83,7 +83,6 @@ class RigidBody : public Body {
double getRestitution() const; // Get the restitution coefficient double getRestitution() const; // Get the restitution coefficient
void setRestitution(double restitution) throw(std::invalid_argument); // Set 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 updateOldPositionAndOrientation(); // Update the previous position and orientation of the body
const OBB* const getOBB() const; // Return the oriented bounding box of the rigid body
void update(); // Update the rigid body in order to reflect a change in the body state void update(); // Update the rigid body in order to reflect a change in the body state
}; };
@ -241,15 +240,11 @@ inline void RigidBody::updateOldPositionAndOrientation() {
oldOrientation = orientation; oldOrientation = orientation;
} }
// Return the oriented bounding box of the rigid body
inline const OBB* const RigidBody::getOBB() const {
return &obb;
}
// Update the rigid body in order to reflect a change in the body state // Update the rigid body in order to reflect a change in the body state
inline void RigidBody::update() { inline void RigidBody::update() {
// Update the orientation of the corresponding bounding volume of the rigid body // Update the orientation of the corresponding bounding volumes of the rigid body
obb.updateOrientation(position, orientation); broadBoundingVolume->update(position, orientation);
narrowBoundingVolume->update(position, orientation);
} }
} // End of the ReactPhyscis3D namespace } // End of the ReactPhyscis3D namespace

13
sources/reactphysics3d/collision/BroadPhaseAlgorithm.h
View File

@ -30,23 +30,24 @@ namespace reactphysics3d {
Class BroadPhaseAlgorithm : Class BroadPhaseAlgorithm :
This class is an abstract class that represents an algorithm This class is an abstract class that represents an algorithm
used to perform the broad-phase of a collision detection. The used to perform the broad-phase of a collision detection. The
goal of the broad-phase algorithm is to compute the bodies that goal of the broad-phase algorithm is to compute the pair of bodies
can collide. But it's important to understand that the that can collide. But it's important to understand that the
broad-phase doesn't compute only body pairs that can collide but broad-phase doesn't compute only body pairs that can collide but
also pairs of body that doesn't collide but are very close. The could also pairs of body that doesn't collide but are very close.
goal of the broad-phase is to remove pairs of body that cannot The goal of the broad-phase is to remove pairs of body that cannot
collide in order to avoid to much bodies to be tested in the collide in order to avoid to much bodies to be tested in the
narrow-phase. narrow-phase.
-------------------------------------------------------------------- --------------------------------------------------------------------
*/ */
class BroadPhaseAlgorithm { class BroadPhaseAlgorithm {
private : protected :
public : public :
BroadPhaseAlgorithm(); // Constructor BroadPhaseAlgorithm(); // Constructor
virtual ~BroadPhaseAlgorithm(); // Destructor virtual ~BroadPhaseAlgorithm(); // Destructor
virtual bool testCollisionPair(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2)=0; // Return true if the two bounding volume can collide virtual void computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs)=0; // Compute the possible collision pairs of bodies
}; };
} // End of reactphysics3d namespace } // End of reactphysics3d namespace

36
sources/reactphysics3d/collision/CollisionDetection.cpp
View File

@ -19,7 +19,7 @@
// Libraries // Libraries
#include "CollisionDetection.h" #include "CollisionDetection.h"
#include "NoBroadPhaseAlgorithm.h" #include "SAPAlgorithm.h"
#include "SATAlgorithm.h" #include "SATAlgorithm.h"
#include "../body/Body.h" #include "../body/Body.h"
#include "../body/OBB.h" #include "../body/OBB.h"
@ -33,10 +33,10 @@ using namespace reactphysics3d;
CollisionDetection::CollisionDetection(PhysicsWorld* world) { CollisionDetection::CollisionDetection(PhysicsWorld* world) {
this->world = world; this->world = world;
// Construct the broad-phase algorithm that will be used (Separating axis with AABB) // Create the broad-phase algorithm that will be used (Sweep and Prune with AABB)
broadPhaseAlgorithm = new NoBroadPhaseAlgorithm(); broadPhaseAlgorithm = new SAPAlgorithm();
// Construct the narrow-phase algorithm that will be used (Separating axis algorithm) // Create the narrow-phase algorithm that will be used (Separating axis algorithm)
narrowPhaseAlgorithm = new SATAlgorithm(); narrowPhaseAlgorithm = new SATAlgorithm();
} }
@ -68,38 +68,22 @@ bool CollisionDetection::computeCollisionDetection() {
// Compute the broad-phase collision detection // Compute the broad-phase collision detection
void CollisionDetection::computeBroadPhase() { void CollisionDetection::computeBroadPhase() {
// For each pair of bodies in the physics world // Clear the set of possible colliding pairs of bodies
for(std::vector<Body*>::const_iterator it1 = world->getBodiesBeginIterator(); it1 != world->getBodiesEndIterator(); ++it1) { possibleCollisionPairs.clear();
for(std::vector<Body*>::const_iterator it2 = it1; it2 != world->getBodiesEndIterator(); ++it2) {
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(*it1); // Compute the set of possible collision pairs of bodies
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(*it2); broadPhaseAlgorithm->computePossibleCollisionPairs(world->getAddedBodies(), world->getRemovedBodies(), possibleCollisionPairs);
// If both bodies are RigidBody and are different and if both have collision activated
if(rigidBody1 && rigidBody2 && rigidBody1 != rigidBody2
&& rigidBody1->getIsCollisionEnabled() && rigidBody2->getIsCollisionEnabled()) {
// Get the oriented bounding boxes of the two bodies
const OBB* obb1 = rigidBody1->getOBB();
const OBB* obb2 = rigidBody2->getOBB();
// Use the broad-phase algorithm to decide if the two bodies can collide
if(broadPhaseAlgorithm->testCollisionPair(obb1, obb2)) {
// If the broad-phase thinks that the two bodies collide, we add the in the possible collision pair list
possibleCollisionPairs.push_back(std::pair<const OBB*, const OBB*>(obb1, obb2));
}
}
}
}
} }
// Compute the narrow-phase collision detection // Compute the narrow-phase collision detection
void CollisionDetection::computeNarrowPhase() { void CollisionDetection::computeNarrowPhase() {
// For each possible collision pair of bodies // For each possible collision pair of bodies
for (unsigned int i=0; i<possibleCollisionPairs.size(); i++) { for (unsigned int i=0; i<possibleCollisionPairs.size(); i++) {
ContactInfo* contactInfo = 0; ContactInfo* contactInfo = 0;
// Use the narrow-phase collision detection algorithm to check if the really are a contact // Use the narrow-phase collision detection algorithm to check if the really are a contact
if (narrowPhaseAlgorithm->testCollision(possibleCollisionPairs.at(i).first, possibleCollisionPairs.at(i).second, contactInfo)) { if (narrowPhaseAlgorithm->testCollision(possibleCollisionPairs.at(i).first->getNarrowBoundingVolume(), possibleCollisionPairs.at(i).second->getNarrowBoundingVolume(), contactInfo)) {
assert(contactInfo != 0); assert(contactInfo != 0);
// Add the contact info the current list of collision informations // Add the contact info the current list of collision informations

16
sources/reactphysics3d/collision/CollisionDetection.h
View File

@ -41,15 +41,15 @@ namespace reactphysics3d {
*/ */
class CollisionDetection { class CollisionDetection {
private : private :
PhysicsWorld* world; // Pointer to the physics world PhysicsWorld* world; // Pointer to the physics world
std::vector<std::pair<const OBB*, const OBB* > > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase) std::vector<std::pair<const Body*, const Body* > > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase)
std::vector<ContactInfo*> contactInfos; // Contact informations (computed by narrowphase) std::vector<ContactInfo*> contactInfos; // Contact informations (computed by narrowphase)
BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm
NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm
void computeBroadPhase(); // Compute the broad-phase collision detection void computeBroadPhase(); // Compute the broad-phase collision detection
void computeNarrowPhase(); // Compute the narrow-phase collision detection void computeNarrowPhase(); // Compute the narrow-phase collision detection
void computeAllContacts(); // Compute all the contacts from the collision info list void computeAllContacts(); // Compute all the contacts from the collision info list
void computeContact(const ContactInfo* const contactInfo); // Compute a contact (and add it to the physics world) for two colliding bodies void computeContact(const ContactInfo* const contactInfo); // Compute a contact (and add it to the physics world) for two colliding bodies
public : public :

47
sources/reactphysics3d/collision/NoBroadPhaseAlgorithm.h
View File

@ -22,31 +22,64 @@
// Libraries // Libraries
#include "BroadPhaseAlgorithm.h" #include "BroadPhaseAlgorithm.h"
#include <algorithm>
// Namespace ReactPhysics3D // Namespace ReactPhysics3D
namespace reactphysics3d { namespace reactphysics3d {
/* -------------------------------------------------------------------- /* --------------------------------------------------------------------
Class BroadPhaseAlgorithm : Class NoBroadPhaseAlgorithm :
This class implements a broad-phase algorithm that does nothing. This class implements a broad-phase algorithm that does nothing.
It should be use if we don't want to perform a broad-phase for It should be use if we don't want to perform a broad-phase for
the collision detection. the collision detection.
-------------------------------------------------------------------- --------------------------------------------------------------------
*/ */
class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm { class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm {
private : protected :
std::vector<Body*> bodies; // All bodies of the engine
public : public :
NoBroadPhaseAlgorithm(); // Constructor NoBroadPhaseAlgorithm(); // Constructor
virtual ~NoBroadPhaseAlgorithm(); // Destructor virtual ~NoBroadPhaseAlgorithm(); // Destructor
virtual bool testCollisionPair(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2); // Return true if the two bounding volume can collide virtual void computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs); // Compute the possible collision pairs of bodies
}; };
// Method that always returns true (meaning that the two bounding volumes can collide).
// We don't want to perform the broad-phase and therefore this method always returns true. // Compute the possible collision pairs of bodies
inline bool NoBroadPhaseAlgorithm::testCollisionPair(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2) { // The arguments "addedBodies" and "removedBodies" are respectively the set
return true; // of bodies that have been added and removed since the last broad-phase
// computation. Before the call, the argument "possibleCollisionPairs"
// correspond to the possible colliding pairs of bodies from the last broad-phase
// computation. This methods computes the current possible collision pairs of
// bodies and update the "possibleCollisionPairs" argument. This broad-phase
// algorithm doesn't do anything and therefore the "possibleCollisionPairs" set
// must contains all the possible pairs of bodies
inline void NoBroadPhaseAlgorithm::computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs) {
// Add the new bodies
for (std::vector<Body*>::iterator it = addedBodies.begin(); it < addedBodies.end(); it++) {
bodies.push_back(*it);
}
// Remove the bodies to be removed
for (std::vector<Body*>::iterator it = removedBodies.begin(); it < removedBodies.end(); it++) {
bodies.erase(std::find(bodies.begin(), bodies.end(), *it));
}
// If the set of bodies have been changed
if (addedBodies.size() + removedBodies.size() > 0) {
// Recompute all the possible pairs of bodies
possibleCollisionPairs.clear();
for (std::vector<Body*>::iterator it1 = addedBodies.begin(); it1 < addedBodies.end(); it1++) {
for (std::vector<Body*>::iterator it2 = addedBodies.begin(); it2 < addedBodies.end(); it2++) {
if (*it1 != *it2) {
possibleCollisionPairs.push_back(std::make_pair(*it1, *it2));
}
}
}
}
} }
} // End of reactphysics3d namespace } // End of reactphysics3d namespace

127
sources/reactphysics3d/collision/SAPAlgorithm.cpp Normal file
View File

@ -0,0 +1,127 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
// Libraries
#include "SAPAlgorithm.h"
#include <algorithm>
// Namespaces
using namespace reactphysics3d;
using namespace std;
// TODO : Take care of the isCollisionEnabled variable of the bodies while performing broad-phase collision detection
// Initialize the static attributes
unsigned short int SAPAlgorithm::sortAxis = 0;
// Constructor
SAPAlgorithm::SAPAlgorithm() {
}
// Destructor
SAPAlgorithm::~SAPAlgorithm() {
}
// Remove the AABB representation of a given set of bodies from the sortedAABBs set
void SAPAlgorithm::removeBodiesAABB(vector<Body*> bodies) {
vector<const AABB*>::iterator elemToRemove;
const AABB* aabb;
// Removed the AABB of the bodies that have been removed
for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); it++) {
aabb = dynamic_cast<const AABB*>((*it)->getBroadBoundingVolume());
assert(aabb);
elemToRemove = find(sortedAABBs.begin(), sortedAABBs.end(), aabb);
assert((*elemToRemove) == aabb);
sortedAABBs.erase(elemToRemove);
}
}
// Compute the possible collision pairs of bodies
// The arguments "addedBodies" and "removedBodies" are respectively the set
// of bodies that have been added and removed since the last broad-phase
// computation. Before the call, the argument "possibleCollisionPairs"
// correspond to the possible colliding pairs of bodies from the last broad-phase
// computation. This methods computes the current possible collision pairs of
// bodies and update the "possibleCollisionPairs" argument.
void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vector<Body*> removedBodies,
vector<pair<const Body*, const Body* > >& 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
vector<const AABB*>::iterator it; // Iterator on the sortedAABBs set
vector<const AABB*>::iterator it2; // Second iterator
Vector3D center3D; // Center of the current AABB
double center[3]; // Coordinates of the center of the current AABB
int i;
uint nbAABBs = sortedAABBs.size(); // Number of AABBs
// Removed the bodies to remove
removeBodiesAABB(removedBodies);
// Add the bodies to add
addBodiesAABB(addedBodies);
// Sort the set of AABBs
sort(sortedAABBs.begin(), sortedAABBs.end(), compareAABBs);
// Sweep the sorted set of AABBs
for (vector<const AABB*>::iterator it = sortedAABBs.begin(); it != sortedAABBs.end(); it++) {
// Center of the current AABB
Vector3D center3D = (*it)->getCenter();
center[0] = center3D.getX();
center[1] = center3D.getY();
center[2] = center3D.getZ();
// Update the esperance and esperanceSquare values to compute the variance
for (i=0; i<3; i++) {
esperance[i] += center[i];
esperanceSquare[i] += center[i] * center[i];
}
// Test collision against all possible overlapping AABBs following the current one
for (it2 = it + 1; it2 < sortedAABBs.end(); it2++) {
// Stop when the tested AABBs are beyond the end of the current AABB
if ((*it2)->getMinValueOnAxis(sortAxis) > (*it)->getMaxValueOnAxis(sortAxis)) {
break;
}
// Test if both AABBs overlap
if ((*it)->testCollision(*(*it2))) {
// Add the current pair of AABBs into the possibleCollisionPairs set
possibleCollisionPairs.push_back(make_pair((*it)->getBodyPointer(), (*it2)->getBodyPointer()));
}
}
}
// Compute the variance of the distribution of the AABBs on the three x,y and z axis
for (i=0; i<3; i++) {
variance[i] = esperanceSquare[i] - esperance[i] * esperance[i] / nbAABBs;
}
// Update the sorted Axis according to the axis with the largest variance
sortAxis = 0;
if (variance[1] > variance[0]) sortAxis = 1;
if (variance[2] > variance[sortAxis]) sortAxis = 2;
}

89
sources/reactphysics3d/collision/SAPAlgorithm.h Normal file
View File

@ -0,0 +1,89 @@
/***************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef SAPALGORITHM_H
#define SAPALGORITHM_H
// Libraries
#include "BroadPhaseAlgorithm.h"
#include "../body/AABB.h"
// Namespace ReactPhysics3D
namespace reactphysics3d {
/* --------------------------------------------------------------------
Class SAPAlgorithm :
This class implements the Sweep and Prune (SAP) broad-phase
algorithm. This algorithm uses the AABB bounding-volume of the
bodies and keep a sorted representation of the intervals of the
bodies' AABB on the three x. y and z axis. Given this sorted
representation, we can efficiently compute the set of possible
colliding pairs of bodies. At each broad-phase computation, we
should sort the AABB according to the axis (x, y or z) with the
largest variance of the AABBs positions in order that the sorted
AABB's set does not change a lot between each computations. To do
so, we compute at each time the variance of each axis and select
the axis (sortAxis) with the largest variance for the next
broad-phase computation.
--------------------------------------------------------------------
*/
class SAPAlgorithm : public BroadPhaseAlgorithm {
protected :
std::vector<const AABB*> sortedAABBs; // Sorted set of AABB of the bodies on one of the x.y or z axis
static unsigned short int sortAxis; // Current sorting axis (0 for x, 1 for y, 2 for z axis)
static bool compareAABBs(const AABB* a, const AABB* b); // Static method that compare two AABBs (in order to sort them)
void removeBodiesAABB(std::vector<Body*> bodies); // Remove the AABB representation of a given set of bodies from the sortedAABBs set
void addBodiesAABB(std::vector<Body*> bodies); // Add the AABB representation of a given set of bodies in the sortedAABBs set
public :
SAPAlgorithm(); // Constructor
virtual ~SAPAlgorithm(); // Destructor
virtual void computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs); // Compute the possible collision pairs of bodies
};
// Static method that compare two AABBs. This method will be used to compare to AABBs
// in order to sort them with the sort() function to obtain the sortedAABBs set.
// This method must return true if the AABB "a" goes before the AABB "b". We
// consider that "a" goes before "b" if the minimum value of "a" on the current
// sorting axis (sortAxis) is smaller than the minimum value of "b" on this same
// axis.
inline bool SAPAlgorithm::compareAABBs(const AABB* a, const AABB* b) {
return (a->getMinValueOnAxis(sortAxis) < b->getMinValueOnAxis(sortAxis));
}
// Add the AABB representation of a given body in the sortedAABBs set
inline void SAPAlgorithm::addBodiesAABB(std::vector<Body*> bodies) {
const AABB* aabb;
for (std::vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); it++) {
aabb = 0;
aabb = dynamic_cast<const AABB*>((*it)->getBroadBoundingVolume());
assert(aabb);
// TODO : Add an assert here to check that the AABB pointer isn't already in the sortedAABBs set
sortedAABBs.push_back(aabb);
}
}
} // End of reactphysics3d namespace
#endif

3
sources/reactphysics3d/engine/PhysicsEngine.cpp
View File

@ -66,6 +66,9 @@ void PhysicsEngine::update() {
if (existCollision) { if (existCollision) {
constraintSolver.freeMemory(); constraintSolver.freeMemory();
} }
// Clear the added and removed bodies from last update() method call
world->clearAddedAndRemovedBodies();
} }
} }
} }

33
sources/reactphysics3d/engine/PhysicsWorld.cpp
View File

@ -19,9 +19,11 @@
// Libraries // Libraries
#include "PhysicsWorld.h" #include "PhysicsWorld.h"
#include <algorithm>
// We want to use the ReactPhysics3D namespace // Namespaces
using namespace reactphysics3d; using namespace reactphysics3d;
using namespace std;
// Constructor // Constructor
PhysicsWorld::PhysicsWorld(const Vector3D& gravity) PhysicsWorld::PhysicsWorld(const Vector3D& gravity)
@ -35,32 +37,37 @@ PhysicsWorld::~PhysicsWorld() {
} }
// Add a body to the physics world // Add a body to the physics world
void PhysicsWorld::addBody(Body* body) throw(std::invalid_argument) { void PhysicsWorld::addBody(Body* body) throw(invalid_argument) {
// Check if the body pointer is not null // Check if the body pointer is not null
if (body != 0) { if (body != 0) {
// Check if the body pointer isn't already in the bodyList // Check if the body pointer isn't already in the bodyList
for(std::vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); ++it) { for(vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); ++it) {
if (*it == body) { if (*it == body) {
// The body is already in the bodyList, therefore we throw an exception // The body is already in the bodyList, therefore we throw an exception
throw std::invalid_argument("Exception in PhysicsWorld::addBody() : The argument body is already in the PhysicsWorld"); throw invalid_argument("Exception in PhysicsWorld::addBody() : The argument body is already in the PhysicsWorld");
} }
} }
// The body isn't already in the bodyList, therefore we add it to the list // The body isn't already in the bodyList, therefore we add it to the list
bodies.push_back(body); bodies.push_back(body);
addedBodies.push_back(body);
vector<Body*>::iterator it = find(removedBodies.begin(), removedBodies.end(), body);
if (it != removedBodies.end()) {
removedBodies.erase(it);
}
} }
else { else {
// Throw an exception // Throw an exception
throw std::invalid_argument("Exception in PhysicsWorld::addBody() : The argument pointer cannot be NULL"); throw invalid_argument("Exception in PhysicsWorld::addBody() : The argument pointer cannot be NULL");
} }
} }
// Remove a body from the physics world // Remove a body from the physics world
void PhysicsWorld::removeBody(Body const* const body) throw(std::invalid_argument) { void PhysicsWorld::removeBody(Body const* const body) throw(invalid_argument) {
// Check if the body pointer is not null // Check if the body pointer is not null
if (body != 0) { if (body != 0) {
// Look for the body to remove in the bodyList // Look for the body to remove in the bodyList
std::vector<Body*>::iterator it = bodies.begin(); vector<Body*>::iterator it = bodies.begin();
while(it != bodies.end() && *it != body) { while(it != bodies.end() && *it != body) {
// Increment the iterator // Increment the iterator
++it; ++it;
@ -70,32 +77,34 @@ void PhysicsWorld::removeBody(Body const* const body) throw(std::invalid_argumen
if (*it == body) { if (*it == body) {
// Remove the body // Remove the body
bodies.erase(it); bodies.erase(it);
addedBodies.erase(it);
removedBodies.push_back(*it);
} else { } else {
// The body is not in the bodyList, therfore we throw an exception // The body is not in the bodyList, therfore we throw an exception
throw std::invalid_argument("Exception in PhysicsWorld::removeBody() : The argument body to remove is not in the PhysicsWorld"); throw invalid_argument("Exception in PhysicsWorld::removeBody() : The argument body to remove is not in the PhysicsWorld");
} }
} }
else { else {
// Throw an exception // Throw an exception
throw std::invalid_argument("Exception in PhysicsWorld::removeBody() : The argument pointer cannot be NULL"); throw invalid_argument("Exception in PhysicsWorld::removeBody() : The argument pointer cannot be NULL");
} }
} }
// Add a constraint into the physics world // Add a constraint into the physics world
void PhysicsWorld::addConstraint(Constraint* constraint) throw(std::invalid_argument) { void PhysicsWorld::addConstraint(Constraint* constraint) throw(invalid_argument) {
assert(constraint != 0); assert(constraint != 0);
constraints.push_back(constraint); constraints.push_back(constraint);
} }
// Remove a constraint // Remove a constraint
void PhysicsWorld::removeConstraint(Constraint* constraint) throw(std::invalid_argument) { void PhysicsWorld::removeConstraint(Constraint* constraint) throw(invalid_argument) {
// TODO : Implement this method // TODO : Implement this method
} }
// Remove all collision contacts constraints // Remove all collision contacts constraints
void PhysicsWorld::removeAllContactConstraints() { void PhysicsWorld::removeAllContactConstraints() {
// For all constraints // For all constraints
for (std::vector<Constraint*>::iterator it = constraints.begin(); it != constraints.end(); ) { for (vector<Constraint*>::iterator it = constraints.begin(); it != constraints.end(); ) {
// Try a downcasting // Try a downcasting
Contact* contact = dynamic_cast<Contact*>(*it); Contact* contact = dynamic_cast<Contact*>(*it);

21
sources/reactphysics3d/engine/PhysicsWorld.h
View File

@ -41,6 +41,8 @@ namespace reactphysics3d {
class PhysicsWorld { class PhysicsWorld {
protected : protected :
std::vector<Body*> bodies; // list that contains all bodies of the physics world std::vector<Body*> bodies; // list that contains all bodies of the physics world
std::vector<Body*> addedBodies; // Added bodies since last update
std::vector<Body*> removedBodies; // Removed bodies since last update
std::vector<Constraint*> constraints; // List that contains all the current constraints std::vector<Constraint*> constraints; // List that contains all the current constraints
Vector3D gravity; // Gravity vector of the world Vector3D gravity; // Gravity vector of the world
bool isGravityOn; // True if the gravity force is on bool isGravityOn; // True if the gravity force is on
@ -51,6 +53,7 @@ class PhysicsWorld {
void addBody(Body* body) throw(std::invalid_argument); // Add a body to the physics world void addBody(Body* body) throw(std::invalid_argument); // Add a body to the physics world
void removeBody(Body const* const body) throw(std::invalid_argument); // Remove a body from the physics world void removeBody(Body const* const body) throw(std::invalid_argument); // Remove a body from the physics world
void clearAddedAndRemovedBodies(); // Clear the addedBodies and removedBodies sets
Vector3D getGravity() const; // Return the gravity vector of the world Vector3D getGravity() const; // Return the gravity vector of the world
bool getIsGravityOn() const; // Return if the gravity is on bool getIsGravityOn() const; // Return if the gravity is on
void setIsGratityOn(bool isGravityOn); // Set the isGravityOn attribute void setIsGratityOn(bool isGravityOn); // Set the isGravityOn attribute
@ -61,10 +64,18 @@ class PhysicsWorld {
std::vector<Constraint*>::iterator getConstraintsEndIterator(); // Return a end iterator on the constraint list std::vector<Constraint*>::iterator getConstraintsEndIterator(); // Return a end iterator on the constraint list
std::vector<Body*>::iterator getBodiesBeginIterator(); // Return an iterator to the beginning of the bodies of the physics world std::vector<Body*>::iterator getBodiesBeginIterator(); // Return an iterator to the beginning of the bodies of the physics world
std::vector<Body*>::iterator getBodiesEndIterator(); // Return an iterator to the end of the bodies of the physics world std::vector<Body*>::iterator getBodiesEndIterator(); // Return an iterator to the end of the bodies of the physics world
std::vector<Body*>& getAddedBodies(); // Return the added bodies since last update of the physics engine
std::vector<Body*>& getRemovedBodies(); // Retrun the removed bodies since last update of the physics engine
}; };
// --- Inline functions --- // // --- Inline functions --- //
// Clear the addedBodies and removedBodies sets
inline void PhysicsWorld::clearAddedAndRemovedBodies() {
addedBodies.clear();
removedBodies.clear();
}
// Return the gravity vector of the world // Return the gravity vector of the world
inline Vector3D PhysicsWorld::getGravity() const { inline Vector3D PhysicsWorld::getGravity() const {
return gravity; return gravity;
@ -100,6 +111,16 @@ inline std::vector<Body*>::iterator PhysicsWorld::getBodiesEndIterator() {
return bodies.end(); return bodies.end();
} }
// Return the added bodies since last update of the physics engine
inline std::vector<Body*>& PhysicsWorld::getAddedBodies() {
return addedBodies;
}
// Retrun the removed bodies since last update of the physics engine
inline std::vector<Body*>& PhysicsWorld::getRemovedBodies() {
return removedBodies;
}
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace
#endif #endif

3
sources/reactphysics3d/reactphysics3d.h
View File

@ -26,6 +26,9 @@
#include "engine/PhysicsEngine.h" #include "engine/PhysicsEngine.h"
#include "body/BoundingVolume.h" #include "body/BoundingVolume.h"
#include "body/OBB.h" #include "body/OBB.h"
#include "body/AABB.h"
// TODO : Use using namespace std in every possible cpp files to increase readability
// Alias to the ReactPhysics3D namespace // Alias to the ReactPhysics3D namespace
namespace rp3d = reactphysics3d; namespace rp3d = reactphysics3d;