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

Merge branch 'raycast' into develop

Browse Source
This commit is contained in:
Daniel Chappuis 2014-11-29 13:08:51 +01:00
commit f598e0c1e9
130 changed files with 9835 additions and 2794 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
CMakeLists.txt
View File

@ -4,8 +4,10 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
# Project configuration
PROJECT(REACTPHYSICS3D)
# Default build type
SET(CMAKE_BUILD_TYPE "Debug")
# Build type
IF (NOT CMAKE_BUILD_TYPE)
SET(CMAKE_BUILD_TYPE "Release")
ENDIF (NOT CMAKE_BUILD_TYPE)
# Where to build the library
SET(LIBRARY_OUTPUT_PATH "lib")
@ -43,12 +45,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/body/RigidBody.cpp"
"src/collision/broadphase/BroadPhaseAlgorithm.h"
"src/collision/broadphase/BroadPhaseAlgorithm.cpp"
"src/collision/broadphase/NoBroadPhaseAlgorithm.h"
"src/collision/broadphase/NoBroadPhaseAlgorithm.cpp"
"src/collision/broadphase/PairManager.h"
"src/collision/broadphase/PairManager.cpp"
"src/collision/broadphase/SweepAndPruneAlgorithm.h"
"src/collision/broadphase/SweepAndPruneAlgorithm.cpp"
"src/collision/broadphase/DynamicAABBTree.h"
"src/collision/broadphase/DynamicAABBTree.cpp"
"src/collision/narrowphase/EPA/EdgeEPA.h"
"src/collision/narrowphase/EPA/EdgeEPA.cpp"
"src/collision/narrowphase/EPA/EPAAlgorithm.h"
@ -81,8 +79,10 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/shapes/CylinderShape.cpp"
"src/collision/shapes/SphereShape.h"
"src/collision/shapes/SphereShape.cpp"
"src/collision/BroadPhasePair.h"
"src/collision/BroadPhasePair.cpp"
"src/collision/RaycastInfo.h"
"src/collision/RaycastInfo.cpp"
"src/collision/ProxyShape.h"
"src/collision/ProxyShape.cpp"
"src/collision/CollisionDetection.h"
"src/collision/CollisionDetection.cpp"
"src/constraint/BallAndSocketJoint.h"
@ -132,9 +132,11 @@ SET (REACTPHYSICS3D_SOURCES
"src/mathematics/Vector2.h"
"src/mathematics/Vector2.cpp"
"src/mathematics/Vector3.h"
"src/mathematics/Ray.h"
"src/mathematics/Vector3.cpp"
"src/memory/MemoryAllocator.h"
"src/memory/MemoryAllocator.cpp"
"src/memory/Stack.h"
)
# Create the library

15
documentation/UserManual/configHTLatex.cfg Normal file
View File

@ -0,0 +1,15 @@
\Preamble{html}
\begin{document}
% Upper case PNG file extensions
\Configure{graphics*}
{PNG}
{\Picture[pict]{\csname Gin@base\endcsname.PNG align=center border=0}}
% Lower case png extensions
\Configure{graphics*}
{png}
{\Picture[pict]{\csname Gin@base\endcsname.png align=center border=0}}
% Problems with spanish babel
\makeatletter
\let\ifes@LaTeXe\iftrue
\makeatother
\EndPreamble

13
documentation/UserManual/generateHTMLManual.sh Executable file
View File

@ -0,0 +1,13 @@
#!/bin/bash
# Delete the /html folder
rm -R html/
# Create the /html folder
mkdir html
# Use the htlatex command to generate the HTML user manual from the .tex file
htlatex ReactPhysics3D-UserManual.tex "configHTLatex.cfg,html" "" -dhtml/
# Copy the images/ folder into the html/ folder
cp -R images/ html/images/

1
examples/CMakeLists.txt
View File

@ -8,3 +8,4 @@ ADD_SUBDIRECTORY(common/)
ADD_SUBDIRECTORY(cubes/)
ADD_SUBDIRECTORY(joints/)
ADD_SUBDIRECTORY(collisionshapes/)
ADD_SUBDIRECTORY(raycast/)

1
examples/collisionshapes/CMakeLists.txt
View File

@ -30,6 +30,7 @@ SET(COLLISION_SHAPES_SOURCES
"../common/Sphere.cpp"
"../common/Cylinder.cpp"
"../common/Cone.cpp"
"../common/Dumbbell.cpp"
"../common/Box.cpp"
"../common/Viewer.cpp"
)

66
examples/collisionshapes/Scene.cpp
View File

@ -46,7 +46,7 @@ Scene::Scene(Viewer* viewer, const std::string& shaderFolderPath, const std::str
mViewer->setScenePosition(center, radiusScene);
// Gravity vector in the dynamics world
rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0);
rp3d::Vector3 gravity(0, -9.81, 0);
// Time step for the physics simulation
rp3d::decimal timeStep = 1.0f / 60.0f;
@ -62,6 +62,25 @@ Scene::Scene(Viewer* viewer, const std::string& shaderFolderPath, const std::str
float radius = 3.0f;
for (int i=0; i<NB_COMPOUND_SHAPES; i++) {
// Position
float angle = i * 30.0f;
openglframework::Vector3 position(radius * cos(angle),
100 + i * (DUMBBELL_HEIGHT + 0.3f),
radius * sin(angle));
// Create a convex mesh and a corresponding rigid in the dynamics world
Dumbbell* dumbbell = new Dumbbell(position, mDynamicsWorld, meshFolderPath);
// Change the material properties of the rigid body
rp3d::Material& material = dumbbell->getRigidBody()->getMaterial();
material.setBounciness(rp3d::decimal(0.2));
// Add the mesh the list of dumbbells in the scene
mDumbbells.push_back(dumbbell);
}
// Create all the boxes of the scene
for (int i=0; i<NB_BOXES; i++) {
@ -83,7 +102,7 @@ Scene::Scene(Viewer* viewer, const std::string& shaderFolderPath, const std::str
}
// Create all the spheres of the scene
for (int i=0; i<NB_SPHERES; i++) {
for (int i=0; i<NB_CUBES; i++) {
// Position
float angle = i * 35.0f;
@ -271,15 +290,17 @@ Scene::~Scene() {
delete (*it);
}
// Destroy all the visual contact points
for (std::vector<VisualContactPoint*>::iterator it = mContactPoints.begin();
it != mContactPoints.end(); ++it) {
// Destroy all the dumbbell of the scene
for (std::vector<Dumbbell*>::iterator it = mDumbbells.begin();
it != mDumbbells.end(); ++it) {
// Destroy the corresponding rigid body from the dynamics world
mDynamicsWorld->destroyRigidBody((*it)->getRigidBody());
// Destroy the convex mesh
delete (*it);
}
// Destroy the static data for the visual contact points
VisualContactPoint::destroyStaticData();
// Destroy the rigid body of the floor
mDynamicsWorld->destroyRigidBody(mFloor->getRigidBody());
@ -342,25 +363,12 @@ void Scene::simulate() {
(*it)->updateTransform();
}
// Destroy all the visual contact points
for (std::vector<VisualContactPoint*>::iterator it = mContactPoints.begin();
it != mContactPoints.end(); ++it) {
delete (*it);
}
mContactPoints.clear();
// Update the position and orientation of the dumbbells
for (std::vector<Dumbbell*>::iterator it = mDumbbells.begin();
it != mDumbbells.end(); ++it) {
// Generate the new visual contact points
const std::vector<rp3d::ContactManifold*>& manifolds = mDynamicsWorld->getContactManifolds();
for (std::vector<rp3d::ContactManifold*>::const_iterator it = manifolds.begin();
it != manifolds.end(); ++it) {
for (unsigned int i=0; i<(*it)->getNbContactPoints(); i++) {
rp3d::ContactPoint* point = (*it)->getContactPoint(i);
const rp3d::Vector3 pos = point->getWorldPointOnBody1();
openglframework::Vector3 position(pos.x, pos.y, pos.z);
VisualContactPoint* visualPoint = new VisualContactPoint(position);
mContactPoints.push_back(visualPoint);
}
// Update the transform used for the rendering
(*it)->updateTransform();
}
mFloor->updateTransform();
@ -422,9 +430,9 @@ void Scene::render() {
(*it)->render(mPhongShader, worldToCameraMatrix);
}
// Render all the visual contact points
for (std::vector<VisualContactPoint*>::iterator it = mContactPoints.begin();
it != mContactPoints.end(); ++it) {
// Render all the dumbbells of the scene
for (std::vector<Dumbbell*>::iterator it = mDumbbells.begin();
it != mDumbbells.end(); ++it) {
(*it)->render(mPhongShader, worldToCameraMatrix);
}

17
examples/collisionshapes/Scene.h
View File

@ -35,16 +35,18 @@
#include "Cylinder.h"
#include "Capsule.h"
#include "ConvexMesh.h"
#include "Dumbbell.h"
#include "VisualContactPoint.h"
#include "../common/Viewer.h"
// Constants
const int NB_BOXES = 3;
const int NB_SPHERES = 3;
const int NB_BOXES = 2;
const int NB_CUBES = 1;
const int NB_CONES = 3;
const int NB_CYLINDERS = 3;
const int NB_CAPSULES = 3;
const int NB_MESHES = 3;
const int NB_CYLINDERS = 2;
const int NB_CAPSULES = 1;
const int NB_MESHES = 2;
const int NB_COMPOUND_SHAPES = 2;
const openglframework::Vector3 BOX_SIZE(2, 2, 2);
const float SPHERE_RADIUS = 1.5f;
const float CONE_RADIUS = 2.0f;
@ -53,6 +55,7 @@ const float CYLINDER_RADIUS = 1.0f;
const float CYLINDER_HEIGHT = 5.0f;
const float CAPSULE_RADIUS = 1.0f;
const float CAPSULE_HEIGHT = 1.0f;
const float DUMBBELL_HEIGHT = 1.0f;
const openglframework::Vector3 FLOOR_SIZE(20, 0.5f, 20); // Floor dimensions in meters
const float BOX_MASS = 1.0f;
const float CONE_MASS = 1.0f;
@ -91,8 +94,8 @@ class Scene {
/// All the convex meshes of the scene
std::vector<ConvexMesh*> mConvexMeshes;
/// All the visual contact points
std::vector<VisualContactPoint*> mContactPoints;
/// All the dumbbell of the scene
std::vector<Dumbbell*> mDumbbells;
/// Box for the floor
Box* mFloor;

61
examples/common/Box.cpp
View File

@ -58,7 +58,7 @@ GLuint Box::mCubeIndices[36] = { 0, 1, 2,
// Constructor
Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position,
float mass, reactphysics3d::DynamicsWorld* dynamicsWorld)
reactphysics3d::CollisionWorld* world)
: openglframework::Object3D(), mColor(0.5f, 0.5f, 0.5f, 1.0f) {
// Initialize the size of the box
@ -77,7 +77,7 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
// Create the collision shape for the rigid body (box shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::BoxShape collisionShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2]));
// Initial position and orientation of the rigid body
@ -85,14 +85,65 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the cube in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
// Create a rigid body in the dynamics world
mRigidBody = world->createCollisionBody(transform);
// Add the collision shape to the body
mRigidBody->addCollisionShape(collisionShape, rp3d::Transform::identity());
// If the Vertex Buffer object has not been created yet
if (!areVBOsCreated) {
// Create the Vertex Buffer
createVBO();
}
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Constructor
Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &position,
float mass, reactphysics3d::DynamicsWorld* world)
: openglframework::Object3D(), mColor(0.5f, 0.5f, 0.5f, 1.0f) {
// Initialize the size of the box
mSize[0] = size.x * 0.5f;
mSize[1] = size.y * 0.5f;
mSize[2] = size.z * 0.5f;
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mSize[0], 0, 0, 0,
0, mSize[1], 0, 0,
0, 0, mSize[2], 0,
0, 0, 0, 1);
// Initialize the position where the cube will be rendered
translateWorld(position);
// Create the collision shape for the rigid body (box shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::BoxShape collisionShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2]));
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body in the dynamics world
rp3d::RigidBody* body = world->createRigidBody(transform);
// Add the collision shape to the body
body->addCollisionShape(collisionShape, rp3d::Transform::identity(), mass);
mRigidBody = body;
// If the Vertex Buffer object has not been created yet
if (!areVBOsCreated) {
// Create the Vertex Buffer
createVBO();
}
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Destructor
@ -160,7 +211,7 @@ void Box::updateTransform() {
rp3d::Transform transform = mRigidBody->getInterpolatedTransform();
// Compute the transform used for rendering the box
float matrix[16];
rp3d::decimal matrix[16];
transform.getOpenGLMatrix(matrix);
openglframework::Matrix4 newMatrix(matrix[0], matrix[4], matrix[8], matrix[12],
matrix[1], matrix[5], matrix[9], matrix[13],

20
examples/common/Box.h
View File

@ -54,7 +54,7 @@ class Box : public openglframework::Object3D {
float mSize[3];
/// Rigid body used to simulate the dynamics of the box
rp3d::RigidBody* mRigidBody;
rp3d::CollisionBody* mRigidBody;
/// Scaling matrix (applied to a cube to obtain the correct box dimensions)
openglframework::Matrix4 mScalingMatrix;
@ -88,13 +88,20 @@ class Box : public openglframework::Object3D {
/// Constructor
Box(const openglframework::Vector3& size, const openglframework::Vector3& position,
float mass, rp3d::DynamicsWorld* dynamicsWorld);
reactphysics3d::CollisionWorld* world);
/// Constructor
Box(const openglframework::Vector3& size, const openglframework::Vector3& position,
float mass, reactphysics3d::DynamicsWorld *world);
/// Destructor
~Box();
/// Return a pointer to the collision body of the box
reactphysics3d::CollisionBody* getCollisionBody();
/// Return a pointer to the rigid body of the box
rp3d::RigidBody* getRigidBody();
reactphysics3d::RigidBody* getRigidBody();
/// Update the transform matrix of the box
void updateTransform();
@ -106,9 +113,14 @@ class Box : public openglframework::Object3D {
void setColor(const openglframework::Color& color);
};
// Return a pointer to the collision body of the box
inline rp3d::CollisionBody* Box::getCollisionBody() {
return mRigidBody;
}
// Return a pointer to the rigid body of the box
inline rp3d::RigidBody* Box::getRigidBody() {
return mRigidBody;
return dynamic_cast<rp3d::RigidBody*>(mRigidBody);
}
// Set the color of the box

54
examples/common/Capsule.cpp
View File

@ -26,6 +26,45 @@
// Libraries
#include "Capsule.h"
// Constructor
Capsule::Capsule(float radius, float height, const openglframework::Vector3& position,
reactphysics3d::CollisionWorld* world,
const std::string& meshFolderPath)
: openglframework::Mesh(), mRadius(radius), mHeight(height) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "capsule.obj", *this);
// Calculate the normals of the mesh
calculateNormals();
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
0, (mHeight + 2.0f * mRadius) / 3.0f, 0,0,
0, 0, mRadius, 0,
0, 0, 0, 1.0f);
// Initialize the position where the sphere will be rendered
translateWorld(position);
// Create the collision shape for the rigid body (sphere shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::CapsuleShape collisionShape(mRadius, mHeight);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding in the dynamics world
mRigidBody = world->createCollisionBody(transform);
// Add a collision shape to the body and specify the mass of the shape
mRigidBody->addCollisionShape(collisionShape, rp3d::Transform::identity());
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Constructor
Capsule::Capsule(float radius, float height, const openglframework::Vector3& position,
@ -50,7 +89,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
// Create the collision shape for the rigid body (sphere shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::CapsuleShape collisionShape(mRadius, mHeight);
// Initial position and orientation of the rigid body
@ -58,8 +97,15 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the sphere in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
// Create a rigid body corresponding in the dynamics world
rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the shape
body->addCollisionShape(collisionShape, rp3d::Transform::identity(), mass);
mRigidBody = body;
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Destructor
@ -121,7 +167,7 @@ void Capsule::updateTransform() {
rp3d::Transform transform = mRigidBody->getInterpolatedTransform();
// Compute the transform used for rendering the sphere
float matrix[16];
rp3d::decimal matrix[16];
transform.getOpenGLMatrix(matrix);
openglframework::Matrix4 newMatrix(matrix[0], matrix[4], matrix[8], matrix[12],
matrix[1], matrix[5], matrix[9], matrix[13],

25
examples/common/Capsule.h
View File

@ -44,7 +44,7 @@ class Capsule : public openglframework::Mesh {
float mHeight;
/// Rigid body used to simulate the dynamics of the sphere
rp3d::RigidBody* mRigidBody;
rp3d::CollisionBody* mRigidBody;
/// Scaling matrix (applied to a sphere to obtain the correct sphere dimensions)
openglframework::Matrix4 mScalingMatrix;
@ -57,13 +57,21 @@ class Capsule : public openglframework::Mesh {
/// Constructor
Capsule(float radius, float height, const openglframework::Vector3& position,
float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath);
reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath);
/// Constructor
Capsule(float radius, float height, const openglframework::Vector3& position,
float mass, reactphysics3d::DynamicsWorld* dynamicsWorld,
const std::string& meshFolderPath);
/// Destructor
~Capsule();
/// Return a pointer to the rigid body of the sphere
rp3d::RigidBody* getRigidBody();
/// Return a pointer to the collision body of the box
reactphysics3d::CollisionBody* getCollisionBody();
/// Return a pointer to the rigid body of the box
reactphysics3d::RigidBody* getRigidBody();
/// Update the transform matrix of the sphere
void updateTransform();
@ -73,9 +81,14 @@ class Capsule : public openglframework::Mesh {
const openglframework::Matrix4& worldToCameraMatrix);
};
// Return a pointer to the rigid body of the sphere
inline rp3d::RigidBody* Capsule::getRigidBody() {
// Return a pointer to the collision body of the box
inline rp3d::CollisionBody* Capsule::getCollisionBody() {
return mRigidBody;
}
// Return a pointer to the rigid body of the box
inline rp3d::RigidBody* Capsule::getRigidBody() {
return dynamic_cast<rp3d::RigidBody*>(mRigidBody);
}
#endif

52
examples/common/Cone.cpp
View File

@ -26,6 +26,45 @@
// Libraries
#include "Cone.h"
// Constructor
Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
reactphysics3d::CollisionWorld* world,
const std::string& meshFolderPath)
: openglframework::Mesh(), mRadius(radius), mHeight(height) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cone.obj", *this);
// Calculate the normals of the mesh
calculateNormals();
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
0, mHeight, 0, 0,
0, 0, mRadius, 0,
0, 0, 0, 1);
// Initialize the position where the cone will be rendered
translateWorld(position);
// Create the collision shape for the rigid body (cone shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::ConeShape collisionShape(mRadius, mHeight);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the cone in the dynamics world
mRigidBody = world->createCollisionBody(transform);
// Add a collision shape to the body and specify the mass of the shape
mRigidBody->addCollisionShape(collisionShape, rp3d::Transform::identity());
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Constructor
Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
@ -50,7 +89,7 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
// Create the collision shape for the rigid body (cone shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::ConeShape collisionShape(mRadius, mHeight);
// Initial position and orientation of the rigid body
@ -59,7 +98,14 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the cone in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the shape
body->addCollisionShape(collisionShape, rp3d::Transform::identity(), mass);
mRigidBody = body;
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Destructor
@ -121,7 +167,7 @@ void Cone::updateTransform() {
rp3d::Transform transform = mRigidBody->getInterpolatedTransform();
// Compute the transform used for rendering the cone
float matrix[16];
rp3d::decimal matrix[16];
transform.getOpenGLMatrix(matrix);
openglframework::Matrix4 newMatrix(matrix[0], matrix[4], matrix[8], matrix[12],
matrix[1], matrix[5], matrix[9], matrix[13],

22
examples/common/Cone.h
View File

@ -44,7 +44,7 @@ class Cone : public openglframework::Mesh {
float mHeight;
/// Rigid body used to simulate the dynamics of the cone
rp3d::RigidBody* mRigidBody;
rp3d::CollisionBody* mRigidBody;
/// Scaling matrix (applied to a sphere to obtain the correct cone dimensions)
openglframework::Matrix4 mScalingMatrix;
@ -55,6 +55,10 @@ class Cone : public openglframework::Mesh {
// -------------------- Methods -------------------- //
/// Constructor
Cone(float radius, float height, const openglframework::Vector3& position,
rp3d::CollisionWorld* world, const std::string& meshFolderPath);
/// Constructor
Cone(float radius, float height, const openglframework::Vector3& position,
float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath);
@ -62,8 +66,11 @@ class Cone : public openglframework::Mesh {
/// Destructor
~Cone();
/// Return a pointer to the rigid body of the cone
rp3d::RigidBody* getRigidBody();
/// Return a pointer to the collision body of the box
reactphysics3d::CollisionBody* getCollisionBody();
/// Return a pointer to the rigid body of the box
reactphysics3d::RigidBody* getRigidBody();
/// Update the transform matrix of the cone
void updateTransform();
@ -73,9 +80,14 @@ class Cone : public openglframework::Mesh {
const openglframework::Matrix4& worldToCameraMatrix);
};
// Return a pointer to the rigid body of the cone
inline rp3d::RigidBody* Cone::getRigidBody() {
// Return a pointer to the collision body of the box
inline rp3d::CollisionBody* Cone::getCollisionBody() {
return mRigidBody;
}
// Return a pointer to the rigid body of the box
inline rp3d::RigidBody* Cone::getRigidBody() {
return dynamic_cast<rp3d::RigidBody*>(mRigidBody);
}
#endif

67
examples/common/ConvexMesh.cpp
View File

@ -26,10 +26,9 @@
// Libraries
#include "ConvexMesh.h"
// Constructor
ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
reactphysics3d::DynamicsWorld* dynamicsWorld,
ConvexMesh::ConvexMesh(const openglframework::Vector3 &position,
reactphysics3d::CollisionWorld* world,
const std::string& meshFolderPath)
: openglframework::Mesh() {
@ -44,7 +43,7 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
// Create the collision shape for the rigid body (convex mesh shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
rp3d::decimal* verticesArray = (rp3d::decimal*) getVerticesPointer();
rp3d::ConvexMeshShape collisionShape(verticesArray, mVertices.size(),
sizeof(openglframework::Vector3));
@ -72,7 +71,63 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the sphere in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
mRigidBody = world->createCollisionBody(transform);
// Add a collision shape to the body and specify the mass of the collision shape
mRigidBody->addCollisionShape(collisionShape, rp3d::Transform::identity());
}
// Constructor
ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
reactphysics3d::DynamicsWorld* dynamicsWorld,
const std::string& meshFolderPath)
: openglframework::Mesh() {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "convexmesh.obj", *this);
// Calculate the normals of the mesh
calculateNormals();
// Initialize the position where the sphere will be rendered
translateWorld(position);
// Create the collision shape for the rigid body (convex mesh shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
rp3d::decimal* verticesArray = (rp3d::decimal*) getVerticesPointer();
rp3d::ConvexMeshShape collisionShape(verticesArray, mVertices.size(),
sizeof(openglframework::Vector3));
// Add the edges information of the mesh into the convex mesh collision shape.
// This is optional but it really speed up the convex mesh collision detection at the
// cost of some additional memory to store the edges inside the collision shape.
for (unsigned int i=0; i<getNbFaces(); i++) { // For each triangle face of the mesh
// Get the three vertex IDs of the vertices of the face
unsigned int v1 = getVertexIndexInFace(i, 0);
unsigned int v2 = getVertexIndexInFace(i, 1);
unsigned int v3 = getVertexIndexInFace(i, 2);
// Add the three edges into the collision shape
collisionShape.addEdge(v1, v2);
collisionShape.addEdge(v1, v3);
collisionShape.addEdge(v2, v3);
}
collisionShape.setIsEdgesInformationUsed(true);// Enable the fast collision detection with edges
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the sphere in the dynamics world
rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the collision shape
body->addCollisionShape(collisionShape, rp3d::Transform::identity(), mass);
mRigidBody = body;
}
// Destructor
@ -134,7 +189,7 @@ void ConvexMesh::updateTransform() {
rp3d::Transform transform = mRigidBody->getInterpolatedTransform();
// Compute the transform used for rendering the sphere
float matrix[16];
rp3d::decimal matrix[16];
transform.getOpenGLMatrix(matrix);
openglframework::Matrix4 newMatrix(matrix[0], matrix[4], matrix[8], matrix[12],
matrix[1], matrix[5], matrix[9], matrix[13],

22
examples/common/ConvexMesh.h
View File

@ -38,7 +38,7 @@ class ConvexMesh : public openglframework::Mesh {
// -------------------- Attributes -------------------- //
/// Rigid body used to simulate the dynamics of the mesh
rp3d::RigidBody* mRigidBody;
rp3d::CollisionBody* mRigidBody;
// -------------------- Methods -------------------- //
@ -46,6 +46,10 @@ class ConvexMesh : public openglframework::Mesh {
// -------------------- Methods -------------------- //
/// Constructor
ConvexMesh(const openglframework::Vector3& position,
rp3d::CollisionWorld* world, const std::string& meshFolderPath);
/// Constructor
ConvexMesh(const openglframework::Vector3& position, float mass,
rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath);
@ -53,8 +57,11 @@ class ConvexMesh : public openglframework::Mesh {
/// Destructor
~ConvexMesh();
/// Return a pointer to the rigid body of the mesh
rp3d::RigidBody* getRigidBody();
/// Return a pointer to the collision body of the box
reactphysics3d::CollisionBody* getCollisionBody();
/// Return a pointer to the rigid body of the box
reactphysics3d::RigidBody* getRigidBody();
/// Update the transform matrix of the mesh
void updateTransform();
@ -64,9 +71,14 @@ class ConvexMesh : public openglframework::Mesh {
const openglframework::Matrix4& worldToCameraMatrix);
};
// Return a pointer to the rigid body of the mesh
inline rp3d::RigidBody* ConvexMesh::getRigidBody() {
// Return a pointer to the collision body of the box
inline rp3d::CollisionBody* ConvexMesh::getCollisionBody() {
return mRigidBody;
}
// Return a pointer to the rigid body of the box
inline rp3d::RigidBody* ConvexMesh::getRigidBody() {
return dynamic_cast<rp3d::RigidBody*>(mRigidBody);
}
#endif

54
examples/common/Cylinder.cpp
View File

@ -26,9 +26,48 @@
// Libraries
#include "Cylinder.h"
// Constructor
Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position,
reactphysics3d::CollisionWorld* world,
const std::string& meshFolderPath)
: openglframework::Mesh(), mRadius(radius), mHeight(height) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "cylinder.obj", *this);
// Calculate the normals of the mesh
calculateNormals();
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
0, mHeight, 0, 0,
0, 0, mRadius, 0,
0, 0, 0, 1);
// Initialize the position where the cylinder will be rendered
translateWorld(position);
// Create the collision shape for the rigid body (cylinder shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::CylinderShape collisionShape(mRadius, mHeight);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the cylinder in the dynamics world
mRigidBody = world->createCollisionBody(transform);
// Add a collision shape to the body and specify the mass of the shape
mRigidBody->addCollisionShape(collisionShape, rp3d::Transform::identity());
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Constructor
Cylinder::Cylinder(float radius, float height, const openglframework::Vector3 &position,
Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& position,
float mass, reactphysics3d::DynamicsWorld* dynamicsWorld,
const std::string& meshFolderPath)
: openglframework::Mesh(), mRadius(radius), mHeight(height) {
@ -50,7 +89,7 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3 &p
// Create the collision shape for the rigid body (cylinder shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::CylinderShape collisionShape(mRadius, mHeight);
// Initial position and orientation of the rigid body
@ -59,7 +98,14 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3 &p
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the cylinder in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the shape
body->addCollisionShape(collisionShape, rp3d::Transform::identity(), mass);
mTransformMatrix = mTransformMatrix * mScalingMatrix;
mRigidBody = body;
}
// Destructor
@ -121,7 +167,7 @@ void Cylinder::updateTransform() {
rp3d::Transform transform = mRigidBody->getInterpolatedTransform();
// Compute the transform used for rendering the cylinder
float matrix[16];
rp3d::decimal matrix[16];
transform.getOpenGLMatrix(matrix);
openglframework::Matrix4 newMatrix(matrix[0], matrix[4], matrix[8], matrix[12],
matrix[1], matrix[5], matrix[9], matrix[13],

22
examples/common/Cylinder.h
View File

@ -44,7 +44,7 @@ class Cylinder : public openglframework::Mesh {
float mHeight;
/// Rigid body used to simulate the dynamics of the cylinder
rp3d::RigidBody* mRigidBody;
rp3d::CollisionBody* mRigidBody;
/// Scaling matrix (applied to a sphere to obtain the correct cylinder dimensions)
openglframework::Matrix4 mScalingMatrix;
@ -55,6 +55,10 @@ class Cylinder : public openglframework::Mesh {
// -------------------- Methods -------------------- //
/// Constructor
Cylinder(float radius, float height, const openglframework::Vector3& position,
rp3d::CollisionWorld* world, const std::string &meshFolderPath);
/// Constructor
Cylinder(float radius, float height, const openglframework::Vector3& position,
float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string &meshFolderPath);
@ -62,8 +66,11 @@ class Cylinder : public openglframework::Mesh {
/// Destructor
~Cylinder();
/// Return a pointer to the rigid body of the cylinder
rp3d::RigidBody* getRigidBody();
/// Return a pointer to the collision body of the box
reactphysics3d::CollisionBody* getCollisionBody();
/// Return a pointer to the rigid body of the box
reactphysics3d::RigidBody* getRigidBody();
/// Update the transform matrix of the cylinder
void updateTransform();
@ -73,9 +80,14 @@ class Cylinder : public openglframework::Mesh {
const openglframework::Matrix4& worldToCameraMatrix);
};
// Return a pointer to the rigid body of the cylinder
inline rp3d::RigidBody* Cylinder::getRigidBody() {
// Return a pointer to the collision body of the box
inline rp3d::CollisionBody* Cylinder::getCollisionBody() {
return mRigidBody;
}
// Return a pointer to the rigid body of the box
inline rp3d::RigidBody* Cylinder::getRigidBody() {
return dynamic_cast<rp3d::RigidBody*>(mRigidBody);
}
#endif

216
examples/common/Dumbbell.cpp Normal file
View File

@ -0,0 +1,216 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2014 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "Dumbbell.h"
// Constructor
Dumbbell::Dumbbell(const openglframework::Vector3 &position,
reactphysics3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath)
: openglframework::Mesh() {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this);
// Calculate the normals of the mesh
calculateNormals();
// Identity scaling matrix
mScalingMatrix.setToIdentity();
// Initialize the position where the sphere will be rendered
translateWorld(position);
// Create a sphere collision shape for the two ends of the dumbbell
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::decimal radiusSphere = rp3d::decimal(1.5);
const rp3d::decimal massSphere = rp3d::decimal(2.0);
const rp3d::SphereShape sphereCollisionShape(radiusSphere);
// Create a cylinder collision shape for the middle of the dumbbell
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::decimal radiusCylinder = rp3d::decimal(0.5);
const rp3d::decimal heightCylinder = rp3d::decimal(8.0);
const rp3d::decimal massCylinder = rp3d::decimal(1.0);
const rp3d::CylinderShape cylinderCollisionShape(radiusCylinder, heightCylinder);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::decimal angleAroundX = 0;//rp3d::PI / 2;
rp3d::Quaternion initOrientation(angleAroundX, 0, 0);
rp3d::Transform transformBody(initPosition, initOrientation);
// Initial transform of the first sphere collision shape of the dumbbell (in local-space)
rp3d::Transform transformSphereShape1(rp3d::Vector3(0, 4.0, 0), rp3d::Quaternion::identity());
// Initial transform of the second sphere collision shape of the dumbell (in local-space)
rp3d::Transform transformSphereShape2(rp3d::Vector3(0, -4.0, 0), rp3d::Quaternion::identity());
// Initial transform of the cylinder collision shape of the dumbell (in local-space)
rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity());
// Create a rigid body corresponding to the dumbbell in the dynamics world
rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transformBody);
// Add the three collision shapes to the body and specify the mass and transform of the shapes
body->addCollisionShape(sphereCollisionShape, transformSphereShape1, massSphere);
body->addCollisionShape(sphereCollisionShape, transformSphereShape2, massSphere);
body->addCollisionShape(cylinderCollisionShape, transformCylinderShape, massCylinder);
mBody = body;
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Constructor
Dumbbell::Dumbbell(const openglframework::Vector3 &position,
reactphysics3d::CollisionWorld* world, const std::string& meshFolderPath)
: openglframework::Mesh() {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "dumbbell.obj", *this);
// Calculate the normals of the mesh
calculateNormals();
// Identity scaling matrix
mScalingMatrix.setToIdentity();
// Initialize the position where the sphere will be rendered
translateWorld(position);
// Create a sphere collision shape for the two ends of the dumbbell
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::decimal radiusSphere = rp3d::decimal(1.5);
const rp3d::decimal massSphere = rp3d::decimal(2.0);
const rp3d::SphereShape sphereCollisionShape(radiusSphere);
// Create a cylinder collision shape for the middle of the dumbbell
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::decimal radiusCylinder = rp3d::decimal(0.5);
const rp3d::decimal heightCylinder = rp3d::decimal(8.0);
const rp3d::decimal massCylinder = rp3d::decimal(1.0);
const rp3d::CylinderShape cylinderCollisionShape(radiusCylinder, heightCylinder);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::decimal angleAroundX = 0;//rp3d::PI / 2;
rp3d::Quaternion initOrientation(angleAroundX, 0, 0);
rp3d::Transform transformBody(initPosition, initOrientation);
// Initial transform of the first sphere collision shape of the dumbbell (in local-space)
rp3d::Transform transformSphereShape1(rp3d::Vector3(0, 4.0, 0), rp3d::Quaternion::identity());
// Initial transform of the second sphere collision shape of the dumbell (in local-space)
rp3d::Transform transformSphereShape2(rp3d::Vector3(0, -4.0, 0), rp3d::Quaternion::identity());
// Initial transform of the cylinder collision shape of the dumbell (in local-space)
rp3d::Transform transformCylinderShape(rp3d::Vector3(0, 0, 0), rp3d::Quaternion::identity());
// Create a rigid body corresponding to the dumbbell in the dynamics world
mBody = world->createCollisionBody(transformBody);
// Add the three collision shapes to the body and specify the mass and transform of the shapes
mBody->addCollisionShape(sphereCollisionShape, transformSphereShape1);
mBody->addCollisionShape(sphereCollisionShape, transformSphereShape2);
mBody->addCollisionShape(cylinderCollisionShape, transformCylinderShape);
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Destructor
Dumbbell::~Dumbbell() {
// Destroy the mesh
destroy();
}
// Render the sphere at the correct position and with the correct orientation
void Dumbbell::render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix) {
// Bind the shader
shader.bind();
// Set the model to camera matrix
const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix;
shader.setMatrix4x4Uniform("localToCameraMatrix", localToCameraMatrix);
// Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the
// model-view matrix)
const openglframework::Matrix3 normalMatrix =
localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
shader.setMatrix3x3Uniform("normalMatrix", normalMatrix);
glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_NORMAL_ARRAY);
if (hasTexture()) {
glEnableClientState(GL_TEXTURE_COORD_ARRAY);
}
glVertexPointer(3, GL_FLOAT, 0, getVerticesPointer());
glNormalPointer(GL_FLOAT, 0, getNormalsPointer());
if(hasTexture()) {
glTexCoordPointer(2, GL_FLOAT, 0, getUVTextureCoordinatesPointer());
}
// For each part of the mesh
for (unsigned int i=0; i<getNbParts(); i++) {
glDrawElements(GL_TRIANGLES, getNbFaces(i) * 3,
GL_UNSIGNED_INT, getIndicesPointer());
}
glDisableClientState(GL_NORMAL_ARRAY);
glDisableClientState(GL_VERTEX_ARRAY);
if (hasTexture()) {
glDisableClientState(GL_TEXTURE_COORD_ARRAY);
}
// Unbind the shader
shader.unbind();
}
// Update the transform matrix of the sphere
void Dumbbell::updateTransform() {
// Get the interpolated transform of the rigid body
rp3d::Transform transform = mBody->getInterpolatedTransform();
// Compute the transform used for rendering the sphere
rp3d::decimal matrix[16];
transform.getOpenGLMatrix(matrix);
openglframework::Matrix4 newMatrix(matrix[0], matrix[4], matrix[8], matrix[12],
matrix[1], matrix[5], matrix[9], matrix[13],
matrix[2], matrix[6], matrix[10], matrix[14],
matrix[3], matrix[7], matrix[11], matrix[15]);
// Apply the scaling matrix to have the correct sphere dimensions
mTransformMatrix = newMatrix * mScalingMatrix;
}

91
examples/common/Dumbbell.h Normal file
View File

@ -0,0 +1,91 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2014 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef DUMBBELL_H
#define DUMBBELL_H
// Libraries
#include "openglframework.h"
#include "reactphysics3d.h"
// Class Sphere
class Dumbbell : public openglframework::Mesh {
private :
// -------------------- Attributes -------------------- //
/// Radius of the spheres
float mRadius;
/// Rigid body used to simulate the dynamics of the sphere
rp3d::CollisionBody* mBody;
/// Scaling matrix (applied to a sphere to obtain the correct sphere dimensions)
openglframework::Matrix4 mScalingMatrix;
// -------------------- Methods -------------------- //
public :
// -------------------- Methods -------------------- //
/// Constructor
Dumbbell(const openglframework::Vector3& position, rp3d::DynamicsWorld* dynamicsWorld,
const std::string& meshFolderPath);
/// Constructor
Dumbbell(const openglframework::Vector3& position, rp3d::CollisionWorld* world,
const std::string& meshFolderPath);
/// Destructor
~Dumbbell();
/// Return a pointer to the rigid body
rp3d::RigidBody* getRigidBody();
/// Return a pointer to the body
rp3d::CollisionBody* getCollisionBody();
/// Update the transform matrix of the sphere
void updateTransform();
/// Render the sphere at the correct position and with the correct orientation
void render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix);
};
// Return a pointer to the rigid body of the sphere
inline rp3d::RigidBody* Dumbbell::getRigidBody() {
return dynamic_cast<rp3d::RigidBody*>(mBody);
}
// Return a pointer to the body
inline rp3d::CollisionBody* Dumbbell::getCollisionBody() {
return mBody;
}
#endif

34
src/collision/broadphase/NoBroadPhaseAlgorithm.cpp → examples/common/Line.cpp
View File

@ -24,20 +24,36 @@
********************************************************************************/
// Libraries
#include "NoBroadPhaseAlgorithm.h"
#include "../CollisionDetection.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
using namespace std;
#include "Line.h"
// Constructor
NoBroadPhaseAlgorithm::NoBroadPhaseAlgorithm(CollisionDetection& collisionDetection)
: BroadPhaseAlgorithm(collisionDetection) {
Line::Line(const openglframework::Vector3& worldPoint1,
const openglframework::Vector3& worldPoint2)
: mWorldPoint1(worldPoint1), mWorldPoint2(worldPoint2) {
}
// Destructor
NoBroadPhaseAlgorithm::~NoBroadPhaseAlgorithm() {
Line::~Line() {
}
// Render the sphere at the correct position and with the correct orientation
void Line::render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix) {
// Bind the shader
shader.bind();
// Set the model to camera matrix
shader.setMatrix4x4Uniform("localToCameraMatrix", worldToCameraMatrix);
glBegin(GL_LINES);
glVertex3f(mWorldPoint1.x, mWorldPoint1.y, mWorldPoint1.z);
glVertex3f(mWorldPoint2.x, mWorldPoint2.y, mWorldPoint2.z);
glEnd();
// Unbind the shader
shader.unbind();
}

54
examples/common/Line.h Normal file
View File

@ -0,0 +1,54 @@
#ifndef LINE_H
#define LINE_H
// Libraries
#include "openglframework.h"
#include "reactphysics3d.h"
// Class Line
class Line : public openglframework::Object3D {
private :
// -------------------- Attributes -------------------- //
openglframework::Vector3 mWorldPoint1, mWorldPoint2;
// -------------------- Methods -------------------- //
public :
// -------------------- Methods -------------------- //
/// Constructor
Line(const openglframework::Vector3& worldPoint1,
const openglframework::Vector3& worldPoint2);
/// Destructor
~Line();
/// Return the first point of the line
openglframework::Vector3 getPoint1() const;
/// Return the second point of the line
openglframework::Vector3 getPoint2() const;
/// Update the transform matrix of the sphere
void updateTransform();
/// Render the line at the correct position and with the correct orientation
void render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix);
};
// Return the first point of the line
inline openglframework::Vector3 Line::getPoint1() const {
return mWorldPoint1;
}
// Return the second point of the line
inline openglframework::Vector3 Line::getPoint2() const {
return mWorldPoint2;
}
#endif

56
examples/common/Sphere.cpp
View File

@ -26,10 +26,9 @@
// Libraries
#include "Sphere.h"
// Constructor
Sphere::Sphere(float radius, const openglframework::Vector3 &position,
float mass, reactphysics3d::DynamicsWorld* dynamicsWorld,
reactphysics3d::CollisionWorld* world,
const std::string& meshFolderPath)
: openglframework::Mesh(), mRadius(radius) {
@ -50,7 +49,7 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position,
// Create the collision shape for the rigid body (sphere shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling Dynamics::createRigidBody()
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::SphereShape collisionShape(mRadius);
// Initial position and orientation of the rigid body
@ -59,7 +58,54 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position,
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the sphere in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
mRigidBody = world->createCollisionBody(transform);
// Add a collision shape to the body and specify the mass of the shape
mRigidBody->addCollisionShape(collisionShape, rp3d::Transform::identity());
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Constructor
Sphere::Sphere(float radius, const openglframework::Vector3 &position,
float mass, reactphysics3d::DynamicsWorld* world,
const std::string& meshFolderPath)
: openglframework::Mesh(), mRadius(radius) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "sphere.obj", *this);
// Calculate the normals of the mesh
calculateNormals();
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
0, mRadius, 0, 0,
0, 0, mRadius, 0,
0, 0, 0, 1);
// Initialize the position where the sphere will be rendered
translateWorld(position);
// Create the collision shape for the rigid body (sphere shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::SphereShape collisionShape(mRadius);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the sphere in the dynamics world
rp3d::RigidBody* body = world->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the shape
body->addCollisionShape(collisionShape, rp3d::Transform::identity(), mass);
mRigidBody = body;
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Destructor
@ -121,7 +167,7 @@ void Sphere::updateTransform() {
rp3d::Transform transform = mRigidBody->getInterpolatedTransform();
// Compute the transform used for rendering the sphere
float matrix[16];
rp3d::decimal matrix[16];
transform.getOpenGLMatrix(matrix);
openglframework::Matrix4 newMatrix(matrix[0], matrix[4], matrix[8], matrix[12],
matrix[1], matrix[5], matrix[9], matrix[13],

22
examples/common/Sphere.h
View File

@ -41,7 +41,7 @@ class Sphere : public openglframework::Mesh {
float mRadius;
/// Rigid body used to simulate the dynamics of the sphere
rp3d::RigidBody* mRigidBody;
rp3d::CollisionBody* mRigidBody;
/// Scaling matrix (applied to a sphere to obtain the correct sphere dimensions)
openglframework::Matrix4 mScalingMatrix;
@ -52,6 +52,10 @@ class Sphere : public openglframework::Mesh {
// -------------------- Methods -------------------- //
/// Constructor
Sphere(float radius, const openglframework::Vector3& position,
rp3d::CollisionWorld* world, const std::string& meshFolderPath);
/// Constructor
Sphere(float radius, const openglframework::Vector3& position,
float mass, rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath);
@ -59,8 +63,11 @@ class Sphere : public openglframework::Mesh {
/// Destructor
~Sphere();
/// Return a pointer to the rigid body of the sphere
rp3d::RigidBody* getRigidBody();
/// Return a pointer to the collision body of the box
reactphysics3d::CollisionBody* getCollisionBody();
/// Return a pointer to the rigid body of the box
reactphysics3d::RigidBody* getRigidBody();
/// Update the transform matrix of the sphere
void updateTransform();
@ -70,9 +77,14 @@ class Sphere : public openglframework::Mesh {
const openglframework::Matrix4& worldToCameraMatrix);
};
// Return a pointer to the rigid body of the sphere
inline rp3d::RigidBody* Sphere::getRigidBody() {
// Return a pointer to the collision body of the box
inline rp3d::CollisionBody* Sphere::getCollisionBody() {
return mRigidBody;
}
// Return a pointer to the rigid body of the box
inline rp3d::RigidBody* Sphere::getRigidBody() {
return dynamic_cast<rp3d::RigidBody*>(mRigidBody);
}
#endif

1396
examples/common/meshes/dumbbell.obj Normal file
View File

File diff suppressed because it is too large Load Diff

9
examples/common/opengl-framework/src/maths/Color.h
View File

@ -59,6 +59,15 @@ struct Color {
// Return the white color
static Color white() { return Color(1.0f, 1.0f, 1.0f, 1.0f);}
// Return the red color
static Color red() { return Color(1.0f, 0.0f, 0.0f, 1.0f);}
// Return the green color
static Color green() { return Color(0.0f, 1.0f, 0.0f, 1.0f);}
// Return the blue color
static Color blue() { return Color(0.0f, 0.0f, 1.0f, 1.0f);}
// = operator
Color& operator=(const Color& color) {
if (&color != this) {

11
examples/cubes/Scene.cpp
View File

@ -46,7 +46,7 @@ Scene::Scene(Viewer* viewer, const std::string& shaderFolderPath)
mViewer->setScenePosition(center, radiusScene);
// Gravity vector in the dynamics world
rp3d::Vector3 gravity(0, rp3d::decimal(-9.81), 0);
rp3d::Vector3 gravity(0, rp3d::decimal(-5.81), 0);
// Time step for the physics simulation
rp3d::decimal timeStep = 1.0f / 60.0f;
@ -60,7 +60,7 @@ Scene::Scene(Viewer* viewer, const std::string& shaderFolderPath)
float radius = 2.0f;
// Create all the cubes of the scene
for (int i=0; i<NB_SPHERES; i++) {
for (int i=0; i<NB_CUBES; i++) {
// Position of the cubes
float angle = i * 30.0f;
@ -92,6 +92,8 @@ Scene::Scene(Viewer* viewer, const std::string& shaderFolderPath)
// Start the simulation
startSimulation();
counter=0;
}
// Destructor
@ -129,6 +131,11 @@ void Scene::simulate() {
// If the physics simulation is running
if (mIsRunning) {
counter++;
if (counter == 400) {
//mIsRunning = false;
}
// Take a simulation step
mDynamicsWorld->update();

4
examples/cubes/Scene.h
View File

@ -33,7 +33,7 @@
#include "Viewer.h"
// Constants
const int NB_SPHERES = 20; // Number of boxes in the scene
const int NB_CUBES = 200; // Number of boxes in the scene
const openglframework::Vector3 BOX_SIZE(2, 2, 2); // Box dimensions in meters
const openglframework::Vector3 FLOOR_SIZE(50, 0.5f, 50); // Floor dimensions in meters
const float BOX_MASS = 1.0f; // Box mass in kilograms
@ -46,6 +46,8 @@ class Scene {
// -------------------- Attributes -------------------- //
int counter;
/// Pointer to the viewer
Viewer* mViewer;

43
examples/raycast/CMakeLists.txt Normal file
View File

@ -0,0 +1,43 @@
# Minimum cmake version required
cmake_minimum_required(VERSION 2.6)
# Project configuration
PROJECT(Raycast)
# Where to build the executables
SET(EXECUTABLE_OUTPUT_PATH "${OUR_EXECUTABLE_OUTPUT_PATH}/collisionshapes")
SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${EXECUTABLE_OUTPUT_PATH})
SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY_RELEASE ${EXECUTABLE_OUTPUT_PATH})
SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY_DEBUG ${EXECUTABLE_OUTPUT_PATH})
# Copy the shaders used for the demo into the build directory
FILE(COPY "${OPENGLFRAMEWORK_DIR}/src/shaders/" DESTINATION "${EXECUTABLE_OUTPUT_PATH}/shaders/")
# Copy the meshes used for the demo into the build directory
FILE(COPY "../common/meshes/" DESTINATION "${EXECUTABLE_OUTPUT_PATH}/meshes/")
# Headers
INCLUDE_DIRECTORIES("${OPENGLFRAMEWORK_DIR}/src/" "../common/glfw/include/" "../common/")
# Source files
SET(RAYCAST_SOURCES
Raycast.cpp
Scene.cpp
Scene.h
"../common/VisualContactPoint.cpp"
"../common/ConvexMesh.cpp"
"../common/Capsule.cpp"
"../common/Sphere.cpp"
"../common/Line.cpp"
"../common/Cylinder.cpp"
"../common/Cone.cpp"
"../common/Dumbbell.cpp"
"../common/Box.cpp"
"../common/Viewer.cpp"
)
# Create the executable
ADD_EXECUTABLE(raycast ${RAYCAST_SOURCES})
# Link with libraries
TARGET_LINK_LIBRARIES(raycast reactphysics3d openglframework glfw ${GLFW_LIBRARIES})

155
examples/raycast/Raycast.cpp Normal file
View File

@ -0,0 +1,155 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "Scene.h"
#include "../common/Viewer.h"
// Declarations
void simulate();
void render();
void update();
void mouseButton(GLFWwindow* window, int button, int action, int mods);
void mouseMotion(GLFWwindow* window, double x, double y);
void keyboard(GLFWwindow* window, int key, int scancode, int action, int mods);
void scroll(GLFWwindow* window, double xAxis, double yAxis);
void init();
// Namespaces
using namespace openglframework;
// Global variables
Viewer* viewer;
Scene* scene;
// Main function
int main(int argc, char** argv) {
// Create and initialize the Viewer
viewer = new Viewer();
Vector2 windowsSize = Vector2(800, 600);
Vector2 windowsPosition = Vector2(100, 100);
viewer->init(argc, argv, "ReactPhysics3D Examples - Raycast",
windowsSize, windowsPosition, true);
// If the shaders and meshes folders are not specified as an argument
if (argc < 3) {
std::cerr << "Error : You need to specify the shaders folder as the first argument"
<< " and the meshes folder as the second argument" << std::endl;
return 1;
}
// Get the path of the shaders folder
std::string shaderFolderPath(argv[1]);
std::string meshFolderPath(argv[2]);
// Register callback methods
viewer->registerUpdateFunction(update);
viewer->registerKeyboardCallback(keyboard);
viewer->registerMouseButtonCallback(mouseButton);
viewer->registerMouseCursorCallback(mouseMotion);
viewer->registerScrollingCallback(scroll);
// Create the scene
scene = new Scene(viewer, shaderFolderPath, meshFolderPath);
init();
viewer->startMainLoop();
delete viewer;
delete scene;
return 0;
}
// Update function that is called each frame
void update() {
// Take a simulation step
simulate();
// Render
render();
}
// Simulate function
void simulate() {
// Physics simulation
scene->simulate();
viewer->computeFPS();
}
// Initialization
void init() {
// Define the background color (black)
glClearColor(0.0, 0.0, 0.0, 1.0);
}
// Callback method to receive keyboard events
void keyboard(GLFWwindow* window, int key, int scancode, int action, int mods) {
if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS) {
glfwSetWindowShouldClose(window, GL_TRUE);
}
if (key == GLFW_KEY_SPACE && action == GLFW_PRESS) {
scene->changeBody();
}
if (key == GLFW_KEY_N && action == GLFW_PRESS) {
scene->showHideNormals();
}
}
// Callback method to receive scrolling events
void scroll(GLFWwindow* window, double xAxis, double yAxis) {
viewer->scrollingEvent(static_cast<float>(yAxis));
}
// Called when a mouse button event occurs
void mouseButton(GLFWwindow* window, int button, int action, int mods) {
viewer->mouseButtonEvent(button, action);
}
// Called when a mouse motion event occurs
void mouseMotion(GLFWwindow* window, double x, double y) {
viewer->mouseMotionEvent(x, y);
}
// Display the scene
void render() {
// Render the scene
scene->render();
// Display the FPS
viewer->displayGUI();
// Check the OpenGL errors
Viewer::checkOpenGLErrors();
}

309
examples/raycast/Scene.cpp Normal file
View File

@ -0,0 +1,309 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "Scene.h"
// Namespaces
using namespace openglframework;
// Constructor
Scene::Scene(Viewer* viewer, const std::string& shaderFolderPath, const std::string& meshFolderPath)
: mViewer(viewer), mLight0(0), mCurrentBodyIndex(-1), mAreNormalsDisplayed(false),
mPhongShader(shaderFolderPath + "phong.vert",
shaderFolderPath +"phong.frag") {
// Move the light 0
mLight0.translateWorld(Vector3(50, 50, 50));
// Compute the radius and the center of the scene
float radiusScene = 30.0f;
openglframework::Vector3 center(0, 0, 0);
// Set the center of the scene
mViewer->setScenePosition(center, radiusScene);
// Create the dynamics world for the physics simulation
mCollisionWorld = new rp3d::CollisionWorld();
// Create the static data for the visual contact points
VisualContactPoint::createStaticData(meshFolderPath);
// ---------- Dumbbell ---------- //
openglframework::Vector3 position1(0, 0, 0);
// Create a convex mesh and a corresponding rigid in the dynamics world
mDumbbell = new Dumbbell(position1, mCollisionWorld, meshFolderPath);
// ---------- Box ---------- //
openglframework::Vector3 position2(0, 0, 0);
// Create a sphere and a corresponding rigid in the dynamics world
mBox = new Box(BOX_SIZE, position2, mCollisionWorld);
mBox->getCollisionBody()->setIsActive(false);
// ---------- Sphere ---------- //
openglframework::Vector3 position3(0, 0, 0);
// Create a sphere and a corresponding rigid in the dynamics world
mSphere = new Sphere(SPHERE_RADIUS, position3, mCollisionWorld,
meshFolderPath);
// ---------- Cone ---------- //
openglframework::Vector3 position4(0, 0, 0);
// Create a cone and a corresponding rigid in the dynamics world
mCone = new Cone(CONE_RADIUS, CONE_HEIGHT, position4, mCollisionWorld,
meshFolderPath);
mCone->getCollisionBody()->setIsActive(false);
// ---------- Cylinder ---------- //
openglframework::Vector3 position5(0, 0, 0);
// Create a cylinder and a corresponding rigid in the dynamics world
mCylinder = new Cylinder(CYLINDER_RADIUS, CYLINDER_HEIGHT, position5,
mCollisionWorld, meshFolderPath);
mCylinder->getCollisionBody()->setIsActive(false);
// ---------- Capsule ---------- //
openglframework::Vector3 position6(0, 0, 0);
// Create a cylinder and a corresponding rigid in the dynamics world
mCapsule = new Capsule(CAPSULE_RADIUS, CAPSULE_HEIGHT, position6 ,
mCollisionWorld, meshFolderPath);
// ---------- Convex Mesh ---------- //
openglframework::Vector3 position7(0, 0, 0);
// Create a convex mesh and a corresponding rigid in the dynamics world
mConvexMesh = new ConvexMesh(position7, mCollisionWorld, meshFolderPath);
// Create the lines that will be used for raycasting
createLines();
changeBody();
}
// Create the raycast lines
void Scene::createLines() {
int nbRaysOneDimension = sqrt(NB_RAYS);
for (int i=0; i<nbRaysOneDimension; i++) {
for (int j=0; j<nbRaysOneDimension; j++) {
float theta = i * 2.0f * M_PI / float(nbRaysOneDimension);
float phi = j * M_PI / float(nbRaysOneDimension);
// Generate a point on a sphere with spherical coordinates
float x = RAY_LENGTH * std::sin(phi) * std::cos(theta);
float y = RAY_LENGTH * std::sin(phi) * std::sin(theta);
float z = RAY_LENGTH * std::cos(phi);
// Create a line from the point on the sphere to the center of
// the scene
openglframework::Vector3 point1(x, y, z);
openglframework::Vector3 point2(0.0f, 0.0f, 0.0f);
Line* line = new Line(point1, point2);
mLines.push_back(line);
}
}
}
// Change the body to raycast and to display
void Scene::changeBody() {
mCurrentBodyIndex++;
if (mCurrentBodyIndex >= NB_BODIES) mCurrentBodyIndex = 0;
mSphere->getCollisionBody()->setIsActive(false);
mBox->getCollisionBody()->setIsActive(false);
mCone->getCollisionBody()->setIsActive(false);
mCylinder->getCollisionBody()->setIsActive(false);
mCapsule->getCollisionBody()->setIsActive(false);
mConvexMesh->getCollisionBody()->setIsActive(false);
mDumbbell->getCollisionBody()->setIsActive(false);
switch(mCurrentBodyIndex) {
case 0: mSphere->getCollisionBody()->setIsActive(true);
break;
case 1: mBox->getCollisionBody()->setIsActive(true);
break;
case 2: mCone->getCollisionBody()->setIsActive(true);
break;
case 3: mCylinder->getCollisionBody()->setIsActive(true);
break;
case 4: mCapsule->getCollisionBody()->setIsActive(true);
break;
case 5: mConvexMesh->getCollisionBody()->setIsActive(true);
break;
case 6: mDumbbell->getCollisionBody()->setIsActive(true);
break;
}
}
// Destructor
Scene::~Scene() {
// Destroy the shader
mPhongShader.destroy();
// Destroy the box rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mBox->getCollisionBody());
delete mBox;
// Destroy the sphere
mCollisionWorld->destroyCollisionBody(mSphere->getCollisionBody());
delete mSphere;
// Destroy the corresponding rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mCone->getCollisionBody());
delete mCone;
// Destroy the corresponding rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mCylinder->getCollisionBody());
// Destroy the sphere
delete mCylinder;
// Destroy the corresponding rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mCapsule->getCollisionBody());
// Destroy the sphere
delete mCapsule;
// Destroy the corresponding rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mConvexMesh->getCollisionBody());
// Destroy the convex mesh
delete mConvexMesh;
// Destroy the corresponding rigid body from the dynamics world
mCollisionWorld->destroyCollisionBody(mDumbbell->getCollisionBody());
// Destroy the convex mesh
delete mDumbbell;
mRaycastManager.resetPoints();
// Destroy the static data for the visual contact points
VisualContactPoint::destroyStaticData();
// Destroy the collision world
delete mCollisionWorld;
// Destroy the lines
for (std::vector<Line*>::iterator it = mLines.begin(); it != mLines.end();
++it) {
delete (*it);
}
}
// Take a step for the simulation
void Scene::simulate() {
mRaycastManager.resetPoints();
// For each line of the scene
for (std::vector<Line*>::iterator it = mLines.begin(); it != mLines.end();
++it) {
Line* line = *it;
// Create a ray corresponding to the line
openglframework::Vector3 p1 = line->getPoint1();
openglframework::Vector3 p2 = line->getPoint2();
rp3d::Vector3 point1(p1.x, p1.y, p1.z);
rp3d::Vector3 point2(p2.x, p2.y, p2.z);
rp3d::Ray ray(point1, point2);
// Perform a raycast query on the physics world by passing a raycast
// callback class in argument.
mCollisionWorld->raycast(ray, &mRaycastManager);
}
}
// Render the scene
void Scene::render() {
glEnable(GL_DEPTH_TEST);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnable(GL_CULL_FACE);
// Get the world-space to camera-space matrix
const Camera& camera = mViewer->getCamera();
const openglframework::Matrix4 worldToCameraMatrix = camera.getTransformMatrix().getInverse();
// Bind the shader
mPhongShader.bind();
openglframework::Vector4 grey(0.7, 0.7, 0.7, 1);
mPhongShader.setVector4Uniform("vertexColor", grey);
// Set the variables of the shader
mPhongShader.setMatrix4x4Uniform("projectionMatrix", camera.getProjectionMatrix());
mPhongShader.setVector3Uniform("light0PosCameraSpace", worldToCameraMatrix * mLight0.getOrigin());
mPhongShader.setVector3Uniform("lightAmbientColor", Vector3(0.3f, 0.3f, 0.3f));
const Color& diffColLight0 = mLight0.getDiffuseColor();
const Color& specColLight0 = mLight0.getSpecularColor();
mPhongShader.setVector3Uniform("light0DiffuseColor", Vector3(diffColLight0.r, diffColLight0.g, diffColLight0.b));
mPhongShader.setVector3Uniform("light0SpecularColor", Vector3(specColLight0.r, specColLight0.g, specColLight0.b));
mPhongShader.setFloatUniform("shininess", 200.0f);
if (mBox->getCollisionBody()->isActive()) mBox->render(mPhongShader, worldToCameraMatrix);
if (mSphere->getCollisionBody()->isActive()) mSphere->render(mPhongShader, worldToCameraMatrix);
if (mCone->getCollisionBody()->isActive()) mCone->render(mPhongShader, worldToCameraMatrix);
if (mCylinder->getCollisionBody()->isActive()) mCylinder->render(mPhongShader, worldToCameraMatrix);
if (mCapsule->getCollisionBody()->isActive()) mCapsule->render(mPhongShader, worldToCameraMatrix);
if (mConvexMesh->getCollisionBody()->isActive()) mConvexMesh->render(mPhongShader, worldToCameraMatrix);
if (mDumbbell->getCollisionBody()->isActive()) mDumbbell->render(mPhongShader, worldToCameraMatrix);
mPhongShader.unbind();
mPhongShader.bind();
mPhongShader.setVector3Uniform("light0SpecularColor", Vector3(0, 0, 0));
openglframework::Vector4 redColor(1, 0, 0, 1);
mPhongShader.setVector4Uniform("vertexColor", redColor);
// Render all the raycast hit points
mRaycastManager.render(mPhongShader, worldToCameraMatrix, mAreNormalsDisplayed);
mPhongShader.unbind();
mPhongShader.bind();
openglframework::Vector4 blueColor(0, 0.62, 0.92, 1);
mPhongShader.setVector4Uniform("vertexColor", blueColor);
// Render the lines
for (std::vector<Line*>::iterator it = mLines.begin(); it != mLines.end();
++it) {
(*it)->render(mPhongShader, worldToCameraMatrix);
}
// Unbind the shader
mPhongShader.unbind();
}

198
examples/raycast/Scene.h Normal file
View File

@ -0,0 +1,198 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef SCENE_H
#define SCENE_H
// Libraries
#include "openglframework.h"
#include "reactphysics3d.h"
#include "Sphere.h"
#include "Box.h"
#include "Cone.h"
#include "Cylinder.h"
#include "Capsule.h"
#include "Line.h"
#include "ConvexMesh.h"
#include "Dumbbell.h"
#include "VisualContactPoint.h"
#include "../common/Viewer.h"
// Constants
const openglframework::Vector3 BOX_SIZE(4, 2, 1);
const float SPHERE_RADIUS = 3.0f;
const float CONE_RADIUS = 3.0f;
const float CONE_HEIGHT = 5.0f;
const float CYLINDER_RADIUS = 3.0f;
const float CYLINDER_HEIGHT = 5.0f;
const float CAPSULE_RADIUS = 3.0f;
const float CAPSULE_HEIGHT = 5.0f;
const float DUMBBELL_HEIGHT = 5.0f;
const int NB_RAYS = 100;
const float RAY_LENGTH = 30.0f;
const int NB_BODIES = 7;
// Raycast manager
class RaycastManager : public rp3d::RaycastCallback {
private:
/// All the visual contact points
std::vector<VisualContactPoint*> mHitPoints;
/// All the normals at hit points
std::vector<Line*> mNormals;
public:
virtual rp3d::decimal notifyRaycastHit(const rp3d::RaycastInfo& raycastInfo) {
rp3d::Vector3 hitPos = raycastInfo.worldPoint;
openglframework::Vector3 position(hitPos.x, hitPos.y, hitPos.z);
VisualContactPoint* point = new VisualContactPoint(position);
mHitPoints.push_back(point);
// Create a line to display the normal at hit point
rp3d::Vector3 n = raycastInfo.worldNormal;
openglframework::Vector3 normal(n.x, n.y, n.z);
Line* normalLine = new Line(position, position + normal);
mNormals.push_back(normalLine);
return raycastInfo.hitFraction;
}
void render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix,
bool showNormals) {
// Render all the raycast hit points
for (std::vector<VisualContactPoint*>::iterator it = mHitPoints.begin();
it != mHitPoints.end(); ++it) {
(*it)->render(shader, worldToCameraMatrix);
}
if (showNormals) {
// Render all the normals at hit points
for (std::vector<Line*>::iterator it = mNormals.begin();
it != mNormals.end(); ++it) {
(*it)->render(shader, worldToCameraMatrix);
}
}
}
void resetPoints() {
// Destroy all the visual contact points
for (std::vector<VisualContactPoint*>::iterator it = mHitPoints.begin();
it != mHitPoints.end(); ++it) {
delete (*it);
}
mHitPoints.clear();
// Destroy all the normals
for (std::vector<Line*>::iterator it = mNormals.begin();
it != mNormals.end(); ++it) {
delete (*it);
}
mNormals.clear();
}
};
// Class Scene
class Scene {
private :
// -------------------- Attributes -------------------- //
/// Pointer to the viewer
Viewer* mViewer;
/// Raycast manager
RaycastManager mRaycastManager;
/// Light 0
openglframework::Light mLight0;
/// Phong shader
openglframework::Shader mPhongShader;
/// All the raycast lines
std::vector<Line*> mLines;
/// Current body index
int mCurrentBodyIndex;
/// True if the hit points normals are displayed
bool mAreNormalsDisplayed;
/// Raycast manager
/// All objects on the scene
Box* mBox;
Sphere* mSphere;
Cone* mCone;
Cylinder* mCylinder;
Capsule* mCapsule;
ConvexMesh* mConvexMesh;
Dumbbell* mDumbbell;
/// Collision world used for the physics simulation
rp3d::CollisionWorld* mCollisionWorld;
/// Create the raycast lines
void createLines();
public:
// -------------------- Methods -------------------- //
/// Constructor
Scene(Viewer* viewer, const std::string& shaderFolderPath,
const std::string& meshFolderPath);
/// Destructor
~Scene();
/// Take a step for the simulation
void simulate();
/// Render the scene
void render();
/// Change the body to raycast
void changeBody();
/// Display or not the surface normals at hit points
void showHideNormals();
};
// Display or not the surface normals at hit points
inline void Scene::showHideNormals() {
mAreNormalsDisplayed = !mAreNormalsDisplayed;
}
#endif

4
src/body/Body.cpp
View File

@ -25,7 +25,7 @@
// Libraries
#include "Body.h"
#include "../collision/shapes/CollisionShape.h"
#include "collision/shapes/CollisionShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -33,7 +33,7 @@ using namespace reactphysics3d;
// Constructor
Body::Body(bodyindex id)
: mID(id), mIsAlreadyInIsland(false), mIsAllowedToSleep(true), mIsActive(true),
mIsSleeping(false), mSleepTime(0) {
mIsSleeping(false), mSleepTime(0), mUserData(NULL) {
}

39
src/body/Body.h
View File

@ -29,11 +29,12 @@
// Libraries
#include <stdexcept>
#include <cassert>
#include "../configuration.h"
#include "configuration.h"
/// Namespace reactphysics3d
namespace reactphysics3d {
// TODO : Make this class abstract
// Class Body
/**
* This class is an abstract class to represent a body of the physics engine.
@ -53,7 +54,14 @@ class Body {
/// True if the body is allowed to go to sleep for better efficiency
bool mIsAllowedToSleep;
/// True if the body is active
/// True if the body is active.
/// An inactive body does not participate in collision detection,
/// is not simulated and will not be hit in a ray casting query.
/// A body is active by default. If you set this
/// value to "false", all the proxy shapes of this body will be
/// removed from the broad-phase. If you set this value to "true",
/// all the proxy shapes will be added to the broad-phase. A joint
/// connected to an inactive body will also be inactive.
bool mIsActive;
/// True if the body is sleeping (for sleeping technique)
@ -62,6 +70,9 @@ class Body {
/// Elapsed time since the body velocity was bellow the sleep velocity
decimal mSleepTime;
/// Pointer that can be used to attach user data to the body
void* mUserData;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -95,9 +106,18 @@ class Body {
/// Return true if the body is active
bool isActive() const;
/// Set whether or not the body is active
virtual void setIsActive(bool isActive);
/// Set the variable to know whether or not the body is sleeping
virtual void setIsSleeping(bool isSleeping);
/// Return a pointer to the user data attached to this body
void* getUserData() const;
/// Attach user data to this body
void setUserData(void* userData);
/// Smaller than operator
bool operator<(const Body& body2) const;
@ -142,6 +162,11 @@ inline bool Body::isActive() const {
return mIsActive;
}
// Set whether or not the body is active
inline void Body::setIsActive(bool isActive) {
mIsActive = isActive;
}
// Set the variable to know whether or not the body is sleeping
inline void Body::setIsSleeping(bool isSleeping) {
@ -157,6 +182,16 @@ inline void Body::setIsSleeping(bool isSleeping) {
mIsSleeping = isSleeping;
}
// Return a pointer to the user data attached to this body
inline void* Body::getUserData() const {
return mUserData;
}
// Attach user data to this body
inline void Body::setUserData(void* userData) {
mUserData = userData;
}
// Smaller than operator
inline bool Body::operator<(const Body& body2) const {
return (mID < body2.mID);

261
src/body/CollisionBody.cpp
View File

@ -25,36 +25,154 @@
// Libraries
#include "CollisionBody.h"
#include "../engine/ContactManifold.h"
#include "engine/CollisionWorld.h"
#include "engine/ContactManifold.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
CollisionBody::CollisionBody(const Transform& transform, CollisionShape *collisionShape,
bodyindex id)
: Body(id), mType(DYNAMIC), mCollisionShape(collisionShape), mTransform(transform),
mHasMoved(false), mContactManifoldsList(NULL) {
assert(collisionShape);
CollisionBody::CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id)
: Body(id), mType(DYNAMIC), mTransform(transform), mProxyCollisionShapes(NULL),
mNbCollisionShapes(0), mContactManifoldsList(NULL), mWorld(world) {
mIsCollisionEnabled = true;
mInterpolationFactor = 0.0;
// Initialize the old transform
mOldTransform = transform;
// Initialize the AABB for broad-phase collision detection
mCollisionShape->updateAABB(mAabb, transform);
}
// Destructor
CollisionBody::~CollisionBody() {
assert(mContactManifoldsList == NULL);
// Remove all the proxy collision shapes of the body
removeAllCollisionShapes();
}
// Add a collision shape to the body.
/// This methods will create a copy of the collision shape you provided inside the world and
/// return a pointer to the actual collision shape in the world. You can use this pointer to
/// remove the collision from the body. Note that when the body is destroyed, all the collision
/// shapes will also be destroyed automatically. Because an internal copy of the collision shape
/// you provided is performed, you can delete it right after calling this method. The second
/// parameter is the transformation that transform the local-space of the collision shape into
/// the local-space of the body.
/// This method will return a pointer to the proxy collision shape that links the body with
/// the collision shape you have added.
ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisionShape,
const Transform& transform) {
// Create an internal copy of the collision shape into the world (if it does not exist yet)
CollisionShape* newCollisionShape = mWorld.createCollisionShape(collisionShape);
// Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
sizeof(ProxyShape))) ProxyShape(this, newCollisionShape,
transform, decimal(1));
// Add it to the list of proxy collision shapes of the body
if (mProxyCollisionShapes == NULL) {
mProxyCollisionShapes = proxyShape;
}
else {
proxyShape->mNext = mProxyCollisionShapes;
mProxyCollisionShapes = proxyShape;
}
// Compute the world-space AABB of the new collision shape
AABB aabb;
newCollisionShape->computeAABB(aabb, mTransform * transform);
// Notify the collision detection about this new collision shape
mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb);
mNbCollisionShapes++;
// Return a pointer to the collision shape
return proxyShape;
}
// Remove a collision shape from the body
void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
ProxyShape* current = mProxyCollisionShapes;
// If the the first proxy shape is the one to remove
if (current == proxyShape) {
mProxyCollisionShapes = current->mNext;
if (mIsActive) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
}
mWorld.removeCollisionShape(proxyShape->mCollisionShape);
current->ProxyShape::~ProxyShape();
mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
mNbCollisionShapes--;
assert(mNbCollisionShapes >= 0);
return;
}
// Look for the proxy shape that contains the collision shape in parameter
while(current->mNext != NULL) {
// If we have found the collision shape to remove
if (current->mNext == proxyShape) {
// Remove the proxy collision shape
ProxyShape* elementToRemove = current->mNext;
current->mNext = elementToRemove->mNext;
if (mIsActive) {
mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
}
mWorld.removeCollisionShape(proxyShape->mCollisionShape);
elementToRemove->ProxyShape::~ProxyShape();
mWorld.mMemoryAllocator.release(elementToRemove, sizeof(ProxyShape));
mNbCollisionShapes--;
return;
}
// Get the next element in the list
current = current->mNext;
}
assert(mNbCollisionShapes >= 0);
}
// Remove all the collision shapes
void CollisionBody::removeAllCollisionShapes() {
// TODO : Remove all the contact manifolds at the end of this call
ProxyShape* current = mProxyCollisionShapes;
// Look for the proxy shape that contains the collision shape in parameter
while(current != NULL) {
// Remove the proxy collision shape
ProxyShape* nextElement = current->mNext;
if (mIsActive) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
}
mWorld.removeCollisionShape(current->mCollisionShape);
current->ProxyShape::~ProxyShape();
mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
// Get the next element in the list
current = nextElement;
}
mProxyCollisionShapes = NULL;
}
// Reset the contact manifold lists
void CollisionBody::resetContactManifoldsList(MemoryAllocator& memoryAllocator) {
void CollisionBody::resetContactManifoldsList() {
// Delete the linked list of contact manifolds of that body
ContactManifoldListElement* currentElement = mContactManifoldsList;
@ -63,11 +181,126 @@ void CollisionBody::resetContactManifoldsList(MemoryAllocator& memoryAllocator)
// Delete the current element
currentElement->ContactManifoldListElement::~ContactManifoldListElement();
memoryAllocator.release(currentElement, sizeof(ContactManifoldListElement));
mWorld.mMemoryAllocator.release(currentElement, sizeof(ContactManifoldListElement));
currentElement = nextElement;
}
mContactManifoldsList = NULL;
assert(mContactManifoldsList == NULL);
}
// Update the broad-phase state for this body (because it has moved for instance)
void CollisionBody::updateBroadPhaseState() const {
// For all the proxy collision shapes of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Recompute the world-space AABB of the collision shape
AABB aabb;
shape->getCollisionShape()->computeAABB(aabb, mTransform *shape->getLocalToBodyTransform());
// Update the broad-phase state for the proxy collision shape
mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb);
}
}
// Set whether or not the body is active
void CollisionBody::setIsActive(bool isActive) {
// If the state does not change
if (mIsActive == isActive) return;
Body::setIsActive(isActive);
// If we have to activate the body
if (isActive) {
// For each proxy shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Compute the world-space AABB of the new collision shape
AABB aabb;
shape->getCollisionShape()->computeAABB(aabb, mTransform * shape->mLocalToBodyTransform);
// Add the proxy shape to the collision detection
mWorld.mCollisionDetection.addProxyCollisionShape(shape, aabb);
}
}
else { // If we have to deactivate the body
// For each proxy shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Remove the proxy shape from the collision detection
mWorld.mCollisionDetection.removeProxyCollisionShape(shape);
}
// Reset the contact manifold list of the body
resetContactManifoldsList();
}
}
// Ask the broad-phase to test again the collision shapes of the body for collision
// (as if the body has moved).
void CollisionBody::askForBroadPhaseCollisionCheck() const {
// For all the proxy collision shapes of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
mWorld.mCollisionDetection.askForBroadPhaseCollisionCheck(shape);
}
}
// Reset the mIsAlreadyInIsland variable of the body and contact manifolds.
/// This method also returns the number of contact manifolds of the body.
int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
mIsAlreadyInIsland = false;
int nbManifolds = 0;
// Reset the mIsAlreadyInIsland variable of the contact manifolds for
// this body
ContactManifoldListElement* currentElement = mContactManifoldsList;
while (currentElement != NULL) {
currentElement->contactManifold->mIsAlreadyInIsland = false;
currentElement = currentElement->next;
nbManifolds++;
}
return nbManifolds;
}
// Return true if a point is inside the collision body
bool CollisionBody::testPointInside(const Vector3& worldPoint) const {
// For each collision shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Test if the point is inside the collision shape
if (shape->testPointInside(worldPoint)) return true;
}
return false;
}
// Raycast method with feedback information
/// The method returns the closest hit among all the collision shapes of the body
bool CollisionBody::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
bool isHit = false;
Ray rayTemp(ray);
// For each collision shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// TODO : Test for broad-phase hit for each shape before testing actual shape raycast
// Test if the ray hits the collision shape
if (shape->raycast(rayTemp, raycastInfo)) {
rayTemp.maxFraction = raycastInfo.hitFraction;
isHit = true;
}
}
return isHit;
}

107
src/body/CollisionBody.h
View File

@ -30,17 +30,20 @@
#include <stdexcept>
#include <cassert>
#include "Body.h"
#include "../mathematics/Transform.h"
#include "../collision/shapes/AABB.h"
#include "../collision/shapes/CollisionShape.h"
#include "../memory/MemoryAllocator.h"
#include "../configuration.h"
#include "mathematics/Transform.h"
#include "collision/shapes/AABB.h"
#include "collision/shapes/CollisionShape.h"
#include "collision/RaycastInfo.h"
#include "memory/MemoryAllocator.h"
#include "configuration.h"
/// Namespace reactphysics3d
namespace reactphysics3d {
// Class declarations
struct ContactManifoldListElement;
class ProxyShape;
class CollisionWorld;
/// Enumeration for the type of a body
/// STATIC : A static body has infinite mass, zero velocity but the position can be
@ -67,9 +70,6 @@ class CollisionBody : public Body {
/// Type of body (static, kinematic or dynamic)
BodyType mType;
/// Collision shape of the body
CollisionShape* mCollisionShape;
/// Position and orientation of the body
Transform mTransform;
@ -82,15 +82,18 @@ class CollisionBody : public Body {
/// True if the body can collide with others bodies
bool mIsCollisionEnabled;
/// AABB for Broad-Phase collision detection
AABB mAabb;
/// First element of the linked list of proxy collision shapes of this body
ProxyShape* mProxyCollisionShapes;
/// True if the body has moved during the last frame
bool mHasMoved;
/// Number of collision shapes
uint mNbCollisionShapes;
/// First element of the linked list of contact manifolds involving this body
ContactManifoldListElement* mContactManifoldsList;
/// Reference to the world the body belongs to
CollisionWorld& mWorld;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -100,20 +103,30 @@ class CollisionBody : public Body {
CollisionBody& operator=(const CollisionBody& body);
/// Reset the contact manifold lists
void resetContactManifoldsList(MemoryAllocator& memoryAllocator);
void resetContactManifoldsList();
/// Remove all the collision shapes
void removeAllCollisionShapes();
/// Update the old transform with the current one.
void updateOldTransform();
/// Update the Axis-Aligned Bounding Box coordinates
void updateAABB();
/// Update the broad-phase state for this body (because it has moved for instance)
virtual void updateBroadPhaseState() const;
/// Ask the broad-phase to test again the collision shapes of the body for collision
/// (as if the body has moved).
void askForBroadPhaseCollisionCheck() const;
/// Reset the mIsAlreadyInIsland variable of the body and contact manifolds
int resetIsAlreadyInIslandAndCountManifolds();
public :
// -------------------- Methods -------------------- //
/// Constructor
CollisionBody(const Transform& transform, CollisionShape* collisionShape, bodyindex id);
CollisionBody(const Transform& transform, CollisionWorld& world, bodyindex id);
/// Destructor
virtual ~CollisionBody();
@ -124,11 +137,8 @@ class CollisionBody : public Body {
/// Set the type of the body
void setType(BodyType type);
/// Return the collision shape
CollisionShape* getCollisionShape() const;
/// Set the collision shape
void setCollisionShape(CollisionShape* collisionShape);
/// Set whether or not the body is active
virtual void setIsActive(bool isActive);
/// Return the current position and orientation
const Transform& getTransform() const;
@ -136,8 +146,12 @@ class CollisionBody : public Body {
/// Set the current position and orientation
void setTransform(const Transform& transform);
/// Return the AAABB of the body
const AABB& getAABB() const;
/// Add a collision shape to the body.
virtual ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
const Transform& transform);
/// Remove a collision shape from the body
virtual void removeCollisionShape(const ProxyShape* proxyShape);
/// Return the interpolated transform for rendering
Transform getInterpolatedTransform() const;
@ -154,10 +168,20 @@ class CollisionBody : public Body {
/// Return the first element of the linked list of contact manifolds involving this body
const ContactManifoldListElement* getContactManifoldsLists() const;
/// Return true if a point is inside the collision body
bool testPointInside(const Vector3& worldPoint) const;
/// Raycast method with feedback information
bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
// -------------------- Friendship -------------------- //
friend class CollisionWorld;
friend class DynamicsWorld;
friend class CollisionDetection;
friend class BroadPhaseAlgorithm;
friend class ConvexMeshShape;
friend class ProxyShape;
};
// Return the type of the body
@ -168,18 +192,12 @@ inline BodyType CollisionBody::getType() const {
// Set the type of the body
inline void CollisionBody::setType(BodyType type) {
mType = type;
}
// Return the collision shape
inline CollisionShape* CollisionBody::getCollisionShape() const {
assert(mCollisionShape);
return mCollisionShape;
}
if (mType == STATIC) {
// Set the collision shape
inline void CollisionBody::setCollisionShape(CollisionShape* collisionShape) {
assert(collisionShape);
mCollisionShape = collisionShape;
// Update the broad-phase state of the body
updateBroadPhaseState();
}
}
// Return the interpolated transform for rendering
@ -201,20 +219,14 @@ inline const Transform& CollisionBody::getTransform() const {
// Set the current position and orientation
inline void CollisionBody::setTransform(const Transform& transform) {
// Check if the body has moved
if (mTransform != transform) {
mHasMoved = true;
}
// Update the transform of the body
mTransform = transform;
// Update the broad-phase state of the body
updateBroadPhaseState();
}
// Return the AAABB of the body
inline const AABB& CollisionBody::getAABB() const {
return mAabb;
}
// Return true if the body can collide with others bodies
// Return true if the body can collide with others bodies
inline bool CollisionBody::isCollisionEnabled() const {
return mIsCollisionEnabled;
}
@ -230,13 +242,6 @@ inline void CollisionBody::updateOldTransform() {
mOldTransform = mTransform;
}
// Update the rigid body in order to reflect a change in the body state
inline void CollisionBody::updateAABB() {
// Update the AABB
mCollisionShape->updateAABB(mAabb, mTransform);
}
// Return the first element of the linked list of contact manifolds involving this body
inline const ContactManifoldListElement* CollisionBody::getContactManifoldsLists() const {
return mContactManifoldsList;

248
src/body/RigidBody.cpp
View File

@ -26,30 +26,21 @@
// Libraries
#include "RigidBody.h"
#include "constraint/Joint.h"
#include "../collision/shapes/CollisionShape.h"
#include "collision/shapes/CollisionShape.h"
#include "engine/DynamicsWorld.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
RigidBody::RigidBody(const Transform& transform, decimal mass, CollisionShape *collisionShape,
bodyindex id)
: CollisionBody(transform, collisionShape, id), mInitMass(mass), mIsGravityEnabled(true),
mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)), mJointsList(NULL) {
assert(collisionShape);
// If the mass is not positive, set it to one
if (mInitMass <= decimal(0.0)) {
mInitMass = decimal(1.0);
}
// Compute the inertia tensor using the collision shape of the body
mCollisionShape->computeLocalInertiaTensor(mInertiaTensorLocal, mInitMass);
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id)
: CollisionBody(transform, world, id), mInitMass(decimal(1.0)),
mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
mJointsList(NULL) {
// Compute the inverse mass
mMassInverse = decimal(1.0) / mass;
mMassInverse = decimal(1.0) / mInitMass;
}
// Destructor
@ -64,6 +55,9 @@ void RigidBody::setType(BodyType type) {
CollisionBody::setType(type);
// Recompute the total mass, center of mass and inertia tensor
recomputeMassInformation();
// If it is a static body
if (mType == STATIC) {
@ -77,7 +71,8 @@ void RigidBody::setType(BodyType type) {
// Reset the inverse mass and inverse inertia tensor to zero
mMassInverse = decimal(0.0);
mInertiaTensorLocalInverse = Matrix3x3::zero();
mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero();
}
else { // If it is a dynamic body
@ -87,36 +82,58 @@ void RigidBody::setType(BodyType type) {
// Awake the body
setIsSleeping(false);
// Remove all the contacts with this body
resetContactManifoldsList();
// Ask the broad-phase to test again the collision shapes of the body for collision
// detection (as if the body has moved)
askForBroadPhaseCollisionCheck();
// Reset the force and torque on the body
mExternalForce.setToZero();
mExternalTorque.setToZero();
}
// Method that set the mass of the body
// Set the local inertia tensor of the body (in local-space coordinates)
void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
if (mType != DYNAMIC) return;
mInertiaTensorLocal = inertiaTensorLocal;
// Compute the inverse local inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
}
// Set the local center of mass of the body (in local-space coordinates)
void RigidBody::setCenterOfMassLocal(const Vector3& centerOfMassLocal) {
if (mType != DYNAMIC) return;
const Vector3 oldCenterOfMass = mCenterOfMassWorld;
mCenterOfMassLocal = centerOfMassLocal;
// Compute the center of mass in world-space coordinates
mCenterOfMassWorld = mTransform * mCenterOfMassLocal;
// Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
}
// Set the mass of the rigid body
void RigidBody::setMass(decimal mass) {
if (mType != DYNAMIC) return;
mInitMass = mass;
// If the mass is negative, set it to one
if (mInitMass <= decimal(0.0)) {
mInitMass = decimal(1.0);
}
// Recompute the inverse mass
if (mType == DYNAMIC) {
if (mInitMass > decimal(0.0)) {
mMassInverse = decimal(1.0) / mInitMass;
}
else {
mMassInverse = decimal(0.0);
}
}
// Set the local inertia tensor of the body (in body coordinates)
void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
mInertiaTensorLocal = inertiaTensorLocal;
// Recompute the inverse local inertia tensor
if (mType == DYNAMIC) {
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
}
else {
mInertiaTensorLocalInverse = Matrix3x3::zero();
mInitMass = decimal(1.0);
mMassInverse = decimal(1.0);
}
}
@ -148,4 +165,155 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
}
}
// Add a collision shape to the body.
/// This methods will create a copy of the collision shape you provided inside the world and
/// return a pointer to the actual collision shape in the world. You can use this pointer to
/// remove the collision from the body. Note that when the body is destroyed, all the collision
/// shapes will also be destroyed automatically. Because an internal copy of the collision shape
/// you provided is performed, you can delete it right after calling this method.
/// The second parameter is the mass of the collision shape (this will used to compute the
/// total mass of the rigid body and its inertia tensor). The mass must be positive. The third
/// parameter is the transformation that transform the local-space of the collision shape into
/// the local-space of the body. By default, the second parameter is the identity transform.
ProxyShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape,
const Transform& transform,
decimal mass) {
assert(mass > decimal(0.0));
// Create an internal copy of the collision shape into the world if it is not there yet
CollisionShape* newCollisionShape = mWorld.createCollisionShape(collisionShape);
// Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
sizeof(ProxyShape))) ProxyShape(this, newCollisionShape,
transform, mass);
// Add it to the list of proxy collision shapes of the body
if (mProxyCollisionShapes == NULL) {
mProxyCollisionShapes = proxyShape;
}
else {
proxyShape->mNext = mProxyCollisionShapes;
mProxyCollisionShapes = proxyShape;
}
// Compute the world-space AABB of the new collision shape
AABB aabb;
newCollisionShape->computeAABB(aabb, mTransform * transform);
// Notify the collision detection about this new collision shape
mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb);
mNbCollisionShapes++;
// Recompute the center of mass, total mass and inertia tensor of the body with the new
// collision shape
recomputeMassInformation();
// Return a pointer to the proxy collision shape
return proxyShape;
}
// Remove a collision shape from the body
void RigidBody::removeCollisionShape(const ProxyShape* proxyCollisionShape) {
// Remove the collision shape
CollisionBody::removeCollisionShape(proxyCollisionShape);
// Recompute the total mass, center of mass and inertia tensor
recomputeMassInformation();
}
// Recompute the center of mass, total mass and inertia tensor of the body using all
// the collision shapes attached to the body.
void RigidBody::recomputeMassInformation() {
mInitMass = decimal(0.0);
mMassInverse = decimal(0.0);
mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero();
mCenterOfMassLocal.setToZero();
// If it is STATIC or KINEMATIC body
if (mType == STATIC || mType == KINEMATIC) {
mCenterOfMassWorld = mTransform.getPosition();
return;
}
assert(mType == DYNAMIC);
// Compute the total mass of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
mInitMass += shape->getMass();
mCenterOfMassLocal += shape->getLocalToBodyTransform().getPosition() * shape->getMass();
}
if (mInitMass > decimal(0.0)) {
mMassInverse = decimal(1.0) / mInitMass;
}
else {
mInitMass = decimal(1.0);
mMassInverse = decimal(1.0);
}
// Compute the center of mass
const Vector3 oldCenterOfMass = mCenterOfMassWorld;
mCenterOfMassLocal *= mMassInverse;
mCenterOfMassWorld = mTransform * mCenterOfMassLocal;
// Compute the total mass and inertia tensor using all the collision shapes
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Get the inertia tensor of the collision shape in its local-space
Matrix3x3 inertiaTensor;
shape->getCollisionShape()->computeLocalInertiaTensor(inertiaTensor, shape->getMass());
// Convert the collision shape inertia tensor into the local-space of the body
const Transform& shapeTransform = shape->getLocalToBodyTransform();
Matrix3x3 rotationMatrix = shapeTransform.getOrientation().getMatrix();
inertiaTensor = rotationMatrix * inertiaTensor * rotationMatrix.getTranspose();
// Use the parallel axis theorem to convert the inertia tensor w.r.t the collision shape
// center into a inertia tensor w.r.t to the body origin.
Vector3 offset = shapeTransform.getPosition() - mCenterOfMassLocal;
decimal offsetSquare = offset.lengthSquare();
Matrix3x3 offsetMatrix;
offsetMatrix[0].setAllValues(offsetSquare, decimal(0.0), decimal(0.0));
offsetMatrix[1].setAllValues(decimal(0.0), offsetSquare, decimal(0.0));
offsetMatrix[2].setAllValues(decimal(0.0), decimal(0.0), offsetSquare);
offsetMatrix[0] += offset * (-offset.x);
offsetMatrix[1] += offset * (-offset.y);
offsetMatrix[2] += offset * (-offset.z);
offsetMatrix *= shape->getMass();
mInertiaTensorLocal += inertiaTensor + offsetMatrix;
}
// Compute the local inverse inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
// Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
}
// Update the broad-phase state for this body (because it has moved for instance)
void RigidBody::updateBroadPhaseState() const {
PROFILE("RigidBody::updateBroadPhaseState()");
DynamicsWorld& world = dynamic_cast<DynamicsWorld&>(mWorld);
const Vector3 displacement = world.mTimer.getTimeStep() * mLinearVelocity;
// For all the proxy collision shapes of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Recompute the world-space AABB of the collision shape
AABB aabb;
shape->getCollisionShape()->computeAABB(aabb, mTransform *shape->getLocalToBodyTransform());
// Update the broad-phase state for the proxy collision shape
mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb, displacement);
}
}

67
src/body/RigidBody.h
View File

@ -29,9 +29,9 @@
// Libraries
#include <cassert>
#include "CollisionBody.h"
#include "../engine/Material.h"
#include "../mathematics/mathematics.h"
#include "../memory/MemoryAllocator.h"
#include "engine/Material.h"
#include "mathematics/mathematics.h"
#include "memory/MemoryAllocator.h"
/// Namespace reactphysics3d
namespace reactphysics3d {
@ -39,7 +39,7 @@ namespace reactphysics3d {
// Class declarations
struct JointListElement;
class Joint;
class DynamicsWorld;
// Class RigidBody
/**
@ -57,6 +57,13 @@ class RigidBody : public CollisionBody {
/// Intial mass of the body
decimal mInitMass;
/// Center of mass of the body in local-space coordinates.
/// The center of mass can therefore be different from the body origin
Vector3 mCenterOfMassLocal;
/// Center of mass of the body in world-space coordinates
Vector3 mCenterOfMassWorld;
/// Linear velocity of the body
Vector3 mLinearVelocity;
@ -69,7 +76,8 @@ class RigidBody : public CollisionBody {
/// Current external torque on the body
Vector3 mExternalTorque;
/// Local inertia tensor of the body (in local-space)
/// Local inertia tensor of the body (in local-space) with respect to the
/// center of mass of the body
Matrix3x3 mInertiaTensorLocal;
/// Inverse of the inertia tensor of the body
@ -104,13 +112,18 @@ class RigidBody : public CollisionBody {
/// Remove a joint from the joints list
void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint);
/// Update the transform of the body after a change of the center of mass
void updateTransformWithCenterOfMass();
/// Update the broad-phase state for this body (because it has moved for instance)
virtual void updateBroadPhaseState() const;
public :
// -------------------- Methods -------------------- //
/// Constructor
RigidBody(const Transform& transform, decimal mass, CollisionShape* collisionShape,
bodyindex id);
RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id);
/// Destructor
virtual ~RigidBody();
@ -121,9 +134,6 @@ class RigidBody : public CollisionBody {
/// Return the mass of the body
decimal getMass() const;
/// Set the mass of the body
void setMass(decimal mass);
/// Return the linear velocity
Vector3 getLinearVelocity() const;
@ -142,6 +152,12 @@ class RigidBody : public CollisionBody {
/// Set the local inertia tensor of the body (in body coordinates)
void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal);
/// Set the local center of mass of the body (in local-space coordinates)
void setCenterOfMassLocal(const Vector3& centerOfMassLocal);
/// Set the mass of the rigid body
void setMass(decimal mass);
/// Return the inertia tensor in world coordinates.
Matrix3x3 getInertiaTensorWorld() const;
@ -178,8 +194,8 @@ class RigidBody : public CollisionBody {
/// Set the variable to know whether or not the body is sleeping
virtual void setIsSleeping(bool isSleeping);
/// Apply an external force to the body at its gravity center.
void applyForceToCenter(const Vector3& force);
/// Apply an external force to the body at its center of mass.
void applyForceToCenterOfMass(const Vector3& force);
/// Apply an external force to the body at a given point (in world-space coordinates).
void applyForce(const Vector3& force, const Vector3& point);
@ -187,6 +203,18 @@ class RigidBody : public CollisionBody {
/// Apply an external torque to the body.
void applyTorque(const Vector3& torque);
/// Add a collision shape to the body.
virtual ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
const Transform& transform,
decimal mass);
/// Remove a collision shape from the body
virtual void removeCollisionShape(const ProxyShape* proxyCollisionShape);
/// Recompute the center of mass, total mass and inertia tensor of the body using all
/// the collision shapes attached to the body.
void recomputeMassInformation();
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
@ -329,12 +357,12 @@ inline void RigidBody::setIsSleeping(bool isSleeping) {
Body::setIsSleeping(isSleeping);
}
// Apply an external force to the body at its gravity center.
// Apply an external force to the body at its center of mass.
/// If the body is sleeping, calling this method will wake it up. Note that the
/// force will we added to the sum of the applied forces and that this sum will be
/// reset to zero at the end of each call of the DynamicsWorld::update() method.
/// You can only apply a force to a dynamic body otherwise, this method will do nothing.
inline void RigidBody::applyForceToCenter(const Vector3& force) {
inline void RigidBody::applyForceToCenterOfMass(const Vector3& force) {
// If it is not a dynamic body, we do nothing
if (mType != DYNAMIC) return;
@ -349,7 +377,7 @@ inline void RigidBody::applyForceToCenter(const Vector3& force) {
}
// Apply an external force to the body at a given point (in world-space coordinates).
/// If the point is not at the center of gravity of the body, it will also
/// If the point is not at the center of mass of the body, it will also
/// generate some torque and therefore, change the angular velocity of the body.
/// If the body is sleeping, calling this method will wake it up. Note that the
/// force will we added to the sum of the applied forces and that this sum will be
@ -367,7 +395,7 @@ inline void RigidBody::applyForce(const Vector3& force, const Vector3& point) {
// Add the force and torque
mExternalForce += force;
mExternalTorque += (point - mTransform.getPosition()).cross(force);
mExternalTorque += (point - mCenterOfMassWorld).cross(force);
}
// Apply an external torque to the body.
@ -389,6 +417,13 @@ inline void RigidBody::applyTorque(const Vector3& torque) {
mExternalTorque += torque;
}
/// Update the transform of the body after a change of the center of mass
inline void RigidBody::updateTransformWithCenterOfMass() {
// Translate the body according to the translation of the center of mass position
mTransform.setPosition(mCenterOfMassWorld - mTransform.getOrientation() * mCenterOfMassLocal);
}
}
#endif

121
src/collision/BroadPhasePair.h
View File

@ -1,121 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_BROAD_PHASE_PAIR_H
#define REACTPHYSICS3D_BROAD_PHASE_PAIR_H
// Libraries
#include "../body/CollisionBody.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Structure BroadPhasePair
/**
* This structure represents a pair of bodies
* during the broad-phase collision detection.
*/
struct BroadPhasePair {
public:
// -------------------- Attributes -------------------- //
/// Pointer to the first body
CollisionBody* body1;
/// Pointer to the second body
CollisionBody* body2;
/// Previous cached separating axis
Vector3 previousSeparatingAxis;
// -------------------- Methods -------------------- //
/// Constructor
BroadPhasePair(CollisionBody* body1, CollisionBody* body2);
/// Destructor
~BroadPhasePair();
/// Return the pair of bodies index
static bodyindexpair computeBodiesIndexPair(CollisionBody* body1, CollisionBody* body2);
/// Return the pair of bodies index
bodyindexpair getBodiesIndexPair() const;
/// Smaller than operator
bool operator<(const BroadPhasePair& broadPhasePair2) const;
/// Larger than operator
bool operator>(const BroadPhasePair& broadPhasePair2) const;
/// Equal operator
bool operator==(const BroadPhasePair& broadPhasePair2) const;
/// Not equal operator
bool operator!=(const BroadPhasePair& broadPhasePair2) const;
};
// Return the pair of bodies index
inline bodyindexpair BroadPhasePair::computeBodiesIndexPair(CollisionBody* body1,
CollisionBody* body2) {
// Construct the pair of body index
bodyindexpair indexPair = body1->getID() < body2->getID() ?
std::make_pair(body1->getID(), body2->getID()) :
std::make_pair(body2->getID(), body1->getID());
assert(indexPair.first != indexPair.second);
return indexPair;
}
// Return the pair of bodies index
inline bodyindexpair BroadPhasePair::getBodiesIndexPair() const {
return computeBodiesIndexPair(body1, body2);
}
// Smaller than operator
inline bool BroadPhasePair::operator<(const BroadPhasePair& broadPhasePair2) const {
return (body1 < broadPhasePair2.body1 ? true : (body2 < broadPhasePair2.body2));
}
// Larger than operator
inline bool BroadPhasePair::operator>(const BroadPhasePair& broadPhasePair2) const {
return (body1 > broadPhasePair2.body1 ? true : (body2 > broadPhasePair2.body2));
}
// Equal operator
inline bool BroadPhasePair::operator==(const BroadPhasePair& broadPhasePair2) const {
return (body1 == broadPhasePair2.body1 && body2 == broadPhasePair2.body2);
}
// Not equal operator
inline bool BroadPhasePair::operator!=(const BroadPhasePair& broadPhasePair2) const {
return (body1 != broadPhasePair2.body1 || body2 != broadPhasePair2.body2);
}
}
#endif

219
src/collision/CollisionDetection.cpp
View File

@ -25,13 +25,11 @@
// Libraries
#include "CollisionDetection.h"
#include "../engine/CollisionWorld.h"
#include "broadphase/SweepAndPruneAlgorithm.h"
#include "broadphase/NoBroadPhaseAlgorithm.h"
#include "../body/Body.h"
#include "../collision/shapes/BoxShape.h"
#include "../body/RigidBody.h"
#include "../configuration.h"
#include "engine/CollisionWorld.h"
#include "body/Body.h"
#include "collision/shapes/BoxShape.h"
#include "body/RigidBody.h"
#include "configuration.h"
#include <cassert>
#include <complex>
#include <set>
@ -44,20 +42,16 @@ using namespace std;
// Constructor
CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator)
: mWorld(world), mMemoryAllocator(memoryAllocator),
: mWorld(world), mBroadPhaseAlgorithm(*this),
mNarrowPhaseGJKAlgorithm(memoryAllocator),
mNarrowPhaseSphereVsSphereAlgorithm(memoryAllocator) {
mNarrowPhaseSphereVsSphereAlgorithm(memoryAllocator),
mIsCollisionShapesAdded(false) {
// Create the broad-phase algorithm that will be used (Sweep and Prune with AABB)
mBroadPhaseAlgorithm = new SweepAndPruneAlgorithm(*this);
assert(mBroadPhaseAlgorithm != NULL);
}
// Destructor
CollisionDetection::~CollisionDetection() {
// Delete the broad-phase algorithm
delete mBroadPhaseAlgorithm;
}
// Compute the collision detection
@ -77,114 +71,185 @@ void CollisionDetection::computeBroadPhase() {
PROFILE("CollisionDetection::computeBroadPhase()");
// Notify the broad-phase algorithm about the bodies that have moved since last frame
for (set<CollisionBody*>::iterator it = mWorld->getBodiesBeginIterator();
it != mWorld->getBodiesEndIterator(); it++) {
// If new collision shapes have been added to bodies
if (mIsCollisionShapesAdded) {
// If the body has moved
if ((*it)->mHasMoved) {
// Notify the broad-phase that the body has moved
mBroadPhaseAlgorithm->updateObject(*it, (*it)->getAABB());
}
}
// Ask the broad-phase to recompute the overlapping pairs of collision
// shapes. This call can only add new overlapping pairs in the collision
// detection.
mBroadPhaseAlgorithm.computeOverlappingPairs();
}
}
// Compute the narrow-phase collision detection
void CollisionDetection::computeNarrowPhase() {
PROFILE("CollisionDetection::computeNarrowPhase()");
map<bodyindexpair, BroadPhasePair*>::iterator it;
// For each possible collision pair of bodies
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); it++) {
map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
ContactPointInfo* contactInfo = NULL;
BroadPhasePair* pair = (*it).second;
assert(pair != NULL);
OverlappingPair* pair = it->second;
CollisionBody* const body1 = pair->body1;
CollisionBody* const body2 = pair->body2;
ProxyShape* shape1 = pair->getShape1();
ProxyShape* shape2 = pair->getShape2();
assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
// Check that the two shapes are overlapping. If the shapes are not overlapping
// anymore, we remove the overlapping pair.
if (!mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2)) {
std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
++it;
// Destroy the overlapping pair
itToRemove->second->OverlappingPair::~OverlappingPair();
mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
mOverlappingPairs.erase(itToRemove);
continue;
}
else {
++it;
}
CollisionBody* const body1 = shape1->getBody();
CollisionBody* const body2 = shape2->getBody();
// Update the contact cache of the overlapping pair
mWorld->updateOverlappingPair(pair);
pair->update();
// Check if the two bodies are allowed to collide, otherwise, we do not test for collision
if (pair->body1->getType() != DYNAMIC && pair->body2->getType() != DYNAMIC) continue;
if (mNoCollisionPairs.count(pair->getBodiesIndexPair()) > 0) continue;
if (body1->getType() != DYNAMIC && body2->getType() != DYNAMIC) continue;
bodyindexpair bodiesIndex = OverlappingPair::computeBodiesIndexPair(body1, body2);
if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
// Check if the two bodies are sleeping, if so, we do no test collision between them
if (body1->isSleeping() && body2->isSleeping()) continue;
// Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm& narrowPhaseAlgorithm = SelectNarrowPhaseAlgorithm(
body1->getCollisionShape(),
body2->getCollisionShape());
shape1->getCollisionShape(),
shape2->getCollisionShape());
// Notify the narrow-phase algorithm about the overlapping pair we are going to test
narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision
if (narrowPhaseAlgorithm.testCollision(body1->getCollisionShape(), body1->getTransform(),
body2->getCollisionShape(), body2->getTransform(),
contactInfo)) {
if (narrowPhaseAlgorithm.testCollision(shape1, shape2, contactInfo)) {
assert(contactInfo != NULL);
// Set the bodies of the contact
contactInfo->body1 = dynamic_cast<RigidBody*>(body1);
contactInfo->body2 = dynamic_cast<RigidBody*>(body2);
assert(contactInfo->body1 != NULL);
assert(contactInfo->body2 != NULL);
// Notify the world about the new narrow-phase contact
mWorld->notifyNewContact(pair, contactInfo);
// Create a new contact
createContact(pair, contactInfo);
// Delete and remove the contact info from the memory allocator
contactInfo->ContactPointInfo::~ContactPointInfo();
mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
}
}
}
// Allow the broadphase to notify the collision detection about an overlapping pair.
/// This method is called by a broad-phase collision detection algorithm
void CollisionDetection::broadPhaseNotifyAddedOverlappingPair(BodyPair* addedPair) {
/// This method is called by the broad-phase collision detection algorithm
void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
// Get the pair of body index
bodyindexpair indexPair = addedPair->getBodiesIndexPair();
assert(shape1->mBroadPhaseID != shape2->mBroadPhaseID);
// Create the corresponding broad-phase pair object
BroadPhasePair* broadPhasePair = new (mMemoryAllocator.allocate(sizeof(BroadPhasePair)))
BroadPhasePair(addedPair->body1, addedPair->body2);
assert(broadPhasePair != NULL);
// If the two proxy collision shapes are from the same body, skip it
if (shape1->getBody()->getID() == shape2->getBody()->getID()) return;
// Add the pair into the set of overlapping pairs (if not there yet)
pair<map<bodyindexpair, BroadPhasePair*>::iterator, bool> check = mOverlappingPairs.insert(
make_pair(indexPair,
broadPhasePair));
// Compute the overlapping pair ID
overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2);
// Check if the overlapping pair already exists
if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return;
// Create the overlapping pair and add it into the set of overlapping pairs
OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair)))
OverlappingPair(shape1, shape2, mWorld->mMemoryAllocator);
assert(newPair != NULL);
std::pair<map<overlappingpairid, OverlappingPair*>::iterator, bool> check =
mOverlappingPairs.insert(make_pair(pairID, newPair));
assert(check.second);
// Notify the world about the new broad-phase overlapping pair
mWorld->notifyAddedOverlappingPair(broadPhasePair);
}
// Allow the broadphase to notify the collision detection about a removed overlapping pair
void CollisionDetection::broadPhaseNotifyRemovedOverlappingPair(BodyPair* removedPair) {
// Remove a body from the collision detection
void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
// Get the pair of body index
bodyindexpair indexPair = removedPair->getBodiesIndexPair();
// Remove all the overlapping pairs involving this proxy shape
std::map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
if (it->second->getShape1()->mBroadPhaseID == proxyShape->mBroadPhaseID||
it->second->getShape2()->mBroadPhaseID == proxyShape->mBroadPhaseID) {
std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
++it;
// Get the broad-phase pair
BroadPhasePair* broadPhasePair = mOverlappingPairs.find(indexPair)->second;
assert(broadPhasePair != NULL);
// Destroy the overlapping pair
itToRemove->second->OverlappingPair::~OverlappingPair();
mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
mOverlappingPairs.erase(itToRemove);
}
else {
++it;
}
}
// Notify the world about the removed broad-phase pair
mWorld->notifyRemovedOverlappingPair(broadPhasePair);
// Remove the overlapping pair from the memory allocator
broadPhasePair->BroadPhasePair::~BroadPhasePair();
mMemoryAllocator.release(broadPhasePair, sizeof(BroadPhasePair));
mOverlappingPairs.erase(indexPair);
// Remove the body from the broad-phase
mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape);
}
// Create a new contact
void CollisionDetection::createContact(OverlappingPair* overlappingPair,
const ContactPointInfo* contactInfo) {
// Create a new contact
ContactPoint* contact = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPoint)))
ContactPoint(*contactInfo);
assert(contact != NULL);
// If it is the first contact since the pair are overlapping
if (overlappingPair->getNbContactPoints() == 0) {
// Trigger a callback event
if (mWorld->mEventListener != NULL) mWorld->mEventListener->beginContact(*contactInfo);
}
// Add the contact to the contact cache of the corresponding overlapping pair
overlappingPair->addContact(contact);
// Add the contact manifold into the list of contact manifolds
// of the two bodies involved in the contact
addContactManifoldToBody(overlappingPair->getContactManifold(),
overlappingPair->getShape1()->getBody(),
overlappingPair->getShape2()->getBody());
// Trigger a callback event for the new contact
if (mWorld->mEventListener != NULL) mWorld->mEventListener->newContact(*contactInfo);
}
// Add a contact manifold to the linked list of contact manifolds of the two bodies involved
// in the corresponding contact
void CollisionDetection::addContactManifoldToBody(ContactManifold* contactManifold,
CollisionBody* body1, CollisionBody* body2) {
assert(contactManifold != NULL);
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement1 = new (allocatedMemory1)
ContactManifoldListElement(contactManifold,
body1->mContactManifoldsList);
body1->mContactManifoldsList = listElement1;
// Add the contact manifold at the beginning of the linked
// list of the contact manifolds of the second body
void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement2 = new (allocatedMemory2)
ContactManifoldListElement(contactManifold,
body2->mContactManifoldsList);
body2->mContactManifoldsList = listElement2;
}

105
src/collision/CollisionDetection.h
View File

@ -27,13 +27,13 @@
#define REACTPHYSICS3D_COLLISION_DETECTION_H
// Libraries
#include "../body/CollisionBody.h"
#include "body/CollisionBody.h"
#include "broadphase/BroadPhaseAlgorithm.h"
#include "BroadPhasePair.h"
#include "engine/OverlappingPair.h"
#include "narrowphase/GJK/GJKAlgorithm.h"
#include "narrowphase/SphereVsSphereAlgorithm.h"
#include "../memory/MemoryAllocator.h"
#include "../constraint/ContactPoint.h"
#include "memory/MemoryAllocator.h"
#include "constraint/ContactPoint.h"
#include <vector>
#include <map>
#include <set>
@ -62,14 +62,11 @@ class CollisionDetection {
/// Pointer to the physics world
CollisionWorld* mWorld;
/// Memory allocator
MemoryAllocator& mMemoryAllocator;
/// Broad-phase overlapping pairs
std::map<bodyindexpair, BroadPhasePair*> mOverlappingPairs;
std::map<overlappingpairid, OverlappingPair*> mOverlappingPairs;
/// Broad-phase algorithm
BroadPhaseAlgorithm* mBroadPhaseAlgorithm;
BroadPhaseAlgorithm mBroadPhaseAlgorithm;
/// Narrow-phase GJK algorithm
GJKAlgorithm mNarrowPhaseGJKAlgorithm;
@ -80,6 +77,9 @@ class CollisionDetection {
/// Set of pair of bodies that cannot collide between each other
std::set<bodyindexpair> mNoCollisionPairs;
/// True if some collision shapes have been added previously
bool mIsCollisionShapesAdded;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -95,8 +95,16 @@ class CollisionDetection {
void computeNarrowPhase();
/// Select the narrow phase algorithm to use given two collision shapes
NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(CollisionShape* collisionShape1,
CollisionShape* collisionShape2);
NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(const CollisionShape* collisionShape1,
const CollisionShape* collisionShape2);
/// Create a new contact
void createContact(OverlappingPair* overlappingPair, const ContactPointInfo* contactInfo);
/// Add a contact manifold to the linked list of contact manifolds of the two bodies
/// involed in the corresponding contact.
void addContactManifoldToBody(ContactManifold* contactManifold,
CollisionBody *body1, CollisionBody *body2);
public :
@ -108,31 +116,44 @@ class CollisionDetection {
/// Destructor
~CollisionDetection();
/// Add a body to the collision detection
void addBody(CollisionBody* body);
/// Add a proxy collision shape to the collision detection
void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb);
/// Remove a body from the collision detection
void removeBody(CollisionBody* body);
/// Remove a proxy collision shape from the collision detection
void removeProxyCollisionShape(ProxyShape* proxyShape);
/// Update a proxy collision shape (that has moved for instance)
void updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb,
const Vector3& displacement = Vector3(0, 0, 0));
/// Add a pair of bodies that cannot collide with each other
void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
/// Remove a pair of bodies that cannot collide with each other
void removeNoCollisionPair(CollisionBody *body1, CollisionBody *body2);
void removeNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
/// Ask for a collision shape to be tested again during broad-phase.
void askForBroadPhaseCollisionCheck(ProxyShape* shape);
/// Compute the collision detection
void computeCollisionDetection();
/// Allow the broadphase to notify the collision detection about a new overlapping pair.
void broadPhaseNotifyAddedOverlappingPair(BodyPair* pair);
/// Ray casting method
void raycast(RaycastCallback* raycastCallback, const Ray& ray) const;
/// Allow the broadphase to notify the collision detection about a removed overlapping pair
void broadPhaseNotifyRemovedOverlappingPair(BodyPair* pair);
/// Allow the broadphase to notify the collision detection about an overlapping pair.
void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
friend class ConvexMeshShape;
};
// Select the narrow-phase collision algorithm to use given two collision shapes
inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm(
CollisionShape* collisionShape1, CollisionShape* collisionShape2) {
const CollisionShape* collisionShape1,
const CollisionShape* collisionShape2) {
// Sphere vs Sphere algorithm
if (collisionShape1->getType() == SPHERE && collisionShape2->getType() == SPHERE) {
@ -144,29 +165,49 @@ inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm(
}
// Add a body to the collision detection
inline void CollisionDetection::addBody(CollisionBody* body) {
inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape,
const AABB& aabb) {
// Add the body to the broad-phase
mBroadPhaseAlgorithm->addObject(body, body->getAABB());
}
mBroadPhaseAlgorithm.addProxyCollisionShape(proxyShape, aabb);
// Remove a body from the collision detection
inline void CollisionDetection::removeBody(CollisionBody* body) {
// Remove the body from the broad-phase
mBroadPhaseAlgorithm->removeObject(body);
}
mIsCollisionShapesAdded = true;
}
// Add a pair of bodies that cannot collide with each other
inline void CollisionDetection::addNoCollisionPair(CollisionBody* body1,
CollisionBody* body2) {
mNoCollisionPairs.insert(BroadPhasePair::computeBodiesIndexPair(body1, body2));
mNoCollisionPairs.insert(OverlappingPair::computeBodiesIndexPair(body1, body2));
}
// Remove a pair of bodies that cannot collide with each other
inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1,
CollisionBody* body2) {
mNoCollisionPairs.erase(BroadPhasePair::computeBodiesIndexPair(body1, body2));
mNoCollisionPairs.erase(OverlappingPair::computeBodiesIndexPair(body1, body2));
}
// Ask for a collision shape to be tested again during broad-phase.
/// We simply put the shape in the list of collision shape that have moved in the
/// previous frame so that it is tested for collision again in the broad-phase.
inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) {
mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID);
}
// Update a proxy collision shape (that has moved for instance)
inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb,
const Vector3& displacement) {
mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
}
// Ray casting method
inline void CollisionDetection::raycast(RaycastCallback* raycastCallback,
const Ray& ray) const {
RaycastTest rayCastTest(raycastCallback);
// Ask the broad-phase algorithm to call the testRaycastAgainstShape()
// callback method for each proxy shape hit by the ray in the broad-phase
mBroadPhaseAlgorithm.raycast(ray, rayCastTest);
}
}

30
src/collision/ProxyShape.cpp Normal file
View File

@ -0,0 +1,30 @@
// Libraries
#include "ProxyShape.h"
using namespace reactphysics3d;
// Constructor
ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform,
decimal mass)
:mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
mNext(NULL), mBroadPhaseID(-1), mCachedCollisionData(NULL), mUserData(NULL) {
}
// Destructor
ProxyShape::~ProxyShape() {
// Release the cached collision data memory
if (mCachedCollisionData != NULL) {
free(mCachedCollisionData);
}
}
// Return true if a point is inside the collision shape
bool ProxyShape::testPointInside(const Vector3& worldPoint) {
const Transform localToWorld = mBody->getTransform() * mLocalToBodyTransform;
const Vector3 localPoint = localToWorld.getInverse() * worldPoint;
return mCollisionShape->testPointInside(localPoint, this);
}

175
src/collision/ProxyShape.h Normal file
View File

@ -0,0 +1,175 @@
#ifndef REACTPHYSICS3D_PROXY_SHAPE_H
#define REACTPHYSICS3D_PROXY_SHAPE_H
// Libraries
#include "body/CollisionBody.h"
#include "shapes/CollisionShape.h"
namespace reactphysics3d {
// Class ProxyShape
/**
* The CollisionShape instances are supposed to be unique for memory optimization. For instance,
* consider two rigid bodies with the same sphere collision shape. In this situation, we will have
* a unique instance of SphereShape but we need to differentiate between the two instances during
* the collision detection. They do not have the same position in the world and they do not
* belong to the same rigid body. The ProxyShape class is used for that purpose by attaching a
* rigid body with one of its collision shape. A body can have multiple proxy shapes (one for
* each collision shape attached to the body).
*/
class ProxyShape {
protected:
// -------------------- Attributes -------------------- //
/// Pointer to the parent body
CollisionBody* mBody;
/// Internal collision shape
CollisionShape* mCollisionShape;
/// Local-space to parent body-space transform (does not change over time)
const Transform mLocalToBodyTransform;
/// Mass (in kilogramms) of the corresponding collision shape
decimal mMass;
/// Pointer to the next proxy shape of the body (linked list)
ProxyShape* mNext;
/// Broad-phase ID (node ID in the dynamic AABB tree)
int mBroadPhaseID;
/// Cached collision data
void* mCachedCollisionData;
/// Pointer to user data
void* mUserData;
// -------------------- Methods -------------------- //
/// Private copy-constructor
ProxyShape(const ProxyShape& proxyShape);
/// Private assignment operator
ProxyShape& operator=(const ProxyShape& proxyShape);
// Return a local support point in a given direction with the object margin
Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
/// Return a local support point in a given direction without the object margin.
Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction);
/// Return the collision shape margin
decimal getMargin() const;
public:
// -------------------- Methods -------------------- //
/// Constructor
ProxyShape(CollisionBody* body, CollisionShape* shape,
const Transform& transform, decimal mass);
/// Destructor
~ProxyShape();
/// Return the collision shape
const CollisionShape* getCollisionShape() const;
/// Return the parent body
CollisionBody* getBody() const;
/// Return the mass of the collision shape
decimal getMass() const;
/// Return a pointer to the user data attached to this body
void* getUserData() const;
/// Attach user data to this body
void setUserData(void* userData);
/// Return the local to parent body transform
const Transform& getLocalToBodyTransform() const;
/// Return the local to world transform
const Transform getLocalToWorldTransform() const;
/// Return true if a point is inside the collision shape
bool testPointInside(const Vector3& worldPoint);
/// Raycast method with feedback information
bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
// -------------------- Friendship -------------------- //
friend class OverlappingPair;
friend class CollisionBody;
friend class RigidBody;
friend class BroadPhaseAlgorithm;
friend class DynamicAABBTree;
friend class CollisionDetection;
friend class EPAAlgorithm;
friend class GJKAlgorithm;
friend class ConvexMeshShape;
};
/// Return the collision shape
inline const CollisionShape* ProxyShape::getCollisionShape() const {
return mCollisionShape;
}
// Return the parent body
inline CollisionBody* ProxyShape::getBody() const {
return mBody;
}
// Return the mass of the collision shape
inline decimal ProxyShape::getMass() const {
return mMass;
}
// Return a pointer to the user data attached to this body
inline void* ProxyShape::getUserData() const {
return mUserData;
}
// Attach user data to this body
inline void ProxyShape::setUserData(void* userData) {
mUserData = userData;
}
// Return the local to parent body transform
inline const Transform& ProxyShape::getLocalToBodyTransform() const {
return mLocalToBodyTransform;
}
// Return the local to world transform
inline const Transform ProxyShape::getLocalToWorldTransform() const {
return mBody->mTransform * mLocalToBodyTransform;
}
// Return a local support point in a given direction with the object margin
inline Vector3 ProxyShape::getLocalSupportPointWithMargin(const Vector3& direction) {
return mCollisionShape->getLocalSupportPointWithMargin(direction, &mCachedCollisionData);
}
// Return a local support point in a given direction without the object margin.
inline Vector3 ProxyShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
return mCollisionShape->getLocalSupportPointWithoutMargin(direction, &mCachedCollisionData);
}
// Return the collision shape margin
inline decimal ProxyShape::getMargin() const {
return mCollisionShape->getMargin();
}
// Raycast method with feedback information
inline bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
return mCollisionShape->raycast(ray, raycastInfo, this);
}
}
#endif

29
src/collision/BroadPhasePair.cpp → src/collision/RaycastInfo.cpp
View File

@ -1,6 +1,6 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
* Copyright (c) 2010-2014 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
@ -24,17 +24,26 @@
********************************************************************************/
// Libraries
#include "BroadPhasePair.h"
#include "decimal.h"
#include "RaycastInfo.h"
#include "ProxyShape.h"
using namespace reactphysics3d;
// Constructor
BroadPhasePair::BroadPhasePair(CollisionBody* body1, CollisionBody* body2)
: body1(body1), body2(body2), previousSeparatingAxis(Vector3(1,1,1)) {
}
// Destructor
BroadPhasePair::~BroadPhasePair() {
// Ray cast test against a proxy shape
decimal RaycastTest::raycastAgainstShape(ProxyShape* shape, const Ray& ray) {
// Ray casting test against the collision shape
RaycastInfo raycastInfo;
bool isHit = shape->raycast(ray, raycastInfo);
// If the ray hit the collision shape
if (isHit) {
// Report the hit to the user and return the
// user hit fraction value
return userCallback->notifyRaycastHit(raycastInfo);
}
return ray.maxFraction;
}

142
src/collision/RaycastInfo.h Normal file
View File

@ -0,0 +1,142 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_RAYCAST_INFO_H
#define REACTPHYSICS3D_RAYCAST_INFO_H
// Libraries
#include "mathematics/Vector3.h"
#include "mathematics/Ray.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declarations
class CollisionBody;
class ProxyShape;
class CollisionShape;
// Structure RaycastInfo
/**
* This structure contains the information about a raycast hit.
*/
struct RaycastInfo {
private:
// -------------------- Methods -------------------- //
/// Private copy constructor
RaycastInfo(const RaycastInfo& raycastInfo);
/// Private assignment operator
RaycastInfo& operator=(const RaycastInfo& raycastInfo);
public:
// -------------------- Attributes -------------------- //
/// Hit point in world-space coordinates
Vector3 worldPoint;
/// Surface normal at hit point in world-space coordinates
Vector3 worldNormal;
/// Fraction distance of the hit point between point1 and point2 of the ray
/// The hit point "p" is such that p = point1 + hitFraction * (point2 - point1)
decimal hitFraction;
/// Pointer to the hit collision body
CollisionBody* body;
/// Pointer to the hit proxy collision shape
ProxyShape* proxyShape;
// -------------------- Methods -------------------- //
/// Constructor
RaycastInfo() : body(NULL), proxyShape(NULL) {
}
/// Destructor
~RaycastInfo() {
}
};
// Class RaycastCallback
/**
* This class can be used to register a callback for ray casting queries.
* You should implement your own class inherited from this one and implement
* the notifyRaycastHit() method. This method will be called for each ProxyShape
* that is hit by the ray.
*/
class RaycastCallback {
public:
// -------------------- Methods -------------------- //
/// Destructor
virtual ~RaycastCallback() {
}
/// This method will be called for each ProxyShape that is hit by the
/// ray. You cannot make any assumptions about the order of the
/// calls. You should use the return value to control the continuation
/// of the ray. The return value is the next maxFraction value to use.
/// If you return a fraction of 0.0, it means that the raycast should
/// terminate. If you return a fraction of 1.0, it indicates that the
/// ray is not clipped and the ray cast should continue as if no hit
/// occurred. If you return the fraction in the parameter (hitFraction
/// value in the RaycastInfo object), the current ray will be clipped
/// to this fraction in the next queries. If you return -1.0, it will
/// ignore this ProxyShape and continue the ray cast.
virtual decimal notifyRaycastHit(const RaycastInfo& raycastInfo)=0;
};
/// Structure RaycastTest
struct RaycastTest {
public:
/// User callback class
RaycastCallback* userCallback;
/// Constructor
RaycastTest(RaycastCallback* callback) {
userCallback = callback;
}
/// Ray cast test against a proxy shape
decimal raycastAgainstShape(ProxyShape* shape, const Ray& ray);
};
}
#endif

219
src/collision/broadphase/BroadPhaseAlgorithm.cpp
View File

@ -25,17 +25,234 @@
// Libraries
#include "BroadPhaseAlgorithm.h"
#include "collision/CollisionDetection.h"
#include "engine/Profiler.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
:mPairManager(collisionDetection), mCollisionDetection(collisionDetection) {
:mDynamicAABBTree(*this), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
mCollisionDetection(collisionDetection) {
// Allocate memory for the array of non-static bodies IDs
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
assert(mMovedShapes != NULL);
// Allocate memory for the array of potential overlapping pairs
mPotentialPairs = (BroadPair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPair));
assert(mPotentialPairs != NULL);
}
// Destructor
BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
// Release the memory for the array of non-static bodies IDs
free(mMovedShapes);
// Release the memory for the array of potential overlapping pairs
free(mPotentialPairs);
}
// Add a collision shape in the array of shapes that have moved in the last simulation step
// and that need to be tested again for broad-phase overlapping.
void BroadPhaseAlgorithm::addMovedCollisionShape(int broadPhaseID) {
// Allocate more elements in the array of bodies that have moved if necessary
if (mNbAllocatedMovedShapes == mNbMovedShapes) {
mNbAllocatedMovedShapes *= 2;
int* oldArray = mMovedShapes;
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
assert(mMovedShapes != NULL);
memcpy(mMovedShapes, oldArray, mNbMovedShapes * sizeof(int));
free(oldArray);
}
// Store the broad-phase ID into the array of bodies that have moved
assert(mNbMovedShapes < mNbAllocatedMovedShapes);
assert(mMovedShapes != NULL);
mMovedShapes[mNbMovedShapes] = broadPhaseID;
mNbMovedShapes++;
}
// Remove a collision shape from the array of shapes that have moved in the last simulation step
// and that need to be tested again for broad-phase overlapping.
void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
assert(mNbNonUsedMovedShapes <= mNbMovedShapes);
// If less than the quarter of allocated elements of the non-static bodies IDs array
// are used, we release some allocated memory
if ((mNbMovedShapes - mNbNonUsedMovedShapes) < mNbAllocatedMovedShapes / 4 &&
mNbAllocatedMovedShapes > 8) {
mNbAllocatedMovedShapes /= 2;
int* oldArray = mMovedShapes;
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
assert(mMovedShapes != NULL);
uint nbElements = 0;
for (uint i=0; i<mNbMovedShapes; i++) {
if (oldArray[i] != -1) {
mMovedShapes[nbElements] = oldArray[i];
nbElements++;
}
}
mNbMovedShapes = nbElements;
mNbNonUsedMovedShapes = 0;
free(oldArray);
}
// Remove the broad-phase ID from the array
for (uint i=0; i<mNbMovedShapes; i++) {
if (mMovedShapes[i] == broadPhaseID) {
mMovedShapes[i] = -1;
mNbNonUsedMovedShapes++;
break;
}
}
}
// Add a proxy collision shape into the broad-phase collision detection
void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
// Add the collision shape into the dynamic AABB tree and get its broad-phase ID
mDynamicAABBTree.addObject(proxyShape, aabb);
// Add the collision shape into the array of bodies that have moved (or have been created)
// during the last simulation step
addMovedCollisionShape(proxyShape->mBroadPhaseID);
}
// Remove a proxy collision shape from the broad-phase collision detection
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
int broadPhaseID = proxyShape->mBroadPhaseID;
// Remove the collision shape from the dynamic AABB tree
mDynamicAABBTree.removeObject(broadPhaseID);
// Remove the collision shape into the array of bodies that have moved (or have been created)
// during the last simulation step
removeMovedCollisionShape(broadPhaseID);
}
// Notify the broad-phase that a collision shape has moved and need to be updated
void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb,
const Vector3& displacement) {
int broadPhaseID = proxyShape->mBroadPhaseID;
assert(broadPhaseID >= 0);
// Update the dynamic AABB tree according to the movement of the collision shape
bool hasBeenReInserted = mDynamicAABBTree.updateObject(broadPhaseID, aabb, displacement);
// If the collision shape has moved out of its fat AABB (and therefore has been reinserted
// into the tree).
if (hasBeenReInserted) {
// Add the collision shape into the array of bodies that have moved (or have been created)
// during the last simulation step
addMovedCollisionShape(broadPhaseID);
}
}
// Compute all the overlapping pairs of collision shapes
void BroadPhaseAlgorithm::computeOverlappingPairs() {
// Reset the potential overlapping pairs
mNbPotentialPairs = 0;
// For all collision shapes that have moved (or have been created) during the
// last simulation step
for (uint i=0; i<mNbMovedShapes; i++) {
uint shapeID = mMovedShapes[i];
if (shapeID == -1) continue;
// Get the fat AABB of the collision shape from the dynamic AABB tree
const AABB& shapeAABB = mDynamicAABBTree.getFatAABB(shapeID);
// Ask the dynamic AABB tree to report all collision shapes that overlap with
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
// by the dynamic AABB tree for each potential overlapping pair.
mDynamicAABBTree.reportAllShapesOverlappingWith(shapeID, shapeAABB);
}
// Reset the array of collision shapes that have move (or have been created) during the
// last simulation step
mNbMovedShapes = 0;
// Sort the array of potential overlapping pairs in order to remove duplicate pairs
std::sort(mPotentialPairs, mPotentialPairs + mNbPotentialPairs, BroadPair::smallerThan);
// Check all the potential overlapping pairs avoiding duplicates to report unique
// overlapping pairs
uint i=0;
while (i < mNbPotentialPairs) {
// Get a potential overlapping pair
BroadPair* pair = mPotentialPairs + i;
i++;
// Get the two collision shapes of the pair
ProxyShape* shape1 = mDynamicAABBTree.getCollisionShape(pair->collisionShape1ID);
ProxyShape* shape2 = mDynamicAABBTree.getCollisionShape(pair->collisionShape2ID);
// Notify the collision detection about the overlapping pair
mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
// Skip the duplicate overlapping pairs
while (i < mNbPotentialPairs) {
// Get the next pair
BroadPair* nextPair = mPotentialPairs + i;
// If the next pair is different from the previous one, we stop skipping pairs
if (nextPair->collisionShape1ID != pair->collisionShape1ID ||
nextPair->collisionShape2ID != pair->collisionShape2ID) {
break;
}
i++;
}
}
// If the number of potential overlapping pairs is less than the quarter of allocated
// number of overlapping pairs
if (mNbPotentialPairs < mNbAllocatedPotentialPairs / 4 && mNbPotentialPairs > 8) {
// Reduce the number of allocated potential overlapping pairs
BroadPair* oldPairs = mPotentialPairs;
mNbAllocatedPotentialPairs /= 2;
mPotentialPairs = (BroadPair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPair));
assert(mPotentialPairs);
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPair));
free(oldPairs);
}
}
// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void BroadPhaseAlgorithm::notifyOverlappingPair(int node1ID, int node2ID) {
// If both the nodes are the same, we do not create store the overlapping pair
if (node1ID == node2ID) return;
// If we need to allocate more memory for the array of potential overlapping pairs
if (mNbPotentialPairs == mNbAllocatedPotentialPairs) {
// Allocate more memory for the array of potential pairs
BroadPair* oldPairs = mPotentialPairs;
mNbAllocatedPotentialPairs *= 2;
mPotentialPairs = (BroadPair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPair));
assert(mPotentialPairs);
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPair));
free(oldPairs);
}
// Add the new potential pair into the array of potential overlapping pairs
mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID);
mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID);
mNbPotentialPairs++;
}

148
src/collision/broadphase/BroadPhaseAlgorithm.h
View File

@ -28,8 +28,9 @@
// Libraries
#include <vector>
#include "../../body/CollisionBody.h"
#include "PairManager.h"
#include "body/CollisionBody.h"
#include "collision/ProxyShape.h"
#include "DynamicAABBTree.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -37,17 +38,42 @@ namespace reactphysics3d {
// Declarations
class CollisionDetection;
// TODO : Check that when a kinematic or static body is manually moved, the dynamic aabb tree
// is correctly updated
// TODO : Replace the names "body, bodies" by "collision shapes"
// TODO : Remove the pair manager
// TODO : RENAME THIS
// Structure BroadPair
/**
* This structure represent a potential overlapping pair during the broad-phase collision
* detection.
*/
struct BroadPair {
// -------------------- Attributes -------------------- //
/// Broad-phase ID of the first collision shape
int collisionShape1ID;
/// Broad-phase ID of the second collision shape
int collisionShape2ID;
// -------------------- Methods -------------------- //
/// Method used to compare two pairs for sorting algorithm
static bool smallerThan(const BroadPair& pair1, const BroadPair& pair2);
};
// Class BroadPhaseAlgorithm
/**
* This class is an abstract class that represents an algorithm
* used to perform the broad-phase of a collision detection. The
* goal of the broad-phase algorithm is to compute the pair of bodies
* that can collide. But it's important to understand that the
* broad-phase doesn't compute only body pairs that can collide but
* could also pairs of body that doesn't collide but are very close.
* 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
* narrow-phase.
* This class represents the broad-phase collision detection. The
* goal of the broad-phase collision detection is to compute the pairs of bodies
* that have their AABBs overlapping. Only those pairs of bodies will be tested
* later for collision during the narrow-phase collision detection. A dynamic AABB
* tree data structure is used for fast broad-phase collision detection.
*/
class BroadPhaseAlgorithm {
@ -55,8 +81,34 @@ class BroadPhaseAlgorithm {
// -------------------- Attributes -------------------- //
/// Pair manager containing the overlapping pairs
PairManager mPairManager;
/// Dynamic AABB tree
DynamicAABBTree mDynamicAABBTree;
/// Array with the broad-phase IDs of all collision shapes that have moved (or have been
/// created) during the last simulation step. Those are the shapes that need to be tested
/// for overlapping in the next simulation step.
int* mMovedShapes;
/// Number of collision shapes in the array of shapes that have moved during the last
/// simulation step.
uint mNbMovedShapes;
/// Number of allocated elements for the array of shapes that have moved during the last
/// simulation step.
uint mNbAllocatedMovedShapes;
/// Number of non-used elements in the array of shapes that have moved during the last
/// simulation step.
uint mNbNonUsedMovedShapes;
/// Temporary array of potential overlapping pairs (with potential duplicates)
BroadPair* mPotentialPairs;
/// Number of potential overlapping pairs
uint mNbPotentialPairs;
/// Number of allocated elements for the array of potential overlapping pairs
uint mNbAllocatedPotentialPairs;
/// Reference to the collision detection object
CollisionDetection& mCollisionDetection;
@ -77,33 +129,65 @@ class BroadPhaseAlgorithm {
BroadPhaseAlgorithm(CollisionDetection& collisionDetection);
/// Destructor
virtual ~BroadPhaseAlgorithm();
~BroadPhaseAlgorithm();
/// Notify the broad-phase about a new object in the world
virtual void addObject(CollisionBody* body, const AABB& aabb)=0;
/// Add a proxy collision shape into the broad-phase collision detection
void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb);
/// Notify the broad-phase about an object that has been removed from the world
virtual void removeObject(CollisionBody* body)=0;
/// Remove a proxy collision shape from the broad-phase collision detection
void removeProxyCollisionShape(ProxyShape* proxyShape);
/// Notify the broad-phase that the AABB of an object has changed
virtual void updateObject(CollisionBody* body, const AABB& aabb)=0;
/// Notify the broad-phase that a collision shape has moved and need to be updated
void updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb,
const Vector3& displacement);
/// Return a pointer to the first active pair (used to iterate over the active pairs)
BodyPair* beginOverlappingPairsPointer() const;
/// Add a collision shape in the array of shapes that have moved in the last simulation step
/// and that need to be tested again for broad-phase overlapping.
void addMovedCollisionShape(int broadPhaseID);
/// Return a pointer to the last active pair (used to iterate over the active pairs)
BodyPair* endOverlappingPairsPointer() const;
/// Remove a collision shape from the array of shapes that have moved in the last simulation
/// step and that need to be tested again for broad-phase overlapping.
void removeMovedCollisionShape(int broadPhaseID);
/// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void notifyOverlappingPair(int node1ID, int node2ID);
/// Compute all the overlapping pairs of collision shapes
void computeOverlappingPairs();
/// Return true if the two broad-phase collision shapes are overlapping
bool testOverlappingShapes(ProxyShape* shape1, ProxyShape* shape2) const;
/// Ray casting method
void raycast(const Ray& ray, RaycastTest& raycastTest) const;
};
// Return a pointer to the first active pair (used to iterate over the overlapping pairs)
inline BodyPair* BroadPhaseAlgorithm::beginOverlappingPairsPointer() const {
return mPairManager.beginOverlappingPairsPointer();
}
// Method used to compare two pairs for sorting algorithm
inline bool BroadPair::smallerThan(const BroadPair& pair1, const BroadPair& pair2) {
// Return a pointer to the last active pair (used to iterate over the overlapping pairs)
inline BodyPair* BroadPhaseAlgorithm::endOverlappingPairsPointer() const {
return mPairManager.endOverlappingPairsPointer();
}
if (pair1.collisionShape1ID < pair2.collisionShape1ID) return true;
if (pair1.collisionShape1ID == pair2.collisionShape1ID) {
return pair1.collisionShape2ID < pair2.collisionShape2ID;
}
return false;
}
// Return true if the two broad-phase collision shapes are overlapping
inline bool BroadPhaseAlgorithm::testOverlappingShapes(ProxyShape* shape1,
ProxyShape* shape2) const {
// Get the two AABBs of the collision shapes
const AABB& aabb1 = mDynamicAABBTree.getFatAABB(shape1->mBroadPhaseID);
const AABB& aabb2 = mDynamicAABBTree.getFatAABB(shape2->mBroadPhaseID);
// Check if the two AABBs are overlapping
return aabb1.testCollision(aabb2);
}
// Ray casting method
inline void BroadPhaseAlgorithm::raycast(const Ray& ray,
RaycastTest& raycastTest) const {
mDynamicAABBTree.raycast(ray, raycastTest);
}
}

754
src/collision/broadphase/DynamicAABBTree.cpp Normal file
View File

@ -0,0 +1,754 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "DynamicAABBTree.h"
#include "BroadPhaseAlgorithm.h"
#include "memory/Stack.h"
#include "engine/Profiler.h"
using namespace reactphysics3d;
// Initialization of static variables
const int TreeNode::NULL_TREE_NODE = -1;
// Constructor
DynamicAABBTree::DynamicAABBTree(BroadPhaseAlgorithm& broadPhase) : mBroadPhase(broadPhase){
mRootNodeID = TreeNode::NULL_TREE_NODE;
mNbNodes = 0;
mNbAllocatedNodes = 8;
// Allocate memory for the nodes of the tree
mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
assert(mNodes);
memset(mNodes, 0, mNbAllocatedNodes * sizeof(TreeNode));
// Initialize the allocated nodes
for (int i=0; i<mNbAllocatedNodes - 1; i++) {
mNodes[i].nextNodeID = i + 1;
mNodes[i].height = -1;
}
mNodes[mNbAllocatedNodes - 1].nextNodeID = TreeNode::NULL_TREE_NODE;
mNodes[mNbAllocatedNodes - 1].height = -1;
mFreeNodeID = 0;
}
// Destructor
DynamicAABBTree::~DynamicAABBTree() {
// Free the allocated memory for the nodes
free(mNodes);
}
// Allocate and return a new node in the tree
int DynamicAABBTree::allocateNode() {
// If there is no more allocated node to use
if (mFreeNodeID == TreeNode::NULL_TREE_NODE) {
assert(mNbNodes == mNbAllocatedNodes);
// Allocate more nodes in the tree
mNbAllocatedNodes *= 2;
TreeNode* oldNodes = mNodes;
mNodes = (TreeNode*) malloc(mNbAllocatedNodes * sizeof(TreeNode));
assert(mNodes);
memcpy(mNodes, oldNodes, mNbNodes * sizeof(TreeNode));
free(oldNodes);
// Initialize the allocated nodes
for (int i=mNbNodes; i<mNbAllocatedNodes - 1; i++) {
mNodes[i].nextNodeID = i + 1;
mNodes[i].height = -1;
}
mNodes[mNbAllocatedNodes - 1].nextNodeID = TreeNode::NULL_TREE_NODE;
mNodes[mNbAllocatedNodes - 1].height = -1;
mFreeNodeID = mNbNodes;
}
// Get the next free node
int freeNodeID = mFreeNodeID;
mFreeNodeID = mNodes[freeNodeID].nextNodeID;
mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE;
mNodes[freeNodeID].leftChildID = TreeNode::NULL_TREE_NODE;
mNodes[freeNodeID].rightChildID = TreeNode::NULL_TREE_NODE;
mNodes[freeNodeID].proxyShape = NULL;
mNodes[freeNodeID].height = 0;
mNbNodes++;
return freeNodeID;
}
// Release a node
void DynamicAABBTree::releaseNode(int nodeID) {
assert(mNbNodes > 0);
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].height >= 0);
mNodes[nodeID].nextNodeID = mFreeNodeID;
mNodes[nodeID].height = -1;
mFreeNodeID = nodeID;
mNbNodes--;
}
// Add an object into the tree. This method creates a new leaf node in the tree and
// returns the ID of the corresponding node.
void DynamicAABBTree::addObject(ProxyShape* proxyShape, const AABB& aabb) {
// Get the next available node (or allocate new ones if necessary)
int nodeID = allocateNode();
// Create the fat aabb to use in the tree
const Vector3 gap(DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP);
mNodes[nodeID].aabb.setMin(aabb.getMin() - gap);
mNodes[nodeID].aabb.setMax(aabb.getMax() + gap);
// Set the collision shape
mNodes[nodeID].proxyShape = proxyShape;
// Set the height of the node in the tree
mNodes[nodeID].height = 0;
// Insert the new leaf node in the tree
insertLeafNode(nodeID);
assert(mNodes[nodeID].isLeaf());
// Set the broad-phase ID of the proxy shape
proxyShape->mBroadPhaseID = nodeID;
assert(nodeID >= 0);
}
// Remove an object from the tree
void DynamicAABBTree::removeObject(int nodeID) {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
// Remove the node from the tree
removeLeafNode(nodeID);
releaseNode(nodeID);
}
// Update the dynamic tree after an object has moved.
/// If the new AABB of the object that has moved is still inside its fat AABB, then
/// nothing is done. Otherwise, the corresponding node is removed and reinserted into the tree.
/// The method returns true if the object has been reinserted into the tree. The "displacement"
/// argument is the linear velocity of the AABB multiplied by the elapsed time between two
/// frames.
bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement) {
PROFILE("DynamicAABBTree::updateObject()");
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
assert(mNodes[nodeID].height >= 0);
// If the new AABB is still inside the fat AABB of the node
if (mNodes[nodeID].aabb.contains(newAABB)) {
return false;
}
// If the new AABB is outside the fat AABB, we remove the corresponding node
removeLeafNode(nodeID);
// Compute the fat AABB by inflating the AABB with a constant gap
mNodes[nodeID].aabb = newAABB;
const Vector3 gap(DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP);
mNodes[nodeID].aabb.mMinCoordinates -= gap;
mNodes[nodeID].aabb.mMaxCoordinates += gap;
// Inflate the fat AABB in direction of the linear motion of the AABB
if (displacement.x < decimal(0.0)) {
mNodes[nodeID].aabb.mMinCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x;
}
else {
mNodes[nodeID].aabb.mMaxCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x;
}
if (displacement.y < decimal(0.0)) {
mNodes[nodeID].aabb.mMinCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y;
}
else {
mNodes[nodeID].aabb.mMaxCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y;
}
if (displacement.z < decimal(0.0)) {
mNodes[nodeID].aabb.mMinCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z;
}
else {
mNodes[nodeID].aabb.mMaxCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z;
}
assert(mNodes[nodeID].aabb.contains(newAABB));
// Reinsert the node into the tree
insertLeafNode(nodeID);
return true;
}
// Insert a leaf node in the tree. The process of inserting a new leaf node
// in the dynamic tree is described in the book "Introduction to Game Physics
// with Box2D" by Ian Parberry.
void DynamicAABBTree::insertLeafNode(int nodeID) {
// If the tree is empty
if (mRootNodeID == TreeNode::NULL_TREE_NODE) {
mRootNodeID = nodeID;
mNodes[mRootNodeID].parentID = TreeNode::NULL_TREE_NODE;
return;
}
assert(mRootNodeID != TreeNode::NULL_TREE_NODE);
// Find the best sibling node for the new node
AABB newNodeAABB = mNodes[nodeID].aabb;
int currentNodeID = mRootNodeID;
while (!mNodes[currentNodeID].isLeaf()) {
int leftChild = mNodes[currentNodeID].leftChildID;
int rightChild = mNodes[currentNodeID].rightChildID;
// Compute the merged AABB
decimal volumeAABB = mNodes[currentNodeID].aabb.getVolume();
AABB mergedAABBs;
mergedAABBs.mergeTwoAABBs(mNodes[currentNodeID].aabb, newNodeAABB);
decimal mergedVolume = mergedAABBs.getVolume();
// Compute the cost of making the current node the sibbling of the new node
decimal costS = decimal(2.0) * mergedVolume;
// Compute the minimum cost of pushing the new node further down the tree (inheritance cost)
decimal costI = decimal(2.0) * (mergedVolume - volumeAABB);
// Compute the cost of descending into the left child
decimal costLeft;
AABB currentAndLeftAABB;
currentAndLeftAABB.mergeTwoAABBs(newNodeAABB, mNodes[leftChild].aabb);
if (mNodes[leftChild].isLeaf()) { // If the left child is a leaf
costLeft = currentAndLeftAABB.getVolume() + costI;
}
else {
decimal leftChildVolume = mNodes[leftChild].aabb.getVolume();
costLeft = costI + currentAndLeftAABB.getVolume() - leftChildVolume;
}
// Compute the cost of descending into the right child
decimal costRight;
AABB currentAndRightAABB;
currentAndRightAABB.mergeTwoAABBs(newNodeAABB, mNodes[rightChild].aabb);
if (mNodes[rightChild].isLeaf()) { // If the right child is a leaf
costRight = currentAndRightAABB.getVolume() + costI;
}
else {
decimal rightChildVolume = mNodes[rightChild].aabb.getVolume();
costRight = costI + currentAndRightAABB.getVolume() - rightChildVolume;
}
// If the cost of making the current node a sibbling of the new node is smaller than
// the cost of going down into the left or right child
if (costS < costLeft && costS < costRight) break;
// It is cheaper to go down into a child of the current node, choose the best child
if (costLeft < costRight) {
currentNodeID = leftChild;
}
else {
currentNodeID = rightChild;
}
}
int siblingNode = currentNodeID;
// Create a new parent for the new node and the sibling node
int oldParentNode = mNodes[siblingNode].parentID;
int newParentNode = allocateNode();
mNodes[newParentNode].parentID = oldParentNode;
mNodes[newParentNode].proxyShape = NULL;
mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB);
mNodes[newParentNode].height = mNodes[siblingNode].height + 1;
assert(mNodes[newParentNode].height > 0);
// If the sibling node was not the root node
if (oldParentNode != TreeNode::NULL_TREE_NODE) {
if (mNodes[oldParentNode].leftChildID == siblingNode) {
mNodes[oldParentNode].leftChildID = newParentNode;
}
else {
mNodes[oldParentNode].rightChildID = newParentNode;
}
mNodes[newParentNode].leftChildID = siblingNode;
mNodes[newParentNode].rightChildID = nodeID;
mNodes[siblingNode].parentID = newParentNode;
mNodes[nodeID].parentID = newParentNode;
}
else { // If the sibling node was the root node
mNodes[newParentNode].leftChildID = siblingNode;
mNodes[newParentNode].rightChildID = nodeID;
mNodes[siblingNode].parentID = newParentNode;
mNodes[nodeID].parentID = newParentNode;
mRootNodeID = newParentNode;
}
// Move up in the tree to change the AABBs that have changed
currentNodeID = mNodes[nodeID].parentID;
while (currentNodeID != TreeNode::NULL_TREE_NODE) {
// Balance the sub-tree of the current node if it is not balanced
currentNodeID = balanceSubTreeAtNode(currentNodeID);
assert(mNodes[nodeID].isLeaf());
int leftChild = mNodes[currentNodeID].leftChildID;
int rightChild = mNodes[currentNodeID].rightChildID;
assert(leftChild != TreeNode::NULL_TREE_NODE);
assert(rightChild != TreeNode::NULL_TREE_NODE);
// Recompute the height of the node in the tree
mNodes[currentNodeID].height = std::max(mNodes[leftChild].height,
mNodes[rightChild].height) + 1;
assert(mNodes[currentNodeID].height > 0);
// Recompute the AABB of the node
mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
currentNodeID = mNodes[currentNodeID].parentID;
}
assert(mNodes[nodeID].isLeaf());
}
// Remove a leaf node from the tree
void DynamicAABBTree::removeLeafNode(int nodeID) {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
// If we are removing the root node (root node is a leaf in this case)
if (mRootNodeID == nodeID) {
mRootNodeID = TreeNode::NULL_TREE_NODE;
return;
}
int parentNodeID = mNodes[nodeID].parentID;
int grandParentNodeID = mNodes[parentNodeID].parentID;
int siblingNodeID;
if (mNodes[parentNodeID].leftChildID == nodeID) {
siblingNodeID = mNodes[parentNodeID].rightChildID;
}
else {
siblingNodeID = mNodes[parentNodeID].leftChildID;
}
// If the parent of the node to remove is not the root node
if (grandParentNodeID != TreeNode::NULL_TREE_NODE) {
// Destroy the parent node
if (mNodes[grandParentNodeID].leftChildID == parentNodeID) {
mNodes[grandParentNodeID].leftChildID = siblingNodeID;
}
else {
assert(mNodes[grandParentNodeID].rightChildID == parentNodeID);
mNodes[grandParentNodeID].rightChildID = siblingNodeID;
}
mNodes[siblingNodeID].parentID = grandParentNodeID;
releaseNode(parentNodeID);
// Now, we need to recompute the AABBs of the node on the path back to the root
// and make sure that the tree is still balanced
int currentNodeID = grandParentNodeID;
while(currentNodeID != TreeNode::NULL_TREE_NODE) {
// Balance the current sub-tree if necessary
currentNodeID = balanceSubTreeAtNode(currentNodeID);
// Get the two children of the current node
int leftChildID = mNodes[currentNodeID].leftChildID;
int rightChildID = mNodes[currentNodeID].rightChildID;
// Recompute the AABB and the height of the current node
mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb,
mNodes[rightChildID].aabb);
mNodes[currentNodeID].height = std::max(mNodes[leftChildID].height,
mNodes[rightChildID].height) + 1;
assert(mNodes[currentNodeID].height > 0);
currentNodeID = mNodes[currentNodeID].parentID;
}
}
else { // If the parent of the node to remove is the root node
// The sibling node becomes the new root node
mRootNodeID = siblingNodeID;
mNodes[siblingNodeID].parentID = TreeNode::NULL_TREE_NODE;
releaseNode(parentNodeID);
}
}
// Balance the sub-tree of a given node using left or right rotations.
/// The rotation schemes are described in in the book "Introduction to Game Physics
/// with Box2D" by Ian Parberry. This method returns the new root node ID.
int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
assert(nodeID != TreeNode::NULL_TREE_NODE);
TreeNode* nodeA = mNodes + nodeID;
// If the node is a leaf or the height of A's sub-tree is less than 2
if (nodeA->isLeaf() || nodeA->height < 2) {
// Do not perform any rotation
return nodeID;
}
// Get the two children nodes
int nodeBID = nodeA->leftChildID;
int nodeCID = nodeA->rightChildID;
assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes);
assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes);
TreeNode* nodeB = mNodes + nodeBID;
TreeNode* nodeC = mNodes + nodeCID;
// Compute the factor of the left and right sub-trees
int balanceFactor = nodeC->height - nodeB->height;
// If the right node C is 2 higher than left node B
if (balanceFactor > 1) {
int nodeFID = nodeC->leftChildID;
int nodeGID = nodeC->rightChildID;
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
TreeNode* nodeF = mNodes + nodeFID;
TreeNode* nodeG = mNodes + nodeGID;
nodeC->leftChildID = nodeID;
nodeC->parentID = nodeA->parentID;
nodeA->parentID = nodeCID;
if (nodeC->parentID != TreeNode::NULL_TREE_NODE) {
if (mNodes[nodeC->parentID].leftChildID == nodeID) {
mNodes[nodeC->parentID].leftChildID = nodeCID;
}
else {
assert(mNodes[nodeC->parentID].rightChildID == nodeID);
mNodes[nodeC->parentID].rightChildID = nodeCID;
}
}
else {
mRootNodeID = nodeCID;
}
// If the right node C was higher than left node B because of the F node
if (nodeF->height > nodeG->height) {
nodeC->rightChildID = nodeFID;
nodeA->rightChildID = nodeGID;
nodeG->parentID = nodeID;
// Recompute the AABB of node A and C
nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeG->aabb);
nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb);
// Recompute the height of node A and C
nodeA->height = std::max(nodeB->height, nodeG->height) + 1;
nodeC->height = std::max(nodeA->height, nodeF->height) + 1;
assert(nodeA->height > 0);
assert(nodeC->height > 0);
}
else { // If the right node C was higher than left node B because of node G
nodeC->rightChildID = nodeGID;
nodeA->rightChildID = nodeFID;
nodeF->parentID = nodeID;
// Recompute the AABB of node A and C
nodeA->aabb.mergeTwoAABBs(nodeB->aabb, nodeF->aabb);
nodeC->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb);
// Recompute the height of node A and C
nodeA->height = std::max(nodeB->height, nodeF->height) + 1;
nodeC->height = std::max(nodeA->height, nodeG->height) + 1;
assert(nodeA->height > 0);
assert(nodeC->height > 0);
}
// Return the new root of the sub-tree
return nodeCID;
}
// If the left node B is 2 higher than right node C
if (balanceFactor < -1) {
int nodeFID = nodeB->leftChildID;
int nodeGID = nodeB->rightChildID;
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
TreeNode* nodeF = mNodes + nodeFID;
TreeNode* nodeG = mNodes + nodeGID;
nodeB->leftChildID = nodeID;
nodeB->parentID = nodeA->parentID;
nodeA->parentID = nodeBID;
if (nodeB->parentID != TreeNode::NULL_TREE_NODE) {
if (mNodes[nodeB->parentID].leftChildID == nodeID) {
mNodes[nodeB->parentID].leftChildID = nodeBID;
}
else {
assert(mNodes[nodeB->parentID].rightChildID == nodeID);
mNodes[nodeB->parentID].rightChildID = nodeBID;
}
}
else {
mRootNodeID = nodeBID;
}
// If the left node B was higher than right node C because of the F node
if (nodeF->height > nodeG->height) {
nodeB->rightChildID = nodeFID;
nodeA->leftChildID = nodeGID;
nodeG->parentID = nodeID;
// Recompute the AABB of node A and B
nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeG->aabb);
nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeF->aabb);
// Recompute the height of node A and B
nodeA->height = std::max(nodeC->height, nodeG->height) + 1;
nodeB->height = std::max(nodeA->height, nodeF->height) + 1;
assert(nodeA->height > 0);
assert(nodeB->height > 0);
}
else { // If the left node B was higher than right node C because of node G
nodeB->rightChildID = nodeGID;
nodeA->leftChildID = nodeFID;
nodeF->parentID = nodeID;
// Recompute the AABB of node A and B
nodeA->aabb.mergeTwoAABBs(nodeC->aabb, nodeF->aabb);
nodeB->aabb.mergeTwoAABBs(nodeA->aabb, nodeG->aabb);
// Recompute the height of node A and B
nodeA->height = std::max(nodeC->height, nodeF->height) + 1;
nodeB->height = std::max(nodeA->height, nodeG->height) + 1;
assert(nodeA->height > 0);
assert(nodeB->height > 0);
}
// Return the new root of the sub-tree
return nodeBID;
}
// If the sub-tree is balanced, return the current root node
return nodeID;
}
// Report all shapes overlapping with the AABB given in parameter.
/// For each overlapping shape with the AABB given in parameter, the
/// BroadPhase::notifyOverlappingPair() method is called to store a
/// potential overlapping pair.
void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aabb) {
// Create a stack with the nodes to visit
Stack<int, 64> stack;
stack.push(mRootNodeID);
// While there are still nodes to visit
while(stack.getNbElements() > 0) {
// Get the next node ID to visit
int nodeIDToVisit = stack.pop();
// Skip it if it is a null node
if (nodeIDToVisit == TreeNode::NULL_TREE_NODE) continue;
// Get the corresponding node
const TreeNode* nodeToVisit = mNodes + nodeIDToVisit;
// If the AABB in parameter overlaps with the AABB of the node to visit
if (aabb.testCollision(nodeToVisit->aabb)) {
// If the node is a leaf
if (nodeToVisit->isLeaf()) {
// Notify the broad-phase about a new potential overlapping pair
mBroadPhase.notifyOverlappingPair(nodeID, nodeIDToVisit);
}
else { // If the node is not a leaf
// We need to visit its children
stack.push(nodeToVisit->leftChildID);
stack.push(nodeToVisit->rightChildID);
}
}
}
}
// Ray casting method
void DynamicAABBTree::raycast(const Ray& ray, RaycastTest& raycastTest) const {
decimal maxFraction = ray.maxFraction;
// Create an AABB for the ray
Vector3 endPoint = ray.point1 +
maxFraction * (ray.point2 - ray.point1);
AABB rayAABB(Vector3::min(ray.point1, endPoint),
Vector3::max(ray.point1, endPoint));
Stack<int, 128> stack;
stack.push(mRootNodeID);
// Walk through the tree from the root looking for proxy shapes
// that overlap with the ray AABB
while (stack.getNbElements() > 0) {
// Get the next node in the stack
int nodeID = stack.pop();
// If it is a null node, skip it
if (nodeID == TreeNode::NULL_TREE_NODE) continue;
// Get the corresponding node
const TreeNode* node = mNodes + nodeID;
// Test if the node AABB overlaps with the ray AABB
if (!rayAABB.testCollision(node->aabb)) continue;
// If the node is a leaf of the tree
if (node->isLeaf()) {
Ray rayTemp(ray.point1, ray.point2, maxFraction);
// Ask the collision detection to perform a ray cast test against
// the proxy shape of this node because the ray is overlapping
// with the shape in the broad-phase
decimal hitFraction = raycastTest.raycastAgainstShape(node->proxyShape,
rayTemp);
// If the user returned a hitFraction of zero, it means that
// the raycasting should stop here
if (hitFraction == decimal(0.0)) {
return;
}
// If the user returned a positive fraction
if (hitFraction > decimal(0.0)) {
// We update the maxFraction value and the ray
// AABB using the new maximum fraction
if (hitFraction < maxFraction) {
maxFraction = hitFraction;
}
endPoint = ray.point1 + maxFraction * (ray.point2 - ray.point1);
rayAABB.mMinCoordinates = Vector3::min(ray.point1, endPoint);
rayAABB.mMaxCoordinates = Vector3::max(ray.point1, endPoint);
}
// If the user returned a negative fraction, we continue
// the raycasting as if the proxy shape did not exist
}
else { // If the node has children
// Push its children in the stack of nodes to explore
stack.push(node->leftChildID);
stack.push(node->rightChildID);
}
}
}
#ifndef NDEBUG
// Check if the tree structure is valid (for debugging purpose)
void DynamicAABBTree::check() const {
// Recursively check each node
checkNode(mRootNodeID);
int nbFreeNodes = 0;
int freeNodeID = mFreeNodeID;
// Check the free nodes
while(freeNodeID != TreeNode::NULL_TREE_NODE) {
assert(0 <= freeNodeID && freeNodeID < mNbAllocatedNodes);
freeNodeID = mNodes[freeNodeID].nextNodeID;
nbFreeNodes++;
}
assert(mNbNodes + nbFreeNodes == mNbAllocatedNodes);
}
// Check if the node structure is valid (for debugging purpose)
void DynamicAABBTree::checkNode(int nodeID) const {
if (nodeID == TreeNode::NULL_TREE_NODE) return;
// If it is the root
if (nodeID == mRootNodeID) {
assert(mNodes[nodeID].parentID == TreeNode::NULL_TREE_NODE);
}
// Get the children nodes
TreeNode* pNode = mNodes + nodeID;
int leftChild = pNode->leftChildID;
int rightChild = pNode->rightChildID;
assert(pNode->height >= 0);
assert(pNode->aabb.getVolume() > 0);
// If the current node is a leaf
if (pNode->isLeaf()) {
// Check that there are no children
assert(leftChild == TreeNode::NULL_TREE_NODE);
assert(rightChild == TreeNode::NULL_TREE_NODE);
assert(pNode->height == 0);
}
else {
// Check that the children node IDs are valid
assert(0 <= leftChild && leftChild < mNbAllocatedNodes);
assert(0 <= rightChild && rightChild < mNbAllocatedNodes);
// Check that the children nodes have the correct parent node
assert(mNodes[leftChild].parentID == nodeID);
assert(mNodes[rightChild].parentID == nodeID);
// Check the height of node
int height = 1 + std::max(mNodes[leftChild].height, mNodes[rightChild].height);
assert(mNodes[nodeID].height == height);
// Check the AABB of the node
AABB aabb;
aabb.mergeTwoAABBs(mNodes[leftChild].aabb, mNodes[rightChild].aabb);
assert(aabb.getMin() == mNodes[nodeID].aabb.getMin());
assert(aabb.getMax() == mNodes[nodeID].aabb.getMax());
// Recursively check the children nodes
checkNode(leftChild);
checkNode(rightChild);
}
}
#endif

194
src/collision/broadphase/DynamicAABBTree.h Normal file
View File

@ -0,0 +1,194 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2014 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_DYNAMIC_AABB_TREE_H
#define REACTPHYSICS3D_DYNAMIC_AABB_TREE_H
// Libraries
#include "configuration.h"
#include "collision/shapes/AABB.h"
#include "body/CollisionBody.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Declarations
class BroadPhaseAlgorithm;
struct RaycastTest;
// Raycast callback method pointer type
typedef decimal (*RaycastTestCallback) (ProxyShape* shape,
RaycastCallback* userCallback,
const Ray& ray);
// Structure TreeNode
/**
* This structure represents a node of the dynamic AABB tree.
*/
struct TreeNode {
// -------------------- Constants -------------------- //
/// Null tree node constant
const static int NULL_TREE_NODE;
// -------------------- Attributes -------------------- //
/// Parent node ID
int parentID;
/// Left and right child of the node
int leftChildID, rightChildID;
/// Next allocated node ID
int nextNodeID;
/// Height of the node in the tree
int height;
/// Fat axis aligned bounding box (AABB) corresponding to the node
AABB aabb;
/// Pointer to the corresponding collision shape (in case this node is a leaf)
ProxyShape* proxyShape;
// -------------------- Methods -------------------- //
/// Return true if the node is a leaf of the tree
bool isLeaf() const;
};
// Class DynamicAABBTree
/**
* This class implements a dynamic AABB tree that is used for broad-phase
* collision detection. This data structure is inspired by Nathanael Presson's
* dynamic tree implementation in BulletPhysics. The following implementation is
* based on the one from Erin Catto in Box2D as described in the book
* "Introduction to Game Physics with Box2D" by Ian Parberry.
*/
class DynamicAABBTree {
private:
// -------------------- Attributes -------------------- //
/// Reference to the broad-phase
BroadPhaseAlgorithm& mBroadPhase;
/// Pointer to the memory location of the nodes of the tree
TreeNode* mNodes;
/// ID of the root node of the tree
int mRootNodeID;
/// ID of the first node of the list of free (allocated) nodes in the tree that we can use
int mFreeNodeID;
/// Number of allocated nodes in the tree
int mNbAllocatedNodes;
/// Number of nodes in the tree
int mNbNodes;
// -------------------- Methods -------------------- //
/// Allocate and return a node to use in the tree
int allocateNode();
/// Release a node
void releaseNode(int nodeID);
/// Insert a leaf node in the tree
void insertLeafNode(int nodeID);
/// Remove a leaf node from the tree
void removeLeafNode(int nodeID);
/// Balance the sub-tree of a given node using left or right rotations.
int balanceSubTreeAtNode(int nodeID);
#ifndef NDEBUG
/// Check if the tree structure is valid (for debugging purpose)
void check() const;
/// Check if the node structure is valid (for debugging purpose)
void checkNode(int nodeID) const;
#endif
public:
// -------------------- Methods -------------------- //
/// Constructor
DynamicAABBTree(BroadPhaseAlgorithm& broadPhase);
/// Destructor
~DynamicAABBTree();
/// Add an object into the tree
void addObject(ProxyShape* proxyShape, const AABB& aabb);
/// Remove an object from the tree
void removeObject(int nodeID);
/// Update the dynamic tree after an object has moved.
bool updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement);
/// Return the fat AABB corresponding to a given node ID
const AABB& getFatAABB(int nodeID) const;
/// Return the collision shape of a given leaf node of the tree
ProxyShape* getCollisionShape(int nodeID) const;
/// Report all shapes overlapping with the AABB given in parameter.
void reportAllShapesOverlappingWith(int nodeID, const AABB& aabb);
/// Ray casting method
void raycast(const Ray& ray, RaycastTest& raycastTest) const;
};
// Return true if the node is a leaf of the tree
inline bool TreeNode::isLeaf() const {
return leftChildID == NULL_TREE_NODE;
}
// Return the fat AABB corresponding to a given node ID
inline const AABB& DynamicAABBTree::getFatAABB(int nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
return mNodes[nodeID].aabb;
}
// Return the collision shape of a given leaf node of the tree
inline ProxyShape* DynamicAABBTree::getCollisionShape(int nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
return mNodes[nodeID].proxyShape;
}
}
#endif

123
src/collision/broadphase/NoBroadPhaseAlgorithm.h
View File

@ -1,123 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_NO_BROAD_PHASE_ALGORITHM_H
#define REACTPHYSICS3D_NO_BROAD_PHASE_ALGORITHM_H
// Libraries
#include "BroadPhaseAlgorithm.h"
#include <algorithm>
#include <set>
#include <iostream>
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class NoBroadPhaseAlgorithm
/**
* 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
* the collision detection.
*/
class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm {
protected :
// -------------------- Attributes -------------------- //
/// All bodies of the world
std::set<CollisionBody*> mBodies;
// -------------------- Methods -------------------- //
/// Private copy-constructor
NoBroadPhaseAlgorithm(const NoBroadPhaseAlgorithm& algorithm);
/// Private assignment operator
NoBroadPhaseAlgorithm& operator=(const NoBroadPhaseAlgorithm& algorithm);
public :
// -------------------- Methods -------------------- //
/// Constructor
NoBroadPhaseAlgorithm(CollisionDetection& collisionDetection);
/// Destructor
virtual ~NoBroadPhaseAlgorithm();
/// Notify the broad-phase about a new object in the world
virtual void addObject(CollisionBody* body, const AABB& aabb);
/// Notify the broad-phase about an object that has been removed from the world
virtual void removeObject(CollisionBody* body);
/// Notify the broad-phase that the AABB of an object has changed
virtual void updateObject(CollisionBody* body, const AABB& aabb);
};
// Notify the broad-phase about a new object in the world
inline void NoBroadPhaseAlgorithm::addObject(CollisionBody* body, const AABB& aabb) {
// For each body that is already in the world
for (std::set<CollisionBody*>::iterator it = mBodies.begin(); it != mBodies.end(); ++it) {
// Add an overlapping pair with the new body
mPairManager.addPair(*it, body);
}
// Add the new body into the list of bodies
mBodies.insert(body);
}
// Notify the broad-phase about an object that has been removed from the world
inline void NoBroadPhaseAlgorithm::removeObject(CollisionBody* body) {
// For each body that is in the world
for (std::set<CollisionBody*>::iterator it = mBodies.begin(); it != mBodies.end(); ++it) {
if ((*it)->getID() != body->getID()) {
// Remove the overlapping pair with the new body
mPairManager.removePair((*it)->getID(), body->getID());
}
}
// Remove the body from the broad-phase
mBodies.erase(body);
}
// Notify the broad-phase that the AABB of an object has changed
inline void NoBroadPhaseAlgorithm::updateObject(CollisionBody* body, const AABB& aabb) {
// Do nothing
return;
}
}
#endif

289
src/collision/broadphase/PairManager.cpp
View File

@ -1,289 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "PairManager.h"
#include "../CollisionDetection.h"
#include <cassert>
#include <cstring>
using namespace reactphysics3d;
// Initialization of static variables
bodyindex PairManager::INVALID_INDEX = std::numeric_limits<reactphysics3d::bodyindex>::max();
// Constructor of PairManager
PairManager::PairManager(CollisionDetection& collisionDetection)
: mCollisionDetection(collisionDetection) {
mHashTable = NULL;
mOverlappingPairs = NULL;
mOffsetNextPair = NULL;
mNbOverlappingPairs = 0;
mHashMask = 0;
mNbElementsHashTable = 0;
}
// Destructor of PairManager
PairManager::~PairManager() {
// Release the allocated memory
free(mOffsetNextPair);
free(mOverlappingPairs);
free(mHashTable);
}
// Add a pair of bodies in the pair manager and returns a pointer to that pair.
/// If the pair to add does not already exist in the set of
/// overlapping pairs, it will be created and if it already exists, we only
/// return a pointer to that pair.
BodyPair* PairManager::addPair(CollisionBody* body1, CollisionBody* body2) {
// Sort the bodies to have the body with smallest ID first
sortBodiesUsingID(body1, body2);
// Get the bodies IDs
bodyindex id1 = body1->getID();
bodyindex id2 = body2->getID();
// Compute the hash value of the two bodies
uint hashValue = computeHashBodies(id1, id2) & mHashMask;
// Try to find the pair in the current overlapping pairs.
BodyPair* pair = findPairWithHashValue(id1, id2, hashValue);
// If the pair is already in the set of overlapping pairs
if (pair) {
// We only return a pointer to that pair
return pair;
}
// If we need to allocate more pairs in the set of overlapping pairs
if (mNbOverlappingPairs >= mNbElementsHashTable) {
// Increase the size of the hash table (always a power of two)
mNbElementsHashTable = computeNextPowerOfTwo(mNbOverlappingPairs + 1);
// Compute the new hash mask with the new hash size
mHashMask = mNbElementsHashTable - 1;
// Reallocate more pairs
reallocatePairs();
// Compute the new hash value (using the new hash size and hash mask)
hashValue = computeHashBodies(id1, id2) & mHashMask;
}
// Create the new overlapping pair
BodyPair* newPair = &mOverlappingPairs[mNbOverlappingPairs];
newPair->body1 = body1;
newPair->body2 = body2;
// Put the new pair as the initial pair with this hash value
mOffsetNextPair[mNbOverlappingPairs] = mHashTable[hashValue];
mHashTable[hashValue] = mNbOverlappingPairs++;
// Notify the collision detection about this new overlapping pair
mCollisionDetection.broadPhaseNotifyAddedOverlappingPair(newPair);
// Return a pointer to the new created pair
return newPair;
}
// Remove a pair of bodies from the pair manager.
/// This method returns true if the pair has been found and removed.
bool PairManager::removePair(bodyindex id1, bodyindex id2) {
// Sort the bodies IDs
sortIDs(id1, id2);
// Compute the hash value of the pair to remove
const uint hashValue = computeHashBodies(id1, id2) & mHashMask;
// Find the pair to remove
BodyPair* pair = findPairWithHashValue(id1, id2, hashValue);
// If we have not found the pair
if (pair == NULL) {
return false;
}
assert(pair->body1->getID() == id1);
assert(pair->body2->getID() == id2);
// Notify the collision detection about this removed overlapping pair
mCollisionDetection.broadPhaseNotifyRemovedOverlappingPair(pair);
// Remove the pair from the set of overlapping pairs
removePairWithHashValue(id1, id2, hashValue, computePairOffset(pair));
// Try to shrink the memory used by the pair manager
shrinkMemory();
return true;
}
// Internal method to remove a pair from the set of overlapping pair
void PairManager::removePairWithHashValue(bodyindex id1, bodyindex id2, luint hashValue,
bodyindex indexPair) {
// Get the initial offset of the pairs with
// the corresponding hash value
bodyindex offset = mHashTable[hashValue];
assert(offset != INVALID_INDEX);
// Look for the pair in the set of overlapping pairs
bodyindex previousPair = INVALID_INDEX;
while(offset != indexPair) {
previousPair = offset;
offset = mOffsetNextPair[offset];
}
// If the pair was the first one with this hash
// value in the hash table
if (previousPair == INVALID_INDEX) {
// Replace the pair to remove in the
// hash table by the next one
mHashTable[hashValue] = mOffsetNextPair[indexPair];
}
else { // If the pair was not the first one
// Replace the pair to remove in the
// hash table by the next one
assert(mOffsetNextPair[previousPair] == indexPair);
mOffsetNextPair[previousPair] = mOffsetNextPair[indexPair];
}
const bodyindex indexLastPair = mNbOverlappingPairs - 1;
// If the pair to remove is the last one in the list
if (indexPair == indexLastPair) {
// We simply decrease the number of overlapping pairs
mNbOverlappingPairs--;
}
else { // If the pair to remove is in the middle of the list
// Now, we want to move the last pair into the location that is
// now free because of the pair we want to remove
// Get the last pair
const BodyPair* lastPair = &mOverlappingPairs[indexLastPair];
const uint lastPairHashValue = computeHashBodies(lastPair->body1->getID(),
lastPair->body2->getID()) & mHashMask;
// Compute the initial offset of the last pair
bodyindex offset = mHashTable[lastPairHashValue];
assert(offset != INVALID_INDEX);
// Go through the pairs with the same hash value
// and find the offset of the last pair
bodyindex previous = INVALID_INDEX;
while(offset != indexLastPair) {
previous = offset;
offset = mOffsetNextPair[offset];
}
// If the last pair is not the first one with this hash value
if (previous != INVALID_INDEX) {
// Remove the offset of the last pair in the "nextOffset" array
assert(mOffsetNextPair[previous] == indexLastPair);
mOffsetNextPair[previous] = mOffsetNextPair[indexLastPair];
}
else { // If the last pair is the first offset with this hash value
// Remove the offset of the last pair in the "nextOffset" array
mHashTable[lastPairHashValue] = mOffsetNextPair[indexLastPair];
}
// Replace the pair to remove by the last pair in
// the overlapping pairs array
mOverlappingPairs[indexPair] = mOverlappingPairs[indexLastPair];
mOffsetNextPair[indexPair] = mHashTable[lastPairHashValue];
mHashTable[lastPairHashValue] = indexPair;
mNbOverlappingPairs--;
}
}
// Look for a pair in the set of overlapping pairs
BodyPair* PairManager::lookForAPair(bodyindex id1, bodyindex id2, luint hashValue) const {
// Look for the pair in the set of overlapping pairs
bodyindex offset = mHashTable[hashValue];
while (offset != INVALID_INDEX && isDifferentPair(mOverlappingPairs[offset], id1, id2)) {
offset = mOffsetNextPair[offset];
}
// If the pair has not been found in the overlapping pairs
if (offset == INVALID_INDEX) {
return NULL;
}
assert(offset < mNbOverlappingPairs);
// The pair has been found in the set of overlapping pairs, then
// we return a pointer to it
return &mOverlappingPairs[offset];
}
// Reallocate more pairs
void PairManager::reallocatePairs() {
// Reallocate the hash table and initialize it
free(mHashTable);
mHashTable = (bodyindex*) malloc(mNbElementsHashTable * sizeof(bodyindex));
assert(mHashTable != NULL);
for (bodyindex i=0; i<mNbElementsHashTable; i++) {
mHashTable[i] = INVALID_INDEX;
}
// Reallocate the overlapping pairs
BodyPair* newOverlappingPairs = (BodyPair*) malloc(mNbElementsHashTable * sizeof(BodyPair));
bodyindex* newOffsetNextPair = (bodyindex*) malloc(mNbElementsHashTable * sizeof(bodyindex));
assert(newOverlappingPairs != NULL);
assert(newOffsetNextPair != NULL);
// If there is already some overlapping pairs
if (mNbOverlappingPairs) {
// Copy the pairs to the new location
memcpy(newOverlappingPairs, mOverlappingPairs, mNbOverlappingPairs * sizeof(BodyPair));
}
// Recompute the hash table with the new hash values
for (bodyindex i=0; i<mNbOverlappingPairs; i++) {
const uint newHashValue = computeHashBodies(mOverlappingPairs[i].body1->getID(),
mOverlappingPairs[i].body2->getID()) & mHashMask;
newOffsetNextPair[i] = mHashTable[newHashValue];
mHashTable[newHashValue] = i;
}
// Delete the old pairs
free(mOffsetNextPair);
free(mOverlappingPairs);
// Replace by the new data
mOverlappingPairs = newOverlappingPairs;
mOffsetNextPair = newOffsetNextPair;
}

331
src/collision/broadphase/PairManager.h
View File

@ -1,331 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_PAIR_MANAGER_H
#define REACTPHYSICS3D_PAIR_MANAGER_H
// Libraries
#include "../../body/CollisionBody.h"
#include <utility>
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Declaration
class CollisionDetection;
// Structure BodyPair
/**
* This structure represents a pair of bodies
* during the broad-phase collision detection.
*/
struct BodyPair {
public:
/// Pointer to the first body
CollisionBody* body1;
/// Pointer to the second body
CollisionBody* body2;
/// Return the pair of bodies index
bodyindexpair getBodiesIndexPair() const {
// Construct the pair of body index
bodyindexpair indexPair = body1->getID() < body2->getID() ?
std::make_pair(body1->getID(), body2->getID()) :
std::make_pair(body2->getID(), body1->getID());
assert(indexPair.first != indexPair.second);
return indexPair;
}
};
// Class PairManager
/**
* This class is a data-structure contains the pairs of bodies that
* are overlapping during the broad-phase collision detection.
* This class implements the pair manager described by Pierre Terdiman
* in www.codercorner.com/SAP.pdf.
*/
class PairManager {
private :
// -------------------- Attributes -------------------- //
/// Number of elements in the hash table
bodyindex mNbElementsHashTable;
/// Hash mask for the hash function
uint mHashMask;
/// Number of overlapping pairs
bodyindex mNbOverlappingPairs;
/// Hash table that contains the offset of the first pair of the list of
/// pairs with the same hash value in the "overlappingPairs" array
bodyindex* mHashTable;
/// Array that contains for each offset, the offset of the next pair with
/// the same hash value for a given same hash value
bodyindex* mOffsetNextPair;
/// Array that contains the overlapping pairs
BodyPair* mOverlappingPairs;
/// Invalid ID
static bodyindex INVALID_INDEX;
/// Reference to the collision detection
CollisionDetection& mCollisionDetection;
// -------------------- Methods -------------------- //
/// Private copy-constructor
PairManager(const PairManager& pairManager);
/// Private assignment operator
PairManager& operator=(const PairManager& pairManager);
/// Sort the bodies according to their IDs (smallest ID first)
void sortBodiesUsingID(CollisionBody*& body1, CollisionBody*& body2) const;
/// Sort the IDs (smallest ID first)
void sortIDs(bodyindex& id1, bodyindex& id2) const;
/// Return true if pair1 and pair2 are the same
bool isDifferentPair(const BodyPair& pair1, bodyindex pair2ID1, bodyindex pair2ID2) const;
/// Compute the hash value of two bodies using their IDs
uint computeHashBodies(uint id1, uint id2) const;
/// This method returns an hash value for a 32 bits key.
int computeHash32Bits(int key) const;
/// Reallocate memory for more pairs
void reallocatePairs();
/// Shrink the allocated memory
void shrinkMemory();
/// Compute the offset of a given pair
bodyindex computePairOffset(const BodyPair* pair) const;
/// Look for a pair in the set of overlapping pairs
BodyPair* lookForAPair(bodyindex id1, bodyindex id2, luint hashValue) const;
/// Find a pair given two body IDs and an hash value.
BodyPair* findPairWithHashValue(bodyindex id1, bodyindex id2, luint hashValue) const;
/// Remove a pair from the set of active pair
void removePairWithHashValue(bodyindex id1, bodyindex id2, luint hashValue,
bodyindex indexPair);
public :
// ----- Methods ----- //
/// Constructor
PairManager(CollisionDetection& collisionDetection);
/// Destructor
~PairManager();
/// Return the number of active pairs
bodyindex getNbOverlappingPairs() const;
/// Add a pair of bodies in the pair manager and returns a pointer to that pair.
BodyPair* addPair(CollisionBody* body1, CollisionBody* body2);
/// Remove a pair of bodies from the pair manager.
bool removePair(bodyindex id1, bodyindex id2);
/// Find a pair given two body IDs
BodyPair* findPair(bodyindex id1, bodyindex id2) const;
/// Return the next power of two
static luint computeNextPowerOfTwo(luint number);
/// Return a pointer to the first overlapping pair (used to
/// iterate over the active pairs).
BodyPair* beginOverlappingPairsPointer() const;
/// Return a pointer to the last overlapping pair (used to
/// iterate over the active pairs).
BodyPair* endOverlappingPairsPointer() const;
/// Register a callback function (using a function pointer) that will be
/// called when a new overlapping pair is added in the pair manager.
void registerAddedOverlappingPairCallback(void (CollisionDetection::*callbackFunction)
(const BodyPair* addedActivePair));
/// Unregister the callback function that will be called
/// when a new active pair is added in the pair manager
void unregisterAddedOverlappingPairCallback();
/// Register a callback function (using a function pointer)
/// that will be called when an overlapping pair is removed from the pair manager
void registerRemovedOverlappingPairCallback(void (CollisionDetection::*callbackFunction)
(const BodyPair* removedActivePair));
/// Unregister a callback function that will be called
/// when a active pair is removed from the pair manager
void unregisterRemovedOverlappingPairCallback();
};
// Return the number of overlapping pairs
inline bodyindex PairManager::getNbOverlappingPairs() const {
return mNbOverlappingPairs;
}
// Compute the hash value of two bodies
inline uint PairManager::computeHashBodies(uint id1, uint id2) const {
return computeHash32Bits(id1 | (id2 << 16));
}
// Return true if pair1 and pair2 are the same
inline bool PairManager::isDifferentPair(const BodyPair& pair1, bodyindex pair2ID1,
bodyindex pair2ID2) const {
return (pair2ID1 != pair1.body1->getID() || pair2ID2 != pair1.body2->getID());
}
// Return the next power of two of a 32bits integer using a SWAR algorithm
inline luint PairManager::computeNextPowerOfTwo(luint number) {
number |= (number >> 1);
number |= (number >> 2);
number |= (number >> 4);
number |= (number >> 8);
number |= (number >> 16);
return number+1;
}
// Sort the bodies according to their IDs (smallest ID first)
inline void PairManager::sortBodiesUsingID(CollisionBody*& body1, CollisionBody*& body2) const {
// If the ID of body1 is larger than the ID of body 2
if (body1->getID() > body2->getID()) {
// Swap the two bodies pointers
CollisionBody* temp = body2;
body2 = body1;
body1 = temp;
}
}
// Sort the IDs (smallest ID first)
inline void PairManager::sortIDs(bodyindex &id1, bodyindex &id2) const {
if (id1 > id2) {
bodyindex temp = id2;
id2 = id1;
id1 = temp;
}
}
// This method returns an hash value for a 32 bits key.
/// using Thomas Wang's hash technique.
/// This hash function can be found at :
/// http://www.concentric.net/~ttwang/tech/inthash.htm
inline int PairManager::computeHash32Bits(int key) const {
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
// Find a pair given two body IDs
inline BodyPair* PairManager::findPair(bodyindex id1, bodyindex id2) const {
// Check if the hash table has been allocated yet
if (mHashTable == NULL) return NULL;
// Sort the IDs
sortIDs(id1, id2);
// Compute the hash value of the pair to find
uint hashValue = computeHashBodies(id1, id2) & mHashMask;
// Look for the pair in the set of overlapping pairs
return lookForAPair(id1, id2, hashValue);
}
// Find a pair given two body IDs and an hash value.
/// This internal version is used to avoid computing multiple times in the
/// caller method
inline BodyPair* PairManager::findPairWithHashValue(bodyindex id1, bodyindex id2,
luint hashValue) const {
// Check if the hash table has been allocated yet
if (mHashTable == NULL) return NULL;
// Look for the pair in the set of overlapping pairs
return lookForAPair(id1, id2, hashValue);
}
// Try to reduce the allocated memory by the pair manager
inline void PairManager::shrinkMemory() {
// Check if the allocated memory can be reduced
const bodyindex correctNbElementsHashTable = computeNextPowerOfTwo(mNbOverlappingPairs);
if (mNbElementsHashTable == correctNbElementsHashTable) return;
// Reduce the allocated memory
mNbElementsHashTable = correctNbElementsHashTable;
mHashMask = mNbElementsHashTable - 1;
reallocatePairs();
}
// Compute the offset of a given pair in the array of overlapping pairs
inline bodyindex PairManager::computePairOffset(const BodyPair* pair) const {
return ((bodyindex)((size_t(pair) - size_t(mOverlappingPairs))) / sizeof(BodyPair));
}
// Return a pointer to the first overlapping pair (used to iterate over the overlapping pairs) or
// returns 0 if there is no overlapping pairs.
inline BodyPair* PairManager::beginOverlappingPairsPointer() const {
return &mOverlappingPairs[0];
}
// Return a pointer to the last overlapping pair (used to iterate over the overlapping pairs) or
// returns 0 if there is no overlapping pairs.
inline BodyPair* PairManager::endOverlappingPairsPointer() const {
if (mNbOverlappingPairs > 0) {
return &mOverlappingPairs[mNbOverlappingPairs-1];
}
else {
return &mOverlappingPairs[0];
}
}
}
#endif

596
src/collision/broadphase/SweepAndPruneAlgorithm.cpp
View File

@ -1,596 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "SweepAndPruneAlgorithm.h"
#include "../CollisionDetection.h"
#include "PairManager.h"
#include <climits>
#include <cstring>
// Namespaces
using namespace reactphysics3d;
using namespace std;
// Initialization of static variables
const bodyindex SweepAndPruneAlgorithm::INVALID_INDEX = std::numeric_limits<bodyindex>::max();
const luint SweepAndPruneAlgorithm::NB_SENTINELS = 2;
// Constructor that takes an AABB as input
AABBInt::AABBInt(const AABB& aabb) {
min[0] = encodeFloatIntoInteger(aabb.getMin().x);
min[1] = encodeFloatIntoInteger(aabb.getMin().y);
min[2] = encodeFloatIntoInteger(aabb.getMin().z);
max[0] = encodeFloatIntoInteger(aabb.getMax().x);
max[1] = encodeFloatIntoInteger(aabb.getMax().y);
max[2] = encodeFloatIntoInteger(aabb.getMax().z);
}
// Constructor that set all the axis with an minimum and maximum value
AABBInt::AABBInt(uint minValue, uint maxValue) {
min[0] = minValue;
min[1] = minValue;
min[2] = minValue;
max[0] = maxValue;
max[1] = maxValue;
max[2] = maxValue;
}
// Constructor
SweepAndPruneAlgorithm::SweepAndPruneAlgorithm(CollisionDetection& collisionDetection)
:BroadPhaseAlgorithm(collisionDetection) {
mBoxes = NULL;
mEndPoints[0] = NULL;
mEndPoints[1] = NULL;
mEndPoints[2] = NULL;
mNbBoxes = 0;
mNbMaxBoxes = 0;
}
// Destructor
SweepAndPruneAlgorithm::~SweepAndPruneAlgorithm() {
delete[] mBoxes;
delete[] mEndPoints[0];
delete[] mEndPoints[1];
delete[] mEndPoints[2];
}
// Notify the broad-phase about a new object in the world
/// This method adds the AABB of the object ion to broad-phase
void SweepAndPruneAlgorithm::addObject(CollisionBody* body, const AABB& aabb) {
bodyindex boxIndex;
assert(encodeFloatIntoInteger(aabb.getMin().x) > encodeFloatIntoInteger(-FLT_MAX));
assert(encodeFloatIntoInteger(aabb.getMin().y) > encodeFloatIntoInteger(-FLT_MAX));
assert(encodeFloatIntoInteger(aabb.getMin().z) > encodeFloatIntoInteger(-FLT_MAX));
assert(encodeFloatIntoInteger(aabb.getMax().x) < encodeFloatIntoInteger(FLT_MAX));
assert(encodeFloatIntoInteger(aabb.getMax().y) < encodeFloatIntoInteger(FLT_MAX));
assert(encodeFloatIntoInteger(aabb.getMax().z) < encodeFloatIntoInteger(FLT_MAX));
// If the index of the first free box is valid (means that
// there is a bucket in the middle of the array that doesn't
// contain a box anymore because it has been removed)
if (!mFreeBoxIndices.empty()) {
boxIndex = mFreeBoxIndices.back();
mFreeBoxIndices.pop_back();
}
else {
// If the array boxes and end-points arrays are full
if (mNbBoxes == mNbMaxBoxes) {
// Resize the arrays to make them larger
resizeArrays();
}
boxIndex = mNbBoxes;
}
// Move the maximum limit end-point two elements further
// at the end-points array in all three axis
const luint indexLimitEndPoint = 2 * mNbBoxes + NB_SENTINELS - 1;
for (uint axis=0; axis<3; axis++) {
EndPoint* maxLimitEndPoint = &mEndPoints[axis][indexLimitEndPoint];
assert(mEndPoints[axis][0].boxID == INVALID_INDEX && mEndPoints[axis][0].isMin);
assert(maxLimitEndPoint->boxID == INVALID_INDEX && !maxLimitEndPoint->isMin);
EndPoint* newMaxLimitEndPoint = &mEndPoints[axis][indexLimitEndPoint + 2];
newMaxLimitEndPoint->setValues(maxLimitEndPoint->boxID, maxLimitEndPoint->isMin,
maxLimitEndPoint->value);
}
// Create a new box
BoxAABB* box = &mBoxes[boxIndex];
box->body = body;
const uint maxEndPointValue = encodeFloatIntoInteger(FLT_MAX) - 1;
const uint minEndPointValue = encodeFloatIntoInteger(FLT_MAX) - 2;
for (uint axis=0; axis<3; axis++) {
box->min[axis] = indexLimitEndPoint;
box->max[axis] = indexLimitEndPoint + 1;
EndPoint* minimumEndPoint = &mEndPoints[axis][box->min[axis]];
minimumEndPoint->setValues(boxIndex, true, minEndPointValue);
EndPoint* maximumEndPoint = &mEndPoints[axis][box->max[axis]];
maximumEndPoint->setValues(boxIndex, false, maxEndPointValue);
}
// Add the body pointer to box index mapping
mMapBodyToBoxIndex.insert(pair<CollisionBody*, bodyindex>(body, boxIndex));
mNbBoxes++;
// Call the update method to put the end-points of the new AABB at the
// correct position in the array. This will also create the overlapping
// pairs in the pair manager if the new AABB is overlapping with others
// AABBs
updateObject(body, aabb);
}
// Notify the broad-phase about an object that has been removed from the world
void SweepAndPruneAlgorithm::removeObject(CollisionBody* body) {
assert(mNbBoxes > 0);
// Call the update method with an AABB that is very far away
// in order to remove all overlapping pairs from the pair manager
const uint maxEndPointValue = encodeFloatIntoInteger(FLT_MAX) - 1;
const uint minEndPointValue = encodeFloatIntoInteger(FLT_MAX) - 2;
const AABBInt aabbInt(minEndPointValue, maxEndPointValue);
updateObjectIntegerAABB(body, aabbInt);
// Get the corresponding box
bodyindex boxIndex = mMapBodyToBoxIndex.find(body)->second;
// Remove the end-points of the box by moving the maximum end-points two elements back in
// the end-points array
const luint indexLimitEndPoint = 2 * mNbBoxes + NB_SENTINELS - 1;
for (uint axis=0; axis < 3; axis++) {
EndPoint* maxLimitEndPoint = &mEndPoints[axis][indexLimitEndPoint];
assert(mEndPoints[axis][0].boxID == INVALID_INDEX && mEndPoints[axis][0].isMin);
assert(maxLimitEndPoint->boxID == INVALID_INDEX && !maxLimitEndPoint->isMin);
EndPoint* newMaxLimitEndPoint = &mEndPoints[axis][indexLimitEndPoint - 2];
assert(mEndPoints[axis][indexLimitEndPoint - 1].boxID == boxIndex);
assert(!mEndPoints[axis][indexLimitEndPoint - 1].isMin);
assert(newMaxLimitEndPoint->boxID == boxIndex);
assert(newMaxLimitEndPoint->isMin);
newMaxLimitEndPoint->setValues(maxLimitEndPoint->boxID, maxLimitEndPoint->isMin,
maxLimitEndPoint->value);
}
// Add the box index into the list of free indices
mFreeBoxIndices.push_back(boxIndex);
mMapBodyToBoxIndex.erase(body);
mNbBoxes--;
// Check if we need to shrink the allocated memory
const luint nextPowerOf2 = PairManager::computeNextPowerOfTwo((mNbBoxes-1) / 100 );
if (nextPowerOf2 * 100 < mNbMaxBoxes) {
shrinkArrays();
}
}
// Notify the broad-phase that the AABB of an object has changed.
/// The input is an AABB with integer coordinates
void SweepAndPruneAlgorithm::updateObjectIntegerAABB(CollisionBody* body, const AABBInt& aabbInt) {
assert(aabbInt.min[0] > encodeFloatIntoInteger(-FLT_MAX));
assert(aabbInt.min[1] > encodeFloatIntoInteger(-FLT_MAX));
assert(aabbInt.min[2] > encodeFloatIntoInteger(-FLT_MAX));
assert(aabbInt.max[0] < encodeFloatIntoInteger(FLT_MAX));
assert(aabbInt.max[1] < encodeFloatIntoInteger(FLT_MAX));
assert(aabbInt.max[2] < encodeFloatIntoInteger(FLT_MAX));
// Get the corresponding box
bodyindex boxIndex = mMapBodyToBoxIndex.find(body)->second;
BoxAABB* box = &mBoxes[boxIndex];
// Current axis
for (uint axis=0; axis<3; axis++) {
// Get the two others axis
const uint otherAxis1 = (1 << axis) & 3;
const uint otherAxis2 = (1 << otherAxis1) & 3;
// Get the starting end-point of the current axis
EndPoint* startEndPointsCurrentAxis = mEndPoints[axis];
// -------- Update the minimum end-point ------------//
EndPoint* currentMinEndPoint = &startEndPointsCurrentAxis[box->min[axis]];
assert(currentMinEndPoint->isMin);
// Get the minimum value of the AABB on the current axis
uint limit = aabbInt.min[axis];
// If the minimum value of the AABB is smaller
// than the current minimum endpoint
if (limit < currentMinEndPoint->value) {
currentMinEndPoint->value = limit;
// The minimum end-point is moving left
EndPoint savedEndPoint = *currentMinEndPoint;
luint indexEndPoint = (size_t(currentMinEndPoint) -
size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
const luint savedEndPointIndex = indexEndPoint;
while ((--currentMinEndPoint)->value > limit) {
BoxAABB* id1 = &mBoxes[currentMinEndPoint->boxID];
const bool isMin = currentMinEndPoint->isMin;
// If it's a maximum end-point
if (!isMin) {
// The minimum end-point is moving to the left and
// passed a maximum end-point. Thus, the boxes start
// overlapping on the current axis. Therefore we test
// for box intersection
if (box != id1) {
if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2) &&
testIntersect1DSortedAABBs(*id1, aabbInt,
startEndPointsCurrentAxis, axis)) {
// Add an overlapping pair to the pair manager
mPairManager.addPair(body, id1->body);
}
}
id1->max[axis] = indexEndPoint--;
}
else {
id1->min[axis] = indexEndPoint--;
}
*(currentMinEndPoint+1) = *currentMinEndPoint;
}
// Update the current minimum endpoint that we are moving
if (savedEndPointIndex != indexEndPoint) {
if (savedEndPoint.isMin) {
mBoxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
}
else {
mBoxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
}
startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
}
}
else if (limit > currentMinEndPoint->value){// The minimum of the box has moved to the right
currentMinEndPoint->value = limit;
// The minimum en-point is moving right
EndPoint savedEndPoint = *currentMinEndPoint;
luint indexEndPoint = (size_t(currentMinEndPoint) -
size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
const luint savedEndPointIndex = indexEndPoint;
// For each end-point between the current position of the minimum
// end-point and the new position of the minimum end-point
while ((++currentMinEndPoint)->value < limit) {
BoxAABB* id1 = &mBoxes[currentMinEndPoint->boxID];
const bool isMin = currentMinEndPoint->isMin;
// If it's a maximum end-point
if (!isMin) {
// The minimum end-point is moving to the right and
// passed a maximum end-point. Thus, the boxes stop
// overlapping on the current axis.
if (box != id1) {
if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2)) {
// Remove the pair from the pair manager
mPairManager.removePair(body->getID(), id1->body->getID());
}
}
id1->max[axis] = indexEndPoint++;
}
else {
id1->min[axis] = indexEndPoint++;
}
*(currentMinEndPoint-1) = *currentMinEndPoint;
}
// Update the current minimum endpoint that we are moving
if (savedEndPointIndex != indexEndPoint) {
if (savedEndPoint.isMin) {
mBoxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
}
else {
mBoxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
}
startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
}
}
// ------- Update the maximum end-point ------------ //
EndPoint* currentMaxEndPoint = &startEndPointsCurrentAxis[box->max[axis]];
assert(!currentMaxEndPoint->isMin);
// Get the maximum value of the AABB on the current axis
limit = aabbInt.max[axis];
// If the new maximum value of the AABB is larger
// than the current maximum end-point value. It means
// that the AABB is moving to the right.
if (limit > currentMaxEndPoint->value) {
currentMaxEndPoint->value = limit;
EndPoint savedEndPoint = *currentMaxEndPoint;
luint indexEndPoint = (size_t(currentMaxEndPoint) -
size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
const luint savedEndPointIndex = indexEndPoint;
while ((++currentMaxEndPoint)->value < limit) {
// Get the next end-point
BoxAABB* id1 = &mBoxes[currentMaxEndPoint->boxID];
const bool isMin = currentMaxEndPoint->isMin;
// If it's a maximum end-point
if (isMin) {
// The maximum end-point is moving to the right and
// passed a minimum end-point. Thus, the boxes start
// overlapping on the current axis. Therefore we test
// for box intersection
if (box != id1) {
if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2) &&
testIntersect1DSortedAABBs(*id1, aabbInt,
startEndPointsCurrentAxis,axis)) {
// Add an overlapping pair to the pair manager
mPairManager.addPair(body, id1->body);
}
}
id1->min[axis] = indexEndPoint++;
}
else {
id1->max[axis] = indexEndPoint++;
}
*(currentMaxEndPoint-1) = *currentMaxEndPoint;
}
// Update the current minimum endpoint that we are moving
if (savedEndPointIndex != indexEndPoint) {
if (savedEndPoint.isMin) {
mBoxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
}
else {
mBoxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
}
startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
}
}
else if (limit < currentMaxEndPoint->value) { // If the AABB is moving to the left
currentMaxEndPoint->value = limit;
EndPoint savedEndPoint = *currentMaxEndPoint;
luint indexEndPoint = (size_t(currentMaxEndPoint) -
size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
const luint savedEndPointIndex = indexEndPoint;
// For each end-point between the current position of the maximum
// end-point and the new position of the maximum end-point
while ((--currentMaxEndPoint)->value > limit) {
BoxAABB* id1 = &mBoxes[currentMaxEndPoint->boxID];
const bool isMin = currentMaxEndPoint->isMin;
// If it's a minimum end-point
if (isMin) {
// The maximum end-point is moving to the right and
// passed a minimum end-point. Thus, the boxes stop
// overlapping on the current axis.
if (box != id1) {
if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2)) {
// Remove the pair from the pair manager
mPairManager.removePair(body->getID(), id1->body->getID());
}
}
id1->min[axis] = indexEndPoint--;
}
else {
id1->max[axis] = indexEndPoint--;
}
*(currentMaxEndPoint+1) = *currentMaxEndPoint;
}
// Update the current minimum endpoint that we are moving
if (savedEndPointIndex != indexEndPoint) {
if (savedEndPoint.isMin) {
mBoxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
}
else {
mBoxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
}
startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
}
}
}
}
// Resize the boxes and end-points arrays when it is full
void SweepAndPruneAlgorithm::resizeArrays() {
// New number of boxes in the array
const luint newNbMaxBoxes = mNbMaxBoxes ? 2 * mNbMaxBoxes : 100;
const luint nbEndPoints = mNbBoxes * 2 + NB_SENTINELS;
const luint newNbEndPoints = newNbMaxBoxes * 2 + NB_SENTINELS;
// Allocate memory for the new boxes and end-points arrays
BoxAABB* newBoxesArray = new BoxAABB[newNbMaxBoxes];
EndPoint* newEndPointsXArray = new EndPoint[newNbEndPoints];
EndPoint* newEndPointsYArray = new EndPoint[newNbEndPoints];
EndPoint* newEndPointsZArray = new EndPoint[newNbEndPoints];
assert(newBoxesArray != NULL);
assert(newEndPointsXArray != NULL);
assert(newEndPointsYArray != NULL);
assert(newEndPointsZArray != NULL);
// If the arrays were not empty before
if (mNbBoxes > 0) {
// Copy the data from the old arrays into the new one
memcpy(newBoxesArray, mBoxes, sizeof(BoxAABB) * mNbBoxes);
const size_t nbBytesNewEndPoints = sizeof(EndPoint) * nbEndPoints;
memcpy(newEndPointsXArray, mEndPoints[0], nbBytesNewEndPoints);
memcpy(newEndPointsYArray, mEndPoints[1], nbBytesNewEndPoints);
memcpy(newEndPointsZArray, mEndPoints[2], nbBytesNewEndPoints);
}
else { // If the arrays were empty
// Add the limits endpoints (sentinels) into the array
const uint min = encodeFloatIntoInteger(-FLT_MAX);
const uint max = encodeFloatIntoInteger(FLT_MAX);
newEndPointsXArray[0].setValues(INVALID_INDEX, true, min);
newEndPointsXArray[1].setValues(INVALID_INDEX, false, max);
newEndPointsYArray[0].setValues(INVALID_INDEX, true, min);
newEndPointsYArray[1].setValues(INVALID_INDEX, false, max);
newEndPointsZArray[0].setValues(INVALID_INDEX, true, min);
newEndPointsZArray[1].setValues(INVALID_INDEX, false, max);
}
// Delete the old arrays
delete[] mBoxes;
delete[] mEndPoints[0];
delete[] mEndPoints[1];
delete[] mEndPoints[2];
// Assign the pointer to the new arrays
mBoxes = newBoxesArray;
mEndPoints[0] = newEndPointsXArray;
mEndPoints[1] = newEndPointsYArray;
mEndPoints[2] = newEndPointsZArray;
mNbMaxBoxes = newNbMaxBoxes;
}
// Shrink the boxes and end-points arrays when too much memory is allocated
void SweepAndPruneAlgorithm::shrinkArrays() {
// New number of boxes and end-points in the array
const luint nextPowerOf2 = PairManager::computeNextPowerOfTwo((mNbBoxes-1) / 100 );
const luint newNbMaxBoxes = (mNbBoxes > 100) ? nextPowerOf2 * 100 : 100;
const luint nbEndPoints = mNbBoxes * 2 + NB_SENTINELS;
const luint newNbEndPoints = newNbMaxBoxes * 2 + NB_SENTINELS;
assert(newNbMaxBoxes < mNbMaxBoxes);
// Sort the list of the free boxes indices in ascending order
mFreeBoxIndices.sort();
// Reorganize the boxes inside the boxes array so that all the boxes are at the
// beginning of the array
std::map<CollisionBody*, bodyindex> newMapBodyToBoxIndex;
std::map<CollisionBody*,bodyindex>::const_iterator it;
for (it = mMapBodyToBoxIndex.begin(); it != mMapBodyToBoxIndex.end(); ++it) {
CollisionBody* body = it->first;
bodyindex boxIndex = it->second;
// If the box index is outside the range of the current number of boxes
if (boxIndex >= mNbBoxes) {
assert(!mFreeBoxIndices.empty());
// Get a new box index for that body (from the list of free box indices)
bodyindex newBoxIndex = mFreeBoxIndices.front();
mFreeBoxIndices.pop_front();
assert(newBoxIndex < mNbBoxes);
// Copy the box to its new location in the boxes array
BoxAABB* oldBox = &mBoxes[boxIndex];
BoxAABB* newBox = &mBoxes[newBoxIndex];
assert(oldBox->body->getID() == body->getID());
newBox->body = oldBox->body;
for (uint axis=0; axis<3; axis++) {
// Copy the minimum and maximum end-points indices
newBox->min[axis] = oldBox->min[axis];
newBox->max[axis] = oldBox->max[axis];
// Update the box index of the end-points
EndPoint* minimumEndPoint = &mEndPoints[axis][newBox->min[axis]];
EndPoint* maximumEndPoint = &mEndPoints[axis][newBox->max[axis]];
assert(minimumEndPoint->boxID == boxIndex);
assert(maximumEndPoint->boxID == boxIndex);
minimumEndPoint->boxID = newBoxIndex;
maximumEndPoint->boxID = newBoxIndex;
}
newMapBodyToBoxIndex.insert(pair<CollisionBody*, bodyindex>(body, newBoxIndex));
}
else {
newMapBodyToBoxIndex.insert(pair<CollisionBody*, bodyindex>(body, boxIndex));
}
}
assert(newMapBodyToBoxIndex.size() == mMapBodyToBoxIndex.size());
mMapBodyToBoxIndex = newMapBodyToBoxIndex;
// Allocate memory for the new boxes and end-points arrays
BoxAABB* newBoxesArray = new BoxAABB[newNbMaxBoxes];
EndPoint* newEndPointsXArray = new EndPoint[newNbEndPoints];
EndPoint* newEndPointsYArray = new EndPoint[newNbEndPoints];
EndPoint* newEndPointsZArray = new EndPoint[newNbEndPoints];
assert(newBoxesArray != NULL);
assert(newEndPointsXArray != NULL);
assert(newEndPointsYArray != NULL);
assert(newEndPointsZArray != NULL);
// Copy the data from the old arrays into the new one
memcpy(newBoxesArray, mBoxes, sizeof(BoxAABB) * mNbBoxes);
const size_t nbBytesNewEndPoints = sizeof(EndPoint) * nbEndPoints;
memcpy(newEndPointsXArray, mEndPoints[0], nbBytesNewEndPoints);
memcpy(newEndPointsYArray, mEndPoints[1], nbBytesNewEndPoints);
memcpy(newEndPointsZArray, mEndPoints[2], nbBytesNewEndPoints);
// Delete the old arrays
delete[] mBoxes;
delete[] mEndPoints[0];
delete[] mEndPoints[1];
delete[] mEndPoints[2];
// Assign the pointer to the new arrays
mBoxes = newBoxesArray;
mEndPoints[0] = newEndPointsXArray;
mEndPoints[1] = newEndPointsYArray;
mEndPoints[2] = newEndPointsZArray;
mNbMaxBoxes = newNbMaxBoxes;
}

245
src/collision/broadphase/SweepAndPruneAlgorithm.h
View File

@ -1,245 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_SWEEP_AND_PRUNE_ALGORITHM_H
#define REACTPHYSICS3D_SWEEP_AND_PRUNE_ALGORITHM_H
// Libraries
#include "BroadPhaseAlgorithm.h"
#include "../../collision/shapes/AABB.h"
#include <map>
#include <vector>
#include <list>
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Structure EndPoint
/**
* EndPoint structure that represent an end-point of an AABB
* on one of the three x,y or z axis.
*/
struct EndPoint {
public:
/// ID of the AABB box corresponding to this end-point
bodyindex boxID;
/// True if the end-point is a minimum end-point of a box
bool isMin;
/// Value (one dimension coordinate) of the end-point
uint value;
/// Set the values of the endpoint
void setValues(bodyindex boxID, bool isMin, uint value) {
this->boxID = boxID;
this->isMin = isMin;
this->value = value;
}
};
// Structure BoxAABB
/**
* This structure represents an AABB in the Sweep-And-Prune algorithm
*/
struct BoxAABB {
public:
/// Index of the 3 minimum end-points of the AABB over the x,y,z axis
bodyindex min[3];
/// Index of the 3 maximum end-points of the AABB over the x,y,z axis
bodyindex max[3];
/// Body that corresponds to the owner of the AABB
CollisionBody* body;
};
// Structure AABBInt
/**
* Axis-Aligned Bounding box with integer coordinates.
*/
struct AABBInt {
public:
/// Minimum values on the three axis
uint min[3];
/// Maximum values on the three axis
uint max[3];
/// Constructor that takes an AABB as input
AABBInt(const AABB& aabb);
/// Constructor that set all the axis with an minimum and maximum value
AABBInt(uint minValue, uint maxValue);
};
// Class SweepAndPruneAlgorithm
/**
* This class implements the Sweep-And-Prune (SAP) broad-phase
* collision detection algorithm. This class implements an
* array-based implementation of the algorithm from Pierre Terdiman
* that is described here : www.codercorner.com/SAP.pdf.
*/
class SweepAndPruneAlgorithm : public BroadPhaseAlgorithm {
protected :
// -------------------- Constants -------------------- //
/// Invalid array index
const static bodyindex INVALID_INDEX;
/// Number of sentinel end-points in the array of a given axis
const static luint NB_SENTINELS;
// -------------------- Attributes -------------------- //
/// Array that contains all the AABB boxes of the broad-phase
BoxAABB* mBoxes;
/// Array of end-points on the three axis
EndPoint* mEndPoints[3];
/// Number of AABB boxes in the broad-phase
bodyindex mNbBoxes;
/// Max number of boxes in the boxes array
bodyindex mNbMaxBoxes;
/// Indices that are not used by any boxes
std::list<bodyindex> mFreeBoxIndices;
/// Map a body pointer to a box index
std::map<CollisionBody*,bodyindex> mMapBodyToBoxIndex;
// -------------------- Methods -------------------- //
/// Private copy-constructor
SweepAndPruneAlgorithm(const SweepAndPruneAlgorithm& algorithm);
/// Private assignment operator
SweepAndPruneAlgorithm& operator=(const SweepAndPruneAlgorithm& algorithm);
/// Resize the boxes and end-points arrays when it's full
void resizeArrays();
/// Shrink the boxes and end-points arrays when too much memory is allocated
void shrinkArrays();
/// Add an overlapping pair of AABBS
void addPair(CollisionBody* body1, CollisionBody* body2);
/// Check for 1D box intersection between two boxes that are sorted on the given axis.
bool testIntersect1DSortedAABBs(const BoxAABB& box1, const AABBInt& box2,
const EndPoint* const baseEndPoint, uint axis) const;
/// Check for 2D box intersection.
bool testIntersect2D(const BoxAABB& box1, const BoxAABB& box2,
luint axis1, uint axis2) const;
/// Notify the broad-phase that the AABB of an object has changed.
void updateObjectIntegerAABB(CollisionBody* body, const AABBInt& aabbInt);
public :
// -------------------- Methods -------------------- //
/// Constructor
SweepAndPruneAlgorithm(CollisionDetection& mCollisionDetection);
/// Destructor
virtual ~SweepAndPruneAlgorithm();
/// Notify the broad-phase about a new object in the world.
virtual void addObject(CollisionBody* body, const AABB& aabb);
/// Notify the broad-phase about a object that has been removed from the world
virtual void removeObject(CollisionBody* body);
/// Notify the broad-phase that the AABB of an object has changed
virtual void updateObject(CollisionBody* body, const AABB& aabb);
};
/// Encode a floating value into a integer value in order to
/// work with integer comparison in the Sweep-And-Prune algorithm
/// because it is faster. The main issue when encoding floating
/// number into integer is to keep to sorting order. This is a
/// problem for negative float number. This article describes
/// how to solve this issue : http://www.stereopsis.com/radix.html
inline uint encodeFloatIntoInteger(float number) {
uint intNumber = (uint&) number;
// If it's a negative number
if(intNumber & 0x80000000)
intNumber = ~intNumber;
else { // If it is a positive number
intNumber |= 0x80000000;
}
return intNumber;
}
// Check for 1D box intersection between two boxes that are sorted on the given axis.
/// Therefore, only one test is necessary here. We know that the
/// minimum of box1 cannot be larger that the maximum of box2 on the axis.
inline bool SweepAndPruneAlgorithm::testIntersect1DSortedAABBs(const BoxAABB& box1,
const AABBInt& box2,
const EndPoint* const endPointsArray,
uint axis) const {
return !(endPointsArray[box1.max[axis]].value < box2.min[axis]);
}
// Check for 2D box intersection. This method is used when we know
/// that two boxes already overlap on one axis and when want to test
/// if they also overlap on the two others axis.
inline bool SweepAndPruneAlgorithm::testIntersect2D(const BoxAABB& box1, const BoxAABB& box2,
luint axis1, uint axis2) const {
return !(box2.max[axis1] < box1.min[axis1] || box1.max[axis1] < box2.min[axis1] ||
box2.max[axis2] < box1.min[axis2] || box1.max[axis2] < box2.min[axis2]);
}
// Notify the broad-phase that the AABB of an object has changed
inline void SweepAndPruneAlgorithm::updateObject(CollisionBody* body, const AABB& aabb) {
// Compute the corresponding AABB with integer coordinates
AABBInt aabbInt(aabb);
// Call the update object method that uses an AABB with integer coordinates
updateObjectIntegerAABB(body, aabbInt);
}
}
#endif

14
src/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -25,7 +25,7 @@
// Libraries
#include "EPAAlgorithm.h"
#include "../GJK/GJKAlgorithm.h"
#include "collision/narrowphase//GJK/GJKAlgorithm.h"
#include "TrianglesStore.h"
// We want to use the ReactPhysics3D namespace
@ -83,9 +83,9 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
/// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
/// the correct penetration depth
bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
CollisionShape* collisionShape1,
ProxyShape* collisionShape1,
const Transform& transform1,
CollisionShape* collisionShape2,
ProxyShape* collisionShape2,
const Transform& transform2,
Vector3& v, ContactPointInfo*& contactInfo) {
@ -392,7 +392,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info
v = transform1.getOrientation().getMatrix() * triangle->getClosestPoint();
v = transform1.getOrientation() * triangle->getClosestPoint();
Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit();
@ -400,9 +400,9 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
assert(penetrationDepth > 0.0);
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pALocal, pBLocal);
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2, normal,
penetrationDepth, pALocal, pBLocal);
return true;
}

14
src/collision/narrowphase/EPA/EPAAlgorithm.h
View File

@ -27,12 +27,12 @@
#define REACTPHYSICS3D_EPA_ALGORITHM_H
// Libraries
#include "../GJK/Simplex.h"
#include "../../shapes/CollisionShape.h"
#include "../../../constraint/ContactPoint.h"
#include "../../../mathematics/mathematics.h"
#include "collision/narrowphase/GJK/Simplex.h"
#include "collision/shapes/CollisionShape.h"
#include "constraint/ContactPoint.h"
#include "mathematics/mathematics.h"
#include "TriangleEPA.h"
#include "../../../memory/MemoryAllocator.h"
#include "memory/MemoryAllocator.h"
#include <algorithm>
/// ReactPhysics3D namespace
@ -119,9 +119,9 @@ class EPAAlgorithm {
/// Compute the penetration depth with EPA algorithm.
bool computePenetrationDepthAndContactPoints(const Simplex& simplex,
CollisionShape* collisionShape1,
ProxyShape* collisionShape1,
const Transform& transform1,
CollisionShape* collisionShape2,
ProxyShape* collisionShape2,
const Transform& transform2,
Vector3& v, ContactPointInfo*& contactInfo);
};

2
src/collision/narrowphase/EPA/EdgeEPA.h
View File

@ -28,7 +28,7 @@
// Libraries
#include "../../../mathematics/mathematics.h"
#include "mathematics/mathematics.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {

4
src/collision/narrowphase/EPA/TriangleEPA.h
View File

@ -27,8 +27,8 @@
#define REACTPHYSICS3D_TRIANGLE_EPA_H
// Libraries
#include "../../../mathematics/mathematics.h"
#include "../../../configuration.h"
#include "mathematics/mathematics.h"
#include "configuration.h"
#include "EdgeEPA.h"
#include <cassert>

226
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -26,8 +26,8 @@
// Libraries
#include "GJKAlgorithm.h"
#include "Simplex.h"
#include "../../../constraint/ContactPoint.h"
#include "../../../configuration.h"
#include "constraint/ContactPoint.h"
#include "configuration.h"
#include <algorithm>
#include <cmath>
#include <cfloat>
@ -47,7 +47,7 @@ GJKAlgorithm::~GJKAlgorithm() {
}
// Return true and compute a contact info if the two bounding volumes collide.
// Return true and compute a contact info if the two collision shapes collide.
/// This method implements the Hybrid Technique for computing the penetration depth by
/// running the GJK algorithm on original objects (without margin).
/// If the objects don't intersect, this method returns false. If they intersect
@ -57,11 +57,8 @@ GJKAlgorithm::~GJKAlgorithm() {
/// algorithm on the enlarged object to obtain a simplex polytope that contains the
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
/// the correct penetration depth and contact points between the enlarged objects.
bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1,
const Transform& transform1,
CollisionShape* collisionShape2,
const Transform& transform2,
ContactPointInfo*& contactInfo) {
bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
ContactPointInfo*& contactInfo) {
Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B
@ -71,6 +68,12 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1,
decimal vDotw;
decimal prevDistSquare;
// Get the local-space to world-space transforms
const Transform transform1 = collisionShape1->getBody()->getTransform() *
collisionShape1->getLocalToBodyTransform();
const Transform transform2 = collisionShape2->getBody()->getTransform() *
collisionShape2->getLocalToBodyTransform();
// Transform a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2Tobody1 = transform1.getInverse() * transform2;
@ -89,7 +92,7 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1,
Simplex simplex;
// Get the previous point V (last cached separating axis)
Vector3 v = mCurrentOverlappingPair->previousSeparatingAxis;
Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis();
// Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST;
@ -110,7 +113,7 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1,
if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence
mCurrentOverlappingPair->previousSeparatingAxis = v;
mCurrentOverlappingPair->setCachedSeparatingAxis(v);
// No intersection, we return false
return false;
@ -130,16 +133,16 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1,
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pA, pB);
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2, normal,
penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
@ -162,16 +165,16 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1,
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pA, pB);
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2, normal,
penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
@ -192,16 +195,16 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1,
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pA, pB);
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2, normal,
penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
@ -229,16 +232,16 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1,
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal,
penetrationDepth,
pA, pB);
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2, normal,
penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
@ -259,9 +262,9 @@ bool GJKAlgorithm::testCollision(CollisionShape* collisionShape1,
/// assumed to intersect in the original objects (without margin). Therefore such
/// a polytope must exist. Then, we give that polytope to the EPA algorithm to
/// compute the correct penetration depth and contact points of the enlarged objects.
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(CollisionShape* collisionShape1,
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* collisionShape1,
const Transform& transform1,
CollisionShape* collisionShape2,
ProxyShape* collisionShape2,
const Transform& transform2,
ContactPointInfo*& contactInfo,
Vector3& v) {
@ -327,3 +330,170 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(CollisionShape* col
transform1, collisionShape2, transform2,
v, contactInfo);
}
// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* collisionShape) {
Vector3 suppA; // Support point of object A
Vector3 w; // Support point of Minkowski difference A-B
decimal vDotw;
decimal prevDistSquare;
// Support point of object B (object B is a single point)
const Vector3 suppB(localPoint);
// Create a simplex set
Simplex simplex;
// Initial supporting direction
Vector3 v(1, 1, 1);
// Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST;
do {
// Compute the support points for original objects (without margins) A and B
suppA = collisionShape->getLocalSupportPointWithoutMargin(-v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
vDotw = v.dot(w);
// Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB);
// If the simplex is affinely dependent
if (simplex.isAffinelyDependent()) {
return false;
}
// Compute the point of the simplex closest to the origin
// If the computation of the closest point fail
if (!simplex.computeClosestPoint(v)) {
return false;
}
// Store and update the squared distance of the closest point
prevDistSquare = distSquare;
distSquare = v.lengthSquare();
// If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
return false;
}
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
simplex.getMaxLengthSquareOfAPoint());
// The point is inside the collision shape
return true;
}
// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
/// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in
/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".
bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastInfo& raycastInfo) {
Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin)
Vector3 suppB; // Support point on the collision shape
const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON;
const decimal epsilon = decimal(0.0001);
// Convert the ray origin and direction into the local-space of the collision shape
const Transform localToWorldTransform = collisionShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
Vector3 point1 = worldToLocalTransform * ray.point1;
Vector3 point2 = worldToLocalTransform * ray.point2;
Vector3 rayDirection = point2 - point1;
// If the points of the segment are two close, return no hit
if (rayDirection.lengthSquare() < machineEpsilonSquare) return false;
Vector3 w;
// Create a simplex set
Simplex simplex;
Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0));
decimal lambda = decimal(0.0);
suppA = point1; // Current lower bound point on the ray (starting at ray's origin)
suppB = collisionShape->getLocalSupportPointWithoutMargin(rayDirection);
Vector3 v = suppA - suppB;
decimal vDotW, vDotR;
decimal distSquare = v.lengthSquare();
int nbIterations = 0;
// GJK Algorithm loop
while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) {
// Compute the support points
suppB = collisionShape->getLocalSupportPointWithoutMargin(v);
w = suppA - suppB;
vDotW = v.dot(w);
if (vDotW > decimal(0)) {
vDotR = v.dot(rayDirection);
if (vDotR >= -machineEpsilonSquare) {
return false;
}
else {
// We have found a better lower bound for the hit point along the ray
lambda = lambda - vDotW / vDotR;
suppA = point1 + lambda * rayDirection;
w = suppA - suppB;
n = v;
}
}
// Add the new support point to the simplex
if (!simplex.isPointInSimplex(w)) {
simplex.addPoint(w, suppA, suppB);
}
// Compute the closest point
if (simplex.computeClosestPoint(v)) {
distSquare = v.lengthSquare();
}
else {
distSquare = decimal(0.0);
}
// If the current lower bound distance is larger than the maximum raycasting distance
if (lambda > ray.maxFraction) return false;
nbIterations++;
}
// If the origin was inside the shape, we return no hit
if (lambda < MACHINE_EPSILON) return false;
// Compute the closet points of both objects (without the margins)
Vector3 pointA;
Vector3 pointB;
simplex.computeClosestPointsOfAandB(pointA, pointB);
// A raycast hit has been found, we fill in the raycast info
raycastInfo.hitFraction = lambda;
raycastInfo.worldPoint = localToWorldTransform * pointB;
raycastInfo.body = collisionShape->getBody();
raycastInfo.proxyShape = collisionShape;
if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * n.getUnit();
}
else { // Degenerated normal vector, we return a zero normal vector
raycastInfo.worldNormal = Vector3(decimal(0), decimal(0), decimal(0));
}
return true;
}

24
src/collision/narrowphase/GJK/GJKAlgorithm.h
View File

@ -27,10 +27,10 @@
#define REACTPHYSICS3D_GJK_ALGORITHM_H
// Libraries
#include "../NarrowPhaseAlgorithm.h"
#include "../../../constraint/ContactPoint.h"
#include "../../../collision/shapes/CollisionShape.h"
#include "../EPA/EPAAlgorithm.h"
#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
#include "constraint/ContactPoint.h"
#include "collision/shapes/CollisionShape.h"
#include "collision/narrowphase/EPA/EPAAlgorithm.h"
/// ReactPhysics3D namespace
@ -39,6 +39,7 @@ namespace reactphysics3d {
// Constants
const decimal REL_ERROR = decimal(1.0e-3);
const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR;
const int MAX_ITERATIONS_GJK_RAYCAST = 32;
// Class GJKAlgorithm
/**
@ -74,9 +75,9 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm);
/// Compute the penetration depth for enlarged objects.
bool computePenetrationDepthForEnlargedObjects(CollisionShape* collisionShape1,
bool computePenetrationDepthForEnlargedObjects(ProxyShape* collisionShape1,
const Transform& transform1,
CollisionShape* collisionShape2,
ProxyShape* collisionShape2,
const Transform& transform2,
ContactPointInfo*& contactInfo, Vector3& v);
@ -91,11 +92,14 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
~GJKAlgorithm();
/// Return true and compute a contact info if the two bounding volumes collide.
virtual bool testCollision(CollisionShape *collisionShape1,
const Transform& transform1,
CollisionShape *collisionShape2,
const Transform& transform2,
virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
ContactPointInfo*& contactInfo);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const Vector3& localPoint, ProxyShape* collisionShape);
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
bool raycast(const Ray& ray, ProxyShape* collisionShape, RaycastInfo& raycastInfo);
};
}

4
src/collision/narrowphase/GJK/Simplex.h
View File

@ -27,7 +27,7 @@
#define REACTPHYSICS3D_SIMPLEX_H
// Libraries
#include "../../../mathematics/mathematics.h"
#include "mathematics/mathematics.h"
#include <vector>
/// ReactPhysics3D namespace
@ -130,7 +130,7 @@ class Simplex {
/// Return true if the simplex contains 4 points
bool isFull() const;
/// Return true if the simple is empty
/// Return true if the simplex is empty
bool isEmpty() const;
/// Return the points of the simplex

20
src/collision/narrowphase/NarrowPhaseAlgorithm.h
View File

@ -27,11 +27,10 @@
#define REACTPHYSICS3D_NARROW_PHASE_ALGORITHM_H
// Libraries
#include "../../body/Body.h"
#include "../../constraint/ContactPoint.h"
#include "../broadphase/PairManager.h"
#include "../../memory/MemoryAllocator.h"
#include "../BroadPhasePair.h"
#include "body/Body.h"
#include "constraint/ContactPoint.h"
#include "memory/MemoryAllocator.h"
#include "engine/OverlappingPair.h"
/// Namespace ReactPhysics3D
@ -54,7 +53,7 @@ class NarrowPhaseAlgorithm {
MemoryAllocator& mMemoryAllocator;
/// Overlapping pair of the bodies currently tested for collision
BroadPhasePair* mCurrentOverlappingPair;
OverlappingPair* mCurrentOverlappingPair;
// -------------------- Methods -------------------- //
@ -75,18 +74,15 @@ class NarrowPhaseAlgorithm {
virtual ~NarrowPhaseAlgorithm();
/// Set the current overlapping pair of bodies
void setCurrentOverlappingPair(BroadPhasePair* overlappingPair);
void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
/// Return true and compute a contact info if the two bounding volume collide
virtual bool testCollision(CollisionShape* collisionShape1,
const Transform& transform1,
CollisionShape* collisionShape2,
const Transform& transform2,
virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
ContactPointInfo*& contactInfo)=0;
};
// Set the current overlapping pair of bodies
inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(BroadPhasePair *overlappingPair) {
inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) {
mCurrentOverlappingPair = overlappingPair;
}

29
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -25,7 +25,7 @@
// Libraries
#include "SphereVsSphereAlgorithm.h"
#include "../../collision/shapes/SphereShape.h"
#include "collision/shapes/SphereShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -41,16 +41,22 @@ SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() {
}
bool SphereVsSphereAlgorithm::testCollision(CollisionShape* collisionShape1,
const Transform& transform1,
CollisionShape* collisionShape2,
const Transform& transform2,
bool SphereVsSphereAlgorithm::testCollision(ProxyShape* collisionShape1,
ProxyShape* collisionShape2,
ContactPointInfo*& contactInfo) {
// Get the sphere collision shapes
const SphereShape* sphereShape1 = dynamic_cast<const SphereShape*>(collisionShape1);
const SphereShape* sphereShape2 = dynamic_cast<const SphereShape*>(collisionShape2);
const CollisionShape* shape1 = collisionShape1->getCollisionShape();
const CollisionShape* shape2 = collisionShape2->getCollisionShape();
const SphereShape* sphereShape1 = dynamic_cast<const SphereShape*>(shape1);
const SphereShape* sphereShape2 = dynamic_cast<const SphereShape*>(shape2);
// Get the local-space to world-space transforms
const Transform transform1 = collisionShape1->getBody()->getTransform() *
collisionShape1->getLocalToBodyTransform();
const Transform transform2 = collisionShape2->getBody()->getTransform() *
collisionShape2->getLocalToBodyTransform();
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
@ -69,9 +75,10 @@ bool SphereVsSphereAlgorithm::testCollision(CollisionShape* collisionShape1,
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(
vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2);
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2,
vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2);
return true;
}

9
src/collision/narrowphase/SphereVsSphereAlgorithm.h
View File

@ -27,8 +27,8 @@
#define REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
// Libraries
#include "../../body/Body.h"
#include "../../constraint/ContactPoint.h"
#include "body/Body.h"
#include "constraint/ContactPoint.h"
#include "NarrowPhaseAlgorithm.h"
@ -63,10 +63,7 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
virtual ~SphereVsSphereAlgorithm();
/// Return true and compute a contact info if the two bounding volume collide
virtual bool testCollision(CollisionShape* collisionShape1,
const Transform& transform1,
CollisionShape* collisionShape2,
const Transform& transform2,
virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
ContactPointInfo*& contactInfo);
};

33
src/collision/shapes/AABB.cpp
View File

@ -25,7 +25,7 @@
// Libraries
#include "AABB.h"
#include "../../configuration.h"
#include "configuration.h"
#include <cassert>
using namespace reactphysics3d;
@ -42,8 +42,39 @@ AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates)
}
// Copy-constructor
AABB::AABB(const AABB& aabb)
: mMinCoordinates(aabb.mMinCoordinates), mMaxCoordinates(aabb.mMaxCoordinates) {
}
// Destructor
AABB::~AABB() {
}
// Replace the current AABB with a new AABB that is the union of two AABBs in parameters
void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) {
mMinCoordinates.x = std::min(aabb1.mMinCoordinates.x, aabb2.mMinCoordinates.x);
mMinCoordinates.y = std::min(aabb1.mMinCoordinates.y, aabb2.mMinCoordinates.y);
mMinCoordinates.z = std::min(aabb1.mMinCoordinates.z, aabb2.mMinCoordinates.z);
mMaxCoordinates.x = std::max(aabb1.mMaxCoordinates.x, aabb2.mMaxCoordinates.x);
mMaxCoordinates.y = std::max(aabb1.mMaxCoordinates.y, aabb2.mMaxCoordinates.y);
mMaxCoordinates.z = std::max(aabb1.mMaxCoordinates.z, aabb2.mMaxCoordinates.z);
}
// Return true if the current AABB contains the AABB given in parameter
bool AABB::contains(const AABB& aabb) {
bool isInside = true;
isInside = isInside && mMinCoordinates.x <= aabb.mMinCoordinates.x;
isInside = isInside && mMinCoordinates.y <= aabb.mMinCoordinates.y;
isInside = isInside && mMinCoordinates.z <= aabb.mMinCoordinates.z;
isInside = isInside && mMaxCoordinates.x >= aabb.mMaxCoordinates.x;
isInside = isInside && mMaxCoordinates.y >= aabb.mMaxCoordinates.y;
isInside = isInside && mMaxCoordinates.z >= aabb.mMaxCoordinates.z;
return isInside;
}

56
src/collision/shapes/AABB.h
View File

@ -27,13 +27,10 @@
#define REACTPHYSICS3D_AABB_H
// Libraries
#include "../../mathematics/mathematics.h"
#include "mathematics/mathematics.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declaration
class Body;
// Class AABB
/**
@ -54,17 +51,6 @@ class AABB {
/// Maximum world coordinates of the AABB on the x,y and z axis
Vector3 mMaxCoordinates;
// -------------------- Methods -------------------- //
/// Private copy-constructor
AABB(const AABB& aabb);
/// Private assignment operator
AABB& operator=(const AABB& aabb);
/// Constructor
AABB(const Transform& transform, const Vector3& extents);
public :
// -------------------- Methods -------------------- //
@ -75,10 +61,11 @@ class AABB {
/// Constructor
AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates);
/// Copy-constructor
AABB(const AABB& aabb);
/// Destructor
virtual ~AABB();
~AABB();
/// Return the center point
Vector3 getCenter() const;
@ -97,11 +84,27 @@ class AABB {
/// Return true if the current AABB is overlapping with the AABB in argument
bool testCollision(const AABB& aabb) const;
/// Return the volume of the AABB
decimal getVolume() const;
/// Replace the current AABB with a new AABB that is the union of two AABBs in parameters
void mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2);
/// Return true if the current AABB contains the AABB given in parameter
bool contains(const AABB& aabb);
/// Assignment operator
AABB& operator=(const AABB& aabb);
// -------------------- Friendship -------------------- //
friend class DynamicAABBTree;
};
// Return the center point of the AABB in world coordinates
inline Vector3 AABB::getCenter() const {
return (mMinCoordinates + mMaxCoordinates) * 0.5;
return (mMinCoordinates + mMaxCoordinates) * decimal(0.5);
}
// Return the minimum coordinates of the AABB
@ -119,7 +122,7 @@ inline const Vector3& AABB::getMax() const {
return mMaxCoordinates;
}
/// Set the maximum coordinates of the AABB
// Set the maximum coordinates of the AABB
inline void AABB::setMax(const Vector3& max) {
mMaxCoordinates = max;
}
@ -136,6 +139,21 @@ inline bool AABB::testCollision(const AABB& aabb) const {
return true;
}
// Return the volume of the AABB
inline decimal AABB::getVolume() const {
const Vector3 diff = mMaxCoordinates - mMinCoordinates;
return (diff.x * diff.y * diff.z);
}
// Assignment operator
inline AABB& AABB::operator=(const AABB& aabb) {
if (this != &aabb) {
mMinCoordinates = aabb.mMinCoordinates;
mMaxCoordinates = aabb.mMaxCoordinates;
}
return *this;
}
}
#endif

75
src/collision/shapes/BoxShape.cpp
View File

@ -25,7 +25,8 @@
// Libraries
#include "BoxShape.h"
#include "../../configuration.h"
#include "collision/ProxyShape.h"
#include "configuration.h"
#include <vector>
#include <cassert>
@ -37,7 +38,6 @@ BoxShape::BoxShape(const Vector3& extent, decimal margin)
assert(extent.x > decimal(0.0) && extent.x > margin);
assert(extent.y > decimal(0.0) && extent.y > margin);
assert(extent.z > decimal(0.0) && extent.z > margin);
assert(margin > decimal(0.0));
}
// Private copy-constructor
@ -45,6 +45,7 @@ BoxShape::BoxShape(const BoxShape& shape) : CollisionShape(shape), mExtent(shape
}
// Destructor
BoxShape::~BoxShape() {
@ -61,3 +62,73 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare));
}
// Raycast method with feedback information
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
const Vector3 point1 = worldToLocalTransform * ray.point1;
const Vector3 point2 = worldToLocalTransform * ray.point2;
Vector3 rayDirection = point2 - point1;
decimal tMin = DECIMAL_SMALLEST;
decimal tMax = DECIMAL_LARGEST;
Vector3 normalDirection(decimal(0), decimal(0), decimal(0));
Vector3 currentNormal;
// For each of the three slabs
for (int i=0; i<3; i++) {
// If ray is parallel to the slab
if (std::abs(rayDirection[i]) < MACHINE_EPSILON) {
// If the ray's origin is not inside the slab, there is no hit
if (point1[i] > mExtent[i] || point1[i] < -mExtent[i]) return false;
}
else {
// Compute the intersection of the ray with the near and far plane of the slab
decimal oneOverD = decimal(1.0) / rayDirection[i];
decimal t1 = (-mExtent[i] - point1[i]) * oneOverD;
decimal t2 = (mExtent[i] - point1[i]) * oneOverD;
currentNormal[0] = (i == 0) ? -mExtent[i] : decimal(0.0);
currentNormal[1] = (i == 1) ? -mExtent[i] : decimal(0.0);
currentNormal[2] = (i == 2) ? -mExtent[i] : decimal(0.0);
// Swap t1 and t2 if need so that t1 is intersection with near plane and
// t2 with far plane
if (t1 > t2) {
std::swap(t1, t2);
currentNormal = -currentNormal;
}
// Compute the intersection of the of slab intersection interval with previous slabs
if (t1 > tMin) {
tMin = t1;
normalDirection = currentNormal;
}
tMax = std::min(tMax, t2);
// If tMin is larger than the maximum raycasting fraction, we return no hit
if (tMin > ray.maxFraction) return false;
// If the slabs intersection is empty, there is no hit
if (tMin > tMax) return false;
}
}
// If tMin is negative, we return no hit
if (tMin < decimal(0.0)) return false;
// The ray intersects the three slabs, we compute the hit point
Vector3 localHitPoint = point1 + tMin * rayDirection;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = tMin;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
normalDirection.normalize();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
return true;
}

36
src/collision/shapes/BoxShape.h
View File

@ -29,7 +29,8 @@
// Libraries
#include <cfloat>
#include "CollisionShape.h"
#include "../../mathematics/mathematics.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
/// ReactPhysics3D namespace
@ -66,6 +67,20 @@ class BoxShape : public CollisionShape {
/// Private assignment operator
BoxShape& operator=(const BoxShape& shape);
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
public :
// -------------------- Methods -------------------- //
@ -88,12 +103,6 @@ class BoxShape : public CollisionShape {
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction);
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
@ -128,7 +137,8 @@ inline size_t BoxShape::getSizeInBytes() const {
}
// Return a local support point in a given direction with the object margin
inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction) {
inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
assert(mMargin > 0.0);
@ -138,7 +148,8 @@ inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction
}
// Return a local support point in a given direction without the objec margin
inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x,
direction.y < 0.0 ? -mExtent.y : mExtent.y,
@ -151,6 +162,13 @@ inline bool BoxShape::isEqualTo(const CollisionShape& otherCollisionShape) const
return (mExtent == otherShape.mExtent);
}
// Return true if a point is inside the collision shape
inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return (localPoint.x < mExtent[0] && localPoint.x > -mExtent[0] &&
localPoint.y < mExtent[1] && localPoint.y > -mExtent[1] &&
localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]);
}
}
#endif

221
src/collision/shapes/CapsuleShape.cpp
View File

@ -25,7 +25,8 @@
// Libraries
#include "CapsuleShape.h"
#include "../../configuration.h"
#include "collision/ProxyShape.h"
#include "configuration.h"
#include <cassert>
using namespace reactphysics3d;
@ -55,7 +56,8 @@ CapsuleShape::~CapsuleShape() {
/// Therefore, in this method, we compute the support points of both top and bottom spheres of
/// the capsule and return the point with the maximum dot product with the direction vector. Note
/// that the object margin is implicitly the radius and height of the capsule.
Vector3 CapsuleShape::getLocalSupportPointWithMargin(const Vector3& direction) {
Vector3 CapsuleShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
// If the direction vector is not the zero vector
if (direction.lengthSquare() >= MACHINE_EPSILON * MACHINE_EPSILON) {
@ -87,7 +89,8 @@ Vector3 CapsuleShape::getLocalSupportPointWithMargin(const Vector3& direction) {
}
// Return a local support point in a given direction without the object margin.
Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
// If the dot product of the direction and the local Y axis (dotProduct = direction.y)
// is positive
@ -123,3 +126,215 @@ void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) co
0.0, Iyy, 0.0,
0.0, 0.0, IxxAndzz);
}
// Return true if a point is inside the collision shape
bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
const decimal diffYCenterSphere1 = localPoint.y - mHalfHeight;
const decimal diffYCenterSphere2 = localPoint.y + mHalfHeight;
const decimal xSquare = localPoint.x * localPoint.x;
const decimal zSquare = localPoint.z * localPoint.z;
const decimal squareRadius = mRadius * mRadius;
// Return true if the point is inside the cylinder or one of the two spheres of the capsule
return ((xSquare + zSquare) < squareRadius &&
localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) ||
(xSquare + zSquare + diffYCenterSphere1 * diffYCenterSphere1) < squareRadius ||
(xSquare + zSquare + diffYCenterSphere2 * diffYCenterSphere2) < squareRadius;
}
// Raycast method with feedback information
bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
// Transform the ray direction and origin in local-space coordinates
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
const Vector3 point1 = worldToLocalTransform * ray.point1;
const Vector3 point2 = worldToLocalTransform * ray.point2;
const Vector3 n = point2 - point1;
const decimal epsilon = decimal(0.01);
Vector3 p(decimal(0), -mHalfHeight, decimal(0));
Vector3 q(decimal(0), mHalfHeight, decimal(0));
Vector3 d = q - p;
Vector3 m = point1 - p;
decimal t;
decimal mDotD = m.dot(d);
decimal nDotD = n.dot(d);
decimal dDotD = d.dot(d);
// Test if the segment is outside the cylinder
decimal vec1DotD = (point1 - Vector3(decimal(0.0), -mHalfHeight - mRadius, decimal(0.0))).dot(d);
if (vec1DotD < decimal(0.0) && vec1DotD + nDotD < decimal(0.0)) return false;
decimal ddotDExtraCaps = decimal(2.0) * mRadius * d.y;
if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) return false;
decimal nDotN = n.dot(n);
decimal mDotN = m.dot(n);
decimal a = dDotD * nDotN - nDotD * nDotD;
decimal k = m.dot(m) - mRadius * mRadius;
decimal c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the capsule axis
if (std::abs(a) < epsilon) {
// If the origin is outside the surface of the capusle's cylinder, we return no hit
if (c > decimal(0.0)) return false;
// Here we know that the segment intersect an endcap of the capsule
// If the ray intersects with the "p" endcap of the capsule
if (mDotD < decimal(0.0)) {
// Check intersection between the ray and the "p" sphere endcap of the capsule
Vector3 hitLocalPoint;
decimal hitFraction;
if (raycastWithSphereEndCap(point1, point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - p).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
return true;
}
return false;
}
else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder
// Check intersection between the ray and the "q" sphere endcap of the capsule
Vector3 hitLocalPoint;
decimal hitFraction;
if (raycastWithSphereEndCap(point1, point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - q).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
return true;
}
return false;
}
else { // If the origin is inside the cylinder, we return no hit
return false;
}
}
decimal b = dDotD * mDotN - nDotD * mDotD;
decimal discriminant = b * b - a * c;
// If the discriminant is negative, no real roots and therfore, no hit
if (discriminant < decimal(0.0)) return false;
// Compute the smallest root (first intersection along the ray)
decimal t0 = t = (-b - std::sqrt(discriminant)) / a;
// If the intersection is outside the finite cylinder of the capsule on "p" endcap side
decimal value = mDotD + t * nDotD;
if (value < decimal(0.0)) {
// Check intersection between the ray and the "p" sphere endcap of the capsule
Vector3 hitLocalPoint;
decimal hitFraction;
if (raycastWithSphereEndCap(point1, point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - p).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
return true;
}
return false;
}
else if (value > dDotD) { // If the intersection is outside the finite cylinder on the "q" side
// Check intersection between the ray and the "q" sphere endcap of the capsule
Vector3 hitLocalPoint;
decimal hitFraction;
if (raycastWithSphereEndCap(point1, point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - q).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
return true;
}
return false;
}
t = t0;
// If the intersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
Vector3 v = localHitPoint - p;
Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
Vector3 normalDirection = (localHitPoint - (p + w)).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
return true;
}
// Raycasting method between a ray one of the two spheres end cap of the capsule
bool CapsuleShape::raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
const Vector3& sphereCenter, decimal maxFraction,
Vector3& hitLocalPoint, decimal& hitFraction) const {
const Vector3 m = point1 - sphereCenter;
decimal c = m.dot(m) - mRadius * mRadius;
// If the origin of the ray is inside the sphere, we return no intersection
if (c < decimal(0.0)) return false;
const Vector3 rayDirection = point2 - point1;
decimal b = m.dot(rayDirection);
// If the origin of the ray is outside the sphere and the ray
// is pointing away from the sphere, there is no intersection
if (b > decimal(0.0)) return false;
decimal raySquareLength = rayDirection.lengthSquare();
// Compute the discriminant of the quadratic equation
decimal discriminant = b * b - raySquareLength * c;
// If the discriminant is negative or the ray length is very small, there is no intersection
if (discriminant < decimal(0.0) || raySquareLength < MACHINE_EPSILON) return false;
// Compute the solution "t" closest to the origin
decimal t = -b - std::sqrt(discriminant);
assert(t >= decimal(0.0));
// If the hit point is withing the segment ray fraction
if (t < maxFraction * raySquareLength) {
// Compute the intersection information
t /= raySquareLength;
hitFraction = t;
hitLocalPoint = point1 + t * rayDirection;
return true;
}
return false;
}

28
src/collision/shapes/CapsuleShape.h
View File

@ -28,7 +28,8 @@
// Libraries
#include "CollisionShape.h"
#include "../../mathematics/mathematics.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
@ -63,6 +64,25 @@ class CapsuleShape : public CollisionShape {
/// Private assignment operator
CapsuleShape& operator=(const CapsuleShape& shape);
/// Return a local support point in a given direction with the object margin.
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
/// Raycasting method between a ray one of the two spheres end cap of the capsule
bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
const Vector3& sphereCenter, decimal maxFraction,
Vector3& hitLocalPoint, decimal& hitFraction) const;
public :
// -------------------- Methods -------------------- //
@ -85,12 +105,6 @@ class CapsuleShape : public CollisionShape {
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
/// Return a local support point in a given direction with the object margin.
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction);
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;

8
src/collision/shapes/CollisionShape.cpp
View File

@ -25,6 +25,8 @@
// Libraries
#include "CollisionShape.h"
#include "engine/Profiler.h"
#include "body/CollisionBody.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -47,8 +49,10 @@ CollisionShape::~CollisionShape() {
assert(mNbSimilarCreatedShapes == 0);
}
// Update the AABB of a body using its collision shape
void CollisionShape::updateAABB(AABB& aabb, const Transform& transform) {
// Compute the world-space AABB of the collision shape given a transform
void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const {
PROFILE("CollisionShape::computeAABB()");
// Get the local bounds in x,y and z direction
Vector3 minBounds;

41
src/collision/shapes/CollisionShape.h
View File

@ -29,9 +29,12 @@
// Libraries
#include <cassert>
#include <typeinfo>
#include "../../mathematics/Vector3.h"
#include "../../mathematics/Matrix3x3.h"
#include "mathematics/Vector3.h"
#include "mathematics/Matrix3x3.h"
#include "mathematics/Ray.h"
#include "AABB.h"
#include "collision/RaycastInfo.h"
#include "memory/MemoryAllocator.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@ -40,7 +43,8 @@ namespace reactphysics3d {
enum CollisionShapeType {BOX, SPHERE, CONE, CYLINDER, CAPSULE, CONVEX_MESH};
// Declarations
class Body;
class ProxyShape;
class CollisionBody;
// Class CollisionShape
/**
@ -70,6 +74,20 @@ class CollisionShape {
/// Private assignment operator
CollisionShape& operator=(const CollisionShape& shape);
// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const=0;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const=0;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0;
public :
// -------------------- Methods -------------------- //
@ -95,20 +113,14 @@ class CollisionShape {
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const = 0;
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction)=0;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction)=0;
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const=0;
/// Return the local inertia tensor of the collision shapes
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0;
/// Update the AABB of a body using its collision shape
virtual void updateAABB(AABB& aabb, const Transform& transform);
/// Compute the world-space AABB of the collision shape given a transform
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
/// Increment the number of similar allocated collision shapes
void incrementNbSimilarCreatedShapes();
@ -121,8 +133,15 @@ class CollisionShape {
/// Test equality between two collision shapes of the same type (same derived classes).
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const=0;
// -------------------- Friendship -------------------- //
friend class ProxyShape;
};
// Return the type of the collision shape
inline CollisionShapeType CollisionShape::getType() const {
return mType;

154
src/collision/shapes/ConeShape.cpp
View File

@ -25,8 +25,9 @@
// Libraries
#include <complex>
#include "../../configuration.h"
#include "configuration.h"
#include "ConeShape.h"
#include "collision/ProxyShape.h"
using namespace reactphysics3d;
@ -35,7 +36,6 @@ ConeShape::ConeShape(decimal radius, decimal height, decimal margin)
: CollisionShape(CONE, margin), mRadius(radius), mHalfHeight(height * decimal(0.5)) {
assert(mRadius > decimal(0.0));
assert(mHalfHeight > decimal(0.0));
assert(margin > decimal(0.0));
// Compute the sine of the semi-angle at the apex point
mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
@ -54,10 +54,11 @@ ConeShape::~ConeShape() {
}
// Return a local support point in a given direction with the object margin
Vector3 ConeShape::getLocalSupportPointWithMargin(const Vector3& direction) {
Vector3 ConeShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
// Compute the support point without the margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction);
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
// Add the margin to the support point
Vector3 unitVec(0.0, -1.0, 0.0);
@ -70,7 +71,8 @@ Vector3 ConeShape::getLocalSupportPointWithMargin(const Vector3& direction) {
}
// Return a local support point in a given direction without the object margin
Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
const Vector3& v = direction;
decimal sinThetaTimesLengthV = mSinTheta * v.length();
@ -92,3 +94,145 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
return supportPoint;
}
// Raycast method with feedback information
// This implementation is based on the technique described by David Eberly in the article
// "Intersection of a Line and a Cone" that can be found at
// http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf
bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
// Transform the ray direction and origin in local-space coordinates
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
const Vector3 point1 = worldToLocalTransform * ray.point1;
const Vector3 point2 = worldToLocalTransform * ray.point2;
const Vector3 r = point2 - point1;
const decimal epsilon = decimal(0.00001);
Vector3 V(0, mHalfHeight, 0);
Vector3 centerBase(0, -mHalfHeight, 0);
Vector3 axis(0, decimal(-1.0), 0);
decimal heightSquare = decimal(4.0) * mHalfHeight * mHalfHeight;
decimal cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius);
decimal factor = decimal(1.0) - cosThetaSquare;
Vector3 delta = point1 - V;
decimal c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y -
cosThetaSquare * delta.z * delta.z;
decimal c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z;
decimal c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z;
decimal tHit[] = {decimal(-1.0), decimal(-1.0), decimal(-1.0)};
Vector3 localHitPoint[3];
Vector3 localNormal[3];
// If c2 is different from zero
if (std::abs(c2) > MACHINE_EPSILON) {
decimal gamma = c1 * c1 - c0 * c2;
// If there is no real roots in the quadratic equation
if (gamma < decimal(0.0)) {
return false;
}
else if (gamma > decimal(0.0)) { // The equation has two real roots
// Compute two intersections
decimal sqrRoot = std::sqrt(gamma);
tHit[0] = (-c1 - sqrRoot) / c2;
tHit[1] = (-c1 + sqrRoot) / c2;
}
else { // If the equation has a single real root
// Compute the intersection
tHit[0] = -c1 / c2;
}
}
else { // If c2 == 0
// If c2 = 0 and c1 != 0
if (std::abs(c1) > MACHINE_EPSILON) {
tHit[0] = -c0 / (decimal(2.0) * c1);
}
else { // If c2 = c1 = 0
// If c0 is different from zero, no solution and if c0 = 0, we have a
// degenerate case, the whole ray is contained in the cone side
// but we return no hit in this case
return false;
}
}
// If the origin of the ray is inside the cone, we return no hit
if (testPointInside(point1, NULL)) return false;
localHitPoint[0] = point1 + tHit[0] * r;
localHitPoint[1] = point1 + tHit[1] * r;
// Only keep hit points in one side of the double cone (the cone we are interested in)
if (axis.dot(localHitPoint[0] - V) < decimal(0.0)) {
tHit[0] = decimal(-1.0);
}
if (axis.dot(localHitPoint[1] - V) < decimal(0.0)) {
tHit[1] = decimal(-1.0);
}
// Only keep hit points that are within the correct height of the cone
if (localHitPoint[0].y < decimal(-mHalfHeight)) {
tHit[0] = decimal(-1.0);
}
if (localHitPoint[1].y < decimal(-mHalfHeight)) {
tHit[1] = decimal(-1.0);
}
// If the ray is in direction of the base plane of the cone
if (r.y > epsilon) {
// Compute the intersection with the base plane of the cone
tHit[2] = (-point1.y - mHalfHeight) / (r.y);
// Only keep this intersection if it is inside the cone radius
localHitPoint[2] = point1 + tHit[2] * r;
if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) {
tHit[2] = decimal(-1.0);
}
// Compute the normal direction
localNormal[2] = axis;
}
// Find the smallest positive t value
int hitIndex = -1;
decimal t = DECIMAL_LARGEST;
for (int i=0; i<3; i++) {
if (tHit[i] < decimal(0.0)) continue;
if (tHit[i] < t) {
hitIndex = i;
t = tHit[hitIndex];
}
}
if (hitIndex < 0) return false;
if (t > ray.maxFraction) return false;
// Compute the normal direction for hit against side of the cone
if (hitIndex != 2) {
decimal h = decimal(2.0) * mHalfHeight;
decimal value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x +
localHitPoint[hitIndex].z * localHitPoint[hitIndex].z);
decimal rOverH = mRadius / h;
decimal value2 = decimal(1.0) + rOverH * rOverH;
decimal factor = decimal(1.0) / std::sqrt(value1 * value2);
decimal x = localHitPoint[hitIndex].x * factor;
decimal z = localHitPoint[hitIndex].z * factor;
localNormal[hitIndex].x = x;
localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH;
localNormal[hitIndex].z = z;
}
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint[hitIndex];
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * localNormal[hitIndex];
return true;
}

31
src/collision/shapes/ConeShape.h
View File

@ -28,7 +28,8 @@
// Libraries
#include "CollisionShape.h"
#include "../../mathematics/mathematics.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@ -70,6 +71,20 @@ class ConeShape : public CollisionShape {
/// Private assignment operator
ConeShape& operator=(const ConeShape& shape);
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
public :
@ -93,12 +108,6 @@ class ConeShape : public CollisionShape {
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction);
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
@ -158,6 +167,14 @@ inline bool ConeShape::isEqualTo(const CollisionShape& otherCollisionShape) cons
return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight);
}
// Return true if a point is inside the collision shape
inline bool ConeShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
const decimal radiusHeight = mRadius * (-localPoint.y + mHalfHeight) /
(mHalfHeight * decimal(2.0));
return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) &&
(localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight);
}
}
#endif

47
src/collision/shapes/ConvexMeshShape.cpp
View File

@ -25,7 +25,7 @@
// Libraries
#include <complex>
#include "../../configuration.h"
#include "configuration.h"
#include "ConvexMeshShape.h"
using namespace reactphysics3d;
@ -35,10 +35,9 @@ using namespace reactphysics3d;
ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride,
decimal margin)
: CollisionShape(CONVEX_MESH, margin), mNbVertices(nbVertices), mMinBounds(0, 0, 0),
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false), mCachedSupportVertex(0) {
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
assert(nbVertices > 0);
assert(stride > 0);
assert(margin > decimal(0.0));
const unsigned char* vertexPointer = (const unsigned char*) arrayVertices;
@ -58,8 +57,8 @@ ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices,
/// the addVertex() method.
ConvexMeshShape::ConvexMeshShape(decimal margin)
: CollisionShape(CONVEX_MESH, margin), mNbVertices(0), mMinBounds(0, 0, 0),
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false), mCachedSupportVertex(0) {
assert(margin > decimal(0.0));
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
}
// Private copy-constructor
@ -67,8 +66,7 @@ ConvexMeshShape::ConvexMeshShape(const ConvexMeshShape& shape)
: CollisionShape(shape), mVertices(shape.mVertices), mNbVertices(shape.mNbVertices),
mMinBounds(shape.mMinBounds), mMaxBounds(shape.mMaxBounds),
mIsEdgesInformationUsed(shape.mIsEdgesInformationUsed),
mEdgesAdjacencyList(shape.mEdgesAdjacencyList),
mCachedSupportVertex(shape.mCachedSupportVertex) {
mEdgesAdjacencyList(shape.mEdgesAdjacencyList) {
assert(mNbVertices == mVertices.size());
}
@ -79,10 +77,11 @@ ConvexMeshShape::~ConvexMeshShape() {
}
// Return a local support point in a given direction with the object margin
Vector3 ConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction) {
Vector3 ConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
// Get the support point without the margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction);
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
// Get the unit direction vector
Vector3 unitDirection = direction;
@ -103,16 +102,24 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction
/// it as a start in a hill-climbing (local search) process to find the new support vertex which
/// will be in most of the cases very close to the previous one. Using hill-climbing, this method
/// runs in almost constant time.
Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
assert(mNbVertices == mVertices.size());
assert(cachedCollisionData != NULL);
// Allocate memory for the cached collision data if not allocated yet
if ((*cachedCollisionData) == NULL) {
*cachedCollisionData = (int*) malloc(sizeof(int));
*((int*)(*cachedCollisionData)) = 0;
}
// If the edges information is used to speed up the collision detection
if (mIsEdgesInformationUsed) {
assert(mEdgesAdjacencyList.size() == mNbVertices);
uint maxVertex = mCachedSupportVertex;
uint maxVertex = *((int*)(*cachedCollisionData));
decimal maxDotProduct = direction.dot(mVertices[maxVertex]);
bool isOptimal;
@ -142,21 +149,21 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
} while(!isOptimal);
// Cache the support vertex
mCachedSupportVertex = maxVertex;
*((int*)(*cachedCollisionData)) = maxVertex;
// Return the support vertex
return mVertices[maxVertex];
}
else { // If the edges information is not used
decimal maxDotProduct = DECIMAL_SMALLEST;
double maxDotProduct = DECIMAL_SMALLEST;
uint indexMaxDotProduct = 0;
// For each vertex of the mesh
for (uint i=0; i<mNbVertices; i++) {
// Compute the dot product of the current vertex
decimal dotProduct = direction.dot(mVertices[i]);
double dotProduct = direction.dot(mVertices[i]);
// If the current dot product is larger than the maximum one
if (dotProduct > maxDotProduct) {
@ -204,11 +211,7 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const
if (mNbVertices != otherShape.mNbVertices) return false;
// If edges information is used, it means that a collison shape object will store
// cached data (previous support vertex) and therefore, we should not reuse the shape
// for another body. Therefore, we consider that all convex mesh shape using edges
// information are different.
if (mIsEdgesInformationUsed) return false;
if (mIsEdgesInformationUsed != otherShape.mIsEdgesInformationUsed) return false;
if (mEdgesAdjacencyList.size() != otherShape.mEdgesAdjacencyList.size()) return false;
@ -226,3 +229,9 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const
return true;
}
// Raycast method with feedback information
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
ray, proxyShape, raycastInfo);
}

41
src/collision/shapes/ConvexMeshShape.h
View File

@ -28,7 +28,9 @@
// Libraries
#include "CollisionShape.h"
#include "../../mathematics/mathematics.h"
#include "engine/CollisionWorld.h"
#include "mathematics/mathematics.h"
#include "collision/narrowphase/GJK/GJKAlgorithm.h"
#include <vector>
#include <set>
#include <map>
@ -36,6 +38,9 @@
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Declaration
class CollisionWorld;
// Class ConvexMeshShape
/**
* This class represents a convex mesh shape. In order to create a convex mesh shape, you
@ -77,9 +82,6 @@ class ConvexMeshShape : public CollisionShape {
/// Adjacency list representing the edges of the mesh
std::map<uint, std::set<uint> > mEdgesAdjacencyList;
/// Cached support vertex index (previous support vertex)
uint mCachedSupportVertex;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -91,6 +93,20 @@ class ConvexMeshShape : public CollisionShape {
/// Recompute the bounds of the mesh
void recalculateBounds();
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin.
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
public :
// -------------------- Methods -------------------- //
@ -111,19 +127,13 @@ class ConvexMeshShape : public CollisionShape {
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
/// Return a local support point in a given direction without the object margin.
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction);
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Return the local inertia tensor of the collision shape.
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
/// Test equality between two cone shapes
/// Test equality between two collision shapes
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
/// Add a vertex into the convex mesh
@ -222,6 +232,15 @@ inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) {
mIsEdgesInformationUsed = isEdgesUsed;
}
// Return true if a point is inside the collision shape
inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
ProxyShape* proxyShape) const {
// Use the GJK algorithm to test if the point is inside the convex mesh
return proxyShape->mBody->mWorld.mCollisionDetection.
mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape);
}
}
#endif

182
src/collision/shapes/CylinderShape.cpp
View File

@ -25,7 +25,8 @@
// Libraries
#include "CylinderShape.h"
#include "../../configuration.h"
#include "collision/ProxyShape.h"
#include "configuration.h"
using namespace reactphysics3d;
@ -35,7 +36,6 @@ CylinderShape::CylinderShape(decimal radius, decimal height, decimal margin)
mHalfHeight(height/decimal(2.0)) {
assert(radius > decimal(0.0));
assert(height > decimal(0.0));
assert(margin > decimal(0.0));
}
// Private copy-constructor
@ -50,10 +50,11 @@ CylinderShape::~CylinderShape() {
}
// Return a local support point in a given direction with the object margin
Vector3 CylinderShape::getLocalSupportPointWithMargin(const Vector3& direction) {
Vector3 CylinderShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
// Compute the support point without the margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction);
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, NULL);
// Add the margin to the support point
Vector3 unitVec(0.0, 1.0, 0.0);
@ -66,7 +67,8 @@ Vector3 CylinderShape::getLocalSupportPointWithMargin(const Vector3& direction)
}
// Return a local support point in a given direction without the object margin
Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
Vector3 supportPoint(0.0, 0.0, 0.0);
decimal uDotv = direction.y;
@ -85,3 +87,173 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
return supportPoint;
}
// Raycast method with feedback information
/// Algorithm based on the one described at page 194 in Real-ime Collision Detection by
/// Morgan Kaufmann.
bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
// Transform the ray direction and origin in local-space coordinates
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
const Vector3 pointA = worldToLocalTransform * ray.point1;
const Vector3 pointB = worldToLocalTransform * ray.point2;
const Vector3 n = pointB - pointA;
const decimal epsilon = decimal(0.01);
Vector3 p(decimal(0), -mHalfHeight, decimal(0));
Vector3 q(decimal(0), mHalfHeight, decimal(0));
Vector3 d = q - p;
Vector3 m = pointA - p;
decimal t;
decimal mDotD = m.dot(d);
decimal nDotD = n.dot(d);
decimal dDotD = d.dot(d);
// Test if the segment is outside the cylinder
if (mDotD < decimal(0.0) && mDotD + nDotD < decimal(0.0)) return false;
if (mDotD > dDotD && mDotD + nDotD > dDotD) return false;
decimal nDotN = n.dot(n);
decimal mDotN = m.dot(n);
decimal a = dDotD * nDotN - nDotD * nDotD;
decimal k = m.dot(m) - mRadius * mRadius;
decimal c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the cylinder axis
if (std::abs(a) < epsilon) {
// If the origin is outside the surface of the cylinder, we return no hit
if (c > decimal(0.0)) return false;
// Here we know that the segment intersect an endcap of the cylinder
// If the ray intersects with the "p" endcap of the cylinder
if (mDotD < decimal(0.0)) {
t = -mDotN / nDotN;
// If the intersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
Vector3 normalDirection(0, decimal(-1), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
return true;
}
else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder
t = (nDotD - mDotN) / nDotN;
// If the intersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
return true;
}
else { // If the origin is inside the cylinder, we return no hit
return false;
}
}
decimal b = dDotD * mDotN - nDotD * mDotD;
decimal discriminant = b * b - a * c;
// If the discriminant is negative, no real roots and therfore, no hit
if (discriminant < decimal(0.0)) return false;
// Compute the smallest root (first intersection along the ray)
decimal t0 = t = (-b - std::sqrt(discriminant)) / a;
// If the intersection is outside the cylinder on "p" endcap side
decimal value = mDotD + t * nDotD;
if (value < decimal(0.0)) {
// If the ray is pointing away from the "p" endcap, we return no hit
if (nDotD <= decimal(0.0)) return false;
// Compute the intersection against the "p" endcap (intersection agains whole plane)
t = -mDotD / nDotD;
// Keep the intersection if the it is inside the cylinder radius
if (k + t * (decimal(2.0) * mDotN + t) > decimal(0.0)) return false;
// If the intersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
Vector3 normalDirection(0, decimal(-1.0), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
return true;
}
else if (value > dDotD) { // If the intersection is outside the cylinder on the "q" side
// If the ray is pointing away from the "q" endcap, we return no hit
if (nDotD >= decimal(0.0)) return false;
// Compute the intersection against the "q" endcap (intersection against whole plane)
t = (dDotD - mDotD) / nDotD;
// Keep the intersection if it is inside the cylinder radius
if (k + dDotD - decimal(2.0) * mDotD + t * (decimal(2.0) * (mDotN - nDotD) + t) >
decimal(0.0)) return false;
// If the intersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
return true;
}
t = t0;
// If the intersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
Vector3 v = localHitPoint - p;
Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
Vector3 normalDirection = (localHitPoint - (p + w)).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
return true;
}

29
src/collision/shapes/CylinderShape.h
View File

@ -28,7 +28,8 @@
// Libraries
#include "CollisionShape.h"
#include "../../mathematics/mathematics.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
/// ReactPhysics3D namespace
@ -68,6 +69,20 @@ class CylinderShape : public CollisionShape {
/// Private assignment operator
CylinderShape& operator=(const CylinderShape& shape);
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
public :
// -------------------- Methods -------------------- //
@ -90,12 +105,6 @@ class CylinderShape : public CollisionShape {
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction);
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
@ -155,6 +164,12 @@ inline bool CylinderShape::isEqualTo(const CollisionShape& otherCollisionShape)
return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight);
}
// Return true if a point is inside the collision shape
inline bool CylinderShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const{
return ((localPoint.x * localPoint.x + localPoint.z * localPoint.z) < mRadius * mRadius &&
localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight);
}
}
#endif

52
src/collision/shapes/SphereShape.cpp
View File

@ -25,7 +25,8 @@
// Libraries
#include "SphereShape.h"
#include "../../configuration.h"
#include "collision/ProxyShape.h"
#include "configuration.h"
#include <cassert>
using namespace reactphysics3d;
@ -45,3 +46,52 @@ SphereShape::SphereShape(const SphereShape& shape)
SphereShape::~SphereShape() {
}
// Raycast method with feedback information
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
// We perform the intersection test in world-space
const Vector3 sphereCenter = proxyShape->getLocalToWorldTransform().getPosition();
const Vector3 m = ray.point1 - sphereCenter;
decimal c = m.dot(m) - mRadius * mRadius;
// If the origin of the ray is inside the sphere, we return no intersection
if (c < decimal(0.0)) return false;
const Vector3 rayDirection = ray.point2 - ray.point1;
decimal b = m.dot(rayDirection);
// If the origin of the ray is outside the sphere and the ray
// is pointing away from the sphere, there is no intersection
if (b > decimal(0.0)) return false;
decimal raySquareLength = rayDirection.lengthSquare();
// Compute the discriminant of the quadratic equation
decimal discriminant = b * b - raySquareLength * c;
// If the discriminant is negative or the ray length is very small, there is no intersection
if (discriminant < decimal(0.0) || raySquareLength < MACHINE_EPSILON) return false;
// Compute the solution "t" closest to the origin
decimal t = -b - std::sqrt(discriminant);
assert(t >= decimal(0.0));
// If the hit point is withing the segment ray fraction
if (t < ray.maxFraction * raySquareLength) {
// Compute the intersection information
t /= raySquareLength;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = ray.point1 + t * rayDirection;
raycastInfo.worldNormal = (raycastInfo.worldPoint - sphereCenter).getUnit();
return true;
}
return false;
}

38
src/collision/shapes/SphereShape.h
View File

@ -28,7 +28,8 @@
// Libraries
#include "CollisionShape.h"
#include "../../mathematics/mathematics.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
@ -58,6 +59,20 @@ class SphereShape : public CollisionShape {
/// Private assignment operator
SphereShape& operator=(const SphereShape& shape);
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
public :
// -------------------- Methods -------------------- //
@ -77,12 +92,6 @@ class SphereShape : public CollisionShape {
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction);
/// Return the local bounds of the shape in x, y and z directions.
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
@ -90,7 +99,7 @@ class SphereShape : public CollisionShape {
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
/// Update the AABB of a body using its collision shape
virtual void updateAABB(AABB& aabb, const Transform& transform);
virtual void computeAABB(AABB& aabb, const Transform& transform);
/// Test equality between two sphere shapes
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
@ -112,7 +121,8 @@ inline size_t SphereShape::getSizeInBytes() const {
}
// Return a local support point in a given direction with the object margin
inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direction) {
inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
// If the direction vector is not the zero vector
if (direction.lengthSquare() >= MACHINE_EPSILON * MACHINE_EPSILON) {
@ -127,7 +137,8 @@ inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direct
}
// Return a local support point in a given direction without the object margin
inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
// Return the center of the sphere (the radius is taken into account in the object margin)
return Vector3(0.0, 0.0, 0.0);
@ -157,7 +168,7 @@ inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal ma
}
// Update the AABB of a body using its collision shape
inline void SphereShape::updateAABB(AABB& aabb, const Transform& transform) {
inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) {
// Get the local extents in x,y and z direction
Vector3 extents(mRadius, mRadius, mRadius);
@ -173,6 +184,11 @@ inline bool SphereShape::isEqualTo(const CollisionShape& otherCollisionShape) co
return (mRadius == otherShape.mRadius);
}
// Return true if a point is inside the collision shape
inline bool SphereShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return (localPoint.lengthSquare() < mRadius * mRadius);
}
}
#endif

10
src/configuration.h
View File

@ -121,6 +121,16 @@ const decimal DEFAULT_SLEEP_LINEAR_VELOCITY = decimal(0.02);
/// might enter sleeping mode
const decimal DEFAULT_SLEEP_ANGULAR_VELOCITY = decimal(3.0 * (PI / 180.0));
/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
/// inflated with a constant gap to allow the collision shape to move a little bit
/// without triggering a large modification of the tree which can be costly
const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1);
/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are
/// also inflated in direction of the linear motion of the body by mutliplying the
/// followin constant with the linear velocity and the elapsed time between two frames.
const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7);
}
#endif

12
src/constraint/BallAndSocketJoint.cpp
View File

@ -25,7 +25,7 @@
// Libraries
#include "BallAndSocketJoint.h"
#include "../engine/ConstraintSolver.h"
#include "engine/ConstraintSolver.h"
using namespace reactphysics3d;
@ -53,9 +53,9 @@ void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintS
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->getTransform().getPosition();
const Vector3& x2 = mBody2->getTransform().getPosition();
// Get the bodies center of mass and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld;
const Vector3& x2 = mBody2->mCenterOfMassWorld;
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
@ -164,7 +164,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
// do not execute this method
if (mPositionCorrectionTechnique != NON_LINEAR_GAUSS_SEIDEL) return;
// Get the bodies positions and orientations
// Get the bodies center of mass and orientations
Vector3& x1 = constraintSolverData.positions[mIndexBody1];
Vector3& x2 = constraintSolverData.positions[mIndexBody2];
Quaternion& q1 = constraintSolverData.orientations[mIndexBody1];
@ -214,7 +214,7 @@ void BallAndSocketJoint::solvePositionConstraint(const ConstraintSolverData& con
const Vector3 v1 = inverseMassBody1 * linearImpulseBody1;
const Vector3 w1 = mI1 * angularImpulseBody1;
// Update the body position/orientation of body 1
// Update the body center of mass and orientation of body 1
x1 += v1;
q1 += Quaternion(0, w1) * q1 * decimal(0.5);
q1.normalize();

2
src/constraint/BallAndSocketJoint.h
View File

@ -28,7 +28,7 @@
// Libraries
#include "Joint.h"
#include "../mathematics/mathematics.h"
#include "mathematics/mathematics.h"
namespace reactphysics3d {

11
src/constraint/ContactPoint.cpp
View File

@ -25,19 +25,24 @@
// Libraries
#include "ContactPoint.h"
#include "collision/ProxyShape.h"
using namespace reactphysics3d;
using namespace std;
// Constructor
ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
: mBody1(contactInfo.body1), mBody2(contactInfo.body2),
: mBody1(contactInfo.shape1->getBody()), mBody2(contactInfo.shape2->getBody()),
mNormal(contactInfo.normal),
mPenetrationDepth(contactInfo.penetrationDepth),
mLocalPointOnBody1(contactInfo.localPoint1),
mLocalPointOnBody2(contactInfo.localPoint2),
mWorldPointOnBody1(contactInfo.body1->getTransform() * contactInfo.localPoint1),
mWorldPointOnBody2(contactInfo.body2->getTransform() * contactInfo.localPoint2),
mWorldPointOnBody1(contactInfo.shape1->getBody()->getTransform() *
contactInfo.shape1->getLocalToBodyTransform() *
contactInfo.localPoint1),
mWorldPointOnBody2(contactInfo.shape2->getBody()->getTransform() *
contactInfo.shape2->getLocalToBodyTransform() *
contactInfo.localPoint2),
mIsRestingContact(false) {
mFrictionVectors[0] = Vector3(0, 0, 0);

38
src/constraint/ContactPoint.h
View File

@ -27,10 +27,10 @@
#define REACTPHYSICS3D_CONTACT_POINT_H
// Libraries
#include "../body/RigidBody.h"
#include "../configuration.h"
#include "../mathematics/mathematics.h"
#include "../configuration.h"
#include "body/CollisionBody.h"
#include "configuration.h"
#include "mathematics/mathematics.h"
#include "configuration.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@ -58,11 +58,11 @@ struct ContactPointInfo {
// -------------------- Attributes -------------------- //
/// First rigid body of the constraint
RigidBody* body1;
/// First proxy collision shape of the contact
ProxyShape* shape1;
/// Second rigid body of the constraint
RigidBody* body2;
/// Second proxy collision shape of the contact
ProxyShape* shape2;
/// Normal vector the the collision contact in world space
const Vector3 normal;
@ -79,10 +79,12 @@ struct ContactPointInfo {
// -------------------- Methods -------------------- //
/// Constructor
ContactPointInfo(const Vector3& normal, decimal penetrationDepth,
const Vector3& localPoint1, const Vector3& localPoint2)
: normal(normal), penetrationDepth(penetrationDepth),
localPoint1(localPoint1), localPoint2(localPoint2) {
ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const Vector3& normal,
decimal penetrationDepth, const Vector3& localPoint1,
const Vector3& localPoint2)
: shape1(proxyShape1), shape2(proxyShape2), normal(normal),
penetrationDepth(penetrationDepth), localPoint1(localPoint1),
localPoint2(localPoint2) {
}
};
@ -99,10 +101,10 @@ class ContactPoint {
// -------------------- Attributes -------------------- //
/// First rigid body of the contact
RigidBody* mBody1;
CollisionBody* mBody1;
/// Second rigid body of the contact
RigidBody* mBody2;
CollisionBody* mBody2;
/// Normal vector of the contact (From body1 toward body2) in world space
const Vector3 mNormal;
@ -156,10 +158,10 @@ class ContactPoint {
~ContactPoint();
/// Return the reference to the body 1
RigidBody* const getBody1() const;
CollisionBody* const getBody1() const;
/// Return the reference to the body 2
RigidBody* const getBody2() const;
CollisionBody* const getBody2() const;
/// Return the normal vector of the contact
Vector3 getNormal() const;
@ -229,12 +231,12 @@ class ContactPoint {
};
// Return the reference to the body 1
inline RigidBody* const ContactPoint::getBody1() const {
inline CollisionBody* const ContactPoint::getBody1() const {
return mBody1;
}
// Return the reference to the body 2
inline RigidBody* const ContactPoint::getBody2() const {
inline CollisionBody* const ContactPoint::getBody2() const {
return mBody2;
}

6
src/constraint/FixedJoint.cpp
View File

@ -25,7 +25,7 @@
// Libraries
#include "FixedJoint.h"
#include "../engine/ConstraintSolver.h"
#include "engine/ConstraintSolver.h"
using namespace reactphysics3d;
@ -62,8 +62,8 @@ void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->getTransform().getPosition();
const Vector3& x2 = mBody2->getTransform().getPosition();
const Vector3& x1 = mBody1->mCenterOfMassWorld;
const Vector3& x2 = mBody2->mCenterOfMassWorld;
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();

2
src/constraint/FixedJoint.h
View File

@ -28,7 +28,7 @@
// Libraries
#include "Joint.h"
#include "../mathematics/mathematics.h"
#include "mathematics/mathematics.h"
namespace reactphysics3d {

6
src/constraint/HingeJoint.cpp
View File

@ -25,7 +25,7 @@
// Libraries
#include "HingeJoint.h"
#include "../engine/ConstraintSolver.h"
#include "engine/ConstraintSolver.h"
#include <cmath>
using namespace reactphysics3d;
@ -77,8 +77,8 @@ void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDat
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->getTransform().getPosition();
const Vector3& x2 = mBody2->getTransform().getPosition();
const Vector3& x1 = mBody1->mCenterOfMassWorld;
const Vector3& x2 = mBody2->mCenterOfMassWorld;
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();

2
src/constraint/HingeJoint.h
View File

@ -28,7 +28,7 @@
// Libraries
#include "Joint.h"
#include "../mathematics/mathematics.h"
#include "mathematics/mathematics.h"
namespace reactphysics3d {

6
src/constraint/Joint.h
View File

@ -27,9 +27,9 @@
#define REACTPHYSICS3D_CONSTRAINT_H
// Libraries
#include "../configuration.h"
#include "../body/RigidBody.h"
#include "../mathematics/mathematics.h"
#include "configuration.h"
#include "body/RigidBody.h"
#include "mathematics/mathematics.h"
// ReactPhysics3D namespace
namespace reactphysics3d {

5
src/constraint/SliderJoint.cpp
View File

@ -76,8 +76,8 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->getTransform().getPosition();
const Vector3& x2 = mBody2->getTransform().getPosition();
const Vector3& x1 = mBody1->mCenterOfMassWorld;
const Vector3& x2 = mBody2->mCenterOfMassWorld;
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
@ -679,6 +679,7 @@ void SliderJoint::enableMotor(bool isMotorEnabled) {
}
// Return the current translation value of the joint
// TODO : Check if we need to compare rigid body position or center of mass here
decimal SliderJoint::getTranslation() const {
// Get the bodies positions and orientations

4
src/constraint/SliderJoint.h
View File

@ -27,8 +27,8 @@
#define REACTPHYSICS3D_SLIDER_JOINT_H
// Libraries
#include "../mathematics/mathematics.h"
#include "../engine/ConstraintSolver.h"
#include "mathematics/mathematics.h"
#include "engine/ConstraintSolver.h"
namespace reactphysics3d {

41
src/engine/CollisionWorld.cpp
View File

@ -33,7 +33,8 @@ using namespace std;
// Constructor
CollisionWorld::CollisionWorld()
: mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0) {
: mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0),
mEventListener(NULL) {
}
@ -43,33 +44,8 @@ CollisionWorld::~CollisionWorld() {
assert(mBodies.empty());
}
// Notify the world about a new broad-phase overlapping pair
void CollisionWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) {
// TODO : Implement this method
}
// Notify the world about a removed broad-phase overlapping pair
void CollisionWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedPair) {
// TODO : Implement this method
}
// Notify the world about a new narrow-phase contact
void CollisionWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
const ContactPointInfo* contactInfo) {
// TODO : Implement this method
}
// Update the overlapping pair
inline void CollisionWorld::updateOverlappingPair(const BroadPhasePair* pair) {
}
// Create a collision body and add it to the world
CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform,
CollisionShape* collisionShape) {
CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
// Get the next available body ID
bodyindex bodyID = computeNextAvailableBodyID();
@ -79,16 +55,13 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform,
// Create the collision body
CollisionBody* collisionBody = new (mMemoryAllocator.allocate(sizeof(CollisionBody)))
CollisionBody(transform, collisionShape, bodyID);
CollisionBody(transform, *this, bodyID);
assert(collisionBody != NULL);
// Add the collision body to the world
mBodies.insert(collisionBody);
// Add the collision body to the collision detection
mCollisionDetection.addBody(collisionBody);
// Return the pointer to the rigid body
return collisionBody;
}
@ -96,8 +69,8 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform,
// Destroy a collision body
void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
// Remove the body from the collision detection
mCollisionDetection.removeBody(collisionBody);
// Remove all the collision shapes of the body
collisionBody->removeAllCollisionShapes();
// Add the body ID to the list of free IDs
mFreeBodiesIDs.push_back(collisionBody->getID());
@ -129,7 +102,7 @@ bodyindex CollisionWorld::computeNextAvailableBodyID() {
return bodyID;
}
// Create a new collision shape.
// Create a new collision shape in the world.
/// First, this methods checks that the new collision shape does not exist yet in the
/// world. If it already exists, we do not allocate memory for a new one but instead
/// we reuse the existing one. The goal is to only allocate memory for a single

55
src/engine/CollisionWorld.h
View File

@ -31,14 +31,15 @@
#include <set>
#include <list>
#include <algorithm>
#include "../mathematics/mathematics.h"
#include "mathematics/mathematics.h"
#include "Profiler.h"
#include "../body/CollisionBody.h"
#include "body/CollisionBody.h"
#include "collision/RaycastInfo.h"
#include "OverlappingPair.h"
#include "../collision/CollisionDetection.h"
#include "../constraint/Joint.h"
#include "../constraint/ContactPoint.h"
#include "../memory/MemoryAllocator.h"
#include "collision/CollisionDetection.h"
#include "constraint/Joint.h"
#include "constraint/ContactPoint.h"
#include "memory/MemoryAllocator.h"
#include "EventListener.h"
/// Namespace reactphysics3d
@ -65,9 +66,6 @@ class CollisionWorld {
/// All the collision shapes of the world
std::list<CollisionShape*> mCollisionShapes;
/// Broad-phase overlapping pairs of bodies
std::map<bodyindexpair, OverlappingPair*> mOverlappingPairs;
/// Current body ID
bodyindex mCurrentBodyID;
@ -77,6 +75,9 @@ class CollisionWorld {
/// Memory allocator
MemoryAllocator mMemoryAllocator;
/// Pointer to an event listener object
EventListener* mEventListener;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -85,28 +86,15 @@ class CollisionWorld {
/// Private assignment operator
CollisionWorld& operator=(const CollisionWorld& world);
/// Notify the world about a new broad-phase overlapping pair
virtual void notifyAddedOverlappingPair(const BroadPhasePair* addedPair);
/// Notify the world about a removed broad-phase overlapping pair
virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair);
/// Notify the world about a new narrow-phase contact
virtual void notifyNewContact(const BroadPhasePair* pair,
const ContactPointInfo* contactInfo);
/// Update the overlapping pair
virtual void updateOverlappingPair(const BroadPhasePair* pair);
/// Return the next available body ID
bodyindex computeNextAvailableBodyID();
/// Create a new collision shape.
CollisionShape* createCollisionShape(const CollisionShape& collisionShape);
/// Remove a collision shape.
void removeCollisionShape(CollisionShape* collisionShape);
/// Create a new collision shape in the world.
CollisionShape* createCollisionShape(const CollisionShape& collisionShape);
public :
// -------------------- Methods -------------------- //
@ -124,15 +112,20 @@ class CollisionWorld {
std::set<CollisionBody*>::iterator getBodiesEndIterator();
/// Create a collision body
CollisionBody* createCollisionBody(const Transform& transform,
CollisionShape* collisionShape);
CollisionBody* createCollisionBody(const Transform& transform);
/// Destroy a collision body
void destroyCollisionBody(CollisionBody* collisionBody);
// -------------------- Friends -------------------- //
/// Ray cast method
void raycast(const Ray& ray, RaycastCallback* raycastCallback) const;
// -------------------- Friendship -------------------- //
friend class CollisionDetection;
friend class CollisionBody;
friend class RigidBody;
friend class ConvexMeshShape;
};
// Return an iterator to the beginning of the bodies of the physics world
@ -145,6 +138,12 @@ inline std::set<CollisionBody*>::iterator CollisionWorld::getBodiesEndIterator()
return mBodies.end();
}
// Ray cast method
inline void CollisionWorld::raycast(const Ray& ray,
RaycastCallback* raycastCallback) const {
mCollisionDetection.raycast(raycastCallback, ray);
}
}
#endif

13
src/engine/ConstraintSolver.cpp
View File

@ -30,14 +30,9 @@
using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver(std::vector<Vector3>& positions,
std::vector<Quaternion>& orientations,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
: mLinearVelocities(NULL), mAngularVelocities(NULL), mPositions(positions),
mOrientations(orientations),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mConstraintSolverData(positions, orientations,
mapBodyToVelocityIndex){
ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
: mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) {
}
@ -51,8 +46,6 @@ void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
PROFILE("ConstraintSolver::initializeForIsland()");
assert(mLinearVelocities != NULL);
assert(mAngularVelocities != NULL);
assert(island != NULL);
assert(island->getNbBodies() > 0);
assert(island->getNbJoints() > 0);

53
src/engine/ConstraintSolver.h
View File

@ -27,9 +27,9 @@
#define REACTPHYSICS3D_CONSTRAINT_SOLVER_H
// Libraries
#include "../configuration.h"
#include "configuration.h"
#include "mathematics/mathematics.h"
#include "../constraint/Joint.h"
#include "constraint/Joint.h"
#include "Island.h"
#include <map>
#include <set>
@ -55,10 +55,10 @@ struct ConstraintSolverData {
Vector3* angularVelocities;
/// Reference to the bodies positions
std::vector<Vector3>& positions;
Vector3* positions;
/// Reference to the bodies orientations
std::vector<Quaternion>& orientations;
Quaternion* orientations;
/// Reference to the map that associates rigid body to their index
/// in the constrained velocities array
@ -68,12 +68,9 @@ struct ConstraintSolverData {
bool isWarmStartingActive;
/// Constructor
ConstraintSolverData(std::vector<Vector3>& refPositions,
std::vector<Quaternion>& refOrientations,
const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
:linearVelocities(NULL),
angularVelocities(NULL),
positions(refPositions), orientations(refOrientations),
ConstraintSolverData(const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
:linearVelocities(NULL), angularVelocities(NULL),
positions(NULL), orientations(NULL),
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
}
@ -155,20 +152,6 @@ class ConstraintSolver {
// -------------------- Attributes -------------------- //
/// Array of constrained linear velocities (state of the linear velocities
/// after solving the constraints)
Vector3* mLinearVelocities;
/// Array of constrained angular velocities (state of the angular velocities
/// after solving the constraints)
Vector3* mAngularVelocities;
/// Reference to the array of bodies positions (for position error correction)
std::vector<Vector3>& mPositions;
/// Reference to the array of bodies orientations (for position error correction)
std::vector<Quaternion>& mOrientations;
/// Reference to the map that associates rigid body to their index in
/// the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
@ -187,8 +170,7 @@ class ConstraintSolver {
// -------------------- Methods -------------------- //
/// Constructor
ConstraintSolver(std::vector<Vector3>& positions, std::vector<Quaternion>& orientations,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
/// Destructor
~ConstraintSolver();
@ -211,6 +193,10 @@ class ConstraintSolver {
/// Set the constrained velocities arrays
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities);
/// Set the constrained positions/orientations arrays
void setConstrainedPositionsArrays(Vector3* constrainedPositions,
Quaternion* constrainedOrientations);
};
// Set the constrained velocities arrays
@ -218,10 +204,17 @@ inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constraine
Vector3* constrainedAngularVelocities) {
assert(constrainedLinearVelocities != NULL);
assert(constrainedAngularVelocities != NULL);
mLinearVelocities = constrainedLinearVelocities;
mAngularVelocities = constrainedAngularVelocities;
mConstraintSolverData.linearVelocities = mLinearVelocities;
mConstraintSolverData.angularVelocities = mAngularVelocities;
mConstraintSolverData.linearVelocities = constrainedLinearVelocities;
mConstraintSolverData.angularVelocities = constrainedAngularVelocities;
}
// Set the constrained positions/orientations arrays
inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrainedPositions,
Quaternion* constrainedOrientations) {
assert(constrainedPositions != NULL);
assert(constrainedOrientations != NULL);
mConstraintSolverData.positions = constrainedPositions;
mConstraintSolverData.orientations = constrainedOrientations;
}
}

7
src/engine/ContactManifold.h
View File

@ -28,9 +28,9 @@
// Libraries
#include <vector>
#include "../body/CollisionBody.h"
#include "../constraint/ContactPoint.h"
#include "../memory/MemoryAllocator.h"
#include "body/CollisionBody.h"
#include "constraint/ContactPoint.h"
#include "memory/MemoryAllocator.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@ -210,6 +210,7 @@ class ContactManifold {
friend class DynamicsWorld;
friend class Island;
friend class CollisionBody;
};
// Return a pointer to the first body of the contact manifold

12
src/engine/ContactSolver.cpp
View File

@ -26,7 +26,7 @@
// Libraries
#include "ContactSolver.h"
#include "DynamicsWorld.h"
#include "../body/RigidBody.h"
#include "body/RigidBody.h"
#include "Profiler.h"
#include <limits>
@ -83,12 +83,14 @@ void ContactSolver::initializeForIsland(decimal dt, Island* island) {
assert(externalManifold->getNbContactPoints() > 0);
// Get the two bodies of the contact
RigidBody* body1 = externalManifold->getContactPoint(0)->getBody1();
RigidBody* body2 = externalManifold->getContactPoint(0)->getBody2();
RigidBody* body1 = dynamic_cast<RigidBody*>(externalManifold->getContactPoint(0)->getBody1());
RigidBody* body2 = dynamic_cast<RigidBody*>(externalManifold->getContactPoint(0)->getBody2());
assert(body1 != NULL);
assert(body2 != NULL);
// Get the position of the two bodies
Vector3 x1 = body1->getTransform().getPosition();
Vector3 x2 = body2->getTransform().getPosition();
const Vector3& x1 = body1->mCenterOfMassWorld;
const Vector3& x2 = body2->mCenterOfMassWorld;
// Initialize the internal contact manifold structure using the external
// contact manifold

6
src/engine/ContactSolver.h
View File

@ -27,9 +27,9 @@
#define REACTPHYSICS3D_CONTACT_SOLVER_H
// Libraries
#include "../constraint/ContactPoint.h"
#include "../configuration.h"
#include "../constraint/Joint.h"
#include "constraint/ContactPoint.h"
#include "configuration.h"
#include "constraint/Joint.h"
#include "ContactManifold.h"
#include "Island.h"
#include "Impulse.h"

249
src/engine/DynamicsWorld.cpp
View File

@ -34,13 +34,15 @@
using namespace reactphysics3d;
using namespace std;
// TODO : Check if we really need to store the contact manifolds also in mContactManifolds.
// Constructor
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityEnabled(true),
mConstrainedLinearVelocities(NULL), mConstrainedAngularVelocities(NULL),
mConstrainedPositions(NULL), mConstrainedOrientations(NULL),
mContactSolver(mMapBodyToConstrainedVelocityIndex),
mConstraintSolver(mConstrainedPositions, mConstrainedOrientations,
mMapBodyToConstrainedVelocityIndex),
mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
mIsSleepingEnabled(SPLEEPING_ENABLED), mSplitLinearVelocities(NULL),
@ -48,21 +50,13 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_
mIslands(NULL), mNbBodiesCapacity(0),
mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY),
mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY),
mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP), mEventListener(NULL) {
mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) {
}
// Destructor
DynamicsWorld::~DynamicsWorld() {
// Delete the remaining overlapping pairs
map<std::pair<bodyindex, bodyindex>, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); it++) {
// Delete the overlapping pair
(*it).second->OverlappingPair::~OverlappingPair();
mMemoryAllocator.release((*it).second, sizeof(OverlappingPair));
}
// Release the memory allocated for the islands
for (uint i=0; i<mNbIslands; i++) {
@ -82,6 +76,8 @@ DynamicsWorld::~DynamicsWorld() {
delete[] mSplitAngularVelocities;
delete[] mConstrainedLinearVelocities;
delete[] mConstrainedAngularVelocities;
delete[] mConstrainedPositions;
delete[] mConstrainedOrientations;
}
#ifdef IS_PROFILING_ACTIVE
@ -113,9 +109,6 @@ void DynamicsWorld::update() {
// While the time accumulator is not empty
while(mTimer.isPossibleToTakeStep()) {
// Remove all contact manifolds
mContactManifolds.clear();
// Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies();
@ -128,9 +121,6 @@ void DynamicsWorld::update() {
// Integrate the velocities
integrateRigidBodiesVelocities();
// Reset the movement boolean variable of each body to false
resetBodiesMovementVariable();
// Update the timer
mTimer.nextStep();
@ -143,10 +133,10 @@ void DynamicsWorld::update() {
// Solve the position correction for constraints
solvePositionCorrection();
if (mIsSleepingEnabled) updateSleepingBodies();
// Update the state (positions and velocities) of the bodies
updateBodiesState();
// Update the AABBs of the bodies
updateRigidBodiesAABB();
if (mIsSleepingEnabled) updateSleepingBodies();
}
// Reset the external force and torque applied to the bodies
@ -178,10 +168,6 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
// Update the linear and angular velocity of the body
bodies[b]->mLinearVelocity = newLinVelocity;
bodies[b]->mAngularVelocity = newAngVelocity;
// Add the split impulse velocity from Contact Solver (only used
// to update the position)
if (mContactSolver.isSplitImpulseActive()) {
@ -191,35 +177,52 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
}
// Get current position and orientation of the body
const Vector3& currentPosition = bodies[b]->getTransform().getPosition();
const Vector3& currentPosition = bodies[b]->mCenterOfMassWorld;
const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
// Compute the new position of the body
Vector3 newPosition = currentPosition + newLinVelocity * dt;
Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * dt;
// Update the new constrained position and orientation of the body
mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * dt;
mConstrainedOrientations[indexArray] = currentOrientation +
Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * dt;
// Update the Transform of the body
Transform newTransform(newPosition, newOrientation.getUnit());
bodies[b]->setTransform(newTransform);
// TODO : DELETE THIS
Vector3 newPos = mConstrainedPositions[indexArray];
Quaternion newOrientation = mConstrainedOrientations[indexArray];
}
}
}
// Update the AABBs of the bodies
void DynamicsWorld::updateRigidBodiesAABB() {
// Update the postion/orientation of the bodies
void DynamicsWorld::updateBodiesState() {
PROFILE("DynamicsWorld::updateRigidBodiesAABB()");
PROFILE("DynamicsWorld::updateBodiesState()");
// For each rigid body of the world
set<RigidBody*>::iterator it;
for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// If the body has moved
if ((*it)->mHasMoved) {
// For each body of the island
RigidBody** bodies = mIslands[islandIndex]->getBodies();
// Update the AABB of the rigid body
(*it)->updateAABB();
for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
// Update the linear and angular velocity of the body
bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index];
bodies[b]->mAngularVelocity = mConstrainedAngularVelocities[index];
// Update the position of the center of mass of the body
bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index];
// Update the orientation of the body
bodies[b]->mTransform.setOrientation(mConstrainedOrientations[index].getUnit());
// Update the transform of the body (using the new center of mass and new orientation)
bodies[b]->updateTransformWithCenterOfMass();
// Update the broad-phase state of the body
bodies[b]->updateBroadPhaseState();
}
}
}
@ -252,14 +255,19 @@ void DynamicsWorld::initVelocityArrays() {
delete[] mSplitAngularVelocities;
}
mNbBodiesCapacity = nbBodies;
// TODO : Use better memory allocation here
mSplitLinearVelocities = new Vector3[mNbBodiesCapacity];
mSplitAngularVelocities = new Vector3[mNbBodiesCapacity];
mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity];
mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity];
mConstrainedPositions = new Vector3[mNbBodiesCapacity];
mConstrainedOrientations = new Quaternion[mNbBodiesCapacity];
assert(mSplitLinearVelocities != NULL);
assert(mSplitAngularVelocities != NULL);
assert(mConstrainedLinearVelocities != NULL);
assert(mConstrainedAngularVelocities != NULL);
assert(mConstrainedPositions != NULL);
assert(mConstrainedOrientations != NULL);
}
// Reset the velocities arrays
@ -370,6 +378,8 @@ void DynamicsWorld::solveContactsAndConstraints() {
mConstrainedAngularVelocities);
mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
mConstrainedAngularVelocities);
mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions,
mConstrainedOrientations);
// ---------- Solve velocity constraints for joints and contacts ---------- //
@ -427,27 +437,9 @@ void DynamicsWorld::solvePositionCorrection() {
// Do not continue if there is no constraints
if (mJoints.empty()) return;
// ---------- Get the position/orientation of the rigid bodies ---------- //
// TODO : Use better memory allocation here
mConstrainedPositions = std::vector<Vector3>(mRigidBodies.size());
mConstrainedOrientations = std::vector<Quaternion>(mRigidBodies.size());
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// For each body of the island
RigidBody** bodies = mIslands[islandIndex]->getBodies();
for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
// Get the position/orientation of the rigid body
const Transform& transform = bodies[b]->getTransform();
mConstrainedPositions[index] = transform.getPosition();
mConstrainedOrientations[index]= transform.getOrientation();
}
// ---------- Solve the position error correction for the constraints ---------- //
// For each iteration of the position (error correction) solver
@ -456,27 +448,11 @@ void DynamicsWorld::solvePositionCorrection() {
// Solve the position constraints
mConstraintSolver.solvePositionConstraints(mIslands[islandIndex]);
}
// ---------- Update the position/orientation of the rigid bodies ---------- //
for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
// Get the new position/orientation of the body
const Vector3& newPosition = mConstrainedPositions[index];
const Quaternion& newOrientation = mConstrainedOrientations[index];
// Update the Transform of the body
Transform newTransform(newPosition, newOrientation.getUnit());
bodies[b]->setTransform(newTransform);
}
}
}
// Create a rigid body into the physics world
RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass,
const CollisionShape& collisionShape) {
RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
// Compute the body ID
bodyindex bodyID = computeNextAvailableBodyID();
@ -484,23 +460,15 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma
// Largest index cannot be used (it is used for invalid index)
assert(bodyID < std::numeric_limits<reactphysics3d::bodyindex>::max());
// Create a collision shape for the rigid body into the world
CollisionShape* newCollisionShape = createCollisionShape(collisionShape);
// Create the rigid body
RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform,
mass,
newCollisionShape,
bodyID);
*this, bodyID);
assert(rigidBody != NULL);
// Add the rigid body to the physics world
mBodies.insert(rigidBody);
mRigidBodies.insert(rigidBody);
// Add the rigid body to the collision detection
mCollisionDetection.addBody(rigidBody);
// Return the pointer to the rigid body
return rigidBody;
}
@ -508,15 +476,12 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma
// Destroy a rigid body and all the joints which it belongs
void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
// Remove the body from the collision detection
mCollisionDetection.removeBody(rigidBody);
// Remove all the collision shapes of the body
rigidBody->removeAllCollisionShapes();
// Add the body ID to the list of free IDs
mFreeBodiesIDs.push_back(rigidBody->getID());
// Remove the collision shape from the world
removeCollisionShape(rigidBody->getCollisionShape());
// Destroy all the joints in which the rigid body to be destroyed is involved
JointListElement* element;
for (element = rigidBody->mJointsList; element != NULL; element = element->next) {
@ -524,7 +489,7 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
}
// Reset the contact manifold list of the body
rigidBody->resetContactManifoldsList(mMemoryAllocator);
rigidBody->resetContactManifoldsList();
// Call the destructor of the rigid body
rigidBody->RigidBody::~RigidBody();
@ -656,29 +621,6 @@ void DynamicsWorld::addJointToBody(Joint* joint) {
joint->mBody2->mJointsList = jointListElement2;
}
// Add a contact manifold to the linked list of contact manifolds of the two bodies involed
// in the corresponding contact
void DynamicsWorld::addContactManifoldToBody(ContactManifold* contactManifold,
CollisionBody* body1, CollisionBody* body2) {
assert(contactManifold != NULL);
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
void* allocatedMemory1 = mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement1 = new (allocatedMemory1)
ContactManifoldListElement(contactManifold,
body1->mContactManifoldsList);
body1->mContactManifoldsList = listElement1;
// Add the joint at the beginning of the linked list of joints of the second body
void* allocatedMemory2 = mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement2 = new (allocatedMemory2)
ContactManifoldListElement(contactManifold,
body2->mContactManifoldsList);
body2->mContactManifoldsList = listElement2;
}
// Reset all the contact manifolds linked list of each body
void DynamicsWorld::resetContactManifoldListsOfBodies() {
@ -686,7 +628,7 @@ void DynamicsWorld::resetContactManifoldListsOfBodies() {
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
// Reset the contact manifold list of the body
(*it)->resetContactManifoldsList(mMemoryAllocator);
(*it)->resetContactManifoldsList();
}
}
@ -723,13 +665,12 @@ void DynamicsWorld::computeIslands() {
}
mNbIslands = 0;
int nbContactManifolds = 0;
// Reset all the isAlreadyInIsland variables of bodies, joints and contact manifolds
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
(*it)->mIsAlreadyInIsland = false;
}
for (std::vector<ContactManifold*>::iterator it = mContactManifolds.begin();
it != mContactManifolds.end(); ++it) {
(*it)->mIsAlreadyInIsland = false;
int nbBodyManifolds = (*it)->resetIsAlreadyInIslandAndCountManifolds();
nbContactManifolds += nbBodyManifolds;
}
for (std::set<Joint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
(*it)->mIsAlreadyInIsland = false;
@ -761,7 +702,8 @@ void DynamicsWorld::computeIslands() {
// Create the new island
void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island));
mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies,mContactManifolds.size(),
mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies,
nbContactManifolds,
mJoints.size(), mMemoryAllocator);
// While there are still some bodies to visit in the stack
@ -910,69 +852,6 @@ void DynamicsWorld::updateSleepingBodies() {
}
}
// Notify the world about a new broad-phase overlapping pair
void DynamicsWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) {
// Get the pair of body index
bodyindexpair indexPair = addedPair->getBodiesIndexPair();
// Add the pair into the set of overlapping pairs (if not there yet)
OverlappingPair* newPair = new (mMemoryAllocator.allocate(sizeof(OverlappingPair)))
OverlappingPair(addedPair->body1, addedPair->body2, mMemoryAllocator);
assert(newPair != NULL);
std::pair<map<bodyindexpair, OverlappingPair*>::iterator, bool> check =
mOverlappingPairs.insert(make_pair(indexPair, newPair));
assert(check.second);
}
// Notify the world about a removed broad-phase overlapping pair
void DynamicsWorld::notifyRemovedOverlappingPair(const BroadPhasePair* removedPair) {
// Get the pair of body index
std::pair<bodyindex, bodyindex> indexPair = removedPair->getBodiesIndexPair();
// Remove the overlapping pair from the memory allocator
mOverlappingPairs.find(indexPair)->second->OverlappingPair::~OverlappingPair();
mMemoryAllocator.release(mOverlappingPairs[indexPair], sizeof(OverlappingPair));
mOverlappingPairs.erase(indexPair);
}
// Notify the world about a new narrow-phase contact
void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
const ContactPointInfo* contactInfo) {
// Create a new contact
ContactPoint* contact = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(
*contactInfo);
assert(contact != NULL);
// Get the corresponding overlapping pair
pair<bodyindex, bodyindex> indexPair = broadPhasePair->getBodiesIndexPair();
OverlappingPair* overlappingPair = mOverlappingPairs.find(indexPair)->second;
assert(overlappingPair != NULL);
// If it is the first contact since the pair are overlapping
if (overlappingPair->getNbContactPoints() == 0) {
// Trigger a callback event
if (mEventListener != NULL) mEventListener->beginContact(*contactInfo);
}
// Add the contact to the contact cache of the corresponding overlapping pair
overlappingPair->addContact(contact);
// Add the contact manifold to the world
mContactManifolds.push_back(overlappingPair->getContactManifold());
// Add the contact manifold into the list of contact manifolds
// of the two bodies involved in the contact
addContactManifoldToBody(overlappingPair->getContactManifold(), overlappingPair->mBody1,
overlappingPair->mBody2);
// Trigger a callback event for the new contact
if (mEventListener != NULL) mEventListener->newContact(*contactInfo);
}
// Enable/Disable the sleeping technique
void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
mIsSleepingEnabled = isSleepingEnabled;

89
src/engine/DynamicsWorld.h
View File

@ -28,13 +28,13 @@
// Libraries
#include "CollisionWorld.h"
#include "../collision/CollisionDetection.h"
#include "collision/CollisionDetection.h"
#include "ContactSolver.h"
#include "ConstraintSolver.h"
#include "../body/RigidBody.h"
#include "body/RigidBody.h"
#include "Timer.h"
#include "Island.h"
#include "../configuration.h"
#include "configuration.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -72,10 +72,6 @@ class DynamicsWorld : public CollisionWorld {
/// All the rigid bodies of the physics world
std::set<RigidBody*> mRigidBodies;
/// All the contact constraints
// TODO : Remove this variable (we will use the ones in the island now)
std::vector<ContactManifold*> mContactManifolds;
/// All the joints of the world
std::set<Joint*> mJoints;
@ -100,10 +96,10 @@ class DynamicsWorld : public CollisionWorld {
Vector3* mSplitAngularVelocities;
/// Array of constrained rigid bodies position (for position error correction)
std::vector<Vector3> mConstrainedPositions;
Vector3* mConstrainedPositions;
/// Array of constrained rigid bodies orientation (for position error correction)
std::vector<Quaternion> mConstrainedOrientations;
Quaternion* mConstrainedOrientations;
/// Map body to their index in the constrained velocities array
std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
@ -130,9 +126,6 @@ class DynamicsWorld : public CollisionWorld {
/// becomes smaller than the sleep velocity.
decimal mTimeBeforeSleep;
/// Pointer to an event listener object
EventListener* mEventListener;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -172,28 +165,15 @@ class DynamicsWorld : public CollisionWorld {
/// Cleanup the constrained velocities array at each step
void cleanupConstrainedVelocitiesArray();
/// Reset the boolean movement variable of each body
void resetBodiesMovementVariable();
/// Compute the islands of awake bodies.
void computeIslands();
/// Update the postion/orientation of the bodies
void updateBodiesState();
/// Put bodies to sleep if needed.
void updateSleepingBodies();
/// Update the overlapping pair
virtual void updateOverlappingPair(const BroadPhasePair* pair);
/// Notify the world about a new broad-phase overlapping pair
virtual void notifyAddedOverlappingPair(const BroadPhasePair* addedPair);
/// Notify the world about a removed broad-phase overlapping pair
virtual void notifyRemovedOverlappingPair(const BroadPhasePair* removedPair);
/// Notify the world about a new narrow-phase contact
virtual void notifyNewContact(const BroadPhasePair* pair,
const ContactPointInfo* contactInfo);
public :
// -------------------- Methods -------------------- //
@ -230,8 +210,7 @@ class DynamicsWorld : public CollisionWorld {
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// Create a rigid body into the physics world.
RigidBody* createRigidBody(const Transform& transform, decimal mass,
const CollisionShape& collisionShape);
RigidBody* createRigidBody(const Transform& transform);
/// Destroy a rigid body and all the joints which it belongs
void destroyRigidBody(RigidBody* rigidBody);
@ -245,11 +224,6 @@ class DynamicsWorld : public CollisionWorld {
/// Add the joint to the list of joints of the two bodies involved in the joint
void addJointToBody(Joint* joint);
/// Add a contact manifold to the linked list of contact manifolds of the two bodies
/// involed in the corresponding contact.
void addContactManifoldToBody(ContactManifold* contactManifold,
CollisionBody *body1, CollisionBody *body2);
/// Reset all the contact manifolds linked list of each body
void resetContactManifoldListsOfBodies();
@ -268,9 +242,6 @@ class DynamicsWorld : public CollisionWorld {
/// Return the number of joints in the world
uint getNbJoints() const;
/// Return the number of contact manifolds in the world
uint getNbContactManifolds() const;
/// Return the current physics time (in seconds)
long double getPhysicsTime() const;
@ -280,9 +251,6 @@ class DynamicsWorld : public CollisionWorld {
/// Return an iterator to the end of the rigid bodies of the physics world
std::set<RigidBody*>::iterator getRigidBodiesEndIterator();
/// Return a reference to the contact manifolds of the world
const std::vector<ContactManifold*>& getContactManifolds() const;
/// Return true if the sleeping technique is enabled
bool isSleepingEnabled() const;
@ -309,6 +277,10 @@ class DynamicsWorld : public CollisionWorld {
/// Set an event listener object to receive events callbacks.
void setEventListener(EventListener* eventListener);
// -------------------- Friendship -------------------- //
friend class RigidBody;
};
// Reset the external force and torque applied to the bodies
@ -369,31 +341,6 @@ inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool
mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
}
// Reset the boolean movement variable of each body
inline void DynamicsWorld::resetBodiesMovementVariable() {
// For each rigid body
for (std::set<RigidBody*>::iterator it = getRigidBodiesBeginIterator();
it != getRigidBodiesEndIterator(); it++) {
// Set the hasMoved variable to false
(*it)->mHasMoved = false;
}
}
// Update the overlapping pair
inline void DynamicsWorld::updateOverlappingPair(const BroadPhasePair* pair) {
// Get the pair of body index
std::pair<bodyindex, bodyindex> indexPair = pair->getBodiesIndexPair();
// Get the corresponding overlapping pair
OverlappingPair* overlappingPair = mOverlappingPairs[indexPair];
// Update the contact cache of the overlapping pair
overlappingPair->update();
}
// Return the gravity vector of the world
inline Vector3 DynamicsWorld::getGravity() const {
return mGravity;
@ -429,16 +376,6 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator()
return mRigidBodies.end();
}
// Return a reference to the contact manifolds of the world
inline const std::vector<ContactManifold*>& DynamicsWorld::getContactManifolds() const {
return mContactManifolds;
}
// Return the number of contact manifolds in the world
inline uint DynamicsWorld::getNbContactManifolds() const {
return mContactManifolds.size();
}
// Return the current physics time (in seconds)
inline long double DynamicsWorld::getPhysicsTime() const {
return mTimer.getPhysicsTime();

Some files were not shown because too many files have changed in this diff Show More