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

Change in the repository structure

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@395 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2010-09-09 22:06:57 +00:00
parent 9c7a458914
commit 78ea7b891d
89 changed files with 12038 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
src/demo/Camera.cpp Normal file
View File

@ -0,0 +1,34 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
// Libraries
#include "Camera.h"
// Initialization of static variables
double Camera::speed=0.5;
// Constructor of the camera
Camera::Camera() {
}
// Destructor of the camera
Camera::~Camera() {
}

91
src/demo/Camera.h Normal file
View File

@ -0,0 +1,91 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef CAMERA_H
#define CAMERA_H
// Libraries
#include "../reactphysics3d/reactphysics3d.h" // We want the mathematics stuff of reactphysics3d
// Namespaces
using namespace reactphysics3d;
// Class Camera (abstract class)
// In the project we will use two different camera. This is the superclass of the two
// cameras. We will use a OutSideCamera that move arround the scene and an OnBoardCamera
// that will simulate the deplacement of a viewer inside the scene
class Camera {
protected :
Vector3D position; // Position of the camera
Vector3D lookAtPoint; // Point where the camera is looking
Vector3D viewVector; // Vector from the camera position to the view point
static double speed; // Speed movement of the camera
public :
Camera(); // Constructor
Camera(const Camera& camera); // Copy-constructor
virtual ~Camera(); // Destructor
virtual Vector3D getPosition() const; // Get the position of the camera
virtual void setPosition(const Vector3D& pos); // Set the position of the camera
virtual Vector3D getLookAtPoint() const; // Return the point where the camera is looking
virtual Vector3D getViewVector() const; // Return the view vector of the camera
virtual void updateViewVector()=0; // Update the view vector of the camera
static void increaseSpeed(); // Increase the speed of camera movement
static void decreaseSpeed(); // Decrease the speed of camera movement
};
// Get the position of the camera (inline function)
inline Vector3D Camera::getPosition() const {
// Return the position of the camera
return position;
}
// Set the position of the camera (inline function)
inline void Camera::setPosition(const Vector3D& pos) {
// Set the position of the camera
position = pos;
}
// Return the point where the camera is looking (inline function)
inline Vector3D Camera::getLookAtPoint() const {
return lookAtPoint;
}
// Return the view vector of the camera (inline function)
inline Vector3D Camera::getViewVector() const {
return viewVector;
}
// Increase the speed movement of the camera (inline function)
inline void Camera::increaseSpeed()
{
speed= speed+0.05;
if(speed>1)
speed=1;
}
// Decrease the speep movement of the camera (inline function)
inline void Camera::decreaseSpeed()
{
speed = speed-0.05;
if(speed<=0)
speed=0.005;
}
#endif

111
src/demo/Context.cpp Executable file
View File

@ -0,0 +1,111 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
// Libraries
#include "Context.h"
#include "../reactphysics3d/reactphysics3d.h"
#include <iostream>
#include <vector>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor of the class Context
Context::Context() {
/*
Cube* cube1 = new Cube(Vector3D(0.0, 10.0, 0.0), Quaternion(1.0, 1.0, 0.0, 0.0), 3.0, Kilogram(1.0));
Cube* cube2 = new Cube(Vector3D(0.0, 0.0, 0.0), Quaternion(1.0, 1.0, 0.0, 0.0), 3.0, Kilogram(1.0));
//cube1->getRigidBody()->setLinearVelocity(Vector3D(0.0, 0.0, 0.0));
//cube2->getRigidBody()->setLinearVelocity(Vector3D(0.0, 0.0, 0.0));
cube2->getRigidBody()->setIsMotionEnabled(false);
cube1->getRigidBody()->setRestitution(0.5);
cube2->getRigidBody()->setRestitution(0.5);
*/
for (int i=20; i>1; i=i-3) {
Cube* cube = new Cube(Vector3D(3.0, i, i*0.2), Quaternion(1.0, 1.0, 0.0, 0.0), 2.0, Kilogram(1.0));
cube->getRigidBody()->setRestitution(0.7);
addObject(cube);
}
Plane* plane1 = new Plane(Vector3D(0.0, 0.0, 0.0), Quaternion(0.0, 1.0, 0.0 , 0.0), 20.0, 30.0, Vector3D(-1.0, 0.0, 0.0), Vector3D(0.0, 0.0, 1.0), Kilogram(1.0));
plane1->getRigidBody()->setRestitution(0.7);
plane1->getRigidBody()->setIsMotionEnabled(false);
addObject(plane1);
/*
Cube* cube1 = new Cube(Vector3D(4.0, 11.0, 5.0), Quaternion(1.0, 0.3, 0.8, 0.0), 2.0, Kilogram(3.0));
//Cube* cube2 = new Cube(Vector3D(3.0, 9, 3.0), Quaternion(1.0, 1.0, 0.0, 0.0), 2.0, Kilogram(2.0));
cube1->getRigidBody()->setRestitution(0.4);
//cube2->getRigidBody()->setRestitution(0.4);
//Cube* cube3 = new Cube(Vector3D(5.0, 13, 0.0), Quaternion(1.0, 1.0, 0.3, 0.0), 2.0, Kilogram(1.0));
//cube3->getRigidBody()->setRestitution(0.8);
Plane* plane1 = new Plane(Vector3D(0.0, 0.0, 0.0), Quaternion(0.0, 1.0, 0.2, 0.0), 20.0, 30.0, Vector3D(-1.0, 0.0, 0.0), Vector3D(0.0, 0.0, 1.0), Kilogram(1.0));
plane1->getRigidBody()->setRestitution(0.4);
plane1->getRigidBody()->setIsMotionEnabled(false);
addObject(cube1);
//addObject(cube2);
//addObject(cube3);
addObject(plane1);
*/
}
// Destructor of the class Context
Context::~Context() {
// Delete all the objects in vectObjects
for(unsigned int i=0; i<vectObjects.size(); ++i) {
delete vectObjects[i];
}
}
// Method to get an object from the context
Object& Context::getObject(int objectIndex) const {
// TODO : WE HAVE TO ADD HERE AN EXCEPTION IMPLEMENTATION
// Return the object from the context
return (*vectObjects.at(objectIndex)); // TODO : THROWN AN EXCEPTION IF INDEX IS OUT OF THE BOUNDS
}
// Method for adding an object into the context
void Context::addObject(Object* object) {
if (object != 0) {
// Add the object into the context
vectObjects.push_back(object);
}
}
// Method to remove an object from the context
void Context::removeObject(int objectIndex) {
// WE HAVE TO ADD HERE AN EXCEPTION IMPLEMENTATION
// Restore the memory of the element
delete vectObjects[objectIndex];
// Erase the element in the vector
vectObjects.erase(vectObjects.begin()+objectIndex);
}

46
src/demo/Context.h Executable file
View File

@ -0,0 +1,46 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef CONTEXT_H
#define CONTEXT_H
// Libraries
#include "Objects.h"
#include <vector>
// Class Context
class Context {
private :
std::vector<Object*> vectObjects; // Vector of Objects in the simulation
public :
Context(); // Constructor of the class
~Context(); // Destructor of the class
int getNbObjects() const; // Return the number of objects in the context
Object& getObject(int objectIndex) const; // Get an object from the context
void addObject(Object* object); // Add an object into the context
void removeObject(int objectIndex); // Remove an object from the context
};
// Method (inline) to get the number of objects in the context
inline int Context::getNbObjects() const {
return vectObjects.size();
}
#endif

152
src/demo/Objects.cpp Executable file
View File

@ -0,0 +1,152 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
// Libraries
#include "Objects.h"
//#include <windows.h> // To avoid an error due to the #include <GL/glut.h>
#include <GL/freeglut.h>
#include <GL/gl.h>
#include <GL/glu.h>
#include <cmath>
#include <iostream>
// ----- Class Object ----- //
// Constructor of the class Object
Object::Object(const Vector3D& position, const Quaternion& orientation, const Kilogram& mass, const Matrix3x3& inertiaTensor, const OBB& obb)
:rigidBody(new RigidBody(position, orientation, mass, inertiaTensor, obb)) {
}
// Destructor of the class Object
Object::~Object() {
// Delete the rigid body object
delete rigidBody;
}
// Return the pointer to the rigid body
RigidBody* Object::getRigidBody() {
return rigidBody;
}
// ----- Class Cube ----- //
// Static attributes
const Matrix3x3 Cube::inertiaTensor;
// Constructor of the class Cube
Cube::Cube(const Vector3D& position, const Quaternion& orientation, float size, const Kilogram& mass)
:Object(position, orientation, mass, Matrix3x3(1.0/12.0*mass.getValue()*2*size*size, 0.0, 0.0,
0.0, 1.0/12.0*mass.getValue()*2*size*size, 0.0,
0.0, 0.0, 1.0/12.0*mass.getValue()*2*size*size), OBB(position, Vector3D(1.0, 0.0, 0.0), Vector3D(0.0, 1.0, 0.0), Vector3D(0.0, 0.0, 1.0),
size/2.0, size/2.0, size/2)) {
this->size = size;
}
// Destructor of the classe Cube
Cube::~Cube() {
}
// Method to draw the cube
void Cube::draw() const {
// Get the interpolated state of the rigid body
BodyState state = rigidBody->getInterpolatedState();
// Position of the cube
double x = state.getPosition().getX();
double y = state.getPosition().getY();
double z = state.getPosition().getZ();
// Orientation of the cube
Vector3D orientationAxis;
double orientationAngle;
state.getOrientation().getRotationAngleAxis(orientationAngle, orientationAxis);
// Translation of the cube to its position
glTranslatef(x, y, z);
// Rotation of the cube according to its orientation
glRotatef(orientationAngle/PI*180.0, orientationAxis.getX(), orientationAxis.getY(), orientationAxis.getZ());
// Draw the cube
glutSolidCube(size);
}
// ----- Class Plane ----- //
// Constructor of the class Plane
Plane::Plane(const Vector3D& position, const Quaternion& orientation, float width, float height, const Vector3D& d1, const Vector3D& d2, const Kilogram& mass)
:Object(position, orientation, mass, Matrix3x3(1.0/12.0*mass.getValue()*height*height, 0.0, 0.0,
0.0, 1.0/12.0*mass.getValue()*(width*width+height*height), 0.0,
0.0, 0.0, 1.0/12.0*mass.getValue()*width*width), OBB(position, Vector3D(1.0, 0.0, 0.0), Vector3D(0.0, 1.0, 0.0), Vector3D(0.0, 0.0, 1.0),
width/2, 0.5, height/2)) { // TODO : Change the height of the OBB
this->width = width;
this->height = height;
this->d1 = d1;
this->d2 = d2;
// By default Planes in the demo cannot move
rigidBody->setIsMotionEnabled(false);
// Compute the unit normal vector of the plane by a cross product
normalVector = d1.crossProduct(d2).getUnit();
}
// Destructor of the class Plane
Plane::~Plane() {
}
// Method used to draw the plane
void Plane::draw() const {
// Get the interpolated state of the rigid body
BodyState state = rigidBody->getInterpolatedState();
// Get the position of the rigid body
double x = state.getPosition().getX();
double y = state.getPosition().getY();
double z = state.getPosition().getZ();
// Translation of the cube to its position
glTranslatef(x, y, z);
float halfWidth = width / 2.0;
float halfHeight = height / 2.0;
// Draw the plane
glBegin(GL_POLYGON);
glColor3f(1.0, 1.0, 1.0);
glVertex3f(x + d1.getX() * halfWidth + d2.getX() * halfHeight , y + d1.getY() * halfWidth + d2.getY() * halfHeight
, z + d1.getZ() * halfWidth + d2.getZ() * halfHeight);
glNormal3f(normalVector.getX(), normalVector.getY(), normalVector.getZ());
glVertex3f(x + d1.getX() * halfWidth - d2.getX() * halfHeight , y + d1.getY() * halfWidth - d2.getY() * halfHeight
, z + d1.getZ() * halfWidth - d2.getZ() * halfHeight);
glNormal3f(normalVector.getX(), normalVector.getY(), normalVector.getZ());
glVertex3f(x - d1.getX() * halfWidth - d2.getX() * halfHeight , y - d1.getY() * halfWidth - d2.getY() * halfHeight
, z - d1.getZ() * halfWidth - d2.getZ() * halfHeight);
glNormal3f(normalVector.getX(), normalVector.getY(), normalVector.getZ());
glVertex3f(x - d1.getX() * halfWidth + d2.getX() * halfHeight , y - d1.getY() * halfWidth + d2.getY() * halfHeight
, z - d1.getZ() * halfWidth + d2.getZ() * halfHeight);
glEnd();
}

76
src/demo/Objects.h Executable file
View File

@ -0,0 +1,76 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef OBJECTS_H
#define OBJECTS_H
// Libraries
#include "../reactphysics3d/reactphysics3d.h"
// Namespaces
using namespace reactphysics3d;
/*
Here we define all the objects that can appear in the simulation like cube, sphere, plane, ...
*/
// ----- Class Object (abstract) ----- //
// Represent an object of the simulation
class Object {
protected :
RigidBody* rigidBody; // Rigid Body that represents the object
public :
Object(const Vector3D& position, const Quaternion& orientation, const Kilogram& mass, const Matrix3x3& inertiaTensor, const OBB& obb); // Constructor of the class Object
virtual ~Object(); // Destructor of the class Object
virtual void draw() const =0; // pure virtual method to draw the object
RigidBody* getRigidBody(); // Return the pointer to the rigid body
};
// ----- Class Cube ----- //
// Represente a Box in the simulation
class Cube : public Object {
private :
float size; // Size of a side in the box
static const Matrix3x3 inertiaTensor; // Inertia tensor of a box
public :
Cube(const Vector3D& position, const Quaternion& orientation, float size, const Kilogram& mass); // Constructor of the class cube
virtual ~Cube(); // Destructor of the class cube
virtual void draw() const; // Method to draw the cube
};
// ----- Class Plane ---- //
// Represent a plane in the simulation
class Plane : public Object {
private :
float width; // Width of the plane
float height; // Height of the plane
Vector3D d1; // Unit vector in the plane
Vector3D d2; // Unit vector in the plane
Vector3D normalVector; // Unit normal vector of the plane
public :
Plane(const Vector3D& position, const Quaternion& orientation, float width, float height, const Vector3D& d1, const Vector3D& d2, const Kilogram& mass); // Constructor of the class Plane
virtual ~Plane(); // Destructor of the class Plane
virtual void draw() const; // Method to draw the plane
};
#endif

112
src/demo/OutSideCamera.cpp Normal file
View File

@ -0,0 +1,112 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
// Libraries
#include "OutSideCamera.h"
#include <cmath>
// Constructor
OutSideCamera::OutSideCamera() {
// Initialize the attributes
heightFromFloor = 10.0;
horizontalAngleRotation = 45;
verticalAngleRotation = 45;
distanceFromOrigin = 30.0;
lookAtPoint.setAllValues(0.0, 0.0, 0.0);
// Update the position of the camera
updatePosition();
// Update the view vector of the camera
updateViewVector();
}
// Destructor
OutSideCamera::~OutSideCamera() {
}
// Compute the new position of the camera
void OutSideCamera::updatePosition() {
// Compute the floor distance from origin
double floorDistance = distanceFromOrigin * cos(PI/180.0 * verticalAngleRotation);
// Update the position of the camera
position.setAllValues(floorDistance*cos(PI/180.0 * horizontalAngleRotation), distanceFromOrigin*sin(PI/180*verticalAngleRotation),
floorDistance*sin(PI/180.0 * horizontalAngleRotation));
}
// Set the camera rotation angle and update the position of the camera
void OutSideCamera::modifyHorizontalAngleRotation(int screenDistance, float fps) {
// Update the horizontal rotation angle of the camera
horizontalAngleRotation = (horizontalAngleRotation + int(screenDistance * 700.0 / fps)) % 360;
// Update the position and the view vector of the camera
updatePosition();
updateViewVector();
}
// Set the vertical camera rotation angle
void OutSideCamera::modifyVerticalAngleRotation(int screenDistance, float fps) {
// Update the vertical rotation angle of the camera
verticalAngleRotation = verticalAngleRotation + (screenDistance * 700.0 / fps);
// Vertical angle limits
if (verticalAngleRotation > 89) {
verticalAngleRotation = 89;
}
if (verticalAngleRotation < 1) {
verticalAngleRotation = 1;
}
// Update the position and the view vector of the camera
updatePosition();
updateViewVector();
}
// Increase the distance from origine of the camera (used for the zoom)
void OutSideCamera::increaseDistance(float fps) {
// Increase the distance from origin
distanceFromOrigin = distanceFromOrigin + (speed * 60 / fps);
// Update the position and the view vector of the camera
updatePosition();
updateViewVector();
}
// Decrease the distance from origine of the camera (used for the zoom)
void OutSideCamera::decreaseDistance(float fps) {
// Decrease the distance from origin
distanceFromOrigin = distanceFromOrigin - (speed * 60 / fps);
// Limit condition
if(distanceFromOrigin < 1) {
distanceFromOrigin=1;
}
// Update the position and the view vector of the camera
updatePosition();
updateViewVector();
}

64
src/demo/OutSideCamera.h Normal file
View File

@ -0,0 +1,64 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef OUTSIDE_CAMERA_H
#define OUTSIDE_CAMERA_H
// Libraries
#include "Camera.h"
// ---- Class OutSideCamera ----
// This camera can be move everywhere around the scene to obtain a outside view of the house
class OutSideCamera : public Camera {
private :
double heightFromFloor; // Height from the floor
int horizontalAngleRotation; // Horizontal rotation angle (in degree)
int verticalAngleRotation; // Vertical rotation angle (in degree)
double distanceFromOrigin; // Distance of the camera from the origin (used to zoom)
public :
OutSideCamera(); // Constructor
OutSideCamera(const OutSideCamera& camera); // Copy-constructor
virtual ~OutSideCamera(); // Destructor
void updatePosition(); // Compute the new position of the camera
void updateViewVector(); // Update the view vector of the camera
double getHeightFromFloor() const; // Get the height of the camera from the floor
void setHeightFromFloor(double height); // Set the height of the camera from the floor
void modifyHorizontalAngleRotation(int screenDistance, float fps); // Modify the horizontal camera rotation angle
void modifyVerticalAngleRotation(int screenDistance, float fps); // Modify the vertical camera rotation angle
void increaseDistance(float fps); // Increase the distance of the camera from the origin
void decreaseDistance(float fps); // Decrease the distance of the camera from the origin
};
// Compute the new view vector of the camera (inline function)
inline void OutSideCamera::updateViewVector() {
viewVector = Vector3D(0.0, 0.0, 0.0) - position;
}
// Get the height of the camera from the floor (inline function)
inline double OutSideCamera::getHeightFromFloor() const {
return heightFromFloor;
}
// Set the height of the camera from the floor (inline function)
inline void OutSideCamera::setHeightFromFloor(double height) {
heightFromFloor = height;
}
#endif

27
src/demo/ReactDemo.h Executable file
View File

@ -0,0 +1,27 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef REACT_DEMO_H
#define REACT_DEMO_H
// Constants
const int WINWIDTH = 640; // Width of the OpenGL windows
const int WINHEIGHT = 480; // Height of the OpenGL windows
#endif

151
src/demo/Scene.cpp Executable file
View File

@ -0,0 +1,151 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
// Librairies
#include "Scene.h"
#include "Objects.h"
#include <GL/gl.h>
#include <GL/glu.h>
// Constructor of the class Scene
Scene::Scene(rp3d::PhysicsWorld* world) {
this->world = world;
// Initialise the material specular color
mat_specular[0] = 1.0;
mat_specular[1] = 1.0;
mat_specular[2] = 1.0;
mat_specular[3] = 1.0;
// Initialize the material shininess
mat_shininess[0] = 50.0;
// Initialise the light source position
light_position[0] = 20.0;
light_position[1] = 9.0;
light_position[2] = 15.0;
light_position[3] = 0.0;
// Initialise the ambient color of the light
ambient_color[0] = 1.0;
ambient_color[1] = 1.0;
ambient_color[2] = 1.0;
ambient_color[3] = 0.7;
// Initialise the diffuse light color
white_light[0] = 1.0;
white_light[1] = 1.0;
white_light[2] = 1.0;
white_light[3] = 1.0;
}
// Destructor of the class Scene
Scene::~Scene() {
}
// Init method
void Scene::init() {
glClearColor(0.0, 0.0, 0.0, 0.0); // Select the color for the background
glShadeModel(GL_SMOOTH);
glClearDepth(1.0);
// Lighting settings
glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular); // Specular color of the material
glMaterialfv(GL_FRONT, GL_SHININESS, mat_shininess); // Shininess of the material
glLightfv(GL_LIGHT0, GL_POSITION, light_position); // Position of the light source
glLightfv(GL_LIGHT0, GL_AMBIENT, ambient_color); // Ambient color of the light
glLightfv(GL_LIGHT0, GL_DIFFUSE, white_light); // Diffuse color of the light
glLightfv(GL_LIGHT0, GL_SPECULAR, white_light); // Specular color of the light
glEnable(GL_LIGHTING); // Activate the lighting
glEnable(GL_LIGHT0); // Activate a light source
glEnable(GL_DEPTH_TEST); // Activate the Depth buffer
//glEnable(GL_CULL_FACE);
}
// Display method
void Scene::display(const Context& context) const {
glClearColor(0,0,0,0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
// Define the position and the direction of the camera
//gluLookAt(30,10,0,0,0,0,0,1,0);
double x = outsideCamera.getPosition().getX();
double y = outsideCamera.getPosition().getY();
double z = outsideCamera.getPosition().getZ();
gluLookAt(x,y,z,0,0,0,0,1,0);
// Draw all objects in the context
for(int i=0; i<context.getNbObjects(); ++i)
{
// Copy the active matrix on the matrix stack
glPushMatrix();
// Draw the object
context.getObject(i).draw();
glPopMatrix();
glPushMatrix();
// Draw the bounding volume
context.getObject(i).getRigidBody()->getOBB()->draw();
// Remove the matrix on the top of the matrix stack
glPopMatrix();
}
// Draw all the contact points
for (std::vector<Constraint*>::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) {
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>((*it)->getBody1());
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>((*it)->getBody2());
//rigidBody1->setIsMotionEnabled(false);
//rigidBody2->setIsMotionEnabled(false);
Contact* contact = dynamic_cast<Contact*>((*it));
assert(contact != 0);
// Draw the contact points
glPushMatrix();
glTranslatef(contact->getPoint().getX(), contact->getPoint().getY(), contact->getPoint().getZ());
contact->draw();
glPopMatrix();
}
glFlush();
// Swap the buffers
SDL_GL_SwapBuffers();
}
// Reshape the window
void Scene::reshape(int width, int height) {
glViewport(0,0,width,height);
glMatrixMode(GL_PROJECTION); // Specify the matrix that will be modified
glLoadIdentity(); // Load the identity matrix before the transformations
gluPerspective(45.0, (float) width/height, 0.1f, 150.0f);
glMatrixMode(GL_MODELVIEW); // Specify the matrix that will be modified
glLoadIdentity(); // Load the identity matrix before the transformations
}

61
src/demo/Scene.h Executable file
View File

@ -0,0 +1,61 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef SCENE_H
#define SCENE_H
// Libraries
#include "Context.h"
#include "OutSideCamera.h"
#include <SDL/SDL.h>
#include <GL/freeglut.h> // Used only to draw cubes
#include <GL/gl.h>
#include <GL/glu.h>
// Scene class
class Scene {
private :
GLfloat mat_specular[4]; // Material specular light color
GLfloat mat_shininess[1]; // Material shininess
GLfloat light_position[4]; // Position of the light source
GLfloat ambient_color[4]; // Ambient color of the light
GLfloat white_light[4]; // White light color
OutSideCamera outsideCamera; // OutSide camera (Camera that can move around the scene)
rp3d::PhysicsWorld* world; //"Pointer to the physics world
public :
Scene(rp3d::PhysicsWorld* world); // constructor of the class
~Scene(); // Destructor of the class
void init(); // Initialize the values of OpenGL
void display(const Context& context) const; // display the scene
void reshape(int width, int height); // Reshape the window
OutSideCamera& getOutSideCamera() ; // Return a reference to the outside camera
float getCameraAngle1() const; // Return the angle of the camera
float getCameraAngle2() const; // Return the angle of the camera
void setCameraAngle1(float angle); // Set the angle of the camera
void setCameraAngle2(float angle); // Set the angle of the camera
};
// Return a reference to the camera
inline OutSideCamera& Scene::getOutSideCamera() {
return outsideCamera;
}
#endif

190
src/demo/Simulation.cpp Executable file
View File

@ -0,0 +1,190 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
// Libraries
#include "Simulation.h"
#include "ReactDemo.h"
#include <iostream>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor of the class Simulation
Simulation::Simulation()
:world(new PhysicsWorld(Vector3D(0.0, -9.8, 0.0))), engine(world, Time(0.01)), scene(this->world) { // TODO : Change the timestep here after debugging
simRunning = false;
mouseButtonPressed = false;
nbFrame = 0;
lastFrameTime = 0.0;
fps = 0.0;
}
// Destructor of the class Simulation
Simulation::~Simulation() {
// Delete the physics world object
delete world;
}
// Method to start the simulation
void Simulation::start() {
// Initialisation of the OpenGL settings for the scene
scene.init();
// Reshape the windows for the first time
scene.reshape(WINWIDTH, WINHEIGHT);
// Add every rigid body to the dynamic world
for (int i=0; i<context.getNbObjects(); ++i) {
world->addBody(context.getObject(i).getRigidBody());
}
// Activation of the simulation
simRunning = true;
// Get the current time
lastFrameTime = SDL_GetTicks();
PhysicsEngine* pEngine = &engine;
// Initialize the display time
pEngine->initializeDisplayTime(Time(SDL_GetTicks()/1000.0));
// Start the physics simulation
pEngine->start();
//double time = 1.0;
// Main loop of the simulation
while(simRunning) {
// Check if an SDL event occured and make the apropriate actions
checkEvents();
double time = SDL_GetTicks()/1000.0;
//time += 0.01;
//std::cout << "************************************************* Time : " << time << std::endl;
// Update the display time
pEngine->updateDisplayTime(Time(time));
// Update the physics
pEngine->update();
// Display the actual scene
scene.display(context);
// Compute the fps (framerate)
computeFps();
//std::cout << "FPS : " << fps << std::endl;
/*
BodyState state = context.getObject(0).getRigidBody()->getInterpolatedState();
Vector3D velocity = context.getObject(0).getRigidBody()->getInterpolatedState().getAngularVelocity();
//std::cout << "Velocity 0 : " << velocity.getX() << ", " << velocity.getY() << ", " << velocity.getZ() << ")" << std::endl;
double x = state.getPosition().getX();
double y = state.getPosition().getY();
double z = state.getPosition().getZ();
std::cout << "Position Cube 0 : (" << x << ", " << y << ", " << z << ")" << std::endl;
std::cout << "angular velocity 0 : " << velocity.length() << std::endl;;
BodyState state1 = context.getObject(1).getRigidBody()->getInterpolatedState();
Vector3D velocity1 = context.getObject(1).getRigidBody()->getInterpolatedState().getAngularVelocity();
//std::cout << "Velocity 1 : " << velocity1.getX() << ", " << velocity1.getY() << ", " << velocity1.getZ() << ")" << std::endl;
double x1 = state1.getPosition().getX();
double y1 = state1.getPosition().getY();
double z1 = state1.getPosition().getZ();
std::cout << "Position Cube 1 : (" << x1 << ", " << y1 << ", " << z1 << ")" << std::endl;
std::cout << "angular velocity 1 : " << velocity1.length() << std::endl;
BodyState state2 = context.getObject(2).getRigidBody()->getInterpolatedState();
Quaternion velocity2 = context.getObject(2).getRigidBody()->getInterpolatedState().getOrientation();
//std::cout << "Velocity 2 : " << velocity2.getX() << ", " << velocity2.getY() << ", " << velocity2.getZ() << ")" << std::endl;
double x2 = state2.getPosition().getX();
double y2 = state2.getPosition().getY();
double z2 = state2.getPosition().getZ();
std::cout << "Position Cube 2: (" << x2 << ", " << y2 << ", " << z2 << ")" << std::endl;
std::cout << "quaternion orientation 2 : " << velocity2.getX() << ", " << velocity2.getY() << ", " << velocity2.getZ() << ", " << velocity2.getW() << ")" << std::endl;;
*/
/*
double a;
if (time > 5.0) {
std::cin >> a;
}
*/
}
}
// This method checks if an events occur and call the apropriate method
void Simulation::checkEvents() {
SDL_Event event; // An SDL event
// Zoom of the outside camera
if (SDL_GetKeyState(NULL)[SDLK_UP]) {
scene.getOutSideCamera().decreaseDistance(fps);
}
else if(SDL_GetKeyState(NULL)[SDLK_DOWN]) {
scene.getOutSideCamera().increaseDistance(fps);
}
// Check in the stack of events
while(SDL_PollEvent(&event)) {
// Check an event
switch(event.type) {
// An QUIT event occur
case SDL_QUIT: simRunning = false;
break;
// A keyboard key has been pushed
case SDL_KEYDOWN: // The Esc key has been pushed then we end the simulation
if (event.key.keysym.sym == SDLK_ESCAPE)
simRunning = false;
break;
// The size of the windows changed then we reshape the windows
case SDL_VIDEORESIZE: scene.reshape(event.resize.w, event.resize.h);
break;
// If the mouse moved
case SDL_MOUSEMOTION: if (SDL_GetMouseState(NULL, NULL)&SDL_BUTTON(1)) {
// Rotation of the outSideCamera
scene.getOutSideCamera().modifyHorizontalAngleRotation(event.motion.xrel, fps);
scene.getOutSideCamera().modifyVerticalAngleRotation(event.motion.yrel, fps);
}
}
}
}
// Compute the framerate (fps) of the application
void Simulation::computeFps() {
// Increment the number of frame in the last second
nbFrame++;
// Get the current time
double currentTime = SDL_GetTicks();
// Compute the framerate
if (currentTime - lastFrameTime > 1000.0) {
fps = nbFrame * 1000.0/(currentTime-lastFrameTime);
lastFrameTime = currentTime;
nbFrame = 0;
}
}

50
src/demo/Simulation.h Executable file
View File

@ -0,0 +1,50 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef SIMULATION_H
#define SIMULATION_H
// Librairies
#include "Context.h"
#include "Scene.h"
#include "../reactphysics3d/reactphysics3d.h"
// Class Simulation
class Simulation {
private :
rp3d::PhysicsWorld* world; // Pointer to the collision world that contains bodies of the simulation
Scene scene; // Scene object for displaying the simulation
Context context; // Context of the simulation
rp3d::PhysicsEngine engine; // Collision engine for the physics of the simulation
bool simRunning; // True if the simulation is running and false otherwise
bool mouseButtonPressed; // True if the left mouse button is pressed
double lastFrameTime; // Last frame time
int nbFrame; // Number of frame (used to compute the framerate)
double fps; // Framerate of the application
void computeFps(); // Compute the framerate of the application
public :
Simulation(); // Constructor of the class
~Simulation(); // Destructor of the class
void start(); // Start the simulation
void checkEvents(); // Check if SDL events occured and make the apropriate actions
};
#endif

103
src/demo/main.cpp Executable file
View File

@ -0,0 +1,103 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
// Libraries
#include "../reactphysics3d/reactphysics3d.h"
#include "Simulation.h"
#include "ReactDemo.h"
#include <iostream>
#include <SDL/SDL.h>
// Prototypes
int initSDL();
// global variables
SDL_Surface * pScreen; // Pointer to the SDL windows
// Main function
int main(int argc, char** argv) {
if(initSDL() > 0) {
// If initSDL return an error then we exit the program
return EXIT_FAILURE;
}
// Initialize the glut library. We will use glut only to display shapes like cube or sphere
glutInit(&argc, argv);
// Create a Simulation object used to simulate a physic world
Simulation simulation;
// Start the simulation
simulation.start();
std::cerr << "Fin normale du programme" << std::endl;
// To avoid warnings notifying that argc et argv aren't used
(argc);
(argv);
return (EXIT_SUCCESS);
}
// Methode to initialise the SDL and the windows
int initSDL() {
// Initialisation of SDL
if( SDL_Init(SDL_INIT_VIDEO) < 0) {
std::cerr << "Echec SDL_Init : " << SDL_GetError() << std::endl;
return 1;
}
// Select the method to exit
atexit( SDL_Quit );
// Active double buffer mode
SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
// Select the Depth Buffer size
SDL_GL_SetAttribute( SDL_GL_DEPTH_SIZE, 16);
// Activation of displaying in windows mode
SDL_Surface* pScreen = SDL_SetVideoMode(WINWIDTH, WINHEIGHT, 32, SDL_OPENGL | SDL_GL_DOUBLEBUFFER | SDL_RESIZABLE | SDL_SWSURFACE);
// Check that displaying is activated
if( !pScreen ) {
std::cerr << "Echec de creation de la fenetre en 640x480 : " << SDL_GetError() << std::endl;
return 1;
}
// Define the window title and the window icon
SDL_WM_SetCaption("ReactPhysics3D Demo 0.01", NULL);
// Get the state of the Double Buffer parameter
int nValue;
if( SDL_GL_GetAttribute(SDL_GL_DOUBLEBUFFER, &nValue) < 0) {
std::cerr << "Echec de recuperation du parametre SDL_GL_DOUBLEBUFFER : " << SDL_GetError() << std::endl;
return 1;
}
// Check that Double Buffer mode is activated
if(nValue != 1) {
std::cerr << "Erreur : SDL_GL_DOUBLEBUFFER inactif" << std::endl;
return 1;
}
return 0;
}

114
src/reactphysics3d/body/AABB.cpp Normal file
View File

@ -0,0 +1,114 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "AABB.h"
#include <GL/freeglut.h> // TODO : Remove this in the final version
#include <GL/gl.h> // TODO : Remove this in the final version
#include <cassert>
using namespace reactphysics3d;
using namespace std;
// Constructor
AABB::AABB(const Vector3D& center,double extentX, double extentY, double extentZ) {
this->center = center;
this->extent[0] = extentX;
this->extent[1] = extentY;
this->extent[2] = extentZ;
this->originalAABBExtent[0] = extentX;
this->originalAABBExtent[1] = extentY;
this->originalAABBExtent[2] = extentZ;
}
// Destructor
AABB::~AABB() {
}
// Draw the OBB (only for testing purpose)
void AABB::draw() const {
Vector3D s1 = center + Vector3D(extent[0], extent[1], -extent[2]);
Vector3D s2 = center + Vector3D(extent[0], extent[1], extent[2]);
Vector3D s3 = center + Vector3D(-extent[0], extent[1], extent[2]);
Vector3D s4 = center + Vector3D(-extent[0], extent[1], -extent[2]);
Vector3D s5 = center + Vector3D(extent[0], -extent[1], -extent[2]);
Vector3D s6 = center + Vector3D(extent[0], -extent[1], extent[2]);
Vector3D s7 = center + Vector3D(-extent[0], -extent[1], extent[2]);
Vector3D s8 = center + Vector3D(-extent[0], -extent[1], -extent[2]);
// Draw in red
glColor3f(1.0, 0.0, 0.0);
// Draw the OBB
glBegin(GL_LINES);
glVertex3f(s1.getX(), s1.getY(), s1.getZ());
glVertex3f(s2.getX(), s2.getY(), s2.getZ());
glVertex3f(s2.getX(), s2.getY(), s2.getZ());
glVertex3f(s3.getX(), s3.getY(), s3.getZ());
glVertex3f(s3.getX(), s3.getY(), s3.getZ());
glVertex3f(s4.getX(), s4.getY(), s4.getZ());
glVertex3f(s4.getX(), s4.getY(), s4.getZ());
glVertex3f(s1.getX(), s1.getY(), s1.getZ());
glVertex3f(s5.getX(), s5.getY(), s5.getZ());
glVertex3f(s6.getX(), s6.getY(), s6.getZ());
glVertex3f(s6.getX(), s6.getY(), s6.getZ());
glVertex3f(s7.getX(), s7.getY(), s7.getZ());
glVertex3f(s7.getX(), s7.getY(), s7.getZ());
glVertex3f(s8.getX(), s8.getY(), s8.getZ());
glVertex3f(s8.getX(), s8.getY(), s8.getZ());
glVertex3f(s5.getX(), s5.getY(), s5.getZ());
glVertex3f(s1.getX(), s1.getY(), s1.getZ());
glVertex3f(s5.getX(), s5.getY(), s5.getZ());
glVertex3f(s4.getX(), s4.getY(), s4.getZ());
glVertex3f(s8.getX(), s8.getY(), s8.getZ());
glVertex3f(s3.getX(), s3.getY(), s3.getZ());
glVertex3f(s7.getX(), s7.getY(), s7.getZ());
glVertex3f(s2.getX(), s2.getY(), s2.getZ());
glVertex3f(s6.getX(), s6.getY(), s6.getZ());
glEnd();
}
// Static method that computes an AABB from a set of vertices. The "center" argument corresponds to the center of the AABB
// This method allocates a new AABB object and return a pointer to the new allocated AABB object
AABB* AABB::computeFromVertices(const vector<Vector3D>& vertices, const Vector3D& center) {
// TODO : Implement this method;
return 0;
}

178
src/reactphysics3d/body/AABB.h Normal file
View File

@ -0,0 +1,178 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef AABB_H
#define AABB_H
// Libraries
#include "BroadBoundingVolume.h"
#include "../mathematics/mathematics.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class AABB :
This class represents a bounding volume of type "Axis Aligned
Bounding Box". It's a box where all the edges are always aligned
with the world coordinate system. The AABB is defined by a center
point and three extent size in the x,y and z directions.
-------------------------------------------------------------------
*/
class AABB : public BroadBoundingVolume {
protected :
Vector3D center; // Center point of the AABB
double extent[3]; // Three extents size in the x, y and z directions
double originalAABBExtent[3]; // Extents of the original AABB (this is used to update the AABB)
public :
AABB(const Vector3D& center, double extentX, double extentY, double extentZ); // Constructor
virtual ~AABB(); // Destructor
Vector3D getCenter() const; // Return the center point
void setCenter(const Vector3D& center); // Set the center point
Vector3D getVertex(uint index) const throw (std::invalid_argument); // Return a vertex of the AABB
double getExtent(uint index) const throw(std::invalid_argument); // Return an extent value
void setExtent(uint index, double extent) throw(std::invalid_argument); // Set an extent value
double getMinValueOnAxis(uint axis) const throw(std::invalid_argument); // Return the minimum position value on the given axis
double getMaxValueOnAxis(uint axis) const throw(std::invalid_argument); // Return the maximum position value on the given axis
bool testCollision(const AABB& aabb) const; // Return true if the current AABB is overlapping is the AABB in argument
virtual void update(const Vector3D& newCenter, const Quaternion& rotationQuaternion); // Update the oriented bounding box orientation according to a new orientation of the rigid body
virtual void draw() const; // Draw the AABB (only for testing purpose)
static AABB* computeFromVertices(const std::vector<Vector3D>& vertices, const Vector3D& center); // Compute an AABB from a set of vertices
};
// Return the center point
inline Vector3D AABB::getCenter() const {
return center;
}
// Set the center point
inline void AABB::setCenter(const Vector3D& center) {
this->center = center;
}
// Return one of the 8 vertices of the AABB
inline Vector3D AABB::getVertex(unsigned int index) const throw (std::invalid_argument) {
// Check if the index value is valid
if (index >= 0 && index <8) {
switch(index) {
case 0 : return center + Vector3D(extent[0], extent[1], -extent[2]);
case 1 : return center + Vector3D(extent[0], extent[1], extent[2]);
case 2 : return center + Vector3D(-extent[0], extent[1], extent[2]);
case 3 : return center + Vector3D(-extent[0], extent[1], -extent[2]);
case 4 : return center + Vector3D(extent[0], -extent[1], -extent[2]);
case 5 : return center + Vector3D(extent[0], -extent[1], extent[2]);
case 6 : return center + Vector3D(-extent[0], -extent[1], extent[2]);
case 7 : return center + Vector3D(-extent[0], -extent[1], -extent[2]);
}
}
else {
// The index value is not valid, we throw an exception
throw std::invalid_argument("Exception : The index value has to be between 0 and 8");
}
}
// Return an extent value
inline double AABB::getExtent(unsigned int index) const throw(std::invalid_argument) {
// Check if the index value is valid
if (index >= 0 && index <3) {
return extent[index];
}
else {
// The index value is not valid, we throw an exception
throw std::invalid_argument("Exception : The index value has to be between 0 and 2");
}
}
// Set an extent value
inline void AABB::setExtent(unsigned int index, double extent) throw(std::invalid_argument) {
// Check if the index value is valid
if (index >= 0 && index <3) {
this->extent[index] = extent;
}
else {
// The index value is not valid, we throw an exception
throw std::invalid_argument("Exception : The index value has to be between 0 and 2");
}
}
// Return the minimum position value on the given axis
inline double AABB::getMinValueOnAxis(uint axis) const throw(std::invalid_argument) {
switch (axis) {
case 0: return center.getX() - extent[0];
case 1: return center.getY() - extent[1];
case 2: return center.getZ() - extent[2];
default: // The index value is not valid, we throw an exception
throw std::invalid_argument("Exception : The index value has to be between 0 and 2");
}
}
// Return the maximum position value on the given axis
inline double AABB::getMaxValueOnAxis(uint axis) const throw(std::invalid_argument) {
switch (axis) {
case 0: return center.getX() + extent[0];
case 1: return center.getY() + extent[1];
case 2: return center.getZ() + extent[2];
default: // The index value is not valid, we throw an exception
throw std::invalid_argument("Exception : The index value has to be between 0 and 2");
}
}
// Return true if the current AABB is overlapping is the AABB in argument
// Two AABB overlap if they overlap in the three x, y and z axis at the same time
inline bool AABB::testCollision(const AABB& aabb) const {
Vector3D center2 = aabb.getCenter();
if (std::abs(center.getX() - center2.getX()) > (extent[0] + aabb.getExtent(0))) return false;
if (std::abs(center.getY() - center2.getY()) > (extent[1] + aabb.getExtent(1))) return false;
if (std::abs(center.getZ() - center2.getZ()) > (extent[2] + aabb.getExtent(2))) return false;
return true;
}
// Update the orientation of the AABB according to the orientation of the rigid body
// In order to compute the new AABB we use the original AABB (represented by the originalAABBExtent
// values). The goal is to rotate the original AABB according to the current rotation (rotationQuaternion)
// and then compute the new extent values from the rotated axis of the original AABB. The three columns of
// the rotation matrix correspond to the rotated axis of the rotated original AABB. The we have to compute
// the projections of the three rotated axis onto the x, y and z axis. The projections are easy to compute
// because for instance if the size of the projection of the vector (4, 5, 6) onto the x axis is simply 4.
inline void AABB::update(const Vector3D& newCenter, const Quaternion& rotationQuaternion) {
// Update the center of the AABB
center = newCenter;
// Recompute the new extents size from the rotated AABB
Matrix rotationMatrix = rotationQuaternion.getMatrix(); // Rotation matrix
for (int i=0; i<3; i++) { // For each x, y and z axis
extent[i] = 0.0;
for (int j=0; j<3; j++) { // For each rotated axis of the rotated original AABB
// Add the size of the projection of the current rotated axis to the extent of the current (x, y, or z) axis
extent[i] += std::abs(rotationMatrix.getValue(i, j)) * originalAABBExtent[j];
}
}
}
}; // End of the ReactPhysics3D namespace
#endif

65
src/reactphysics3d/body/Body.cpp Normal file
View File

@ -0,0 +1,65 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "Body.h"
#include "BroadBoundingVolume.h"
#include "NarrowBoundingVolume.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
Body::Body(double mass) throw(std::invalid_argument)
: mass(mass), broadBoundingVolume(0), narrowBoundingVolume(0) {
// Check if the mass is not larger than zero
if (mass <= 0.0) {
// We throw an exception
throw std::invalid_argument("Exception in Body constructor : the mass has to be different larger than zero");
}
}
// Destructor
Body::~Body() {
if (broadBoundingVolume) {
delete broadBoundingVolume;
}
if (narrowBoundingVolume) {
delete narrowBoundingVolume;
}
}
// Set the broad-phase bounding volume
void Body::setBroadBoundingVolume(BroadBoundingVolume* broadBoundingVolume) {
assert(broadBoundingVolume);
this->broadBoundingVolume = broadBoundingVolume;
broadBoundingVolume->setBodyPointer(this);
}
// Set the narrow-phase bounding volume
void Body::setNarrowBoundingVolume(NarrowBoundingVolume* narrowBoundingVolume) {
assert(narrowBoundingVolume);
this->narrowBoundingVolume = narrowBoundingVolume;
narrowBoundingVolume->setBodyPointer(this);
}

112
src/reactphysics3d/body/Body.h Normal file
View File

@ -0,0 +1,112 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef BODY_H
#define BODY_H
// Libraries
#include <stdexcept>
#include <cassert>
// Namespace reactphysics3d
namespace reactphysics3d {
class BroadBoundingVolume;
class NarrowBoundingVolume;
/* -------------------------------------------------------------------
Class Body :
This class is an abstract class to represent body of the physics
engine.
-------------------------------------------------------------------
*/
class Body {
protected :
double mass; // Mass of the body
BroadBoundingVolume* broadBoundingVolume; // Bounding volume used for the broad-phase collision detection
NarrowBoundingVolume* narrowBoundingVolume; // Bounding volume used for the narrow-phase collision detection
bool isMotionEnabled; // True if the body is able to move
bool isCollisionEnabled; // True if the body can collide with others bodies
void setBroadBoundingVolume(BroadBoundingVolume* broadBoundingVolume); // Set the broad-phase bounding volume
void setNarrowBoundingVolume(NarrowBoundingVolume* narrowBoundingVolume); // Set the narrow-phase bounding volume
public :
Body(double mass) throw(std::invalid_argument); // Constructor
virtual ~Body(); // Destructor
double getMass() const; // Return the mass of the body
void setMass(double mass); // Set the mass of the body
bool getIsMotionEnabled() const; // Return if the rigid body can move
void setIsMotionEnabled(bool isMotionEnabled); // Set the value to true if the body can move
bool getIsCollisionEnabled() const; // Return true if the body can collide with others bodies
void setIsCollisionEnabled(bool isCollisionEnabled); // Set the isCollisionEnabled value
const BroadBoundingVolume* getBroadBoundingVolume() const; // Return the broad-phase bounding volume
const NarrowBoundingVolume* getNarrowBoundingVolume() const; // Return the narrow-phase bounding volume of the body
};
// Method that return the mass of the body
inline double Body::getMass() const {
return mass;
};
// Return if the rigid body can move
inline bool Body::getIsMotionEnabled() const {
return isMotionEnabled;
}
// Set the value to true if the body can move
inline void Body::setIsMotionEnabled(bool isMotionEnabled) {
this->isMotionEnabled = isMotionEnabled;
}
// Method that set the mass of the body
inline void Body::setMass(double mass) {
this->mass = mass;
}
// Return true if the body can collide with others bodies
inline bool Body::getIsCollisionEnabled() const {
return isCollisionEnabled;
}
// Set the isCollisionEnabled value
inline void Body::setIsCollisionEnabled(bool isCollisionEnabled) {
this->isCollisionEnabled = isCollisionEnabled;
}
// Return the broad-phase bounding volume
inline const BroadBoundingVolume* Body::getBroadBoundingVolume() const {
return broadBoundingVolume;
}
// Return the oriented bounding box of the rigid body
inline const NarrowBoundingVolume* Body::getNarrowBoundingVolume() const {
return narrowBoundingVolume;
}
}
#endif

39
src/reactphysics3d/body/BoundingVolume.cpp Normal file
View File

@ -0,0 +1,39 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "BoundingVolume.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
BoundingVolume::BoundingVolume() {
this->body = 0;
}
// Destructor
BoundingVolume::~BoundingVolume() {
}

73
src/reactphysics3d/body/BoundingVolume.h Normal file
View File

@ -0,0 +1,73 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef BOUNDING_VOLUME_H
#define BOUNDING_VOLUME_H
// Libraries
#include "Body.h"
#include "../mathematics/mathematics.h"
#include <cassert>
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class BoundingVolume :
This class represents the volume that contains a rigid body.
This volume will be used to compute the collisions with others
bodies.
-------------------------------------------------------------------
*/
class BoundingVolume {
protected :
Body* body; // Pointer to the body
public :
BoundingVolume(); // Constructor
virtual ~BoundingVolume(); // Destructor
Body* getBodyPointer() const; // Return the body pointer
void setBodyPointer(Body* body); // Set the body pointer
virtual void update(const Vector3D& newCenter, const Quaternion& rotationQuaternion)=0; // Update the orientation of the bounding volume according to the new orientation of the body
virtual void draw() const=0; // Display the bounding volume (only for testing purpose)
};
// Return the body pointer
inline Body* BoundingVolume::getBodyPointer() const {
assert(body != 0);
return body;
}
// Set the body pointer
inline void BoundingVolume::setBodyPointer(Body* bodyPointer) {
this->body = bodyPointer;
}
} // End of the ReactPhysics3D namespace
#endif

39
src/reactphysics3d/body/BroadBoundingVolume.cpp Normal file
View File

@ -0,0 +1,39 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "BroadBoundingVolume.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
BroadBoundingVolume::BroadBoundingVolume() : BoundingVolume() {
}
// Destructor
BroadBoundingVolume::~BroadBoundingVolume() {
}

52
src/reactphysics3d/body/BroadBoundingVolume.h Normal file
View File

@ -0,0 +1,52 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef BROAD_BOUNDING_VOLUME_H
#define BROAD_BOUNDING_VOLUME_H
// Libraries
#include "BoundingVolume.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class BroadBoundingVolume :
This class represents the volume that contains a rigid body
This volume will be used to compute the broad-phase collision
detection.
-------------------------------------------------------------------
*/
class BroadBoundingVolume : public BoundingVolume {
protected :
public :
BroadBoundingVolume(); // Constructor
virtual ~BroadBoundingVolume(); // Destructor
};
}
#endif

39
src/reactphysics3d/body/NarrowBoundingVolume.cpp Normal file
View File

@ -0,0 +1,39 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "NarrowBoundingVolume.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
NarrowBoundingVolume::NarrowBoundingVolume() : BoundingVolume() {
}
// Destructor
NarrowBoundingVolume::~NarrowBoundingVolume() {
}

56
src/reactphysics3d/body/NarrowBoundingVolume.h Normal file
View File

@ -0,0 +1,56 @@
/****************************************************************************
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef NARROW_BOUNDING_VOLUME_H
#define NARROW_BOUNDING_VOLUME_H
// Libraries
#include "BoundingVolume.h"
#include "AABB.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class NarrowBoundingVolume :
This class represents the volume that contains a rigid body
This volume will be used to compute the narrow-phase collision
detection.
-------------------------------------------------------------------
*/
class NarrowBoundingVolume : public BoundingVolume {
protected :
public :
NarrowBoundingVolume(); // Constructor
virtual ~NarrowBoundingVolume(); // Destructor
virtual AABB* computeAABB() const=0; // Return the corresponding AABB
};
}
#endif

210
src/reactphysics3d/body/OBB.cpp Normal file
View File

@ -0,0 +1,210 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "OBB.h"
#include <vector>
#include <GL/freeglut.h> // TODO : Remove this in the final version
#include <GL/gl.h> // TODO : Remove this in the final version
#include <cassert>
using namespace reactphysics3d;
using namespace std;
// Constructor
OBB::OBB(const Vector3D& center, const Vector3D& axis1, const Vector3D& axis2,
const Vector3D& axis3, double extent1, double extent2, double extent3) {
this->center = center;
oldAxis[0] = axis1.getUnit();
oldAxis[1] = axis2.getUnit();
oldAxis[2] = axis3.getUnit();
this->axis[0] = oldAxis[0];
this->axis[1] = oldAxis[1];
this->axis[2] = oldAxis[2];
this->extent[0] = extent1;
this->extent[1] = extent2;
this->extent[2] = extent3;
}
// Destructor
OBB::~OBB() {
}
// Draw the OBB (only for testing purpose)
void OBB::draw() const {
double e0 = extent[0];
double e1 = extent[1];
double e2 = extent[2];
Vector3D s1 = center + (axis[0]*e0) + (axis[1]*e1) - (axis[2]*e2);
Vector3D s2 = center + (axis[0]*e0) + (axis[1]*e1) + (axis[2]*e2);
Vector3D s3 = center - (axis[0]*e0) + (axis[1]*e1) + (axis[2]*e2);
Vector3D s4 = center - (axis[0]*e0) + (axis[1]*e1) - (axis[2]*e2);
Vector3D s5 = center + (axis[0]*e0) - (axis[1]*e1) - (axis[2]*e2);
Vector3D s6 = center + (axis[0]*e0) - (axis[1]*e1) + (axis[2]*e2);
Vector3D s7 = center - (axis[0]*e0) - (axis[1]*e1) + (axis[2]*e2);
Vector3D s8 = center - (axis[0]*e0) - (axis[1]*e1) - (axis[2]*e2);
// Draw in red
glColor3f(1.0, 0.0, 0.0);
// Draw the OBB
glBegin(GL_LINES);
glVertex3f(s1.getX(), s1.getY(), s1.getZ());
glVertex3f(s2.getX(), s2.getY(), s2.getZ());
glVertex3f(s2.getX(), s2.getY(), s2.getZ());
glVertex3f(s3.getX(), s3.getY(), s3.getZ());
glVertex3f(s3.getX(), s3.getY(), s3.getZ());
glVertex3f(s4.getX(), s4.getY(), s4.getZ());
glVertex3f(s4.getX(), s4.getY(), s4.getZ());
glVertex3f(s1.getX(), s1.getY(), s1.getZ());
glVertex3f(s5.getX(), s5.getY(), s5.getZ());
glVertex3f(s6.getX(), s6.getY(), s6.getZ());
glVertex3f(s6.getX(), s6.getY(), s6.getZ());
glVertex3f(s7.getX(), s7.getY(), s7.getZ());
glVertex3f(s7.getX(), s7.getY(), s7.getZ());
glVertex3f(s8.getX(), s8.getY(), s8.getZ());
glVertex3f(s8.getX(), s8.getY(), s8.getZ());
glVertex3f(s5.getX(), s5.getY(), s5.getZ());
glVertex3f(s1.getX(), s1.getY(), s1.getZ());
glVertex3f(s5.getX(), s5.getY(), s5.getZ());
glVertex3f(s4.getX(), s4.getY(), s4.getZ());
glVertex3f(s8.getX(), s8.getY(), s8.getZ());
glVertex3f(s3.getX(), s3.getY(), s3.getZ());
glVertex3f(s7.getX(), s7.getY(), s7.getZ());
glVertex3f(s2.getX(), s2.getY(), s2.getZ());
glVertex3f(s6.getX(), s6.getY(), s6.getZ());
glVertex3f(center.getX(), center.getY(), center.getZ());
glVertex3f(center.getX() + 8.0 * axis[1].getX(), center.getY() + 8.0 * axis[1].getY(), center.getZ() + 8.0 * axis[1].getZ());
glEnd();
}
// Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis.
// If the extreme vertices are part of a face of the OBB, the returned vertices will be ordered according to the face.
vector<Vector3D> OBB::getExtremeVertices(const Vector3D& directionAxis) const {
assert(directionAxis.length() != 0);
vector<Vector3D> extremeVertices;
// Check if the given axis is parallel to an axis on the OBB
if (axis[0].isParallelWith(directionAxis)) {
if (axis[0].scalarProduct(directionAxis) >= 0) { // If both axis are in the same direction
extremeVertices = getFace(0); // The extreme is the face 0
}
else {
extremeVertices = getFace(1); // The extreme is the face 1
}
}
else if(axis[1].isParallelWith(directionAxis)) {
if (axis[1].scalarProduct(directionAxis) >= 0) { // If both axis are in the same direction
extremeVertices = getFace(2); // The extreme is the face 2
}
else {
extremeVertices = getFace(3); // The extreme is the face 3
}
}
else if(axis[2].isParallelWith(directionAxis)) {
if (axis[2].scalarProduct(directionAxis) >= 0) { // If both axis are in the same direction
extremeVertices = getFace(4); // The extreme is the face 4
}
else {
extremeVertices = getFace(5); // The extreme is the face 5
}
}
else { // The extreme is made of an unique vertex or an edge
double maxProjectionLength = 0.0; // Longest projection length of a vertex onto the projection axis
// For each vertex of the OBB
for (unsigned int i=0; i<8; ++i) {
Vector3D vertex = getVertex(i);
// Compute the projection length of the current vertex onto the projection axis
double projectionLength = directionAxis.scalarProduct(vertex-center) / directionAxis.length();
// If we found a bigger projection length
if (projectionLength > maxProjectionLength + EPSILON) {
maxProjectionLength = projectionLength;
extremeVertices.clear();
extremeVertices.push_back(vertex);
}
else if (approxEqual(projectionLength, maxProjectionLength)) {
extremeVertices.push_back(vertex);
}
}
assert(extremeVertices.size() == 1 || extremeVertices.size() == 2);
}
// An extreme should be a unique vertex, an edge or a face
assert(extremeVertices.size() == 1 || extremeVertices.size() == 2 || extremeVertices.size() == 4);
// Return the extreme vertices
return extremeVertices;
}
// Static method that computes an OBB from a set of vertices. The "center" argument corresponds to the center of the OBB
// This method allocates a new OBB object and return a pointer to the new allocated OBB object
OBB* OBB::computeFromVertices(const vector<Vector3D>& vertices, const Vector3D& center) {
// TODO : Implement this method;
return 0;
}
// Return the corresponding AABB
AABB* OBB::computeAABB() const {
double maxLength[] = {0.0, 0.0, 0.0}; // Maximum length for each of the three x,y and z axis
Vector3D vertex;
double length;
// For each vertex of the OBB
for (int i = 0; i<8; i++) {
vertex = getVertex(i) - center;
for (int j=0; j<3; j++) {
length = std::abs(vertex.getValue(j));
if (length > maxLength[j]) {
maxLength[j] = length;
}
}
}
// Create and return the AABB
return new AABB(center, maxLength[0], maxLength[1], maxLength[2]);
}

228
src/reactphysics3d/body/OBB.h Normal file
View File

@ -0,0 +1,228 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef OBB_H
#define OBB_H
// Libraries
#include <cfloat>
#include "NarrowBoundingVolume.h"
#include "../mathematics/mathematics.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class OBB :
This class represents a bounding volume of type "Oriented Bounding
Box". It's a box that has a given orientation is space. The three
axis of the OBB are three axis that give the orientation of the
OBB. The three axis are normal vectors to the faces of the OBB.
Those axis are unit length. The three extents are half-widths
of the box along the three axis of the OBB.
-------------------------------------------------------------------
*/
class OBB : public NarrowBoundingVolume {
protected :
Vector3D center; // Center point of the OBB
Vector3D oldAxis[3]; // Array that contains the three unit length axis at the beginning
Vector3D axis[3]; // Array that contains the three unit length axis of the OBB
double extent[3]; // Array that contains the three extents size of the OBB
public :
OBB(const Vector3D& center, const Vector3D& axis1, const Vector3D& axis2,
const Vector3D& axis3, double extent1, double extent2, double extent3); // Constructor
virtual ~OBB(); // Destructor
Vector3D getCenter() const; // Return the center point of the OBB
void setCenter(const Vector3D& center); // Set the center point
Vector3D getAxis(unsigned int index) const throw(std::invalid_argument); // Return an axis of the OBB
void setAxis(unsigned int index, const Vector3D& axis) throw(std::invalid_argument); // Set an axis
Vector3D getVertex(unsigned int index) const throw (std::invalid_argument); // Return a vertex of the OBB
std::vector<Vector3D> getFace(unsigned int index) const throw(std::invalid_argument); // Return the 4 vertices the OBB's face in the direction of a given axis
double getExtent(unsigned int index) const throw(std::invalid_argument); // Return an extent value
void setExtent(unsigned int index, double extent) throw(std::invalid_argument); // Set an extent value
virtual std::vector<Vector3D> getExtremeVertices(const Vector3D& axis) const; // Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis
virtual void update(const Vector3D& newCenter, const Quaternion& rotationQuaternion); // Update the oriented bounding box orientation according to a new orientation of the rigid body
virtual AABB* computeAABB() const; // Return the corresponding AABB
virtual void draw() const; // Draw the OBB (only for testing purpose)
static OBB* computeFromVertices(const std::vector<Vector3D>& vertices, const Vector3D& center); // Compute an OBB from a set of vertices
};
// Return the center point of the OBB
inline Vector3D OBB::getCenter() const {
return center;
}
// Set the center point
inline void OBB::setCenter(const Vector3D& center) {
this->center = center;
}
// Return an axis of the OBB
inline Vector3D OBB::getAxis(unsigned int index) const throw(std::invalid_argument) {
// Check if the index value is valid
if (index >= 0 && index <3) {
return axis[index];
}
else {
// The index value is not valid, we throw an exception
throw std::invalid_argument("Exception : The index value has to be between 0 and 2");
}
}
// Set an axis
inline void OBB::setAxis(unsigned int index, const Vector3D& axis) throw(std::invalid_argument) {
// Check if the index value is valid
if (index >= 0 && index <3) {
this->axis[index] = axis;
}
else {
// The index value is not valid, we throw an exception
throw std::invalid_argument("Exception : The index value has to be between 0 and 2");
}
}
// Return a vertex of the OBB
inline Vector3D OBB::getVertex(unsigned int index) const throw (std::invalid_argument) {
// Check if the index value is valid
if (index >= 0 && index <8) {
Vector3D vertex;
switch(index) {
case 0 : vertex = center + (axis[0]*extent[0]) + (axis[1]*extent[1]) - (axis[2]*extent[2]);
break;
case 1 : vertex = center + (axis[0]*extent[0]) + (axis[1]*extent[1]) + (axis[2]*extent[2]);
break;
case 2 : vertex = center - (axis[0]*extent[0]) + (axis[1]*extent[1]) + (axis[2]*extent[2]);
break;
case 3 : vertex = center - (axis[0]*extent[0]) + (axis[1]*extent[1]) - (axis[2]*extent[2]);
break;
case 4 : vertex = center + (axis[0]*extent[0]) - (axis[1]*extent[1]) - (axis[2]*extent[2]);
break;
case 5 : vertex = center + (axis[0]*extent[0]) - (axis[1]*extent[1]) + (axis[2]*extent[2]);
break;
case 6 : vertex = center - (axis[0]*extent[0]) - (axis[1]*extent[1]) + (axis[2]*extent[2]);
break;
case 7 : vertex = center - (axis[0]*extent[0]) - (axis[1]*extent[1]) - (axis[2]*extent[2]);
break;
}
// Return the vertex
return vertex;
}
else {
// The index value is not valid, we throw an exception
throw std::invalid_argument("Exception : The index value has to be between 0 and 8");
}
}
// Return the 4 vertices of a face of the OBB. The 4 vertices will be ordered. The convention is that the index 0 corresponds to
// the face in the direction of the axis[0], 1 corresponds to the face in the opposite direction of the axis[0], 2 corresponds to
// the face in the direction of the axis[1], etc.
inline std::vector<Vector3D> OBB::getFace(unsigned int index) const throw(std::invalid_argument) {
// Check the argument
if (index >=0 && index <6) {
std::vector<Vector3D> vertices;
switch(index) {
case 0: vertices.push_back(center + (axis[0]*extent[0]) + (axis[1]*extent[1]) - (axis[2]*extent[2]));
vertices.push_back(center + (axis[0]*extent[0]) + (axis[1]*extent[1]) + (axis[2]*extent[2]));
vertices.push_back(center + (axis[0]*extent[0]) - (axis[1]*extent[1]) + (axis[2]*extent[2]));
vertices.push_back(center + (axis[0]*extent[0]) - (axis[1]*extent[1]) - (axis[2]*extent[2]));
break;
case 1: vertices.push_back(center - (axis[0]*extent[0]) + (axis[1]*extent[1]) - (axis[2]*extent[2]));
vertices.push_back(center - (axis[0]*extent[0]) + (axis[1]*extent[1]) + (axis[2]*extent[2]));
vertices.push_back(center - (axis[0]*extent[0]) - (axis[1]*extent[1]) + (axis[2]*extent[2]));
vertices.push_back(center - (axis[0]*extent[0]) - (axis[1]*extent[1]) - (axis[2]*extent[2]));
break;
case 2: vertices.push_back(center + (axis[1]*extent[1]) + (axis[0]*extent[0]) - (axis[2]*extent[2]));
vertices.push_back(center + (axis[1]*extent[1]) + (axis[0]*extent[0]) + (axis[2]*extent[2]));
vertices.push_back(center + (axis[1]*extent[1]) - (axis[0]*extent[0]) + (axis[2]*extent[2]));
vertices.push_back(center + (axis[1]*extent[1]) - (axis[0]*extent[0]) - (axis[2]*extent[2]));
break;
case 3: vertices.push_back(center - (axis[1]*extent[1]) + (axis[0]*extent[0]) - (axis[2]*extent[2]));
vertices.push_back(center - (axis[1]*extent[1]) + (axis[0]*extent[0]) + (axis[2]*extent[2]));
vertices.push_back(center - (axis[1]*extent[1]) - (axis[0]*extent[0]) + (axis[2]*extent[2]));
vertices.push_back(center - (axis[1]*extent[1]) - (axis[0]*extent[0]) - (axis[2]*extent[2]));
break;
case 4: vertices.push_back(center + (axis[2]*extent[2]) + (axis[0]*extent[0]) - (axis[1]*extent[1]));
vertices.push_back(center + (axis[2]*extent[2]) + (axis[0]*extent[0]) + (axis[1]*extent[1]));
vertices.push_back(center + (axis[2]*extent[2]) - (axis[0]*extent[0]) + (axis[1]*extent[1]));
vertices.push_back(center + (axis[2]*extent[2]) - (axis[0]*extent[0]) - (axis[1]*extent[1]));
break;
case 5: vertices.push_back(center - (axis[2]*extent[2]) + (axis[0]*extent[0]) - (axis[1]*extent[1]));
vertices.push_back(center - (axis[2]*extent[2]) + (axis[0]*extent[0]) + (axis[1]*extent[1]));
vertices.push_back(center - (axis[2]*extent[2]) - (axis[0]*extent[0]) + (axis[1]*extent[1]));
vertices.push_back(center - (axis[2]*extent[2]) - (axis[0]*extent[0]) - (axis[1]*extent[1]));
break;
}
// Return the vertices
assert(vertices.size() == 4);
return vertices;
}
else {
// Throw an exception
throw std::invalid_argument("Exception: The argument must be between 0 and 5");
}
}
// Return an extent value
inline double OBB::getExtent(unsigned int index) const throw(std::invalid_argument) {
// Check if the index value is valid
if (index >= 0 && index <3) {
return extent[index];
}
else {
// The index value is not valid, we throw an exception
throw std::invalid_argument("Exception : The index value has to be between 0 and 2");
}
}
// Set an extent value
inline void OBB::setExtent(unsigned int index, double extent) throw(std::invalid_argument) {
// Check if the index value is valid
if (index >= 0 && index <3) {
this->extent[index] = extent;
}
else {
// The index value is not valid, we throw an exception
throw std::invalid_argument("Exception : The index value has to be between 0 and 2");
}
}
// Update the orientation of the OBB according to the orientation of the rigid body
inline void OBB::update(const Vector3D& newCenter, const Quaternion& rotationQuaternion) {
// Update the center of the OBB
center = newCenter;
// Update the three axis of the OBB by rotating and normalize then
axis[0] = rotateVectorWithQuaternion(oldAxis[0], rotationQuaternion).getUnit();
axis[1] = rotateVectorWithQuaternion(oldAxis[1], rotationQuaternion).getUnit();
axis[2] = rotateVectorWithQuaternion(oldAxis[2], rotationQuaternion).getUnit();
}
}; // End of the ReactPhysics3D namespace
#endif

67
src/reactphysics3d/body/RigidBody.cpp Normal file
View File

@ -0,0 +1,67 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "RigidBody.h"
#include "BroadBoundingVolume.h"
#include "NarrowBoundingVolume.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
RigidBody::RigidBody(const Vector3D& position, const Quaternion& orientation, double mass, const Matrix3x3& inertiaTensorLocal,
NarrowBoundingVolume* narrowBoundingVolume)
: Body(mass), position(position), orientation(orientation.getUnit()), inertiaTensorLocal(inertiaTensorLocal),
inertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), massInverse(1.0/mass), oldPosition(position), oldOrientation(orientation) {
restitution = 1.0;
isMotionEnabled = true;
isCollisionEnabled = true;
interpolationFactor = 0.0;
// Set the bounding volume for the narrow-phase collision detection
setNarrowBoundingVolume(narrowBoundingVolume);
// Compute the broad-phase bounding volume (an AABB)
setBroadBoundingVolume(narrowBoundingVolume->computeAABB());
// Update the orientation of the OBB according to the orientation of the rigid body
update();
assert(broadBoundingVolume);
assert(narrowBoundingVolume);
}
// Destructor
RigidBody::~RigidBody() {
};
// Update the rigid body in order to reflect a change in the body state
void RigidBody::update() {
// Update the orientation of the corresponding bounding volumes of the rigid body
broadBoundingVolume->update(position, orientation);
narrowBoundingVolume->update(position, orientation);
}

246
src/reactphysics3d/body/RigidBody.h Normal file
View File

@ -0,0 +1,246 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef RIGIDBODY_H
#define RIGIDBODY_H
// Libraries
#include <cassert>
#include "Body.h"
#include "../mathematics/mathematics.h"
// Namespace reactphysics3d
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class RigidBody :
This class represents a rigid body of the physics
engine. A rigid body is a non-deformable body that
has a constant mass.
-------------------------------------------------------------------
*/
class RigidBody : public Body {
protected :
Vector3D position; // Position of the center of mass of the body
Vector3D oldPosition; // Old position used to compute the interpolated position
Quaternion orientation; // Orientation quaternion of the body
Quaternion oldOrientation; // Old orientation used to compute the interpolated orientation
Vector3D linearVelocity; // Linear velocity of the body
Vector3D angularVelocity; // Angular velocity of the body
Vector3D externalForce; // Current external force on the body
Vector3D externalTorque; // Current external torque on the body
Matrix3x3 inertiaTensorLocal; // Local inertia tensor of the body (in body coordinates)
Matrix3x3 inertiaTensorLocalInverse; // Inverse of the inertia tensor of the body (in body coordinates)
double massInverse; // Inverse of the mass of the body
double interpolationFactor; // Interpolation factor used for the state interpolation
double restitution; // Coefficient of restitution (between 0 and 1), 1 for a very boucing body
public :
RigidBody(const Vector3D& position, const Quaternion& orientation, double mass,
const Matrix3x3& inertiaTensorLocal, NarrowBoundingVolume* narrowBoundingVolume); // Constructor // Copy-constructor
virtual ~RigidBody(); // Destructor
Vector3D getPosition() const; // Return the position of the body
void setPosition(const Vector3D& position); // Set the position of the body
Quaternion getOrientation() const; // Return the orientation quaternion
void setOrientation(const Quaternion& orientation); // Set the orientation quaternion
Vector3D getLinearVelocity() const; // Return the linear velocity
void setLinearVelocity(const Vector3D& linearVelocity); // Set the linear velocity of the body
Vector3D getAngularVelocity() const; // Return the angular velocity
void setAngularVelocity(const Vector3D& angularVelocity); // Set the angular velocity
void setMassInverse(double massInverse); // Set the inverse of the mass
Vector3D getExternalForce() const; // Return the current external force of the body
void setExternalForce(const Vector3D& force); // Set the current external force on the body
Vector3D getExternalTorque() const; // Return the current external torque of the body
void setExternalTorque(const Vector3D& torque); // Set the current external torque of the body
double getMassInverse() const; // Return the inverse of the mass of the body
Matrix3x3 getInertiaTensorLocal() const; // Return the local inertia tensor of the body (in body coordinates)
void setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal); // Set the local inertia tensor of the body (in body coordinates)
Matrix3x3 getInertiaTensorLocalInverse() const; // Get the inverse of the inertia tensor
Matrix3x3 getInertiaTensorWorld() const; // Return the inertia tensor in world coordinates
Matrix3x3 getInertiaTensorInverseWorld() const; // Return the inverse of the inertia tensor in world coordinates
void setInterpolationFactor(double factor); // Set the interpolation factor of the body
Vector3D getInterpolatedPosition() const; // Return the interpolated position
Quaternion getInterpolatedOrientation() const; // Return the interpolated orientation
double getRestitution() const; // Get the restitution coefficient
void setRestitution(double restitution) throw(std::invalid_argument); // Set the restitution coefficient
void updateOldPositionAndOrientation(); // Update the previous position and orientation of the body
void update(); // Update the rigid body in order to reflect a change in the body state
};
// --- Inline functions --- //
// Return the position of the body
inline Vector3D RigidBody::getPosition() const {
return position;
}
// Set the position of the body
inline void RigidBody::setPosition(const Vector3D& position) {
this->position = position;
}
// Return the orientation quaternion of the body
inline Quaternion RigidBody::getOrientation() const {
return orientation;
}
// Set the orientation quaternion
inline void RigidBody::setOrientation(const Quaternion& orientation) {
this->orientation = orientation;
// Normalize the orientation quaternion
orientation.getUnit();
}
// Return the linear velocity
inline Vector3D RigidBody::getLinearVelocity() const {
return linearVelocity;
}
// Return the angular velocity of the body
inline Vector3D RigidBody::getAngularVelocity() const {
return angularVelocity;
}
inline void RigidBody::setAngularVelocity(const Vector3D& angularVelocity) {
this->angularVelocity = angularVelocity;
}
// Set the inverse of the mass
inline void RigidBody::setMassInverse(double massInverse) {
this->massInverse = massInverse;
}
// Get the inverse of the inertia tensor
inline Matrix3x3 RigidBody::getInertiaTensorLocalInverse() const {
return inertiaTensorLocalInverse;
}
// Return the external force on the body
inline Vector3D RigidBody::getExternalForce() const {
return externalForce;
}
// Set the external force on the body
inline void RigidBody::setExternalForce(const Vector3D& force) {
this->externalForce = force;
}
// Return the current external torque on the body
inline Vector3D RigidBody::getExternalTorque() const {
return externalTorque;
}
// Set the current external torque on the body
inline void RigidBody::setExternalTorque(const Vector3D& torque) {
this->externalTorque = torque;
}
// Return the inverse of the mass of the body
inline double RigidBody::getMassInverse() const {
return massInverse;
}
// Return the local inertia tensor of the body (in body coordinates)
inline Matrix3x3 RigidBody::getInertiaTensorLocal() const {
return inertiaTensorLocal;
}
// Set the local inertia tensor of the body (in body coordinates)
inline void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
this->inertiaTensorLocal = inertiaTensorLocal;
}
// Return the inertia tensor in world coordinates
// The inertia tensor I_w in world coordinates in computed with the local inertia tensor I_b in body coordinates
// by I_w = R * I_b * R^T
// where R is the rotation matrix (and R^T its transpose) of the current orientation quaternion of the body
inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
// Compute and return the inertia tensor in world coordinates
return orientation.getMatrix() * inertiaTensorLocal * orientation.getMatrix().getTranspose();
}
// Return the inverse of the inertia tensor in world coordinates
// The inertia tensor I_w in world coordinates in computed with the local inverse inertia tensor I_b^-1 in body coordinates
// by I_w = R * I_b^-1 * R^T
// where R is the rotation matrix (and R^T its transpose) of the current orientation quaternion of the body
inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
// Compute and return the inertia tensor in world coordinates
return orientation.getMatrix() * inertiaTensorLocalInverse * orientation.getMatrix().getTranspose();
}
// Set the interpolation factor of the body
inline void RigidBody::setInterpolationFactor(double factor) {
// Set the factor
interpolationFactor = factor;
}
// Return the interpolated position
inline Vector3D RigidBody::getInterpolatedPosition() const {
// Compute the interpolated position
return oldPosition * (1-interpolationFactor) + position * interpolationFactor;
}
// Return the interpolated orientation
inline Quaternion RigidBody::getInterpolatedOrientation() const {
// Compute the interpolated orientation
return Quaternion::slerp(oldOrientation, orientation, interpolationFactor);
}
// Set the linear velocity of the rigid body
inline void RigidBody::setLinearVelocity(const Vector3D& linearVelocity) {
// If the body is able to move
if (isMotionEnabled) {
// Update the linear velocity of the current body state
this->linearVelocity = linearVelocity;
}
}
// Get the restitution coeffficient of the rigid body
inline double RigidBody::getRestitution() const {
return restitution;
}
// Set the restitution coefficient
inline void RigidBody::setRestitution(double restitution) throw(std::invalid_argument) {
// Check if the restitution coefficient is between 0 and 1
if (restitution >= 0.0 && restitution <= 1.0) {
this->restitution = restitution;
}
else {
throw std::invalid_argument("Error : the restitution coefficent must be between 0 and 1");
}
}
// Update the previous body state of the body
// This is used to compute the interpolated position and orientation of the body
inline void RigidBody::updateOldPositionAndOrientation() {
oldPosition = position;
oldOrientation = orientation;
}
} // End of the ReactPhyscis3D namespace
#endif

39
src/reactphysics3d/collision/BroadPhaseAlgorithm.cpp Normal file
View File

@ -0,0 +1,39 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "BroadPhaseAlgorithm.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
BroadPhaseAlgorithm::BroadPhaseAlgorithm() {
}
// Destructor
BroadPhaseAlgorithm::~BroadPhaseAlgorithm() {
}

61
src/reactphysics3d/collision/BroadPhaseAlgorithm.h Normal file
View File

@ -0,0 +1,61 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef BROADPHASEALGORITHM_H
#define BROADPHASEALGORITHM_H
// Libraries
#include "../body/BoundingVolume.h"
// Namespace ReactPhysics3D
namespace reactphysics3d {
/* --------------------------------------------------------------------
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.
--------------------------------------------------------------------
*/
class BroadPhaseAlgorithm {
protected :
public :
BroadPhaseAlgorithm(); // Constructor
virtual ~BroadPhaseAlgorithm(); // Destructor
virtual void computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs)=0; // Compute the possible collision pairs of bodies
};
} // End of reactphysics3d namespace
#endif

253
src/reactphysics3d/collision/CollisionDetection.cpp Normal file
View File

@ -0,0 +1,253 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "CollisionDetection.h"
#include "SAPAlgorithm.h"
#include "SATAlgorithm.h"
#include "../body/Body.h"
#include "../body/OBB.h"
#include "../body/RigidBody.h"
#include <cassert>
#include <complex>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
using namespace std;
// Constructor
CollisionDetection::CollisionDetection(PhysicsWorld* world) {
this->world = world;
// Create the broad-phase algorithm that will be used (Sweep and Prune with AABB)
broadPhaseAlgorithm = new SAPAlgorithm();
// Create the narrow-phase algorithm that will be used (Separating axis algorithm)
narrowPhaseAlgorithm = new SATAlgorithm();
}
// Destructor
CollisionDetection::~CollisionDetection() {
}
// Compute the collision detection
bool CollisionDetection::computeCollisionDetection() {
world->removeAllContactConstraints();
possibleCollisionPairs.clear();
contactInfos.clear();
// Compute the broad-phase collision detection
computeBroadPhase();
// Compute the narrow-phase collision detection
computeNarrowPhase();
// Compute all the new contacts
computeAllContacts();
// Return true if at least one contact has been found
return (contactInfos.size() > 0);
}
// Compute the broad-phase collision detection
void CollisionDetection::computeBroadPhase() {
// Clear the set of possible colliding pairs of bodies
possibleCollisionPairs.clear();
// Compute the set of possible collision pairs of bodies
broadPhaseAlgorithm->computePossibleCollisionPairs(world->getAddedBodies(), world->getRemovedBodies(), possibleCollisionPairs);
}
// Compute the narrow-phase collision detection
void CollisionDetection::computeNarrowPhase() {
// For each possible collision pair of bodies
for (unsigned int i=0; i<possibleCollisionPairs.size(); i++) {
ContactInfo* contactInfo = 0;
// Use the narrow-phase collision detection algorithm to check if the really are a contact
if (narrowPhaseAlgorithm->testCollision(possibleCollisionPairs.at(i).first->getNarrowBoundingVolume(), possibleCollisionPairs.at(i).second->getNarrowBoundingVolume(), contactInfo)) {
assert(contactInfo != 0);
// Add the contact info the current list of collision informations
contactInfos.push_back(contactInfo);
}
}
}
// Compute all the contacts from the contact info list
void CollisionDetection::computeAllContacts() {
// For each possible contact info (computed during narrow-phase collision detection)
for (unsigned int i=0; i<contactInfos.size(); i++) {
ContactInfo* contactInfo = contactInfos.at(i);
assert(contactInfo != 0);
// Compute one or several new contacts and add them into the physics world
computeContact(contactInfo);
}
}
// Compute a contact (and add it to the physics world) for two colliding bodies
void CollisionDetection::computeContact(const ContactInfo* const contactInfo) {
// Extract informations from the contact info structure
const OBB* const obb1 = contactInfo->obb1;
const OBB* const obb2 = contactInfo->obb2;
Vector3D normal = contactInfo->normal;
double penetrationDepth = contactInfo->penetrationDepth;
const vector<Vector3D> obb1ExtremePoints = obb1->getExtremeVertices(normal);
const vector<Vector3D> obb2ExtremePoints = obb2->getExtremeVertices(normal.getOpposite());
unsigned int nbVerticesExtremeOBB1 = obb1ExtremePoints.size();
unsigned int nbVerticesExtremeOBB2 = obb2ExtremePoints.size();
assert(nbVerticesExtremeOBB1==1 || nbVerticesExtremeOBB1==2 || nbVerticesExtremeOBB1==4);
assert(nbVerticesExtremeOBB2==1 || nbVerticesExtremeOBB2==2 || nbVerticesExtremeOBB2==4);
assert(approxEqual(normal.length(), 1.0));
// If it's a Vertex-Something contact
if (nbVerticesExtremeOBB1 == 1) {
// Create a new contact and add it to the physics world
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, obb1ExtremePoints.at(0)));
}
else if(nbVerticesExtremeOBB2 == 1) { // If its a Vertex-Something contact
// Create a new contact and add it to the physics world
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, obb2ExtremePoints.at(0)));
}
else if (nbVerticesExtremeOBB1 == 2 && nbVerticesExtremeOBB2 == 2) { // If it's an edge-edge contact
// Compute the two vectors of the segment lines
Vector3D d1 = obb1ExtremePoints[1] - obb1ExtremePoints[0];
Vector3D d2 = obb2ExtremePoints[1] - obb2ExtremePoints[0];
double alpha, beta;
vector<Vector3D> contactSet;
// If the two edges are parallel
if (d1.isParallelWith(d2)) {
Vector3D contactPointA;
Vector3D contactPointB;
// Compute the intersection between the two edges
computeParallelSegmentsIntersection(obb1ExtremePoints[0], obb1ExtremePoints[1], obb2ExtremePoints[0], obb2ExtremePoints[1],
contactPointA, contactPointB);
// Add the two contact points in the contact set
contactSet.push_back(contactPointA);
contactSet.push_back(contactPointB);
}
else { // If the two edges are not parallel
// Compute the closest two points between the two line segments
closestPointsBetweenTwoLines(obb1ExtremePoints[0], d1, obb2ExtremePoints[0], d2, &alpha, &beta);
Vector3D pointA = obb1ExtremePoints[0] + d1 * alpha;
Vector3D pointB = obb2ExtremePoints[0] + d2 * beta;
// Compute the contact point as halfway between the 2 closest points
Vector3D contactPoint = 0.5 * (pointA + pointB);
// Add the contact point into the contact set
contactSet.push_back(contactPoint);
}
// For each point of the set of contact points
for (unsigned int i=0; i<contactSet.size(); i++) {
// Create a new contact and add it to the physics world
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, contactSet.at(i)));
}
}
else if(nbVerticesExtremeOBB1 == 2 && nbVerticesExtremeOBB2 == 4) { // If it's an edge-face contact
// Compute the projection of the edge of OBB1 onto the same plane of the face of OBB2
vector<Vector3D> edge = projectPointsOntoPlane(obb1ExtremePoints, obb2ExtremePoints[0], normal);
// Clip the edge of OBB1 using the face of OBB2
vector<Vector3D> clippedEdge = clipSegmentWithRectangleInPlane(edge, obb2ExtremePoints);
// TODO : Correct this bug
// The following code is to correct a bug when the projected "edge" is not inside the clip rectangle
// of obb1ExtremePoints. Therefore, we compute the nearest two points that are on the rectangle.
if (clippedEdge.size() != 2) {
edge.clear();
edge.push_back(computeNearestPointOnRectangle(edge[0], obb2ExtremePoints));
edge.push_back(computeNearestPointOnRectangle(edge[1], obb2ExtremePoints));
clippedEdge = clipSegmentWithRectangleInPlane(edge, obb2ExtremePoints);
}
// Move the clipped edge halway between the edge of OBB1 and the face of OBB2
clippedEdge = movePoints(clippedEdge, penetrationDepth/2.0 * normal.getOpposite());
assert(clippedEdge.size() == 2);
// For each point of the contact set
for (unsigned int i=0; i<clippedEdge.size(); i++) {
// Create a new contact and add it to the physics world
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedEdge.at(i)));
}
}
else if(nbVerticesExtremeOBB1 == 4 && nbVerticesExtremeOBB2 == 2) { // If it's an edge-face contact
// Compute the projection of the edge of OBB2 onto the same plane of the face of OBB1
vector<Vector3D> edge = projectPointsOntoPlane(obb2ExtremePoints, obb1ExtremePoints[0], normal);
// Clip the edge of OBB2 using the face of OBB1
vector<Vector3D> clippedEdge = clipSegmentWithRectangleInPlane(edge, obb1ExtremePoints);
// TODO : Correct this bug
// The following code is to correct a bug when the projected "edge" is not inside the clip rectangle
// of obb1ExtremePoints. Therefore, we compute the nearest two points that are on the rectangle.
if (clippedEdge.size() != 2) {
edge.clear();
edge.push_back(computeNearestPointOnRectangle(edge[0], obb1ExtremePoints));
edge.push_back(computeNearestPointOnRectangle(edge[1], obb1ExtremePoints));
clippedEdge = clipSegmentWithRectangleInPlane(edge, obb1ExtremePoints);
}
// Move the clipped edge halfway between the face of OBB1 and the edge of OBB2
clippedEdge = movePoints(clippedEdge, penetrationDepth/2.0 * normal);
assert(clippedEdge.size() == 2);
// For each point of the contact set
for (unsigned int i=0; i<clippedEdge.size(); i++) {
// Create a new contact and add it to the physics world
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedEdge.at(i)));
}
}
else { // If it's a face-face contact
// Compute the projection of the face vertices of OBB2 onto the plane of the face of OBB1
vector<Vector3D> faceOBB2 = projectPointsOntoPlane(obb2ExtremePoints, obb1ExtremePoints[0], normal);
// Clip the face of OBB2 using the face of OBB1
vector<Vector3D> clippedFace = clipPolygonWithRectangleInPlane(faceOBB2, obb1ExtremePoints);
// Move the clipped face halfway between the face of OBB1 and the face of OBB2
clippedFace = movePoints(clippedFace, penetrationDepth/2.0 * normal);
assert(clippedFace.size() >= 3);
// For each point of the contact set
for (unsigned int i=0; i<clippedFace.size(); i++) {
// Create a new contact and add it to the physics world
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedFace.at(i)));
}
}
}

70
src/reactphysics3d/collision/CollisionDetection.h Normal file
View File

@ -0,0 +1,70 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef COLLISIONDETECTION_H
#define COLLISIONDETECTION_H
// Libraries
#include "BroadPhaseAlgorithm.h"
#include "NarrowPhaseAlgorithm.h"
#include "../body/Body.h"
#include "../engine/PhysicsWorld.h"
#include "ContactInfo.h"
#include <vector>
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class CollisionDetection :
This class computes the collision detection algorithms. We first
perfom a broad-phase algorithm to know wich pairs of bodies can
collide and then we run a narrow-phase algorithm to compute the
collision contacts between bodies.
-------------------------------------------------------------------
*/
class CollisionDetection {
private :
PhysicsWorld* world; // Pointer to the physics world
std::vector<std::pair<const Body*, const Body* > > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase)
std::vector<ContactInfo*> contactInfos; // Contact informations (computed by narrowphase)
BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm
NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm
void computeBroadPhase(); // Compute the broad-phase collision detection
void computeNarrowPhase(); // Compute the narrow-phase collision detection
void computeAllContacts(); // Compute all the contacts from the collision info list
void computeContact(const ContactInfo* const contactInfo); // Compute a contact (and add it to the physics world) for two colliding bodies
void computeContact2(const ContactInfo* const contactInfo); // Compute a contact (and add it to the physics world) for two colliding bodies
public :
CollisionDetection(PhysicsWorld* physicsWorld); // Constructor
~CollisionDetection(); // Destructor
bool computeCollisionDetection(); // Compute the collision detection
};
} // End of the ReactPhysics3D namespace
#endif

34
src/reactphysics3d/collision/ContactInfo.cpp Normal file
View File

@ -0,0 +1,34 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "ContactInfo.h"
using namespace reactphysics3d;
// Constructor
ContactInfo::ContactInfo(const OBB* const obb1, const OBB* const obb2, const Vector3D& normal, double penetrationDepth)
: obb1(obb1), obb2(obb2), normal(normal), penetrationDepth(penetrationDepth) {
}

57
src/reactphysics3d/collision/ContactInfo.h Normal file
View File

@ -0,0 +1,57 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef CONTACTINFO_H
#define CONTACTINFO_H
// Libraries
#include "../body/OBB.h"
#include "../mathematics/mathematics.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Structure ContactInfo :
This structure contains informations about a collision contact
computed durring the narow phase collision detection. Those
informations are use to compute the contact set for a contact
between two bodies.
-------------------------------------------------------------------
*/
struct ContactInfo {
public:
// TODO : Use polymorphism here (change OBB into BoundingVolume to be more general)
const OBB* const obb1; // Body pointer of the first bounding volume
const OBB* const obb2; // Body pointer of the second bounding volume
const Vector3D normal; // Normal vector the the collision contact
const double penetrationDepth; // Penetration depth of the contact
ContactInfo(const OBB* const obb1, const OBB* const obb2, const Vector3D& normal, double penetrationDepth); // Constructor
};
} // End of the ReactPhysics3D namespace
#endif

39
src/reactphysics3d/collision/NarrowPhaseAlgorithm.cpp Normal file
View File

@ -0,0 +1,39 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "NarrowPhaseAlgorithm.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
NarrowPhaseAlgorithm::NarrowPhaseAlgorithm() {
}
// Destructor
NarrowPhaseAlgorithm::~NarrowPhaseAlgorithm() {
}

57
src/reactphysics3d/collision/NarrowPhaseAlgorithm.h Normal file
View File

@ -0,0 +1,57 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef NARROWPHASEALGORITHM_H
#define NARROWPHASEALGORITHM_H
// Libraries
#include "../body/BoundingVolume.h"
#include "ContactInfo.h"
// Namespace ReactPhysics3D
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class NarrowPhaseAlgorithm :
This class is an abstract class that represents an algorithm
used to perform the narrow phase of a collision detection. The
goal of the narrow phase algorithm is to compute contact
informations of a collision between two bodies.
-------------------------------------------------------------------
*/
class NarrowPhaseAlgorithm {
private :
public :
NarrowPhaseAlgorithm(); // Constructor
virtual ~NarrowPhaseAlgorithm(); // Destructor
virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide
};
} // End of reactphysics3d namespace
#endif

41
src/reactphysics3d/collision/NoBroadPhaseAlgorithm.cpp Normal file
View File

@ -0,0 +1,41 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "NoBroadPhaseAlgorithm.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
NoBroadPhaseAlgorithm::NoBroadPhaseAlgorithm() {
}
// Destructor
NoBroadPhaseAlgorithm::~NoBroadPhaseAlgorithm() {
}

94
src/reactphysics3d/collision/NoBroadPhaseAlgorithm.h Normal file
View File

@ -0,0 +1,94 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef NOBROADPHASEALGORITHM_H
#define NOBROADPHASEALGORITHM_H
// Libraries
#include "BroadPhaseAlgorithm.h"
#include <algorithm>
// 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 :
std::vector<Body*> bodies; // All bodies of the engine
public :
NoBroadPhaseAlgorithm(); // Constructor
virtual ~NoBroadPhaseAlgorithm(); // Destructor
virtual void computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs); // Compute the possible collision pairs of bodies
};
// Compute the possible collision pairs of bodies
// The arguments "addedBodies" and "removedBodies" are respectively the set
// of bodies that have been added and removed since the last broad-phase
// computation. Before the call, the argument "possibleCollisionPairs"
// correspond to the possible colliding pairs of bodies from the last broad-phase
// computation. This methods computes the current possible collision pairs of
// bodies and update the "possibleCollisionPairs" argument. This broad-phase
// algorithm doesn't do anything and therefore the "possibleCollisionPairs" set
// must contains all the possible pairs of bodies
inline void NoBroadPhaseAlgorithm::computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs) {
// Add the new bodies
for (std::vector<Body*>::iterator it = addedBodies.begin(); it < addedBodies.end(); it++) {
bodies.push_back(*it);
}
// Remove the bodies to be removed
for (std::vector<Body*>::iterator it = removedBodies.begin(); it < removedBodies.end(); it++) {
bodies.erase(std::find(bodies.begin(), bodies.end(), *it));
}
// If the set of bodies have been changed
if (addedBodies.size() + removedBodies.size() > 0) {
// Recompute all the possible pairs of bodies
possibleCollisionPairs.clear();
for (std::vector<Body*>::iterator it1 = addedBodies.begin(); it1 < addedBodies.end(); it1++) {
for (std::vector<Body*>::iterator it2 = addedBodies.begin(); it2 < addedBodies.end(); it2++) {
if (*it1 != *it2) {
possibleCollisionPairs.push_back(std::make_pair(*it1, *it2));
}
}
}
}
}
} // End of reactphysics3d namespace
#endif

151
src/reactphysics3d/collision/SAPAlgorithm.cpp Normal file
View File

@ -0,0 +1,151 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "SAPAlgorithm.h"
#include <algorithm>
// Namespaces
using namespace reactphysics3d;
using namespace std;
// Initialize the static attributes
unsigned short int SAPAlgorithm::sortAxis = 0;
// Constructor
SAPAlgorithm::SAPAlgorithm() {
}
// Destructor
SAPAlgorithm::~SAPAlgorithm() {
}
// Remove the AABB representation of a given set of bodies from the sortedAABBs set
void SAPAlgorithm::removeBodiesAABB(vector<Body*> bodies) {
vector<const AABB*>::iterator elemToRemove;
const AABB* aabb;
// Removed the AABB of the bodies that have been removed
for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); it++) {
aabb = dynamic_cast<const AABB*>((*it)->getBroadBoundingVolume());
assert(aabb);
elemToRemove = find(sortedAABBs.begin(), sortedAABBs.end(), aabb);
assert((*elemToRemove) == aabb);
sortedAABBs.erase(elemToRemove);
}
}
// Add the AABB representation of a given body in the sortedAABBs set
void SAPAlgorithm::addBodiesAABB(vector<Body*> bodies) {
const AABB* aabb;
for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); it++) {
aabb = 0;
aabb = dynamic_cast<const AABB*>((*it)->getBroadBoundingVolume());
assert(aabb);
sortedAABBs.push_back(aabb);
}
}
// Compute the possible collision pairs of bodies
// The arguments "addedBodies" and "removedBodies" are respectively the set
// of bodies that have been added and removed since the last broad-phase
// computation. Before the call, the argument "possibleCollisionPairs"
// correspond to the possible colliding pairs of bodies from the last broad-phase
// computation. This methods computes the current possible collision pairs of
// bodies and update the "possibleCollisionPairs" argument.
void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vector<Body*> removedBodies,
vector<pair<const Body*, const Body* > >& possibleCollisionPairs) {
double variance[3]; // Variance of the distribution of the AABBs on the three x, y and z axis
double esperance[] = {0.0, 0.0, 0.0}; // Esperance of the distribution of the AABBs on the three x, y and z axis
double esperanceSquare[] = {0.0, 0.0, 0.0}; // Esperance of the square of the distribution values of the AABBs on the three x, y and z axis
vector<const AABB*>::iterator it; // Iterator on the sortedAABBs set
vector<const AABB*>::iterator it2; // Second iterator
Vector3D center3D; // Center of the current AABB
double center[3]; // Coordinates of the center of the current AABB
int i;
const Body* body; // Body pointer on the body corresponding to an AABB
uint nbAABBs = sortedAABBs.size(); // Number of AABBs
// Removed the bodies to remove
removeBodiesAABB(removedBodies);
// Add the bodies to add
addBodiesAABB(addedBodies);
// Sort the set of AABBs
sort(sortedAABBs.begin(), sortedAABBs.end(), compareAABBs);
// Sweep the sorted set of AABBs
for (vector<const AABB*>::iterator it = sortedAABBs.begin(); it != sortedAABBs.end(); it++) {
// If the collision of the AABB's corresponding body is disabled
if (!(*it)->getBodyPointer()->getIsCollisionEnabled()) {
// Go to the next AABB to test
continue;
}
// Center of the current AABB
center3D = (*it)->getCenter();
center[0] = center3D.getX();
center[1] = center3D.getY();
center[2] = center3D.getZ();
// Update the esperance and esperanceSquare values to compute the variance
for (i=0; i<3; i++) {
esperance[i] += center[i];
esperanceSquare[i] += center[i] * center[i];
}
// Test collision against all possible overlapping AABBs following the current one
for (it2 = it + 1; it2 < sortedAABBs.end(); it2++) {
// Stop when the tested AABBs are beyond the end of the current AABB
if ((*it2)->getMinValueOnAxis(sortAxis) > (*it)->getMaxValueOnAxis(sortAxis)) {
break;
}
body = (*it2)->getBodyPointer();
// Test if both AABBs overlap
if (body->getIsCollisionEnabled() && (*it)->testCollision(*(*it2))) {
// Add the current pair of AABBs into the possibleCollisionPairs set
possibleCollisionPairs.push_back(make_pair((*it)->getBodyPointer(), (*it2)->getBodyPointer()));
}
}
}
// Compute the variance of the distribution of the AABBs on the three x,y and z axis
for (i=0; i<3; i++) {
variance[i] = esperanceSquare[i] - esperance[i] * esperance[i] / nbAABBs;
}
// Update the sorted Axis according to the axis with the largest variance
sortAxis = 0;
if (variance[1] > variance[0]) sortAxis = 1;
if (variance[2] > variance[sortAxis]) sortAxis = 2;
}

82
src/reactphysics3d/collision/SAPAlgorithm.h Normal file
View File

@ -0,0 +1,82 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef SAPALGORITHM_H
#define SAPALGORITHM_H
// Libraries
#include "BroadPhaseAlgorithm.h"
#include "../body/AABB.h"
// Namespace ReactPhysics3D
namespace reactphysics3d {
/* --------------------------------------------------------------------
Class SAPAlgorithm :
This class implements the Sweep and Prune (SAP) broad-phase
algorithm. This algorithm uses the AABB bounding-volume of the
bodies and keep a sorted representation of the intervals of the
bodies' AABB on the three x. y and z axis. Given this sorted
representation, we can efficiently compute the set of possible
colliding pairs of bodies. At each broad-phase computation, we
should sort the AABB according to the axis (x, y or z) with the
largest variance of the AABBs positions in order that the sorted
AABB's set does not change a lot between each computations. To do
so, we compute at each time the variance of each axis and select
the axis (sortAxis) with the largest variance for the next
broad-phase computation.
--------------------------------------------------------------------
*/
class SAPAlgorithm : public BroadPhaseAlgorithm {
protected :
std::vector<const AABB*> sortedAABBs; // Sorted set of AABB of the bodies on one of the x.y or z axis
static unsigned short int sortAxis; // Current sorting axis (0 for x, 1 for y, 2 for z axis)
static bool compareAABBs(const AABB* a, const AABB* b); // Static method that compare two AABBs (in order to sort them)
void removeBodiesAABB(std::vector<Body*> bodies); // Remove the AABB representation of a given set of bodies from the sortedAABBs set
void addBodiesAABB(std::vector<Body*> bodies); // Add the AABB representation of a given set of bodies in the sortedAABBs set
public :
SAPAlgorithm(); // Constructor
virtual ~SAPAlgorithm(); // Destructor
virtual void computePossibleCollisionPairs(std::vector<Body*> addedBodies, std::vector<Body*> removedBodies,
std::vector<std::pair<const Body*, const Body* > >& possibleCollisionPairs); // Compute the possible collision pairs of bodies
};
// Static method that compare two AABBs. This method will be used to compare to AABBs
// in order to sort them with the sort() function to obtain the sortedAABBs set.
// This method must return true if the AABB "a" goes before the AABB "b". We
// consider that "a" goes before "b" if the minimum value of "a" on the current
// sorting axis (sortAxis) is smaller than the minimum value of "b" on this same
// axis.
inline bool SAPAlgorithm::compareAABBs(const AABB* a, const AABB* b) {
return (a->getMinValueOnAxis(sortAxis) < b->getMinValueOnAxis(sortAxis));
}
} // End of reactphysics3d namespace
#endif

420
src/reactphysics3d/collision/SATAlgorithm.cpp Normal file
View File

@ -0,0 +1,420 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "SATAlgorithm.h"
#include "../body/OBB.h"
#include "../body/RigidBody.h"
#include "../constraint/Contact.h"
#include <algorithm>
#include <cfloat>
#include <cmath>
#include <cassert>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
SATAlgorithm::SATAlgorithm() {
}
// Destructor
SATAlgorithm::~SATAlgorithm() {
}
// Return true and compute a contact info if the two bounding volume collide.
// The method returns false if there is no collision between the two bounding volumes.
bool SATAlgorithm::testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo) {
assert(boundingVolume1 != boundingVolume2);
// If the two bounding volumes are OBB
//const OBB* const obb1 = dynamic_cast<const OBB* const>(boundingVolume1);
//const OBB* const obb2 = dynamic_cast<const OBB* const>(boundingVolume2);
const OBB* const obb1 = dynamic_cast<const OBB* const>(boundingVolume1);
const OBB* const obb2 = dynamic_cast<const OBB* const>(boundingVolume2);
// If the two bounding volumes are OBB
if (obb1 && obb2) {
// Compute the collision test between two OBB
return computeCollisionTest(obb1, obb2, contactInfo);
}
else {
return false;
}
}
// This method returns true and computes a contact info if the two OBB intersect.
// This method implements the separating algorithm between two OBBs. The goal of this method is to test if the
// two OBBs intersect or not. If they intersect we report a contact info and the method returns true. If
// they don't intersect, the method returns false. The separation axis that have to be tested for two
// OBB are the six face normals (3 for each OBB) and the nine vectors V = Ai x Bj where Ai is the ith face normal
// vector of OBB 1 and Bj is the jth face normal vector of OBB 2. We will use the notation Ai for the ith face
// normal of OBB 1 and Bj for the jth face normal of OBB 2.
bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const obb2, ContactInfo*& contactInfo) const {
double center; // Center of a projection interval
double radius1; // Radius of projection interval [min1, max1]
double radius2; // Radius of projection interval [min2, max2]
double min1; // Minimum of interval 1
double max1; // Maximum of interval 1
double min2; // Minimm of interval 2
double max2; // Maximum of interval 2
Vector3D normal; // Contact normal (correspond to the separation axis with the smallest positive penetration depth)
// The contact normal point out of OBB1 toward OBB2
double minPenetrationDepth = DBL_MAX; // Minimum penetration depth detected among all separated axis
const double cutoff = 0.99; // Cutoff for cosine of angles between box axes
bool existsParallelPair = false; // True if there exists two face normals that are parallel.
// This is used because if a parallel pair exists, it is sufficient
// to test only the face normals of the OBBs for separation. Two nearly
// parallel faces can lead to all face normal tests reporting no separation
// along those directions. The cross product directions are tested next, but
// Ai x Bj is nearly the zero vector and can cause a report that the two OBBs
// are not intersecting when in fact they are.
double c[3][3]; // c[i][j] = DotProduct(obb1.Ai, obb2.Bj)
double absC[3][3]; // absC[i][j] = abs(DotProduct(obb1.Ai, obb2.Bj))
double udc1[3]; // DotProduct(obb1.Ai, obb2.center - obb1.center)
double udc2[3]; // DotProduct(obb2.Ai, obb2.center - obb1.center)
Vector3D boxDistance = obb2->getCenter() - obb1->getCenter(); // Vector between the centers of the OBBs
// Axis A0
for (int i=0; i<3; ++i) {
c[0][i] = obb1->getAxis(0).scalarProduct(obb2->getAxis(i));
absC[0][i] = fabs(c[0][i]);
if (absC[0][i] > cutoff) {
existsParallelPair = true;
}
}
udc1[0] = obb1->getAxis(0).scalarProduct(boxDistance);
center = udc1[0];
radius1 = obb1->getExtent(0);
radius2 = obb2->getExtent(0)*absC[0][0] + obb2->getExtent(1)*absC[0][1] + obb2->getExtent(2) * absC[0][2];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(0), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A1
for (int i=0; i<3; ++i) {
c[1][i] = obb1->getAxis(1).scalarProduct(obb2->getAxis(i));
absC[1][i] = fabs(c[1][i]);
if (absC[1][i] > cutoff) {
existsParallelPair = true;
}
}
udc1[1] = obb1->getAxis(1).scalarProduct(boxDistance);
center = udc1[1];
radius1 = obb1->getExtent(1);
radius2 = obb2->getExtent(0)*absC[1][0] + obb2->getExtent(1)*absC[1][1] + obb2->getExtent(2) * absC[1][2];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(1), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A2
for (int i=0; i<3; ++i) {
c[2][i] = obb1->getAxis(2).scalarProduct(obb2->getAxis(i));
absC[2][i] = fabs(c[2][i]);
if (absC[2][i] > cutoff) {
existsParallelPair = true;
}
}
udc1[2] = obb1->getAxis(2).scalarProduct(boxDistance);
center = udc1[2];
radius1 = obb1->getExtent(2);
radius2 = obb2->getExtent(0)*absC[2][0] + obb2->getExtent(1)*absC[2][1] + obb2->getExtent(2)*absC[2][2];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
}
// Axis B0
udc2[0] = obb2->getAxis(0).scalarProduct(boxDistance);
center = udc2[0];
radius1 = obb1->getExtent(0)*absC[0][0] + obb1->getExtent(1)*absC[1][0] + obb1->getExtent(2) * absC[2][0];
radius2 = obb2->getExtent(0);
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb2->getAxis(0), boxDistance); // Compute the contact normal with the correct sign
}
// Axis B1
udc2[1] = obb2->getAxis(1).scalarProduct(boxDistance);
center = udc2[1];
radius1 = obb1->getExtent(0)*absC[0][1] + obb1->getExtent(1)*absC[1][1] + obb1->getExtent(2) * absC[2][1];
radius2 = obb2->getExtent(1);
min1 = - radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb2->getAxis(1), boxDistance); // Compute the contact normal with the correct sign
}
// Axis B2
udc2[2] = obb2->getAxis(2).scalarProduct(boxDistance);
center = udc2[2];
radius1 = obb1->getExtent(0)*absC[0][2] + obb1->getExtent(1)*absC[1][2] + obb1->getExtent(2)*absC[2][2];
radius2 = obb2->getExtent(2);
min1 = - radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb2->getAxis(2), boxDistance); // Compute the contact normal with the correct sign
}
// If there exists a parallel pair of face normals
if (existsParallelPair) {
// There exists a parallel pair of face normals and we have already checked all the face
// normals for separation. Therefore the OBBs must intersect
// Compute the contact info
contactInfo = new ContactInfo(obb1, obb2, normal.getUnit(), minPenetrationDepth);
return true;
}
// Axis A0 x B0
center = udc1[2] * c[1][0] - udc1[1] * c[2][0];
radius1 = obb1->getExtent(1) * absC[2][0] + obb1->getExtent(2) * absC[1][0];
radius2 = obb2->getExtent(1) * absC[0][2] + obb2->getExtent(2) * absC[0][1];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A0 x B1
center = udc1[2] * c[1][1] - udc1[1] * c[2][1];
radius1 = obb1->getExtent(1) * absC[2][1] + obb1->getExtent(2) * absC[1][1];
radius2 = obb2->getExtent(0) * absC[0][2] + obb2->getExtent(2) * absC[0][0];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A0 x B2
center = udc1[2] * c[1][2] - udc1[1] * c[2][2];
radius1 = obb1->getExtent(1) * absC[2][2] + obb1->getExtent(2) * absC[1][2];
radius2 = obb2->getExtent(0) * absC[0][1] + obb2->getExtent(1) * absC[0][0];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A1 x B0
center = udc1[0] * c[2][0] - udc1[2] * c[0][0];
radius1 = obb1->getExtent(0) * absC[2][0] + obb1->getExtent(2) * absC[0][0];
radius2 = obb2->getExtent(1) * absC[1][2] + obb2->getExtent(2) * absC[1][1];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A1 x B1
center = udc1[0] * c[2][1] - udc1[2] * c[0][1];
radius1 = obb1->getExtent(0) * absC[2][1] + obb1->getExtent(2) * absC[0][1];
radius2 = obb2->getExtent(0) * absC[1][2] + obb2->getExtent(2) * absC[1][0];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A1 x B2
center = udc1[0] * c[2][2] - udc1[2] * c[0][2];
radius1 = obb1->getExtent(0) * absC[2][2] + obb1->getExtent(2) * absC[0][2];
radius2 = obb2->getExtent(0) * absC[1][1] + obb2->getExtent(1) * absC[1][0];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A2 x B0
center = udc1[1] * c[0][0] - udc1[0] * c[1][0];
radius1 = obb1->getExtent(0) * absC[1][0] + obb1->getExtent(1) * absC[0][0];
radius2 = obb2->getExtent(1) * absC[2][2] + obb2->getExtent(2) * absC[2][1];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A2 x B1
center = udc1[1] * c[0][1] - udc1[0] * c[1][1];
radius1 = obb1->getExtent(0) * absC[1][1] + obb1->getExtent(1) * absC[0][1];
radius2 = obb2->getExtent(0) * absC[2][2] + obb2->getExtent(2) * absC[2][0];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign
}
// Axis A2 x B2
center = udc1[1] * c[0][2] - udc1[0] * c[1][2];
radius1 = obb1->getExtent(0) * absC[1][2] + obb1->getExtent(1) * absC[0][2];
radius2 = obb2->getExtent(0) * absC[2][1] + obb2->getExtent(1) * absC[2][0];
min1 = -radius1;
max1 = radius1;
min2 = center - radius2;
max2 = center + radius2;
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false;
}
else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis
minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth
normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign
}
// Compute the contact info
contactInfo = new ContactInfo(obb1, obb2, normal.getUnit(), minPenetrationDepth);
return true;
}
// This method computes and returns the penetration depth between two intervals. This method returns the computed
// penetration depth (note that it could return a negative penetration depth if the intervals are separated.
double SATAlgorithm::computePenetrationDepth(double min1, double max1, double min2, double max2) const {
// Compute the length of both intervals
double lengthInterval1 = max1 - min1;
double lengthInterval2 = max2 - min2;
// Compute the total length of both intervals
double minExtreme = std::min(min1, min2);
double maxExtreme = std::max(max1, max2);
double lengthBothIntervals = maxExtreme - minExtreme;
// Compute the current penetration depth
return (lengthInterval1 + lengthInterval2) - lengthBothIntervals;
}

76
src/reactphysics3d/collision/SATAlgorithm.h Normal file
View File

@ -0,0 +1,76 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef SATALGORITHM_H
#define SATALGORITHM_H
// Libraries
#include "NarrowPhaseAlgorithm.h"
#include "../constraint/Contact.h"
#include "../body/OBB.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class SATAlgorithm :
This class implements a narrow-phase algorithm. This algorithm
uses a separating axis theorem (SAT) to check if two bounding
volumes collide or not. If the
two bounding volumes collide we have to create a contact object
to describe the collision contact. The idea is to check if there
exists an axis where, if we project the two bounding volumes on
this axis, the two projections are separated. If we find at
least an axis where the projections of the two bounding volumes
are separated then we know that the two bounding volumes don't
intersect.
-------------------------------------------------------------------
*/
class SATAlgorithm : public NarrowPhaseAlgorithm {
private :
bool computeCollisionTest(const OBB* const obb1, const OBB* const obb2, ContactInfo*& contactInfo) const; // Return true and compute a contact info if the two OBB collide
double computePenetrationDepth(double min1, double max1, double min2, double max2) const; // Compute the penetration depth of two projection intervals
Vector3D computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const; // Compute a contact normal
public :
SATAlgorithm(); // Constructor
~SATAlgorithm(); // Destructor
virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide
};
// Return the contact normal with the correct sign (from obb1 toward obb2). "axis" is the axis vector direction where the
// collision occur and "distanceOfOBBs" is the vector (obb2.center - obb1.center).
inline Vector3D SATAlgorithm::computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const {
if (distanceOfOBBs.scalarProduct(axis) >= 0.0) {
return axis;
}
else {
return axis.getOpposite();
}
}
} // End of the ReactPhysics3D namespace
#endif

40
src/reactphysics3d/constraint/Constraint.cpp Normal file
View File

@ -0,0 +1,40 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "Constraint.h"
// We want ot use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
Constraint::Constraint(Body* const body1, Body* const body2, uint nbAuxConstraints, bool active)
:body1(body1), body2(body2), active(active), nbAuxConstraints(nbAuxConstraints) {
}
// Destructor
Constraint::~Constraint() {
}

96
src/reactphysics3d/constraint/Constraint.h Normal file
View File

@ -0,0 +1,96 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef CONSTRAINT_H
#define CONSTRAINT_H
// Libraries
#include "../body/Body.h"
#include "../mathematics/mathematics.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class Constraint :
This abstract class represents a constraint in the physics engine.
A constraint can be a collision contact or a joint for
instance. Each constraint have a jacobian matrix associated with
the first body of the constraint and a jacobian matrix associated
with the second body. Each constraint can also have some auxiliary
constraints. Those auxiliary constraint are all represented in the
auxiliary Jacobian matrix. Auxiliary constraints represents some
constraints associated with a main constraint. For instance, a
contact constraint between two bodies can have two associated
auxiliary constraints to represent the frictions force constraints
in two directions.
-------------------------------------------------------------------
*/
class Constraint {
protected :
Body* const body1; // Pointer to the first body of the constraint
Body* const body2; // Pointer to the second body of the constraint
bool active; // True if the constraint is active
uint nbAuxConstraints; // Number of auxiliary constraints associated with this constraint
public :
Constraint(Body* const body1, Body* const body2, uint nbAuxConstraints, bool active); // Constructor // Constructor
virtual ~Constraint(); // Destructor
Body* const getBody1() const; // Return the reference to the body 1
Body* const getBody2() const; // Return the reference to the body 2 // Evaluate the constraint
bool isActive() const; // Return true if the constraint is active // Return the jacobian matrix of body 2
virtual void computeJacobian(int noBody, Matrix& jacobian) const=0; // Compute a part of the jacobian for a given body
virtual void computeAuxJacobian(int noBody, int noAuxConstraint, Matrix& auxJacobian) const=0; // Compute a part of the jacobian for an auxiliary constraint
virtual double computeLowerBound() const=0; // Compute the lowerbound of the constraint
virtual double computeUpperBound() const=0; // Compute the upperbound of the constraint
virtual void computeAuxLowerBounds(int beginIndex, Vector& auxLowerBounds) const=0; // Compute lowerbounds for the auxiliary constraints
virtual void computeAuxUpperBounds(int beginIndex, Vector& auxUpperBounds) const=0; // Compute upperbounds for the auxiliary constraints
virtual double computeErrorValue() const=0; // Compute the error value for the constraint
virtual void computeAuxErrorValues(int beginIndex, Vector& errorValues) const=0; // Compute the errors values of the auxiliary constraints
unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints // Return the number of auxiliary constraints
};
// Return the reference to the body 1
inline Body* const Constraint::getBody1() const {
return body1;
}
// Return the reference to the body 2
inline Body* const Constraint::getBody2() const {
return body2;
}
// Return true if the constraint is active
inline bool Constraint::isActive() const {
return active;
}
// Return the number auxiliary constraints
inline uint Constraint::getNbAuxConstraints() const {
return nbAuxConstraints;
}
} // End of the ReactPhysics3D namespace
#endif

47
src/reactphysics3d/constraint/Contact.cpp Normal file
View File

@ -0,0 +1,47 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "Contact.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const Vector3D& point)
:Constraint(body1, body2, 2, true), normal(normal), penetrationDepth(penetrationDepth), point(point) {
// Compute the auxiliary lower and upper bounds
// TODO : Now mC is only the mass of the first body but it is probably wrong
// TODO : Now g is 9.81 but we should use the true gravity value of the physics world.
mu_mc_g = FRICTION_COEFFICIENT * body1->getMass() * 9.81;
// Compute the friction vectors that span the tangential friction plane
computeFrictionVectors();
}
// Destructor
Contact::~Contact() {
}

246
src/reactphysics3d/constraint/Contact.h Normal file
View File

@ -0,0 +1,246 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef CONTACT_H
#define CONTACT_H
// Libraries
#include "../typeDefinitions.h"
#include "Constraint.h"
#include "../body/RigidBody.h"
#include "../mathematics/mathematics.h"
#include <GL/freeglut.h> // TODO : Remove this in the final version
#include <GL/gl.h> // TODO : Remove this in the final version
// ReactPhysics3D namespace
namespace reactphysics3d {
// Constants
const double FRICTION_COEFFICIENT = 0.3; // Friction coefficient
const double PENETRATION_FACTOR = 0.0; // Penetration factor (between 0 and 1) which specify the importance of the
// penetration depth in order to calculate the correct impulse for the contact
/* -------------------------------------------------------------------
Class Contact :
This class represents a collision contact between two bodies in
the physics engine. The contact class inherits from the
Constraint class. The collision detection system computes
contact informations that will be used to perform the collision
response. Each contact contains only one contact point. A contact
constraint has two auxiliary constraints in order two represent
the two friction forces at the contact surface.
-------------------------------------------------------------------
*/
class Contact : public Constraint {
protected :
const Vector3D normal; // Normal vector of the contact (From body1 toward body2)
const double penetrationDepth; // Penetration depth
const Vector3D point; // Contact point
std::vector<Vector3D> frictionVectors; // Two orthogonal vectors that span the tangential friction plane
double mu_mc_g;
void computeFrictionVectors(); // Compute the two friction vectors that span the tangential friction plane
public :
Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const Vector3D& point); // Constructor
virtual ~Contact(); // Destructor
Vector3D getNormal() const; // Return the normal vector of the contact
Vector3D getPoint() const; // Return the contact point
virtual void computeJacobian(int noBody, Matrix& jacobian) const; // Compute a part of the jacobian for a given body
virtual void computeAuxJacobian(int noBody, int noAuxConstraint, Matrix& auxJacobian) const; // Compute a part of the jacobian for an auxiliary constraint
virtual double computeLowerBound() const; // Compute the lowerbound of the constraint
virtual double computeUpperBound() const; // Compute the upperbound of the constraint
virtual void computeAuxLowerBounds(int beginIndex, Vector& auxLowerBounds) const; // Compute the lowerbounds for the auxiliary constraints
virtual void computeAuxUpperBounds(int beginIndex, Vector& auxLowerBounds) const; // Compute the upperbounds for the auxiliary constraints
virtual double computeErrorValue() const; // Compute the error value for the constraint
virtual void computeAuxErrorValues(int beginIndex, Vector& errorValues) const; // Compute the errors values of the auxiliary constraints
uint getNbAuxConstraints() const; // Return the number of auxiliary constraints
double getPenetrationDepth() const; // Return the penetration depth
void draw() const; // TODO : Delete this (Used to debug collision detection)
};
// Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane
// The two vectors have to be such that : v1 x v2 = contactNormal
inline void Contact::computeFrictionVectors() {
// Delete the current friction vectors
frictionVectors.clear();
// Compute the first orthogonal vector
Vector3D vector1 = normal.getOneOrthogonalVector();
frictionVectors.push_back(vector1);
// Compute the second orthogonal vector using the cross product
frictionVectors.push_back(normal.crossProduct(vector1));
}
// Return the normal vector of the contact
inline Vector3D Contact::getNormal() const {
return normal;
}
// Return the contact points
inline Vector3D Contact::getPoint() const {
return point;
}
// Return the penetration depth of the contact
inline double Contact::getPenetrationDepth() const {
return penetrationDepth;
}
// This method computes a part of the jacobian matrix for a given body.
// The argument "noBody" is 1 or 2 and corresponds to which one of the two
// bodies of the constraint we will compute the jacobian part. The argument
// "jacobian" is a 1x6 jacobian matrix of the constraint corresponding to one of
// the two bodies of the constraint.
inline void Contact::computeJacobian(int noBody, Matrix& jacobian) const {
RigidBody* rigidBody;
Vector3D rCrossN;
Vector3D r;
Vector3D norm = normal;
assert(noBody == 1 || noBody == 2);
assert(jacobian.getNbRow() == 1 && jacobian.getNbColumn() == 6);
if (noBody == 1) {
rigidBody = dynamic_cast<RigidBody*>(body1);
assert(rigidBody);
r = point - rigidBody->getPosition();
rCrossN = r.crossProduct(normal).getOpposite();
norm = normal.getOpposite();
}
else {
rigidBody = dynamic_cast<RigidBody*>(body2);
assert(rigidBody);
r = point - rigidBody->getPosition();
rCrossN = r.crossProduct(normal);
}
// Compute the jacobian matrix for the body 1
jacobian.setValue(0, 0, norm.getX());
jacobian.setValue(0, 1, norm.getY());
jacobian.setValue(0, 2, norm.getZ());
jacobian.setValue(0, 3, rCrossN.getX());
jacobian.setValue(0, 4, rCrossN.getY());
jacobian.setValue(0, 5, rCrossN.getZ());
}
// Compute a part of the jacobian matrix for an auxiliary constraint (given by "noAuxConstraint")
// and one of the two bodies (given by "noBody") of the contact. The argument "noBody" is 1 or 2 and
// argument auxJacobian is a 1x6 matrix.
inline void Contact::computeAuxJacobian(int noBody, int noAuxConstraint, Matrix& auxJacobian) const {
Vector3D r;
Vector3D rCrossU;
RigidBody* rigidBody;
double sign;
assert(noBody == 1 || noBody == 2);
assert(noAuxConstraint == 1 || noAuxConstraint == 2);
assert(auxJacobian.getNbRow() == 1 && auxJacobian.getNbColumn() == 6);
if (noBody == 1) {
rigidBody = dynamic_cast<RigidBody*>(body1);
assert(rigidBody);
r = point - rigidBody->getPosition();
sign = -1.0;
}
else {
rigidBody = dynamic_cast<RigidBody*>(body2);
assert(rigidBody);
r = point - rigidBody->getPosition();
sign = 1.0;
}
rCrossU = r.crossProduct(frictionVectors[noAuxConstraint-1]);
auxJacobian.setValue(0, 0, sign * frictionVectors[noAuxConstraint-1].getX());
auxJacobian.setValue(0, 1, sign * frictionVectors[noAuxConstraint-1].getY());
auxJacobian.setValue(0, 2, sign * frictionVectors[noAuxConstraint-1].getZ());
auxJacobian.setValue(0, 3, sign * rCrossU.getX());
auxJacobian.setValue(0, 4, sign * rCrossU.getY());
auxJacobian.setValue(0, 5, sign * rCrossU.getZ());
}
// Compute the lowerbounds for the auxiliary constraints
inline double Contact::computeLowerBound() const {
return 0.0;
}
// Compute the upperbounds for the auxiliary constraints
inline double Contact::computeUpperBound() const {
return INFINITY_CONST;
}
// Compute the lowerbounds for the auxiliary constraints. This method fills the "auxLowerBounds"
// vector starting at the index "beginIndex" in this vector.
inline void Contact::computeAuxLowerBounds(int beginIndex, Vector& auxLowerBounds) const {
assert(beginIndex + nbAuxConstraints <= auxLowerBounds.getNbComponent());
auxLowerBounds.setValue(beginIndex, -mu_mc_g);
auxLowerBounds.setValue(beginIndex + 1, -mu_mc_g);
}
// Compute the upperbounds for the auxiliary constraints. This method fills the "auxUpperBounds"
// vector starting at the index "beginIndex" in this vector.
inline void Contact::computeAuxUpperBounds(int beginIndex, Vector& auxUpperBounds) const {
assert(beginIndex + nbAuxConstraints <= auxUpperBounds.getNbComponent());
auxUpperBounds.setValue(beginIndex, mu_mc_g);
auxUpperBounds.setValue(beginIndex + 1, mu_mc_g);
}
// Compute the error value for the constraint
inline double Contact::computeErrorValue() const {
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
assert(rigidBody1);
assert(rigidBody2);
Vector3D velocity1 = rigidBody1->getLinearVelocity();
Vector3D velocity2 = rigidBody2->getLinearVelocity();
double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
double errorValue = restitutionCoeff * (normal.scalarProduct(velocity1) - normal.scalarProduct(velocity2)) + PENETRATION_FACTOR * penetrationDepth;
return errorValue;
}
// Compute the errors values of the auxiliary constraints
inline void Contact::computeAuxErrorValues(int beginIndex, Vector& errorValues) const {
assert(beginIndex + nbAuxConstraints <= errorValues.getNbComponent());
errorValues.setValue(beginIndex, 0.0);
errorValues.setValue(beginIndex + 1, 0.0);
}
// TODO : Delete this (Used to debug collision detection)
inline void Contact::draw() const {
glColor3f(1.0, 0.0, 0.0);
glutSolidSphere(0.3, 20, 20);
}
} // End of the ReactPhysics3D namespace
#endif

382
src/reactphysics3d/engine/ConstraintSolver.cpp Normal file
View File

@ -0,0 +1,382 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "ConstraintSolver.h"
#include "../mathematics/lcp/LCPProjectedGaussSeidel.h"
#include "../body/RigidBody.h"
using namespace reactphysics3d;
using namespace std;
// Constructor
ConstraintSolver::ConstraintSolver(PhysicsWorld* world)
:physicsWorld(world), bodyMapping(0), nbConstraints(0), constraintsCapacity(0),
bodiesCapacity(0), avConstraintsCapacity(0), avBodiesCapacity(0), avBodiesNumber(0),
avConstraintsNumber(0), avBodiesCounter(0), avConstraintsCounter(0),
lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
}
// Destructor
ConstraintSolver::~ConstraintSolver() {
}
// Initialize the constraint solver before each solving
void ConstraintSolver::initialize() {
Constraint* constraint;
nbConstraints = 0;
// For each constraint
vector<Constraint*>::iterator it;
for (it = physicsWorld->getConstraintsBeginIterator(); it != physicsWorld->getConstraintsEndIterator(); it++) {
constraint = *it;
// If the constraint is active
if (constraint->isActive()) {
activeConstraints.push_back(constraint);
// Add the two bodies of the constraint in the constraintBodies list
constraintBodies.insert(constraint->getBody1());
constraintBodies.insert(constraint->getBody2());
// Fill in the body number maping
bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody2(), bodyNumberMapping.size()));
// Update the size of the jacobian matrix
nbConstraints += (1 + constraint->getNbAuxConstraints());
}
}
// Compute the number of bodies that are part of some active constraint
nbBodies = bodyNumberMapping.size();
assert(nbConstraints > 0);
assert(nbBodies > 0);
// Update the average bodies and constraints capacities
if (avBodiesCounter > AV_COUNTER_LIMIT) {
avBodiesCounter = 0;
avBodiesNumber = 0;
}
if (avConstraintsCounter > AV_COUNTER_LIMIT) {
avConstraintsCounter = 0;
avConstraintsNumber = 0;
}
avBodiesCounter++;
avConstraintsCounter++;
avBodiesNumber += nbBodies;
avConstraintsNumber += nbConstraints;
avBodiesCapacity += (avBodiesNumber / avBodiesCounter);
avConstraintsCapacity += (avConstraintsNumber / avConstraintsCounter);
// Allocate the memory needed for the constraint solver
allocate();
}
// Allocate all the memory needed to solve the LCP problem
// The goal of this method is to avoid to free and allocate the memory
// each time the constraint solver is called but only if the we effectively
// need more memory. Therefore if for instance the number of constraints to
// be solved is smaller than the constraints capacity, we don't free and reallocate
// memory because we don't need to. The problem now is that the constraints capacity
// can grow indefinitely. Therefore we use a way to free and reallocate the memory
// if the average number of constraints currently solved is far less than the current
// constraints capacity
void ConstraintSolver::allocate() {
// If we need to allocate more memory for the bodies
if (nbBodies > bodiesCapacity || avBodiesCapacity < AV_PERCENT_TO_FREE * bodiesCapacity) {
freeMemory(true);
bodiesCapacity = nbBodies;
Minv_sp = new Matrix[nbBodies];
V1 = new Vector[nbBodies];
Vconstraint = new Vector[nbBodies];
Fext = new Vector[nbBodies];
avBodiesNumber = 0;
avBodiesCounter = 0;
}
// If we need to allocate more memory for the constraints
if (nbConstraints > constraintsCapacity || constraintsCapacity < AV_PERCENT_TO_FREE * constraintsCapacity) {
freeMemory(false);
constraintsCapacity = nbConstraints;
bodyMapping = new Body**[nbConstraints];
J_sp = new Matrix*[nbConstraints];
B_sp = new Matrix*[2];
B_sp[0] = new Matrix[nbConstraints];
B_sp[1] = new Matrix[nbConstraints];
for (uint i=0; i<nbConstraints; i++) {
bodyMapping[i] = new Body*[2];
J_sp[i] = new Matrix[2];
}
errorValues.changeSize(nbConstraints);
b.changeSize(nbConstraints);
lambda.changeSize(nbConstraints);
lambdaInit.changeSize(nbConstraints);
lowerBounds.changeSize(nbConstraints);
upperBounds.changeSize(nbConstraints);
avConstraintsNumber = 0;
avConstraintsCounter = 0;
}
}
// Free the memory that was allocated in the allocate() method
// If the argument is true the method will free the memory
// associated to the bodies. In the other case, it will free
// the memory associated with the constraints
void ConstraintSolver::freeMemory(bool freeBodiesMemory) {
// If we need to free the bodies memory
if (freeBodiesMemory && bodiesCapacity > 0) {
delete[] Minv_sp;
delete[] V1;
delete[] Vconstraint;
delete[] Fext;
}
else if (constraintsCapacity > 0) { // If we need to free the constraints memory
// Free the bodyMaping array
for (uint i=0; i<constraintsCapacity; i++) {
delete[] bodyMapping[i];
delete[] J_sp[i];
}
delete[] bodyMapping;
delete[] J_sp;
delete[] B_sp[0];
delete[] B_sp[1];
delete[] B_sp;
}
}
// Fill in all the matrices needed to solve the LCP problem
// Notice that all the active constraints should have been evaluated first
void ConstraintSolver::fillInMatrices() {
Constraint* constraint;
Contact* contact;
ContactCachingInfo* contactInfo;
// For each active constraint
uint noConstraint = 0;
uint nbAuxConstraints = 0;
for (uint c=0; c<activeConstraints.size(); c++) {
constraint = activeConstraints.at(c);
// Fill in the J_sp matrix
J_sp[noConstraint][0].changeSize(1, 6);
J_sp[noConstraint][1].changeSize(1, 6);
constraint->computeJacobian(1, J_sp[noConstraint][0]);
constraint->computeJacobian(2, J_sp[noConstraint][1]);
// Fill in the body mapping matrix
bodyMapping[noConstraint][0] = constraint->getBody1();
bodyMapping[noConstraint][1] = constraint->getBody2();
// Fill in the limit vectors for the constraint
lowerBounds.setValue(noConstraint, constraint->computeLowerBound());
upperBounds.setValue(noConstraint, constraint->computeUpperBound());
// Fill in the error vector
errorValues.setValue(noConstraint, constraint->computeErrorValue());
// If it's a contact constraint
contact = dynamic_cast<Contact*>(constraint);
if (contact) {
// Get the lambda init value from the cache if exists
contactInfo = contactCache.getContactCachingInfo(contact->getBody1(), contact->getBody2(), contact->getPoint());
if (contactInfo) {
// The last lambda init value was in the cache
lambdaInit.setValue(noConstraint, contactInfo->lambda);
}
else {
// The las lambda init value was not in the cache
lambdaInit.setValue(noConstraint, 0.0);
}
}
else {
// Set the lambda init value
lambdaInit.setValue(noConstraint, 0.0);
}
nbAuxConstraints = constraint->getNbAuxConstraints();
// If the current constraint has auxiliary constraints
if (nbAuxConstraints > 0) {
// For each auxiliary constraints
for (uint i=1; i<=nbAuxConstraints; i++) {
// Fill in the J_sp matrix
J_sp[noConstraint+i][0].changeSize(1, 6);
J_sp[noConstraint+i][1].changeSize(1, 6);
constraint->computeAuxJacobian(1, i, J_sp[noConstraint+i][0]);
constraint->computeAuxJacobian(2, i, J_sp[noConstraint+i][1]);
// Fill in the body mapping matrix
bodyMapping[noConstraint+i][0] = constraint->getBody1();
bodyMapping[noConstraint+i][1] = constraint->getBody2();
// Fill in the init lambda value for the constraint
lambdaInit.setValue(noConstraint+i, 0.0);
}
// Fill in the limit vectors for the auxilirary constraints
constraint->computeAuxLowerBounds(noConstraint+1, lowerBounds);
constraint->computeAuxUpperBounds(noConstraint+1, upperBounds);
// Fill in the errorValues vector for the auxiliary constraints
constraint->computeAuxErrorValues(noConstraint+1, errorValues);
}
noConstraint += 1 + nbAuxConstraints;
}
// For each current body that is implied in some constraint
RigidBody* rigidBody;
Body* body;
Vector v(6);
Vector f(6);
Matrix identity = Matrix::identity(3);
Matrix mInv(6,6);
uint b=0;
for (set<Body*>::iterator it = constraintBodies.begin(); it != constraintBodies.end(); it++, b++) {
body = *it;
uint bodyNumber = bodyNumberMapping.at(body);
// TODO : Use polymorphism and remove this downcasting
rigidBody = dynamic_cast<RigidBody*>(body);
assert(rigidBody != 0);
// Compute the vector V1 with initial velocities values
v.fillInSubVector(0, rigidBody->getLinearVelocity());
v.fillInSubVector(3, rigidBody->getAngularVelocity());
V1[bodyNumber].changeSize(6);
V1[bodyNumber] = v;
// Compute the vector Vconstraint with final constraint velocities
Vconstraint[bodyNumber].changeSize(6);
Vconstraint[bodyNumber].initWithValue(0.0);
// Compute the vector with forces and torques values
f.fillInSubVector(0, rigidBody->getExternalForce());
f.fillInSubVector(3, rigidBody->getExternalTorque());
Fext[bodyNumber].changeSize(6);
Fext[bodyNumber] = f;
// Compute the inverse sparse mass matrix
mInv.initWithValue(0.0);
if (rigidBody->getIsMotionEnabled()) {
mInv.fillInSubMatrix(0, 0, rigidBody->getMassInverse() * identity);
mInv.fillInSubMatrix(3, 3, rigidBody->getInertiaTensorInverseWorld());
}
Minv_sp[bodyNumber].changeSize(6, 6);
Minv_sp[bodyNumber] = mInv;
}
}
// Compute the vector b
void ConstraintSolver::computeVectorB(double dt) {
uint indexBody1, indexBody2;
double oneOverDT = 1.0/dt;
b = errorValues * oneOverDT;
for (uint c = 0; c<nbConstraints; c++) {
// Substract 1.0/dt*J*V to the vector b
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
b.setValue(c, b.getValue(c) - (J_sp[c][0] * V1[indexBody1]).getValue(0,0) * oneOverDT);
b.setValue(c, b.getValue(c) - (J_sp[c][1] * V1[indexBody2]).getValue(0,0) * oneOverDT);
// Substract J*M^-1*F_ext to the vector b
b.setValue(c, b.getValue(c) - ((J_sp[c][0] * Minv_sp[indexBody1]) * Fext[indexBody1]
+ (J_sp[c][1] * Minv_sp[indexBody2])*Fext[indexBody2]).getValue(0,0));
}
}
// Compute the matrix B_sp
void ConstraintSolver::computeMatrixB_sp() {
uint indexBody1, indexBody2;
// For each constraint
for (uint c = 0; c<nbConstraints; c++) {
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
B_sp[0][c].changeSize(6,1);
B_sp[1][c].changeSize(6,1);
B_sp[0][c] = Minv_sp[indexBody1] * J_sp[c][0].getTranspose();
B_sp[1][c] = Minv_sp[indexBody2] * J_sp[c][1].getTranspose();
}
}
// Compute the vector V_constraint (which corresponds to the constraint part of
// the final V2 vector) according to the formula
// V_constraint = dt * (M^-1 * J^T * lambda)
// Note that we use the vector V to store both V1 and V_constraint.
// Note that M^-1 * J^T = B.
// This method is called after that the LCP solver have computed lambda
void ConstraintSolver::computeVectorVconstraint(double dt) {
uint indexBody1, indexBody2;
// Compute dt * (M^-1 * J^T * lambda
for (uint i=0; i<nbConstraints; i++) {
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
Vconstraint[indexBody1] = Vconstraint[indexBody1] + (B_sp[0][i] * lambda.getValue(i)).getVector() * dt;
Vconstraint[indexBody2] = Vconstraint[indexBody2] + (B_sp[1][i] * lambda.getValue(i)).getVector() * dt;
}
}
// Clear and Fill in the contact cache with the new lambda values
void ConstraintSolver::updateContactCache() {
Contact* contact;
ContactCachingInfo* contactInfo;
// Clear the contact cache
contactCache.clear();
// For each active constraint
uint noConstraint = 0;
for (uint c=0; c<activeConstraints.size(); c++) {
// If it's a contact constraint
contact = dynamic_cast<Contact*>(activeConstraints.at(c));
if (contact) {
// Create a new ContactCachingInfo
contactInfo = new ContactCachingInfo(contact->getBody1(), contact->getBody2(), contact->getPoint(), lambda.getValue(noConstraint));
// Add it to the contact cache
contactCache.addContactCachingInfo(contactInfo);
}
noConstraint += 1 + activeConstraints.at(c)->getNbAuxConstraints();
}
}

171
src/reactphysics3d/engine/ConstraintSolver.h Normal file
View File

@ -0,0 +1,171 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef CONSTRAINTSOLVER_H
#define CONSTRAINTSOLVER_H
// Libraries
#include "../typeDefinitions.h"
#include "../constraint/Constraint.h"
#include "../mathematics/lcp/LCPSolver.h"
#include "ContactCache.h"
#include "PhysicsWorld.h"
#include <map>
#include <set>
// ReactPhysics3D namespace
namespace reactphysics3d {
// Constants
const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem
const double AV_COUNTER_LIMIT = 500; // Maximum number value of the avBodiesCounter or avConstraintsCounter
const double AV_PERCENT_TO_FREE = 0.5; // We will free the memory if the current nb of bodies (or constraints) is
// less than AV_PERCENT_TO_FREE * bodiesCapacity (or constraintsCapacity). This
// is used to avoid to keep to much memory for a long time if the system doesn't
// need that memory. This value is between 0.0 and 1.0
/* -------------------------------------------------------------------
Class ConstrainSolver :
This class represents the constraint solver. The goal is to
solve A constraint-base LCP problem.
-------------------------------------------------------------------
*/
class ConstraintSolver {
protected:
PhysicsWorld* physicsWorld; // Reference to the physics world
LCPSolver* lcpSolver; // LCP Solver
ContactCache contactCache; // Contact cache
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
uint nbConstraints; // Total number of constraints (with the auxiliary constraints)
uint nbBodies; // Current number of bodies in the physics world
uint constraintsCapacity; // Number of constraints that are currently allocated in memory in the solver
uint bodiesCapacity; // Number of bodies that are currently allocated in memory in the solver
uint avConstraintsCapacity; // Average constraint capacity
uint avBodiesCapacity; // Average bodies capacity
uint avBodiesNumber; // Total bodies number for average computation
uint avConstraintsNumber; // Total constraints number for average computation
uint avBodiesCounter; // Counter used to compute the average
uint avConstraintsCounter;
std::set<Body*> constraintBodies; // Bodies that are implied in some constraint
std::map<Body*, uint> bodyNumberMapping; // Map a body pointer with its index number
Body*** bodyMapping; // 2-dimensional array that contains the mapping of body reference
// in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
// the pointer to the body that correspond to the 1x6 J_ij matrix in the
// J_sp matrix. A integer body index refers to its index in the "bodies" std::vector
Matrix** J_sp; // 2-dimensional array thar correspond to the sparse representation of the jacobian matrix of all constraints
// The dimension of this array is nbConstraints times 2. Each cell will contain
// a 1x6 matrix
Matrix** B_sp; // 2-dimensional array that correspond to a useful matrix in sparse representation
// The dimension of this array is 2 times nbConstraints. Each cell will contain
// a 6x1 matrix
Vector b; // Vector "b" of the LCP problem
Vector lambda; // Lambda vector of the LCP problem
Vector lambdaInit; // Lambda init vector for the LCP solver
Vector errorValues; // Error vector of all constraints
Vector lowerBounds; // Vector that contains the low limits for the variables of the LCP problem
Vector upperBounds; // Vector that contains the high limits for the variables of the LCP problem
Matrix* Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body
// This is an array of size nbBodies that contains in each cell a 6x6 matrix
Vector* V1; // Array that contains for each body the Vector that contains linear and angular velocities
// Each cell contains a 6x1 vector with linear and angular velocities
Vector* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities
Vector* Fext; // Array that contains for each body the vector that contains external forces and torques
// Each cell contains a 6x1 vector with external force and torque.
void initialize(); // Initialize the constraint solver before each solving
void allocate(); // Allocate all the memory needed to solve the LCP problem
void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem
void computeVectorB(double dt); // Compute the vector b
void computeMatrixB_sp(); // Compute the matrix B_sp
void computeVectorVconstraint(double dt); // Compute the vector V2
void updateContactCache(); // Clear and Fill in the contact cache with the new lambda values
void freeMemory(bool freeBodiesMemory); // Free some memory previously allocated for the constraint solver
public:
ConstraintSolver(PhysicsWorld* world); // Constructor
virtual ~ConstraintSolver(); // Destructor
void solve(double dt); // Solve the current LCP problem
bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint
Vector3D getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem
Vector3D getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem
void cleanup();
};
// Return true if the body is in at least one constraint
inline bool ConstraintSolver::isConstrainedBody(Body* body) const {
if(constraintBodies.find(body) != constraintBodies.end()) {
return true;
}
return false;
}
// Return the constrained linear velocity of a body after solving the LCP problem
inline Vector3D ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) {
assert(isConstrainedBody(body));
Vector vec = Vconstraint[bodyNumberMapping[body]].getSubVector(0, 3);
return Vector3D(vec.getValue(0), vec.getValue(1), vec.getValue(2));
}
// Return the constrained angular velocity of a body after solving the LCP problem
inline Vector3D ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) {
assert(isConstrainedBody(body));
Vector vec = Vconstraint[bodyNumberMapping[body]].getSubVector(3, 3);
return Vector3D(vec.getValue(0), vec.getValue(1), vec.getValue(2));
}
// Solve the current LCP problem
inline void ConstraintSolver::solve(double dt) {
// Allocate memory for the matrices
initialize();
// Fill-in all the matrices needed to solve the LCP problem
fillInMatrices();
// Compute the vector b
computeVectorB(dt);
// Compute the matrix B
computeMatrixB_sp();
// Solve the LCP problem (computation of lambda)
lcpSolver->setLambdaInit(lambdaInit);
lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
// Update the contact chaching informations
updateContactCache();
// Compute the vector Vconstraint
computeVectorVconstraint(dt);
}
// Cleanup of the constraint solver
inline void ConstraintSolver::cleanup() {
bodyNumberMapping.clear();
constraintBodies.clear();
activeConstraints.clear();
}
} // End of ReactPhysics3D namespace
#endif

92
src/reactphysics3d/engine/ContactCache.cpp Normal file
View File

@ -0,0 +1,92 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "ContactCache.h"
using namespace reactphysics3d;
using namespace std;
// Constructor
ContactCache::ContactCache() {
}
// Destructor
ContactCache::~ContactCache() {
}
// Remove all the contact caching info of the cache
void ContactCache::clear() {
// Clear the cache
cache.clear();
}
// Add a new contact caching info in the cache
void ContactCache::addContactCachingInfo(ContactCachingInfo* info) {
// Check if there is already an entry for that pair of body in the cache
map<pair<Body*, Body*>, vector<ContactCachingInfo*> >::iterator entry = cache.find(make_pair(info->body1, info->body2));
if (entry != cache.end()) {
(*entry).second.push_back(info);
}
else {
// Add a new entry in the cache
vector<ContactCachingInfo*> vec;
vec.push_back(info);
cache.insert(make_pair(make_pair(info->body1, info->body2), vec));
}
}
// Return the ContactCachingInfo (if exists) corresponding to the arguments
// If the contact pair between the two bodies in argument doesn't exist this or there is no ContactCachingInfo
// compatible (with approximatively the same position), this method returns NULL.
ContactCachingInfo* ContactCache::getContactCachingInfo(Body* body1, Body* body2, const Vector3D& position) {
// Check if there is an entry for that pair of body in the cache
map<pair<Body*, Body*>, vector<ContactCachingInfo*> >::iterator entry = cache.find(make_pair(body1, body2));
if (entry != cache.end()) {
vector<ContactCachingInfo*> vec = (*entry).second;
assert((*entry).first.first == body1);
assert((*entry).first.second == body2);
double posX = position.getX();
double posY = position.getY();
double posZ = position.getZ();
// Check among all the old contacts for this pair of body if there is an approximative match for the contact position
for(vector<ContactCachingInfo*>::iterator it = vec.begin(); it != vec.end(); it++) {
Vector3D& contactPos = (*it)->position;
if (posX <= contactPos.getX() + POSITION_TOLERANCE && posX >= contactPos.getX() - POSITION_TOLERANCE &&
posY <= contactPos.getY() + POSITION_TOLERANCE && posY >= contactPos.getY() - POSITION_TOLERANCE &&
posZ <= contactPos.getZ() + POSITION_TOLERANCE && posZ >= contactPos.getZ() - POSITION_TOLERANCE) {
// Return the ContactCachingInfo
return *it;
}
}
}
else {
return 0;
}
}

63
src/reactphysics3d/engine/ContactCache.h Normal file
View File

@ -0,0 +1,63 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef CONTACTCACHE_H
#define CONTACTCACHE_H
// Libraries
#include <map>
#include <vector>
#include <utility>
#include "ContactCachingInfo.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
// Constant
const double POSITION_TOLERANCE = 0.01 ; // Tolerance used to consider two similar positions to be considered as the same
/* -------------------------------------------------------------------
Class ContactCache :
This class is used to cache the contact in order to be able to
use the lambda values in the last time step in the next time step in the
constraint solver to improve the convergence rate of the PGS algorithm.
This class will cache the ContactCachingInfo objects at each time step.
-------------------------------------------------------------------
*/
class ContactCache {
private:
std::map<std::pair<Body*, Body*>, std::vector<ContactCachingInfo*> > cache;
public:
ContactCache(); // Constructor
~ContactCache(); // Destructor
void clear(); // Remove all the contact caching info of the cache
void addContactCachingInfo(ContactCachingInfo* contactCachingInfo); // Add a new contact caching info in the cache
ContactCachingInfo* getContactCachingInfo(Body* body1, Body* body2, const Vector3D& position); // Return the ContactCachingInfo (if exists) corresponding to the arguments
};
} // End of the ReactPhysics3D namespace
#endif

34
src/reactphysics3d/engine/ContactCachingInfo.cpp Normal file
View File

@ -0,0 +1,34 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "ContactCachingInfo.h"
using namespace reactphysics3d;
// Constructor
ContactCachingInfo::ContactCachingInfo(Body* body1, Body* body2, const Vector3D& position, double lambda)
: body1(body1), body2(body2), position(position), lambda(lambda) {
}

56
src/reactphysics3d/engine/ContactCachingInfo.h Normal file
View File

@ -0,0 +1,56 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef CONTACTCACHINGINFO_H
#define CONTACTCACHINGINFO_H
// Libraries
#include "../body/OBB.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Structure ContactCachingInfo :
This structure contains informations used to cache the last lambda
value of each contact constraint. The last lambda value is used
as the init lambda value in the constraint solver in the next
time step to improve the convergence rate of the constraint solver.
-------------------------------------------------------------------
*/
struct ContactCachingInfo {
public:
// TODO : Use polymorphism here (change OBB into BoundingVolume to be more general)
Body* body1; // Body pointer of the first bounding volume
Body* body2; // Body pointer of the second bounding volume
Vector3D position; // Contact point
double lambda; // Last lambda value for the constraint
ContactCachingInfo(Body* body1, Body* body2, const Vector3D& position, double lambda); // Constructor
};
} // End of the ReactPhysics3D namespace
#endif

207
src/reactphysics3d/engine/PhysicsEngine.cpp Normal file
View File

@ -0,0 +1,207 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "PhysicsEngine.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
using namespace std;
// Constructor
PhysicsEngine::PhysicsEngine(PhysicsWorld* world, double timeStep) throw (invalid_argument)
: world(world), timer(timeStep), collisionDetection(world), constraintSolver(world) {
// Check if the pointer to the world is not NULL
if (world == 0) {
// Throw an exception
throw invalid_argument("Error : The argument world to the PhysicsEngine constructor cannot be NULL");
}
// Check if the timeStep is positive
if (timeStep <= 0.0) {
// Throw an exception
throw invalid_argument("Error : The timeStep argument to the PhysicsEngine constructor have to be greater than zero");
}
}
// Destructor
PhysicsEngine::~PhysicsEngine() {
}
// Update the physics simulation
void PhysicsEngine::update() throw (logic_error) {
bool existCollision = false;
// Check that the timer is running
if (timer.getIsRunning()) {
// Compute the time since the last update() call and update the timer
timer.update();
// Apply the gravity force to all bodies
applyGravity();
// While the time accumulator is not empty
while(timer.isPossibleToTakeStep()) {
existCollision = false;
// Compute the collision detection
if (collisionDetection.computeCollisionDetection()) {
existCollision = true;
// Solve constraints
constraintSolver.solve(timer.getTimeStep());
}
// Update the timer
timer.nextStep();
// Update the position and orientation of each body
updateAllBodiesMotion();
// Cleanup of the constraint solver
if (existCollision) {
constraintSolver.cleanup();
}
// Clear the added and removed bodies from last update() method call
world->clearAddedAndRemovedBodies();
}
// Compute and set the interpolation factor to all the bodies
setInterpolationFactorToAllBodies();
}
else { // Call to update() but the timer is not running
// Throw an exception
throw logic_error("Error : The PhysicsEngine::start() method have to be called before calling PhysicsEngine::update()");
}
}
// Compute the motion of all bodies and update their positions and orientations
// First this method compute the vector V2 = V_constraint + V_forces + V1 where
// V_constraint = dt * (M^-1 * J^T * lambda) and V_forces = dt * (M^-1 * F_ext)
// V2 is the final velocity after the timestep and V1 is the velocity before the
// timestep.
// After having computed the velocity V2, this method will update the position
// and orientation of each body.
// This method uses the semi-implicit Euler method to update the position and
// orientation of the body
void PhysicsEngine::updateAllBodiesMotion() {
double dt = timer.getTimeStep();
Vector3D newLinearVelocity;
Vector3D newAngularVelocity;
// For each body of thephysics world
for (vector<Body*>::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) {
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
assert(rigidBody);
// If the body is able to move
if (rigidBody->getIsMotionEnabled()) {
newLinearVelocity.setAllValues(0.0, 0.0, 0.0);
newAngularVelocity.setAllValues(0.0, 0.0, 0.0);
// If it's a constrained body
if (constraintSolver.isConstrainedBody(*it)) {
// Get the constrained linear and angular velocities from the constraint solver
newLinearVelocity = constraintSolver.getConstrainedLinearVelocityOfBody(*it);
newAngularVelocity = constraintSolver.getConstrainedAngularVelocityOfBody(*it);
}
// Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
// external forces and torques.
newLinearVelocity = newLinearVelocity + dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
newAngularVelocity = newAngularVelocity + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
// Add the velocity V1 to the new velocity
newLinearVelocity = newLinearVelocity + rigidBody->getLinearVelocity();
newAngularVelocity = newAngularVelocity + rigidBody->getAngularVelocity();
// Update the position and the orientation of the body according to the new velocity
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
// If the body state has changed, we have to update some informations in the rigid body
rigidBody->update();
}
}
}
// Update the position and orientation of a body
// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new
// orientation of the body
void PhysicsEngine::updatePositionAndOrientationOfBody(Body* body, const Vector3D& newLinVelocity, const Vector3D& newAngVelocity) {
double dt = timer.getTimeStep();
RigidBody* rigidBody = dynamic_cast<RigidBody*>(body);
assert(rigidBody);
// Update the old position and orientation of the body
rigidBody->updateOldPositionAndOrientation();
// Normalize the orientation quaternion
rigidBody->setOrientation(rigidBody->getOrientation().getUnit());
// Update the linear and angular velocity of the body
rigidBody->setLinearVelocity(newLinVelocity);
rigidBody->setAngularVelocity(newAngVelocity);
// Update the position and the orientation of the body
rigidBody->setPosition(rigidBody->getPosition() + newLinVelocity * dt);
rigidBody->setOrientation(rigidBody->getOrientation() + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * rigidBody->getOrientation() * 0.5 * dt);
}
// Compute and set the interpolation factor to all bodies
void PhysicsEngine::setInterpolationFactorToAllBodies() {
// Compute the interpolation factor
double factor = timer.computeInterpolationFactor();
assert(factor >= 0.0 && factor <= 1.0);
// Set the factor to all bodies
for (vector<Body*>::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) {
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
assert(rigidBody);
rigidBody->setInterpolationFactor(factor);
}
}
// Apply the gravity force to all bodies of the physics world
void PhysicsEngine::applyGravity() {
// For each body of the physics world
for (vector<Body*>::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) {
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
assert(rigidBody);
// If the gravity force is on
if(world->getIsGravityOn()) {
// Apply the current gravity force to the body
rigidBody->setExternalForce(rigidBody->getMass() * world->getGravity());
}
}
}

78
src/reactphysics3d/engine/PhysicsEngine.h Normal file
View File

@ -0,0 +1,78 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef PHYSICSENGINE_H
#define PHYSICSENGINE_H
// Libraries
#include "PhysicsWorld.h"
#include "../collision/CollisionDetection.h"
#include "ConstraintSolver.h"
#include "../body/RigidBody.h"
#include "Timer.h"
// Namespace ReactPhysics3D
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class PhysicsEngine :
This class represents the physics engine
of the library.
-------------------------------------------------------------------
*/
class PhysicsEngine {
protected :
PhysicsWorld* world; // Pointer to the physics world of the physics engine
Timer timer; // Timer of the physics engine
CollisionDetection collisionDetection; // Collision detection
ConstraintSolver constraintSolver; // Constraint solver
void updateAllBodiesMotion(); // Compute the motion of all bodies and update their positions and orientations
void updatePositionAndOrientationOfBody(Body* body, const Vector3D& newLinVelocity, const Vector3D& newAngVelocity); // Update the position and orientation of a body
void setInterpolationFactorToAllBodies(); // Compute and set the interpolation factor to all bodies
void applyGravity(); // Apply the gravity force to all bodies
public :
PhysicsEngine(PhysicsWorld* world, double timeStep) throw (std::invalid_argument); // Constructor
~PhysicsEngine(); // Destructor
void start(); // Start the physics simulation
void stop(); // Stop the physics simulation
void update() throw (std::logic_error); // Update the physics simulation
};
// --- Inline functions --- //
// Start the physics simulation
inline void PhysicsEngine::start() {
timer.start();
}
inline void PhysicsEngine::stop() {
timer.stop();
}
}
#endif

72
src/reactphysics3d/engine/PhysicsWorld.cpp Normal file
View File

@ -0,0 +1,72 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "PhysicsWorld.h"
#include <algorithm>
// Namespaces
using namespace reactphysics3d;
using namespace std;
// Constructor
PhysicsWorld::PhysicsWorld(const Vector3D& gravity)
: gravity(gravity), isGravityOn(true) {
}
// Destructor
PhysicsWorld::~PhysicsWorld() {
// Remove and free the memory of all constraints
removeAllConstraints();
}
// Remove all collision contacts constraints
void PhysicsWorld::removeAllContactConstraints() {
// For all constraints
for (vector<Constraint*>::iterator it = constraints.begin(); it != constraints.end(); ) {
// Try a downcasting
Contact* contact = dynamic_cast<Contact*>(*it);
// If the constraint is a contact
if (contact != 0) {
// Delete the contact
delete (*it);
it = constraints.erase(it);
}
else {
++it;
}
}
}
// Remove all constraints in the physics world and also delete them (free their memory)
void PhysicsWorld::removeAllConstraints() {
for (vector<Constraint*>::iterator it = constraints.begin(); it != constraints.end(); it++) {
delete *it;
}
constraints.clear();
}

178
src/reactphysics3d/engine/PhysicsWorld.h Normal file
View File

@ -0,0 +1,178 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef PHYSICSWORLD_H
#define PHYSICSWORLD_H
// Libraries
#include <vector>
#include <algorithm>
#include "../mathematics/mathematics.h"
#include "../body/Body.h"
#include "../constraint/Constraint.h"
#include "../constraint/Contact.h"
// Namespace reactphysics3d
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class PhysicsWorld :
This class represents the world of the
physics engine. The physics world contains all the bodies of the physics
engine.
-------------------------------------------------------------------
*/
class PhysicsWorld {
protected :
std::vector<Body*> bodies; // list that contains all bodies of the physics world
std::vector<Body*> addedBodies; // Added bodies since last update
std::vector<Body*> removedBodies; // Removed bodies since last update
std::vector<Constraint*> constraints; // List that contains all the current constraints
Vector3D gravity; // Gravity vector of the world
bool isGravityOn; // True if the gravity force is on
public :
PhysicsWorld(const Vector3D& gravity); // Constructor
virtual ~PhysicsWorld(); // Destructor
void addBody(Body* body); // Add a body to the physics world
void removeBody(Body const* const body); // Remove a body from the physics world
void clearAddedAndRemovedBodies(); // Clear the addedBodies and removedBodies sets
Vector3D getGravity() const; // Return the gravity vector of the world
bool getIsGravityOn() const; // Return if the gravity is on
void setIsGratityOn(bool isGravityOn); // Set the isGravityOn attribute
void addConstraint(Constraint* constraint); // Add a constraint
void removeConstraint(Constraint* constraint); // Remove a constraint
void removeAllContactConstraints(); // Remove all collision contacts constraints
void removeAllConstraints(); // Remove all constraints and delete them (free their memory)
std::vector<Constraint*>::iterator getConstraintsBeginIterator(); // Return a start iterator on the constraint list
std::vector<Constraint*>::iterator getConstraintsEndIterator(); // Return a end iterator on the constraint list
std::vector<Body*>::iterator getBodiesBeginIterator(); // Return an iterator to the beginning of the bodies of the physics world
std::vector<Body*>::iterator getBodiesEndIterator(); // Return an iterator to the end of the bodies of the physics world
std::vector<Body*>& getAddedBodies(); // Return the added bodies since last update of the physics engine
std::vector<Body*>& getRemovedBodies(); // Retrun the removed bodies since last update of the physics engine
};
// Add a body to the physics world
inline void PhysicsWorld::addBody(Body* body) {
std::vector<Body*>::iterator it;
assert(body);
it = std::find(bodies.begin(), bodies.end(), body);
assert(it == bodies.end());
// The body isn't already in the bodyList, therefore we add it to the list
bodies.push_back(body);
addedBodies.push_back(body);
it = std::find(removedBodies.begin(), removedBodies.end(), body);
if (it != removedBodies.end()) {
removedBodies.erase(it);
}
}
// Remove a body from the physics world
inline void PhysicsWorld::removeBody(Body const* const body) {
std::vector<Body*>::iterator it;
assert(body);
it = std::find(bodies.begin(), bodies.end(), body);
assert(*it == body);
// Remove the body
bodies.erase(it);
addedBodies.erase(it);
removedBodies.push_back(*it);
}
// Add a constraint into the physics world
inline void PhysicsWorld::addConstraint(Constraint* constraint) {
assert(constraint != 0);
constraints.push_back(constraint);
}
// Remove a constraint and free its memory
inline void PhysicsWorld::removeConstraint(Constraint* constraint) {
std::vector<Constraint*>::iterator it;
assert(constraint);
it = std::find(constraints.begin(), constraints.end(), constraint);
assert(*it == constraint);
delete *it;
constraints.erase(it);
}
// Clear the addedBodies and removedBodies sets
inline void PhysicsWorld::clearAddedAndRemovedBodies() {
addedBodies.clear();
removedBodies.clear();
}
// Return the gravity vector of the world
inline Vector3D PhysicsWorld::getGravity() const {
return gravity;
}
// Return if the gravity is on
inline bool PhysicsWorld::getIsGravityOn() const {
return isGravityOn;
}
// Set the isGravityOn attribute
inline void PhysicsWorld::setIsGratityOn(bool isGravityOn) {
this->isGravityOn = isGravityOn;
}
// Return a start iterator on the constraint list
inline std::vector<Constraint*>::iterator PhysicsWorld::getConstraintsBeginIterator() {
return constraints.begin();
}
// Return a end iterator on the constraint list
inline std::vector<Constraint*>::iterator PhysicsWorld::getConstraintsEndIterator() {
return constraints.end();
}
// Return an iterator to the beginning of the bodies of the physics world
inline std::vector<Body*>::iterator PhysicsWorld::getBodiesBeginIterator() {
return bodies.begin();
}
// Return an iterator to the end of the bodies of the physics world
inline std::vector<Body*>::iterator PhysicsWorld::getBodiesEndIterator() {
return bodies.end();
}
// Return the added bodies since last update of the physics engine
inline std::vector<Body*>& PhysicsWorld::getAddedBodies() {
return addedBodies;
}
// Retrun the removed bodies since last update of the physics engine
inline std::vector<Body*>& PhysicsWorld::getRemovedBodies() {
return removedBodies;
}
} // End of the ReactPhysics3D namespace
#endif

48
src/reactphysics3d/engine/Timer.cpp Normal file
View File

@ -0,0 +1,48 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "Timer.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
Timer::Timer(double timeStep) : timeStep(timeStep), isRunning(false) {
assert(timeStep > 0.0);
}
// Destructor
Timer::~Timer() {
}

152
src/reactphysics3d/engine/Timer.h Normal file
View File

@ -0,0 +1,152 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef TIMER_H
#define TIMER_H
// Libraries
#include <stdexcept>
#include <iostream>
#include <ctime>
#include <cassert>
// Namespace ReactPhysics3D
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class Timer :
This class will take care of the time in the physics engine.
-------------------------------------------------------------------
*/
class Timer {
private :
double timeStep; // Timestep dt of the physics engine (timestep > 0.0)
long double time; // Current time of the physics engine
long double lastUpdateTime; // Last time the timer has been updated
long double deltaTime; // Time difference between the two last timer update() calls
double accumulator; // Used to fix the time step and avoid strange time effects
bool isRunning; // True if the timer is running
public :
Timer(double timeStep); // Constructor
virtual ~Timer(); // Destructor
double getTimeStep() const; // Return the timestep of the physics engine
void setTimeStep(double timeStep) throw(std::invalid_argument); // Set the timestep of the physics engine
long double getTime() const; // Return the current time
void start(); // Start the timer
void stop(); // Stop the timer
bool getIsRunning() const; // Return true if the timer is running
bool isPossibleToTakeStep() const; // True if it's possible to take a new step
void update(); // Compute the time since the last update() call and add it to the accumulator
void nextStep(); // Take a new step => update the timer by adding the timeStep value to the current time
double computeInterpolationFactor(); // Compute the interpolation factor
};
// --- Inline functions --- //
// Return the timestep of the physics engine
inline double Timer::getTimeStep() const {
return timeStep;
}
// Set the timestep of the physics engine
inline void Timer::setTimeStep(double timeStep) throw(std::invalid_argument) {
// Check if the timestep is different from zero
if (timeStep != 0.0) {
this->timeStep = timeStep;
}
else {
// We throw an exception
throw std::invalid_argument("Exception in Timer : the timestep has to be different from zero");
}
}
// Return the current time
inline long double Timer::getTime() const {
return time;
}
// Return if the timer is running
inline bool Timer::getIsRunning() const {
return isRunning;
}
// Start the timer
inline void Timer::start() {
if (!isRunning) {
// Initialize the lastUpdateTime with the current time in seconds
lastUpdateTime = std::clock() / double(CLOCKS_PER_SEC);
accumulator = 0.0;
isRunning = true;
}
}
// Stop the timer
inline void Timer::stop() {
if (isRunning) {
isRunning = false;
}
}
// True if it's possible to take a new step
inline bool Timer::isPossibleToTakeStep() const {
return (accumulator >= timeStep);
}
// Take a new step => update the timer by adding the timeStep value to the current time
inline void Timer::nextStep() {
assert(isRunning);
// Update the current time of the physics engine
time += timeStep;
// Update the accumulator value
accumulator -= timeStep;
}
// Compute the interpolation factor
inline double Timer::computeInterpolationFactor() {
return (accumulator / timeStep);
}
// Compute the time since the last update() call and add it to the accumulator
inline void Timer::update() {
// Compute the current time is seconds
long double currentTime = std::clock() / double(CLOCKS_PER_SEC);
// Compute the delta display time between two display frames
deltaTime = currentTime - lastUpdateTime;
// Update the current display time
lastUpdateTime = currentTime;
// Update the accumulator value
accumulator += deltaTime;
}
} // End of the ReactPhysics3D namespace
#endif

549
src/reactphysics3d/mathematics/Matrix.cpp Normal file
View File

@ -0,0 +1,549 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include <cassert>
#include "Matrix.h"
// Namespaces
using namespace reactphysics3d;
// Constructor without argument
Matrix::Matrix()
:nbRow(0), nbColumn(0) {
array = 0;
}
// Constructor of the class Matrix
Matrix::Matrix(int nbRow, int nbColumn) throw(std::invalid_argument)
:nbRow(nbRow),nbColumn(nbColumn) {
// Check the arguments
if (nbRow>0 && nbColumn>0) {
// Create the two dimensional dynamic array
array = new double*[nbRow];
assert(array != 0); // Array pointer musn't be null
for(int i=0; i<nbRow; ++i) {
array[i] = new double[nbColumn];
assert(array[i] != 0);
}
}
else {
// Throw an exception
throw std::invalid_argument("Exception : The size of the matrix has to be positive !");
}
}
// Copy-constructor of the class Matrix
Matrix::Matrix(const Matrix& matrix)
:nbRow(matrix.nbRow), nbColumn(matrix.nbColumn) {
// Create the two dimensional dynamic array
array = new double*[nbRow];
assert(array != 0); // Array pointer musn't be null
for(int i=0; i<nbRow; ++i) {
array[i] = new double[nbColumn];
}
// Copy the matrix
for (int i=0; i<nbRow; ++i) {
for(int j=0; j<nbColumn; ++j) {
setValue(i,j, matrix.getValue(i,j));
}
}
}
// Conversion from Matrix3x3
Matrix::Matrix(const Matrix3x3& matrix)
:nbRow(3), nbColumn(3) {
// Create the two dimensional dynamic array
array = new double*[nbRow];
assert(array != 0); // Array pointer musn't be null
for(int i=0; i<nbRow; ++i) {
array[i] = new double[nbColumn];
}
// Copy the matrix
for (int i=0; i<nbRow; ++i) {
for(int j=0; j<nbColumn; ++j) {
setValue(i,j, matrix.getValue(i,j));
}
}
}
// Conversion from Vector to Matrix
Matrix::Matrix(const Vector& vector) {
// Create the two dimensional dynamic array
array = new double*[vector.getNbComponent()];
assert(array != 0);
for(int i=0; i<nbRow; ++i) {
array[i] = new double[1];
array[i][0] = vector.getValue(i);
}
}
// Destructor of the class Matrix
Matrix::~Matrix() {
// Destruction of the dynamic array
for(int i=0; i<nbRow; ++i) {
delete array[i];
}
delete this->array;
}
void Matrix::changeSize(uint newNbRows, uint newNbColumns) {
if (array) {
// Destruction of the dynamic array
for(int i=0; i<nbRow; ++i) {
delete array[i];
}
delete this->array;
}
// Create the two dimensional dynamic array
array = new double*[newNbRows];
assert(array != 0); // Array pointer musn't be null
for(int i=0; i<newNbRows; ++i) {
array[i] = new double[newNbColumns];
}
nbColumn = newNbColumns;
nbRow = newNbRows;
}
// Function that return the cofactor matrix by removing row i and column j
Matrix Matrix::getCofactor(int i, int j) const throw(std::invalid_argument) {
// If i and j are in the matrix
if (0<= i && i < nbRow && 0<= j && j<nbColumn) {
// Create the cofactor matrix
Matrix cofactor(nbRow-1,nbColumn-1);
int u=0; // Row coordinate in the cofactor matrix
int v=0; // Column coordinate in the cofactor matrix
// For every element in the matrix
for (int s=0; s<nbColumn; ++s) {
for(int r=0; r<nbRow; ++r) {
// If the element is not in row i or in column j
if (r!=i && s!=j) {
// Add the element in the cofactor matrix
cofactor.setValue(u,v, getValue(r,s));
++u;
if (u==cofactor.nbRow) {
u = 0;
++v;
}
}
}
}
// Return the cofactor matrix
return cofactor;
}
else {
// We Throw an out_of_range exception
throw std::invalid_argument("Exception : The index i or j is outside the matrix size !");
}
}
// This function return the transposed matrix
Matrix Matrix::getTranspose() const {
// Create the new matrix
Matrix transposedMatrix(nbColumn, nbRow);
// Transposition of the matrix
for (int i=0; i<nbRow; ++i) {
for(int j=0; j<nbColumn; ++j) {
transposedMatrix.setValue(j,i, array[i][j]);
}
}
// Return the transposed matrix
return transposedMatrix;
}
// Function that return the inverse of the matrix if there exists
Matrix Matrix::getInverse() const throw(MathematicsException) {
// Check if the matrix is a square-matrix
if (nbRow==nbColumn) {
// Compute the determinant of the matrix
double determinant = getDeterminant();
// Check if the matrix is invertible
if (determinant != 0.0) {
// Create a temp matrix
Matrix tempMatrix(nbRow, nbColumn);
double k=1.0;
// Compute the inverse matrix
for(int i=0; i<nbRow; ++i) {
for(int j=0; j<nbColumn; ++j) {
if ( (i+j) % 2 == 0) {
k=1.0;
}
else {
k=-1.0;
}
tempMatrix.setValue(i,j, k * getCofactor(i,j).getDeterminant());
}
}
// Create the inverse matrix
Matrix inverseMatrix = tempMatrix.getTranspose() * (1.0 / determinant);
// Return the inverse matrix
return inverseMatrix;
}
else {
// We throw a MathematicsException
throw MathematicsException("MathematicsException : Inverse of the matrix can't be computed because the determinant is zero !");
}
}
else {
// We throw an Matrix Exception
throw MathematicsException("MathematicsException : Inverse can't be computed for a non-square matrix !");
}
}
// Function that return the determinant of the matrix
double Matrix::getDeterminant() const throw(MathematicsException) {
// If the matrix is a square matrix
if (nbRow == nbColumn) {
if(nbRow == 1) {
return getValue(0,0);
}
else if (nbRow == 2) {
return (getValue(0,0) * getValue(1,1) - getValue(1,0) * getValue(0,1));
}
else {
double determinant = 0.0;
double k=1.0;
// For every element in the first row
for(int j=0; j<nbColumn; ++j) {
determinant = determinant + k * getValue(0,j) * getCofactor(0,j).getDeterminant();
if (k==1.0) {
k=-1.0;
}
else {
k=1.0;
}
}
// Return the determinant value
return determinant;
}
}
else {
// Throw a MathematicsException
throw MathematicsException("MathematicsException : The determinant of a non-square matrix isn't computable !");
}
}
// Return the trace of the matrix
double Matrix::getTrace() const throw(MathematicsException) {
// Check if the matrix is a square-matrix
if (nbRow == nbColumn) {
double sum = 0.0;
// Compute the trace of the matrix
for(int i=0; i<nbRow; ++i) {
sum = sum + array[i][i];
}
// Return the trace
return sum;
}
else {
// We throw an exception because the matrix is non-square
throw MathematicsException("MathematicsException : Impossible to compute the trace for a non-square matrix");
}
}
// Return a sub matrix of size of the current matrix
Matrix Matrix::getSubMatrix(unsigned int i, unsigned int j,
unsigned int sizeRows, unsigned int sizeColumns) const throw(std::invalid_argument) {
// Check the arguments
if (i<0 || j<0 || i+sizeRows > nbRow || j+sizeColumns > nbColumn) {
// Throw an exception
throw std::invalid_argument("Error : The arguments are out of matrix bounds");
}
// Compute the sub-matrix
Matrix resultMatrix(sizeRows, sizeColumns);
for (unsigned int k=0; k<sizeRows; k++) {
for (unsigned int l=0; l<sizeColumns; l++) {
resultMatrix.array[k][l] = array[i+k][j+l];
}
}
// Return the sub-matrix
return resultMatrix;
}
// Static function that return a identity matrix of size nxn
Matrix Matrix::identity(int dimension) throw(std::invalid_argument) {
// Argument verification
if (dimension > 0) {
// Create a new matrix
Matrix identityMatrix(dimension,dimension);
// Fill in the identity matrix
for(int i=0; i<dimension; ++i) {
for(int j=0; j<dimension; ++j) {
if (i==j) {
identityMatrix.setValue(i, j, 1.0);
}
else {
identityMatrix.setValue(i, j, 0.0);
}
}
}
// Return the identity matrix
return identityMatrix;
}
else {
// Throw an exception
throw std::invalid_argument("Exception : The argument of identity() has to be positive !");
}
}
// Fill in a sub-matrix of the current matrix with another matrix.
// This method replaces the sub-matrix with the upper-left index (i,j) in the current matrix by another
// "subMatrix" matrix.
void Matrix::fillInSubMatrix(unsigned int rowIndex, unsigned int colIndex, const Matrix& subMatrix) {
assert(nbRow-rowIndex >= subMatrix.nbColumn);
assert(nbColumn-colIndex >= subMatrix.nbColumn);
// Fill in the sub-matrix
for (unsigned int i=0; i<subMatrix.nbRow; ++i) {
for (unsigned int j=0; j<subMatrix.nbColumn; ++j) {
array[rowIndex + i][colIndex + j] = subMatrix.array[i][j];
}
}
}
// Initialize all the matrix with the given value
void Matrix::initWithValue(double value) {
for (unsigned int i=0; i<nbRow; ++i) {
for(unsigned int j=0; j<nbColumn; ++j) {
array[i][j] = value;
}
}
}
Vector Matrix::getVector() const {
assert(nbColumn == 1);
Vector vec(nbRow);
for (int i=0; i<nbRow; i++) {
vec.setValue(i, array[i][0]);
}
return vec;
}
// Definition of the operator + for the sum of two matrices with references
Matrix Matrix::operator+(const Matrix& matrix2) const throw(MathematicsException) {
if (nbRow == matrix2.nbRow && nbColumn == matrix2.nbColumn) {
// Create a new matrix
Matrix sumMatrix(nbRow,nbColumn);
// Sum the two matrices
for(int i=0; i<nbRow; ++i) {
for(int j=0; j<nbColumn; ++j) {
sumMatrix.setValue(i, j, this->getValue(i,j) + matrix2.getValue(i,j));
}
}
// Return the sum matrix
return sumMatrix;
}
else {
// We throw an MathematicsException
throw MathematicsException("MathematicsException : Addition of the matrices isn't possible beacause the size of the matrices aren't the same");
}
}
// Definition of the operator - for the substraction of two matrices with references
Matrix Matrix::operator-(const Matrix& matrix2) const throw(MathematicsException) {
if (nbRow == matrix2.nbRow && nbColumn == matrix2.nbColumn) {
// Create a new matrix
Matrix sumMatrix(nbRow, nbColumn);
// Substract the two matrices
for(int i=0; i<nbRow; ++i) {
for(int j=0; j<this->nbColumn; ++j) {
sumMatrix.setValue(i, j, this->getValue(i,j) - matrix2.getValue(i,j));
}
}
// Return the sum matrix
return sumMatrix;
}
else {
// We throw a MathematicsException
throw MathematicsException("MathematicsException : Substraction of the matrices isn't possible beacause the size of the matrices aren't the same");
}
}
// Overloaded operator * for the multiplication of the matrix with a number
Matrix Matrix::operator*(double nb) const {
// Creation of the result matrix
Matrix result(nbRow, nbColumn);
// Multiplication of the matrix with the number
for(int i=0; i<nbRow; ++i) {
for(int j=0; j<nbColumn; ++j) {
result.setValue(i,j, getValue(i,j) * nb);
}
}
// Return the result matrix
return result;
}
// Overloaded operator for multiplication with a matrix
Matrix Matrix::operator*(const Matrix& matrix2) const throw(MathematicsException) {
// Check the sizes of the matrices
if (nbColumn == matrix2.nbRow) {
// Compute the result of the multiplication
Matrix result(nbRow, matrix2.nbColumn);
double sum;
for(int i=0; i<nbRow; ++i) {
for(int j=0; j<matrix2.nbColumn; ++j) {
sum = 0.0;
for(int k=0; k<nbColumn; ++k) {
sum = sum + array[i][k] * matrix2.array[k][j];
}
result.array[i][j] = sum;
}
}
// Return the result matrix
return result;
}
else {
// Throw an exception because the multiplication is impossible
throw MathematicsException("MathematicsException : The sizes of the matrices aren't compatible for the multiplication");
}
}
// Overloaded operator for multiplication with a vector
Matrix Matrix::operator*(const Vector& vector) const throw(MathematicsException) {
// Check the sizes of the matrices
if (nbColumn == vector.getNbComponent()) {
Matrix result(nbRow, 1);
for (int i=0; i<nbRow; i++) {
double sum = 0.0;
for (int j=0; j<nbColumn; j++) {
sum += array[i][j] * vector.getValue(j);
}
result.array[i][0] = sum;
}
return result;
}
else {
// Throw an exception because the multiplication is impossible
throw MathematicsException("MathematicsException : The sizes of the matrix and the vector aren't compatible for the multiplication");
}
}
// Overloaded operator = for the assignment
Matrix& Matrix::operator=(const Matrix& matrix2) throw(MathematicsException) {
// Check for self-assignement
if(this == &matrix2) {
return *this;
}
// Check the size of the matrix
if (nbRow==matrix2.nbRow && nbColumn==matrix2.nbColumn) {
// Check for self-assignment
if (this != &matrix2) {
for(int i=0; i<nbRow; ++i) {
for(int j=0; j<nbColumn; ++j) {
this->setValue(i,j, matrix2.getValue(i,j));
}
}
}
// Return a reference to the matrix
return *this;
}
else {
// Throw a MathematicsException
throw MathematicsException("MathematicsException : Assignment impossible because the size of the matrices aren't the same !");
}
}
// Overloaded operator for equality condition
bool Matrix::operator==(const Matrix& matrix2) const throw(MathematicsException) {
// Check if the matrices dimensions are compatible
if (nbRow == matrix2.nbRow && nbColumn == matrix2.nbColumn) {
for (int i=0; i<nbRow; ++i) {
for(int j=0; j<nbColumn; ++j) {
if (array[i][j] != matrix2.array[i][j]) {
return false;
}
}
}
return true;
}
else {
// Throw an exception because the matrices dimensions aren't the same
throw MathematicsException("MathematicsException : Impossible to check if the matrices are equal because they don't have the same dimension");
}
}
// TO DELETE, THIS IS JUST FOR TESTING MATRICES
void Matrix::display() const {
for(int i=0; i<nbRow; ++i) {
for(int j=0; j<nbColumn; ++j) {
std::cout << array[i][j] << " ";
}
std::cout << std::endl;
}
}

129
src/reactphysics3d/mathematics/Matrix.h Normal file
View File

@ -0,0 +1,129 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef MATRIX_H
#define MATRIX_H
// Libraries
#include "exceptions.h"
#include "Matrix3x3.h"
#include "Vector.h"
#include <stdexcept>
#include <iostream>
// TODO : Replace the "int" by "unsigned int"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class Matrix :
This class represents a matrix.
-------------------------------------------------------------------
*/
class Matrix {
private :
unsigned int nbRow; // Number of row in the matrix
unsigned int nbColumn; // Number of colum in the matrix
double** array; // Dynamic array that contains the values of the matrix
public :
Matrix(); // Constructor without argument
Matrix(int nbRow, int nbColum) throw(std::invalid_argument); // Constructor of the class Matrix
Matrix(const Matrix& matrix); // Copy constructor of the class Matrix
Matrix(const Matrix3x3& matrix); // Conversion from Matrix3x3
Matrix(const Vector& vector); // Conversion from Vector to Matrix
virtual ~Matrix(); // Destructor of the class Matrix
double getValue(int i, int j) const throw(std::invalid_argument); // Return a value in the matrix
void setValue(int i, int j, double value) throw(std::invalid_argument); // Set a value in the matrix
int getNbRow() const; // Return the number of row of the matrix
int getNbColumn() const; // Return the number of column of the matrix
Matrix getCofactor(int i, int j) const throw(std::invalid_argument); // Return the cofactor matrix by removing row i and column j
Matrix getTranspose() const; // Return the transposed matrixs
Matrix getInverse() const throw(MathematicsException); // Return the inverse of the matrix if there exists
double getDeterminant() const throw(MathematicsException); // Return the determinant of the matrix
double getTrace() const throw(MathematicsException); // Return the trace of a square matrix
Matrix getSubMatrix(unsigned int i, unsigned int j,
unsigned int nbRows, unsigned int nbColumns) const throw(std::invalid_argument); // Return a sub matrix of size of the current matrix
Vector getVector() const;
static Matrix identity(int dimension) throw(std::invalid_argument); // Return the identity matrix I of the given dimension
void fillInSubMatrix(unsigned int i, unsigned int j, const Matrix& subMatrix); // Fill in a sub-matrix of the current matrix with another matrix
void initWithValue(double value); // Initialize all the matrix with the given value
void display() const; // TO DELETE
void changeSize(uint nbRows, uint nbColumns);
// --- Overloaded operators --- //
Matrix operator+(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for addition
Matrix operator-(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for substraction
Matrix operator*(double nb) const; // Overloaded operator for multiplication with a number
Matrix operator*(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for multiplication with a matrix
Matrix operator*(const Vector& vector) const throw(MathematicsException); // Overloaded operator for multiplication with a vector
Matrix& operator=(const Matrix& matrix2) throw(MathematicsException); // Overloaded operator for assignment
bool operator==(const Matrix& matrix2) const throw(MathematicsException); // Overloaded operator for equality condition
};
// Function to get a value in the matrix (inline)
inline double Matrix::getValue(int i, int j) const throw(std::invalid_argument) {
if (0 <= i && i < nbRow && 0 <= j && j < nbColumn) {
// get the value in the matrix
return array[i][j];
}
else {
// We Throw an out_of_range exception
throw std::invalid_argument("Exception : The index i or j is outside the matrix size !");
}
}
// Function to set a value in the matrix (inline)
inline void Matrix::setValue(int i, int j, double value) throw(std::invalid_argument) {
if (0 <= i && i < nbRow && 0 <= j && j < nbColumn) {
// Set the value in the matrix
this->array[i][j] = value;
}
else {
// We Throw an out_of_range exception
throw std::invalid_argument("Exception : The index i or j is outside the matrix size !");
}
}
// Function that return the number of row of the matrix (inline)
inline int Matrix::getNbRow() const {
return nbRow;
}
// Function that return the number of colum of the matrix (inline)
inline int Matrix::getNbColumn() const {
return nbColumn;
}
// Overloaded operator for multiplication between a number and a Matrix (inline)
inline Matrix operator*(double number, const Matrix& matrix) {
// Return the result matrix
return matrix * number;
}
} // End of the ReactPhysics3D namespace
#endif

141
src/reactphysics3d/mathematics/Matrix3x3.cpp Normal file
View File

@ -0,0 +1,141 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include <iostream>
#include "Matrix3x3.h"
// Namespaces
using namespace reactphysics3d;
// Constructor of the class Matrix3x3
Matrix3x3::Matrix3x3() {
// Initialize all values in the matrix to zero
setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
}
// Constructor with arguments
Matrix3x3::Matrix3x3(double a1, double a2, double a3, double b1, double b2, double b3, double c1, double c2, double c3) {
// Initialize the matrix with the values
setAllValues(a1, a2, a3, b1, b2, b3, c1, c2, c3);
}
// Copy-constructor
// TODO : Test if this copy-constructor is correct (check if the the copy matrix use
// the same memory place for its array)
Matrix3x3::Matrix3x3(const Matrix3x3& matrix2) {
// Copy the values in the matrix
setAllValues(matrix2.array[0][0], matrix2.array[0][1], matrix2.array[0][2],
matrix2.array[1][0], matrix2.array[1][1], matrix2.array[1][2],
matrix2.array[2][0], matrix2.array[2][1], matrix2.array[2][2]);
}
// Destructor
Matrix3x3::~Matrix3x3() {
}
// Return the inverse matrix
Matrix3x3 Matrix3x3::getInverse() const throw(MathematicsException) {
// Compute the determinant of the matrix
double determinant = getDeterminant();
// Check if the determinant is equal to zero
if (determinant != 0) {
double invDeterminant = 1.0 / determinant;
Matrix3x3 tempMatrix;
// Compute the inverse of the matrix
tempMatrix.setAllValues((array[1][1]*array[2][2]-array[2][1]*array[1][2]), -(array[1][0]*array[2][2]-array[2][0]*array[1][2]), (array[1][0]*array[2][1]-array[2][0]*array[1][1]),
-(array[0][1]*array[2][2]-array[2][1]*array[0][2]), (array[0][0]*array[2][2]-array[2][0]*array[0][2]), -(array[0][0]*array[2][1]-array[2][0]*array[0][1]),
(array[0][1]*array[1][2]-array[0][2]*array[1][1]), -(array[0][0]*array[1][2]-array[1][0]*array[0][2]), (array[0][0]*array[1][1]-array[0][1]*array[1][0]));
// Return the inverse matrix
return (invDeterminant * tempMatrix.getTranspose());
}
else {
// Throw an exception because the inverse of the matrix doesn't exist if the determinant is equal to zero
throw MathematicsException("MathematicsException : Impossible to compute the inverse of the matrix because the determinant is equal to zero");
}
}
// Return the 3x3 identity matrix
Matrix3x3 Matrix3x3::identity() {
// Return the isdentity matrix
return Matrix3x3(1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0);
}
// Overloaded operator for addition
Matrix3x3 Matrix3x3::operator+(const Matrix3x3& matrix2) const {
// Return the sum matrix
return Matrix3x3(array[0][0] + matrix2.array[0][0], array[0][1] + matrix2.array[0][1], array[0][2] + matrix2.array[0][2],
array[1][0] + matrix2.array[1][0], array[1][1] + matrix2.array[1][1], array[1][2] + matrix2.array[1][2],
array[2][0] + matrix2.array[2][0], array[2][1] + matrix2.array[2][1], array[2][2] + matrix2.array[2][2]);
}
// Overloaded operator for substraction
Matrix3x3 Matrix3x3::operator-(const Matrix3x3& matrix2) const {
// Return the substraction matrix
return Matrix3x3(array[0][0] - matrix2.array[0][0], array[0][1] - matrix2.array[0][1], array[0][2] - matrix2.array[0][2],
array[1][0] - matrix2.array[1][0], array[1][1] - matrix2.array[1][1], array[1][2] - matrix2.array[1][2],
array[2][0] - matrix2.array[2][0], array[2][1] - matrix2.array[2][1], array[2][2] - matrix2.array[2][2]);
}
// Overloaded operator for multiplication with a number
Matrix3x3 Matrix3x3::operator*(double nb) const {
// Return multiplied matrix
return Matrix3x3(array[0][0] * nb, array[0][1] * nb, array[0][2] * nb,
array[1][0] * nb, array[1][1] * nb, array[1][2] * nb,
array[2][0] * nb, array[2][1] * nb, array[2][2] * nb);
}
// Overloaded operator for multiplication with a matrix
Matrix3x3 Matrix3x3::operator*(const Matrix3x3& matrix2) const {
// Compute and return the multiplication of the matrices
return Matrix3x3(array[0][0]*matrix2.array[0][0] + array[0][1]*matrix2.array[1][0] + array[0][2]*matrix2.array[2][0],
array[0][0]*matrix2.array[0][1] + array[0][1]*matrix2.array[1][1] + array[0][2]*matrix2.array[2][1],
array[0][0]*matrix2.array[0][2] + array[0][1]*matrix2.array[1][2] + array[0][2]*matrix2.array[2][2],
array[1][0]*matrix2.array[0][0] + array[1][1]*matrix2.array[1][0] + array[1][2]*matrix2.array[2][0],
array[1][0]*matrix2.array[0][1] + array[1][1]*matrix2.array[1][1] + array[1][2]*matrix2.array[2][1],
array[1][0]*matrix2.array[0][2] + array[1][1]*matrix2.array[1][2] + array[1][2]*matrix2.array[2][2],
array[2][0]*matrix2.array[0][0] + array[2][1]*matrix2.array[1][0] + array[2][2]*matrix2.array[2][0],
array[2][0]*matrix2.array[0][1] + array[2][1]*matrix2.array[1][1] + array[2][2]*matrix2.array[2][1],
array[2][0]*matrix2.array[0][2] + array[2][1]*matrix2.array[1][2] + array[2][2]*matrix2.array[2][2]);
}
// Overloaded operator for assignment
Matrix3x3& Matrix3x3::operator=(const Matrix3x3& matrix2) {
// Check for self-assignment
if (this != &matrix2) {
setAllValues(matrix2.array[0][0], matrix2.array[0][1], matrix2.array[0][2],
matrix2.array[1][0], matrix2.array[1][1], matrix2.array[1][2],
matrix2.array[2][0], matrix2.array[2][1], matrix2.array[2][2]);
}
// Return a reference to the matrix
return *this;
}

161
src/reactphysics3d/mathematics/Matrix3x3.h Normal file
View File

@ -0,0 +1,161 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef MATRIX3X3_H
#define MATRIX3X3_H
// Libraries
#include "exceptions.h"
#include "Vector3D.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class Matrix3x3 :
This class represents a 3x3 matrix.
-------------------------------------------------------------------
*/
class Matrix3x3 {
private :
double array[3][3]; // Array with the values of the matrix
public :
Matrix3x3(); // Constructor of the class Matrix3x3
Matrix3x3(double a1, double a2, double a3, double b1, double b2, double b3,
double c1, double c2, double c3); // Constructor with arguments
Matrix3x3(const Matrix3x3& matrix); // Copy-constructor
virtual ~Matrix3x3(); // Destructor
double getValue(int i, int j) const throw(std::invalid_argument); // Get a value in the matrix
void setValue(int i, int j, double value) throw(std::invalid_argument); // Set a value in the matrix
void setAllValues(double a1, double a2, double a3, double b1, double b2, double b3,
double c1, double c2, double c3); // Set all the values in the matrix
Matrix3x3 getTranspose() const; // Return the transpose matrix
double getDeterminant() const; // Return the determinant of the matrix
double getTrace() const; // Return the trace of the matrix
Matrix3x3 getInverse() const throw(MathematicsException); // Return the inverse matrix
static Matrix3x3 identity(); // Return the 3x3 identity matrix
// --- Overloaded operators --- //
Matrix3x3 operator+(const Matrix3x3& matrix2) const; // Overloaded operator for addition
Matrix3x3 operator-(const Matrix3x3& matrix2) const ; // Overloaded operator for substraction
Matrix3x3 operator*(double nb) const; // Overloaded operator for multiplication with a number
Matrix3x3 operator*(const Matrix3x3& matrix2) const; // Overloaded operator for multiplication with a matrix
Vector3D operator*(const Vector3D& vector3d) const; // Overloaded operator for multiplication with a vector
Matrix3x3& operator=(const Matrix3x3& matrix2); // Overloaded operator for assignment
bool operator==(const Matrix3x3& matrix2) const; // Overloaded operator for equality condition
};
// Method to get a value in the matrix (inline)
inline double Matrix3x3::getValue(int i, int j) const throw(std::invalid_argument) {
// Check the argument
if (i>=0 && i<3 && j>=0 && j<3) {
// Return the value
return array[i][j];
}
else {
// Throw an exception because of the wrong argument
throw std::invalid_argument("Exception : The argument isn't in the bounds of the 3x3 matrix");
}
}
// Method to set a value in the matrix (inline)
inline void Matrix3x3::setValue(int i, int j, double value) throw(std::invalid_argument) {
// Check the argument
if (i>=0 && i<3 && j>=0 && j<3) {
// Set the value
array[i][j] = value;
}
else {
// Throw an exception because of the wrong argument
throw std::invalid_argument("Exception : The argument isn't in the bounds of the 3x3 matrix");
}
}
// Method to set all the values in the matrix
inline void Matrix3x3::setAllValues(double a1, double a2, double a3, double b1, double b2, double b3,
double c1, double c2, double c3) {
// Set all the values of the matrix
array[0][0] = a1;
array[0][1] = a2;
array[0][2] = a3;
array[1][0] = b1;
array[1][1] = b2;
array[1][2] = b3;
array[2][0] = c1;
array[2][1] = c2;
array[2][2] = c3;
}
// Return the transpose matrix
inline Matrix3x3 Matrix3x3::getTranspose() const {
// Return the transpose matrix
return Matrix3x3(array[0][0], array[1][0], array[2][0],
array[0][1], array[1][1], array[2][1],
array[0][2], array[1][2], array[2][2]);
}
// Return the determinant of the matrix
inline double Matrix3x3::getDeterminant() const {
// Compute and return the determinant of the matrix
return (array[0][0]*(array[1][1]*array[2][2]-array[2][1]*array[1][2]) - array[0][1]*(array[1][0]*array[2][2]-array[2][0]*array[1][2]) +
array[0][2]*(array[1][0]*array[2][1]-array[2][0]*array[1][1]));
}
// Return the trace of the matrix
inline double Matrix3x3::getTrace() const {
// Compute and return the trace
return (array[0][0] + array[1][1] + array[2][2]);
}
// Overloaded operator for multiplication between a number and a Matrix3x3 (inline)
inline Matrix3x3 operator*(double number, const Matrix3x3& matrix) {
// Return the multiplied matrix
return matrix * number;
}
// Overloaded operator for multiplication with a vector
inline Vector3D Matrix3x3::operator*(const Vector3D& vector3d) const {
// Compute and return the result
return Vector3D(array[0][0]*vector3d.getX() + array[0][1]*vector3d.getY() + array[0][2]*vector3d.getZ(),
array[1][0]*vector3d.getX() + array[1][1]*vector3d.getY() + array[1][2]*vector3d.getZ(),
array[2][0]*vector3d.getX() + array[2][1]*vector3d.getY() + array[2][2]*vector3d.getZ());
}
// Overloaded operator for equality condition
inline bool Matrix3x3::operator==(const Matrix3x3& matrix2) const {
return (array[0][0] == matrix2.array[0][0] && array[0][1] == matrix2.array[0][1] && array[0][2] == matrix2.array[0][2] &&
array[1][0] == matrix2.array[1][0] && array[1][1] == matrix2.array[1][1] && array[1][2] == matrix2.array[1][2] &&
array[2][0] == matrix2.array[2][0] && array[2][1] == matrix2.array[2][1] && array[2][2] == matrix2.array[2][2]);
}
} // End of the ReactPhysics3D namespace
#endif

228
src/reactphysics3d/mathematics/Quaternion.cpp Executable file
View File

@ -0,0 +1,228 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "Quaternion.h"
#include "Vector3D.h"
#include <cassert>
// Namespace
using namespace reactphysics3d;
// Constructor of the class
Quaternion::Quaternion()
:x(0.0), y(0.0), z(0.0), w(0.0) {
}
// Constructor with arguments
Quaternion::Quaternion(double x, double y, double z, double w)
:x(x), y(y), z(z), w(w) {
}
// Constructor with the component w and the vector v=(x y z)
Quaternion::Quaternion(double w, const Vector3D& v)
:x(v.getX()), y(v.getY()), z(v.getZ()), w(w) {
}
// Copy-constructor
Quaternion::Quaternion(const Quaternion& quaternion)
:x(quaternion.x), y(quaternion.y), z(quaternion.z), w(quaternion.w) {
}
// Create a unit quaternion from a rotation matrix
Quaternion::Quaternion(const Matrix3x3& matrix) {
// Get the trace of the matrix
double trace = matrix.getTrace();
double array[3][3];
for (int i=0; i<3; i++) {
for (int j=0; j<3; j++) {
array[i][j] = matrix.getValue(i, j);
}
}
double r;
double s;
if (trace < 0.0) {
if (array[1][1] > array[0][0]) {
if(array[2][2] > array[1][1]) {
r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0);
s = 0.5 / r;
// Compute the quaternion
x = (array[2][0] + array[0][2])*s;
y = (array[1][2] + array[2][1])*s;
z = 0.5*r;
w = (array[1][0] - array[0][1])*s;
}
else {
r = sqrt(array[1][1] - array[2][2] - array[0][0] + 1.0);
s = 0.5 / r;
// Compute the quaternion
x = (array[0][1] + array[1][0])*s;
y = 0.5 * r;
z = (array[1][2] + array[2][1])*s;
w = (array[0][2] - array[2][0])*s;
}
}
else if (array[2][2] > array[0][0]) {
r = sqrt(array[2][2] - array[0][0] - array[1][1] + 1.0);
s = 0.5 / r;
// Compute the quaternion
x = (array[2][0] + array[0][2])*s;
y = (array[1][2] + array[2][1])*s;
z = 0.5 * r;
w = (array[1][0] - array[0][1])*s;
}
else {
r = sqrt(array[0][0] - array[1][1] - array[2][2] + 1.0);
s = 0.5 / r;
// Compute the quaternion
x = 0.5 * r;
y = (array[0][1] + array[1][0])*s;
z = (array[2][0] - array[0][2])*s;
w = (array[2][1] - array[1][2])*s;
}
}
else {
r = sqrt(trace + 1.0);
s = 0.5/r;
// Compute the quaternion
x = (array[2][1]-array[1][2])*s;
y = (array[0][2]-array[2][0])*s;
z = (array[1][0]-array[0][1])*s;
w = 0.5 * r;
}
}
// Destructor
Quaternion::~Quaternion() {
}
// Compute the rotation angle (in radians) and the 3D rotation axis
// This method is used to get the rotation angle (in radian) and the unit
// rotation axis of an orientation quaternion.
void Quaternion::getRotationAngleAxis(double& angle, Vector3D& axis) const {
Quaternion quaternion;
// If the quaternion is unit
if (length() == 1.0) {
quaternion = *this;
}
else {
// We compute the unit quaternion
quaternion = getUnit();
}
// Compute the roation angle
angle = acos(quaternion.w) * 2.0;
// Compute the 3D rotation axis
Vector3D rotationAxis(quaternion.x, quaternion.y, quaternion.z);
// Normalize the rotation axis
rotationAxis = rotationAxis.getUnit();
// Set the rotation axis values
axis.setAllValues(rotationAxis.getX(), rotationAxis.getY(), rotationAxis.getZ());
}
// Return the orientation matrix corresponding to this quaternion
Matrix3x3 Quaternion::getMatrix() const {
double nQ = x*x + y*y + z*z + w*w;
double s = 0.0;
if (nQ > 0.0) {
s = 2.0/nQ;
}
// Computations used for optimization (less multiplications)
double xs = x*s;
double ys = y*s;
double zs = z*s;
double wxs = w*xs;
double wys = w*ys;
double wzs = w*zs;
double xxs = x*xs;
double xys = x*ys;
double xzs = x*zs;
double yys = y*ys;
double yzs = y*zs;
double zzs = z*zs;
// Create the matrix corresponding to the quaternion
return Matrix3x3(1.0-yys-zzs, xys-wzs, xzs + wys,
xys + wzs, 1.0-xxs-zzs, yzs-wxs,
xzs-wys, yzs + wxs, 1.0-xxs-yys);
}
// Compute the spherical linear interpolation between two quaternions.
// The t argument has to be such that 0 <= t <= 1. This method is static.
Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& quaternion2, double t) {
assert(t >= 0.0 && t <= 1.0);
double invert = 1.0;
// Compute cos(theta) using the quaternion scalar product
double cosineTheta = quaternion1.scalarProduct(quaternion2);
// Take care of the sign of cosineTheta
if (cosineTheta < 0.0) {
cosineTheta = -cosineTheta;
invert = -1.0;
}
// Because of precision, if cos(theta) is nearly 1, therefore theta is nearly 0 and we can write
// sin((1-t)*theta) as (1-t) and sin(t*theta) as t
if(1-cosineTheta < EPSILON) {
return quaternion1 * (1.0-t) + quaternion2 * (t * invert);
}
// Compute the theta angle
double theta = acos(cosineTheta);
// Compute sin(theta)
double sineTheta = sin(theta);
// Compute the two coefficients that are in the spherical linear interpolation formula
double coeff1 = sin((1.0-t)*theta) / sineTheta;
double coeff2 = sin(t*theta) / sineTheta * invert;
// Compute and return the interpolated quaternion
return quaternion1 * coeff1 + quaternion2 * coeff2;
}

226
src/reactphysics3d/mathematics/Quaternion.h Normal file
View File

@ -0,0 +1,226 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef QUATERNION_H
#define QUATERNION_H
// Libraries
#include <cmath>
#include "Vector3D.h"
#include "Matrix3x3.h"
#include "exceptions.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class Quaternion :
This class represents a quaternion. We use the notation :
q = (x*i, y*j, z*k, w) to represent a quaternion.
-------------------------------------------------------------------
*/
class Quaternion {
private :
double x; // Component x of the quaternion
double y; // Component y of the quaternion
double z; // Component z of the quaternion
double w; // Component w of the quaternion
public :
Quaternion(); // Constructor
Quaternion(double x, double y, double z, double w); // Constructor with arguments
Quaternion(double w, const Vector3D& v); // Constructor with the component w and the vector v=(x y z)
Quaternion(const Quaternion& quaternion); // Copy-constructor
Quaternion(const Matrix3x3& matrix); // Create a unit quaternion from a rotation matrix
~Quaternion(); // Destructor
double getX() const; // Return the component x of the quaternion
double getY() const; // Return the component y of the quaternion
double getZ() const; // Return the component z of the quaternion
double getW() const; // Return the component w of the quaternion
void setX(double x); // Set the value x
void setY(double y); // Set the value y
void setZ(double z); // Set the value z
void setW(double w); // Set the value w
Vector3D vectorV() const; // Return the vector v=(x y z) of the quaternion
double length() const; // Return the length of the quaternion
Quaternion getUnit() const throw (MathematicsException); // Return the unit quaternion
Quaternion getConjugate() const; // Return the conjugate quaternion
Quaternion getInverse() const throw (MathematicsException); // Return the inverse of the quaternion
Matrix3x3 getMatrix() const; // Return the orientation matrix corresponding to this quaternion
double scalarProduct(const Quaternion& quaternion) const; // Scalar product between two quaternions
void getRotationAngleAxis(double& angle, Vector3D& axis) const; // Compute the rotation angle (in radians) and the axis
static Quaternion slerp(const Quaternion& quaternion1,
const Quaternion& quaternion2, double t); // Compute the spherical linear interpolation between two quaternions
// --- Overloaded operators --- //
Quaternion operator+(const Quaternion& quaternion) const; // Overloaded operator for the addition
Quaternion operator-(const Quaternion& quaternion) const; // Overloaded operator for the substraction
Quaternion operator*(double nb) const; // Overloaded operator for the multiplication with a constant
Quaternion operator*(const Quaternion& quaternion) const; // Overloaded operator for the multiplication
Quaternion& operator=(const Quaternion& quaternion); // Overloaded operator for assignment
bool operator==(const Quaternion& quaternion) const; // Overloaded operator for equality condition
};
// --- Inline functions --- //
// Get the value x (inline)
inline double Quaternion::getX() const {
return x;
}
// Get the value y (inline)
inline double Quaternion::getY() const {
return y;
}
// Get the value z (inline)
inline double Quaternion::getZ() const {
return z;
}
// Get the value w (inline)
inline double Quaternion::getW() const {
return w;
}
// Set the value x (inline)
inline void Quaternion::setX(double x) {
this->x = x;
}
// Set the value y (inline)
inline void Quaternion::setY(double y) {
this->y = y;
}
// Set the value z (inline)
inline void Quaternion::setZ(double z) {
this->z = z;
}
// Set the value w (inline)
inline void Quaternion::setW(double w) {
this->w = w;
}
// Return the vector v=(x y z) of the quaternion
inline Vector3D Quaternion::vectorV() const {
// Return the vector v
return Vector3D(x, y, z);
}
// Return the length of the quaternion (inline)
inline double Quaternion::length() const {
return sqrt(x*x + y*y + z*z + w*w);
}
// Return the unit quaternion
inline Quaternion Quaternion::getUnit() const throw(MathematicsException) {
double lengthQuaternion = length();
// Check if the length is not equal to zero
if (lengthQuaternion != 0.0) {
// Compute and return the unit quaternion
return Quaternion(x/lengthQuaternion, y/lengthQuaternion, z/lengthQuaternion, w/lengthQuaternion);
}
else {
// Throw an exception because it's impossible to compute a unit quaternion if its length is equal to zero
throw MathematicsException("MathematicsException : Impossible to compute the unit quaternion if the length of the quaternion is zero");
}
}
// Return the conjugate of the quaternion (inline)
inline Quaternion Quaternion::getConjugate() const {
return Quaternion(-x, -y, -z, w);
}
// Return the inverse of the quaternion (inline)
inline Quaternion Quaternion::getInverse() const throw(MathematicsException) {
double lengthQuaternion = length();
lengthQuaternion = lengthQuaternion * lengthQuaternion;
// Check if the length is not equal to zero
if (lengthQuaternion != 0.0) {
// Compute and return the inverse quaternion
return Quaternion(-x/lengthQuaternion, -y/lengthQuaternion, -z/lengthQuaternion, w/lengthQuaternion);
}
else {
// Throw an exception because the inverse cannot be computed
throw MathematicsException("MathematicsException : Impossible to compute the inverse of the quaternion because it's length is zero");
}
}
// Scalar product between two quaternions
inline double Quaternion::scalarProduct(const Quaternion& quaternion) const {
return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w);
}
// Overloaded operator for the addition of two quaternions
inline Quaternion Quaternion::operator+(const Quaternion& quaternion) const {
// Return the result quaternion
return Quaternion(x + quaternion.x, y + quaternion.y, z + quaternion.z, w + quaternion.w);
}
// Overloaded operator for the substraction of two quaternions
inline Quaternion Quaternion::operator-(const Quaternion& quaternion) const {
// Return the result of the substraction
return Quaternion(x-quaternion.x, y - quaternion.y, z - quaternion.z, w - quaternion.w);
}
// Overloaded operator for the multiplication with a constant
inline Quaternion Quaternion::operator*(double nb) const {
// Return the result
return Quaternion(nb*x, nb*y, nb*z, nb*w);
}
// Overloaded operator for the multiplication of two quaternions
inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const {
// Return the result of the multiplication
return Quaternion(w*quaternion.w - vectorV().scalarProduct(quaternion.vectorV()), w*quaternion.vectorV()+quaternion.w*vectorV() + vectorV().crossProduct(quaternion.vectorV()));
}
// Overloaded operator for the assignment
inline Quaternion& Quaternion::operator=(const Quaternion& quaternion) {
// Check for self-assignment
if (this != &quaternion) {
x = quaternion.x;
y = quaternion.y;
z = quaternion.z;
w = quaternion.w;
}
// Return this quaternion
return *this;
}
// Overloaded operator for equality condition
inline bool Quaternion::operator==(const Quaternion& quaternion) const {
return (x == quaternion.x && y == quaternion.y && z == quaternion.z && w == quaternion.w);
}
} // End of the ReactPhysics3D namespace
#endif

266
src/reactphysics3d/mathematics/Vector.cpp Normal file
View File

@ -0,0 +1,266 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "Vector.h"
// Namespaces
using namespace reactphysics3d;
// Constructor without argument
Vector::Vector()
:nbComponent(0) {
tab = 0;
}
// Constructor of the class Vector
Vector::Vector(int n) throw(std::invalid_argument) {
// Check the argument
if (n > 0) {
// Create the array that contains the values of the vector
nbComponent = n;
tab = new double[nbComponent];
// TODO : Remove this initialization
// Fill the array with zero's value
for(int i=0; i<nbComponent; ++i) {
tab[i] = 0.0;
}
}
else {
// Throw an exception because of the wrong argument
throw std::invalid_argument("Exception : The size of the vector has to be positive !");
}
}
// Copy-constructor of the class Vector
Vector::Vector(const Vector& vector) {
nbComponent = vector.nbComponent;
tab = new double[nbComponent];
// Fill the array with the value of the vector
for (int i=0; i<nbComponent; ++i) {
tab[i] = vector.tab[i];
}
}
// Conversion from Vector3D to Vector
Vector::Vector(const Vector3D& vector3d) {
nbComponent = 3;
tab = new double[3];
tab[0] = vector3d.getX();
tab[1] = vector3d.getY();
tab[2] = vector3d.getZ();
}
// Destructor of the class Vector
Vector::~Vector() {
// Erase the array with the values of the vector
delete [] tab;
}
// Return the corresponding unit vector
Vector Vector::getUnit() const throw(MathematicsException) {
double lengthVector = length();
// Check if the length of the vector is equal to zero
if (lengthVector!= 0) {
double lengthInv = 1.0 / lengthVector;
Vector unitVector(nbComponent);
// Compute the unit vector
for(int i=0; i<nbComponent; ++i) {
unitVector.setValue(i, getValue(i) * lengthInv);
}
// Return the unit vector
return unitVector;
}
else {
// Throw an exception because the length of the vector is zero
throw MathematicsException("MathematicsException : Impossible to compute the unit vector because the length of the vector is zero");
}
}
void Vector::setVector(const Vector& vector) {
assert(nbComponent == vector.nbComponent);
for (int i=0; i<nbComponent; i++) {
tab[i] = vector.tab[i];
}
}
// Method to compute the scalar product of two vectors
double Vector::scalarProduct(const Vector& vector) const throw(MathematicsException) {
// Check the sizes of the two vectors
if (nbComponent == vector.nbComponent) {
double result = 0.0;
// Compute the scalar product
for (int i=0; i<nbComponent; ++i) {
result = result + vector.tab[i] * tab[i];
}
// Return the result of the scalar product
return result;
}
else {
// Throw an exception because the two vectors haven't the same size
throw MathematicsException("MathematicsException : Impossible to compute the scalar product because the vectors haven't the same size");
}
}
// Method to compute the cross product of two vectors
Vector Vector::crossProduct(const Vector& vector) const throw(MathematicsException) {
// Check if the vectors have 3 components
if (nbComponent == 3 && vector.nbComponent == 3) {
Vector result(3);
// Compute the cross product
result.tab[0] = tab[1] * vector.tab[2] - tab[2] * vector.tab[1];
result.tab[1] = tab[2] * vector.tab[0] - tab[0] * vector.tab[2];
result.tab[2] = tab[0] * vector.tab[1] - tab[1] * vector.tab[0];
// Return the result of the cross product
return result;
}
else {
// Throw an exception because the vectors haven't three components
throw MathematicsException("MathematicsException : Impossible to compute the cross product because the vectors haven't 3 components");
}
}
void Vector::changeSize(uint newSize) {
if (tab) {
delete[] tab;
}
nbComponent = newSize;
tab = new double[nbComponent];
// Fill the array with the value of the vector
for (int i=0; i<nbComponent; ++i) {
tab[i] = 0.0;
}
}
// Overloaded operator for addition
Vector Vector::operator+(const Vector& vector) const throw(MathematicsException) {
// Check the size of the two vectors
if (nbComponent == vector.nbComponent) {
Vector sum(nbComponent);
// Compute the sum of the two vectors
for (int i=0; i<nbComponent; ++i) {
sum.setValue(i, vector.tab[i] + tab[i]);
}
// Return the sum vector
return sum;
}
else {
// Throw an exception because the sizes of the two vectors aren't the same
throw MathematicsException("MathematicsException : Impossible two sum the two vectors because the sizes aren't the same !");
}
}
// Overloaded operator for substraction
Vector Vector::operator-(const Vector& vector) const throw(MathematicsException) {
// Check the size of the two vectors
if (nbComponent == vector.nbComponent) {
Vector substraction(nbComponent);
// Compute the subraction of the two vectors
for (int i=0; i<nbComponent; ++i) {
substraction.setValue(i, tab[i] - vector.tab[i]);
}
// Return the subraction vector
return substraction;
}
else {
// Throw an exception because the sizes of the two vectors aren't the same
throw MathematicsException("MathematicsException : Impossible two substract the two vectors because the sizes aren't the same !");
}
}
// Overloaded operator for multiplication with a number
Vector Vector::operator*(double number) const {
Vector result(nbComponent);
// Compute the multiplication
for (int i=0; i<nbComponent; ++i) {
result.setValue(i, number * tab[i]);
}
// Return the result vector
return result;
}
// Overloaded operator for assigment to a Vector
Vector& Vector::operator=(const Vector& vector) throw(MathematicsException) {
// Check for self-assignment
if (this == &vector) {
return *this;
}
// Check the size of the vectors
if (nbComponent == vector.nbComponent) {
// Check for self-assignment
if (this != &vector) {
for (int i=0; i<nbComponent; ++i) {
tab[i] = vector.tab[i];
}
}
// Return a reference to the vector
return *this;
}
else {
// Throw an exception because the sizes of the vectors aren't the same
throw MathematicsException("MathematicsException : The assigment to a Vector is impossible because the size of the vectors aren't the same");
}
}
// Overloaded operator for the equality condition
bool Vector::operator==(const Vector& vector) const throw(MathematicsException) {
// Check if the sizes of the vectors are compatible
if (nbComponent == vector.nbComponent) {
for (int i=0; i<nbComponent; ++i) {
if (tab[i] != vector.tab[i]) {
return false;
}
}
return true;
}
else {
// Throw an exception because the sizes of the vectors aren't the same
throw MathematicsException("MathematicsException : Impossible to check if the vectors are equal because they don't have the same size");
}
}

174
src/reactphysics3d/mathematics/Vector.h Normal file
View File

@ -0,0 +1,174 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef VECTOR_H
#define VECTOR_H
// Libraries
#include "Vector3D.h"
#include "../typeDefinitions.h"
#include "mathematics_functions.h"
#include "exceptions.h"
#include <cmath>
#include <cassert>
#include <iostream>
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class Vector :
This class represents a Vector.
-------------------------------------------------------------------
*/
class Vector {
private :
double* tab; // Array of the vector's components
uint nbComponent; // number of components in the vector
public :
Vector(); // Constructor without argument
Vector(int n) throw(std::invalid_argument); // Constructor of the class Vector
Vector(const Vector& vector); // Copy-constructor of the class Vector
Vector(const Vector3D& vector3d); // Conversion from Vector3D to Vector
virtual ~Vector(); // Destructor of the class Vector
double getValue(int n) const throw(std::invalid_argument); // Get a component of the vector
void setValue(int n, double value) throw(std::invalid_argument); // Set the value of a component of the vector
int getNbComponent() const; // Get the number of components in the vector
void initWithValue(double value); // Init all the elements with a given value
double length() const; // Get the length of the vector
Vector getUnit() const throw(MathematicsException); // Return the corresponding unit vector
double scalarProduct(const Vector& vector) const throw(MathematicsException); // Scalar product of two vectors
Vector crossProduct(const Vector& vector) const throw(MathematicsException); // Cross product of two vectors (in 3D only)
void fillInSubVector(uint index, const Vector& subVector); // Replace a part of the current vector with another sub-vector
Vector getSubVector(uint index, uint nbElements) const throw(std::invalid_argument); // Return a sub-vector of the current vector
void setVector(const Vector& vector);
bool isUnit() const; // Return true if the vector is unit and false otherwise
void changeSize(uint newSize);
// --- Overloaded operators --- //
Vector operator+(const Vector& vector) const throw(MathematicsException); // Overloaded operator for addition
Vector operator-(const Vector& vector) const throw(MathematicsException); // Overloaded operator for substraction
Vector operator*(double number) const; // Overloaded operator for multiplication with a number
Vector& operator=(const Vector& vector) throw(MathematicsException); // Overloaded operator for the assignement to a Vector
bool operator==(const Vector& vector) const throw(MathematicsException); // Overloaded operator for the equality condition
};
// ------ Definition of inlines functions ------ //
// Method to get the value of a component of the vector (inline)
inline double Vector::getValue(int n) const throw(std::invalid_argument) {
// Check the argument
if (n>=0 && n<nbComponent) {
// Return the value of the component
return tab[n];
}
else {
// Throw an exception because of the wrong argument
throw std::invalid_argument("The argument is outside the bounds of the Vector");
}
}
// Method to set the value of a component of the vector
inline void Vector::setValue(int n, double value) throw(std::invalid_argument) {
// Check the argument
if (n >= 0 && n<nbComponent) {
// Set the value
tab[n] = value;
}
else {
// Throw an exception because of the wrong argument
throw std::invalid_argument("Exception : The argument is outside the bounds of the Vector");
}
}
// Method to get the number of components in the vector (inline)
inline int Vector::getNbComponent() const {
// Return the number of components in the vector
return nbComponent;
}
// Method to get the length of the vector
inline double Vector::length() const {
// Compute the length of the vector
double sum = 0.0;
for(int i=0; i<nbComponent; ++i) {
sum = sum + tab[i] * tab[i];
}
// Return the length of the vector
return sqrt(sum);
}
// Init all the elements with a given value
inline void Vector::initWithValue(double value) {
for (uint i=0; i<nbComponent; i++) {
tab[i] = value;
}
}
// Replace a part of the current vector with another sub-vector.
// The argument "rowIndex" is the row index where the subVector starts.
inline void Vector::fillInSubVector(uint rowIndex, const Vector& subVector) {
assert(nbComponent-rowIndex >= subVector.nbComponent);
// For each value of the sub-vector
for (uint i=0; i<subVector.nbComponent; ++i) {
tab[rowIndex + i] = subVector.getValue(i);
}
}
// Return a sub-vector of the current vector
inline Vector Vector::getSubVector(uint index, uint nbElements) const throw(std::invalid_argument) {
// Check if the arguments are valid
if (index < 0 || index+nbElements > nbComponent) {
throw std::invalid_argument("Error : arguments are out of bounds");
}
// Compute the sub-vector
Vector subVector(nbElements);
for (uint i=0, j=index; i<nbElements; i++, j++) {
subVector.tab[i] = tab[j];
}
// Return the sub-vector
return subVector;
}
// Return true if the vector is unit and false otherwise
inline bool Vector::isUnit() const {
return approxEqual(1.0, length());
}
// Overloaded operator for multiplication between a number and a Vector (inline)
inline Vector operator*(double number, const Vector& vector) {
// Compute and return the result
return vector * number;
}
} // End of the ReactPhysics3D namespace
#endif

130
src/reactphysics3d/mathematics/Vector3D.cpp Normal file
View File

@ -0,0 +1,130 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "Vector3D.h"
#include <iostream>
#include <cassert>
#include <vector>
// Namespaces
using namespace reactphysics3d;
// Constructor of the class Vector3D
Vector3D::Vector3D()
:x(0.0), y(0.0), z(0.0) {
}
// Constructor with arguments
Vector3D::Vector3D(double x, double y, double z)
:x(x), y(y), z(z) {
}
// Copy-constructor
Vector3D::Vector3D(const Vector3D& vector)
:x(vector.x), y(vector.y), z(vector.z) {
}
// Destructor
Vector3D::~Vector3D() {
}
// Return the corresponding unit vector
Vector3D Vector3D::getUnit() const throw(MathematicsException) {
double lengthVector = length();
// Check if the length is equal to zero
if (lengthVector != 0) {
// Compute and return the unit vector
double lengthInv = 1.0 / lengthVector;
return Vector3D(x * lengthInv, y * lengthInv, z*lengthInv);
}
else {
// Throw an exception because the length of the vector is zero
throw MathematicsException("MathematicsException : Impossible to compute the unit vector because the length of the vector is zero");
}
}
// Return two unit orthogonal vectors of the current vector
Vector3D Vector3D::getOneOrthogonalVector() const {
assert(!this->isZero());
Vector3D unitVector = this->getUnit();
// Compute a first orthogonal vector
Vector3D vector1;
if (!approxEqual(x, 0.0)) { // If x != 0
vector1.setY(x);
vector1.setZ((-2*x*y*z + 2*x*z)/(2*(z*z + x*x)));
vector1.setX((-x*y-z*vector1.getZ())/x);
}
else if (!approxEqual(y, 0.0)) { // If y != 0
vector1.setZ(y);
vector1.setX((-2*x*y*z + 2*x*y)/(2*(y*y + x*x)));
vector1.setY((-z*y-x*vector1.getX())/y);
}
else if (!approxEqual(z, 0.0)) { // If z != 0
vector1.setX(z);
vector1.setY((-2*x*y*z + 2*y*z)/(2*(z*z + y*y)));
vector1.setZ((-x*z-y*vector1.getY())/z);
}
assert(vector1.isUnit());
return vector1;
}
// Overloaded operator for addition
Vector3D Vector3D::operator+(const Vector3D& vector) const {
// Compute and return the sum of the two vectors
return Vector3D(x + vector.x, y + vector.y, z + vector.z);
}
// Overloaded operator for substraction
Vector3D Vector3D::operator-(const Vector3D& vector) const {
// Compute and return the substraction of the two vectors
return Vector3D(x - vector.x, y - vector.y, z - vector.z);
}
// Overloaded operator for multiplication with a number
Vector3D Vector3D::operator*(double number) const {
// Compute and return the result
return Vector3D(x * number, y * number, z * number);
}
// Overloaded operator for the assignement to a Vector
Vector3D& Vector3D::operator=(const Vector3D& vector) {
// Check for self-assignment
if (this != &vector) {
// Copy the vector
x = vector.x;
y = vector.y;
z = vector.z;
}
// Return a reference to the vector
return *this;
}

194
src/reactphysics3d/mathematics/Vector3D.h Normal file
View File

@ -0,0 +1,194 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef VECTOR3D_H
#define VECTOR3D_H
// Libraries
#include <cmath>
#include "exceptions.h"
#include "mathematics_functions.h"
// TODO : Remove the methods getX(), getY(), getZ(), setX(), setY(), setZ() and replace the attributes
// x, y and z by an array values[3]
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class Vector3D :
This classrepresents 3 dimensionnal vector in space.
-------------------------------------------------------------------
*/
class Vector3D {
private :
double x; // X component of the vector
double y; // Y component of the vector
double z; // Z component of the vector
public :
Vector3D(); // Constructor of the class Vector3D
Vector3D(double x, double y, double z); // Constructor with arguments
Vector3D(const Vector3D& vector); // Copy-constructor
virtual ~Vector3D(); // Destructor
double getValue(int index) const throw(std::invalid_argument); // Get a component of the vector
void setValue(int index, double value) throw(std::invalid_argument); // Set a component of the vector
double getX() const; // Get the x component of the vector
double getY() const; // Get the y component of the vector
double getZ() const; // Get the z component of the vector
void setX(double x); // Set the x component of the vector
void setY(double y); // Set the y component of the vector
void setZ(double z); // Set the z component of the vector
void setAllValues(double x, double y, double z); // Set all the values of the vector
double length() const; // Return the lenght of the vector
Vector3D getUnit() const throw(MathematicsException); // Return the corresponding unit vector
bool isUnit() const; // Return true if the vector is unit and false otherwise
bool isZero() const; // Return true if the current vector is the zero vector
Vector3D getOpposite() const; // Return the vector in the opposite direction
Vector3D getOneOrthogonalVector() const; // Return one unit orthogonal vectors of the current vector
double scalarProduct(const Vector3D& vector) const; // Scalar product of two vectors
Vector3D crossProduct(const Vector3D& vector) const; // Cross product of two vectors
bool isParallelWith(const Vector3D& vector) const; // Return true if two vectors are parallel
// --- Overloaded operators --- //
Vector3D operator+(const Vector3D& vector) const; // Overloaded operator for addition
Vector3D operator-(const Vector3D& vector) const ; // Overloaded operator for substraction
Vector3D operator*(double number) const; // Overloaded operator for multiplication with a number
Vector3D& operator=(const Vector3D& vector); // Overloaded operator for the assignement to a Vector
bool operator==(const Vector3D& vector) const; // Overloaded operator for the equality condition
};
// Get the x component of the vector (inline)
inline double Vector3D::getX() const {
return x;
}
// Get the y component of the vector (inline)
inline double Vector3D::getY() const {
return y;
}
// Get the z component of the vector (inline)
inline double Vector3D::getZ() const {
return z;
}
// Set the x component of the vector (inline)
inline void Vector3D::setX(double x) {
this->x = x;
}
// Set the y component of the vector (inline)
inline void Vector3D::setY(double y) {
this->y = y;
}
// Set the z component of the vector (inline)
inline void Vector3D::setZ(double z) {
this->z = z;
}
// Get a component of the vector
inline double Vector3D::getValue(int index) const throw(std::invalid_argument) {
switch(index) {
case 0: return x;
case 1: return y;
case 2: return z;
default: // Throw an exception because of the wrong argument
throw std::invalid_argument("The argument is outside the bounds of the Vector3D");
}
}
// Set a component of the vector
inline void Vector3D::setValue(int index, double value) throw(std::invalid_argument) {
switch(index) {
case 0: x = value;
case 1: y = value;
case 2: z = value;
default: // Throw an exception because of the wrong argument
throw std::invalid_argument("The argument is outside the bounds of the Vector3D");
}
}
// Set all the values of the vector (inline)
inline void Vector3D::setAllValues(double x, double y, double z) {
this->x = x;
this->y = y;
this->z = z;
}
// Return the length of the vector (inline)
inline double Vector3D::length() const {
// Compute and return the length of the vector
return sqrt(x*x + y*y + z*z);
}
// Return the vector in the opposite direction
inline Vector3D Vector3D::getOpposite() const {
return (Vector3D(0.0, 0.0, 0.0) - *this);
}
// Scalar product of two vectors (inline)
inline double Vector3D::scalarProduct(const Vector3D& vector) const {
// Compute and return the result of the scalar product
return (x * vector.x + y * vector.y + z * vector.z);
}
// Cross product of two vectors (inline)
inline Vector3D Vector3D::crossProduct(const Vector3D& vector) const {
// Compute and return the cross product
return Vector3D(y * vector.z - z * vector.y, z * vector.x - x * vector.z , x * vector.y - y * vector.x);
}
// Return true if two vectors are parallel
inline bool Vector3D::isParallelWith(const Vector3D& vector) const {
double scalarProd = this->scalarProduct(vector);
return approxEqual(std::abs(scalarProd), length() * vector.length());
}
// Return true if the vector is unit and false otherwise
inline bool Vector3D::isUnit() const {
return approxEqual(x*x+y*y+z*z, 1.0);
}
// Return true if the vector is the zero vector
inline bool Vector3D::isZero() const {
return approxEqual(x*x+y*y+z*z, 0.0);
}
// Overloaded operator for multiplication between a number and a Vector3D (inline)
inline Vector3D operator * (double number, const Vector3D& vector) {
// Compute and return the result vector
return vector * number;
}
// Overloaded operator for the equality condition
inline bool Vector3D::operator == (const Vector3D& vector) const {
return (x == vector.x && y == vector.y && z == vector.z);
}
} // End of the ReactPhysics3D namespace
#endif

37
src/reactphysics3d/mathematics/constants.h Normal file
View File

@ -0,0 +1,37 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef CONSTANTS_H
#define CONSTANTS_H
// Libraries
#include <limits>
// Constants
const double EPSILON = 0.00001;
const double ONE_MINUS_EPSILON = 0.99999;
const double INFINITY_CONST = std::numeric_limits<double>::infinity();
const double PI = 3.14159265; // Pi constant
#endif

63
src/reactphysics3d/mathematics/exceptions.cpp Executable file
View File

@ -0,0 +1,63 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
// Libraries
#include "exceptions.h"
// Namespaces
using namespace reactphysics3d;
// Constructor of the MathematicsException class
MathematicsException::MathematicsException(const std::string& msg)
:std::runtime_error(msg) {
}
// Destructor of the MathException class
MathematicsException::~MathematicsException() throw() {
}
// Overriden exception base class method
const char* MathematicsException::what() const throw() {
return std::runtime_error::what();
}
// Constructor of the DivisionByZeroException class
DivisionByZeroException::DivisionByZeroException(const std::string& msg)
:MathematicsException(msg) {
}
// Destructor of the DivisionByZeroException class
DivisionByZeroException::~DivisionByZeroException() throw() {
}
// Overriden exception base class method
const char* DivisionByZeroException::what() const throw() {
return MathematicsException::what();
}

57
src/reactphysics3d/mathematics/exceptions.h Executable file
View File

@ -0,0 +1,57 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef EXCEPTIONS_H
#define EXCEPTIONS_H
// Libraries
#include <stdexcept>
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Exception class for the mathematics library
-------------------------------------------------------------------
*/
// Class MathematicsException
class MathematicsException : public std::runtime_error {
public:
MathematicsException(const std::string& msg="MathException"); // Constructor
virtual ~MathematicsException() throw(); // Destructor
virtual const char* what() const throw(); // Overriding the base exception method
};
// Class DivisionByZeroException
class DivisionByZeroException : public MathematicsException {
public:
DivisionByZeroException(const std::string& msg="DivisionByZeroException : Division by zero !"); // Constructor
virtual ~DivisionByZeroException() throw(); // Destructor
virtual const char* what() const throw(); // Overriding the base exception method
};
} // End of the ReactPhysics3D namespace
#endif

101
src/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.cpp Normal file
View File

@ -0,0 +1,101 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
// Libraries
#include "LCPProjectedGaussSeidel.h"
#include <cmath>
using namespace reactphysics3d;
// Constructor
LCPProjectedGaussSeidel::LCPProjectedGaussSeidel(uint maxIterations)
:LCPSolver(maxIterations) {
}
// Destructor
LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() {
}
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
// This method outputs the result in the lambda vector
void LCPProjectedGaussSeidel::solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints,
uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const {
lambda = lambdaInit;
double* d = new double[nbConstraints]; // TODO : Avoid those kind of memory allocation here for optimization (allocate once in the object)
uint indexBody1, indexBody2;
double deltaLambda;
double lambdaTemp;
uint i, iter;
Vector* a = new Vector[nbBodies]; // Array that contains nbBodies vector of dimension 6x1
for (i=0; i<nbBodies; i++) {
a[i].changeSize(6);
}
// Compute the vector a
computeVectorA(lambda, nbConstraints, bodyMapping, B_sp, bodyNumberMapping, a, nbBodies);
// For each constraint
for (i=0; i<nbConstraints; i++) {
d[i] = (J_sp[i][0] * B_sp[0][i] + J_sp[i][1] * B_sp[1][i]).getValue(0,0);
}
for(iter=0; iter<maxIterations; iter++) {
for (i=0; i<nbConstraints; i++) {
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
deltaLambda = (b.getValue(i) - (J_sp[i][0] * a[indexBody1]).getValue(0,0) - (J_sp[i][1] * a[indexBody2]).getValue(0,0)) / d[i];
lambdaTemp = lambda.getValue(i);
lambda.setValue(i, std::max(lowLimits.getValue(i), std::min(lambda.getValue(i) + deltaLambda, highLimits.getValue(i))));
deltaLambda = lambda.getValue(i) - lambdaTemp;
a[indexBody1] = a[indexBody1] + (B_sp[0][i] * deltaLambda).getVector();
a[indexBody2] = a[indexBody2] + (B_sp[1][i] * deltaLambda).getVector();
}
}
// Clean
delete[] d;
delete[] a;
}
// Compute the vector a used in the solve() method
// Note that a = B * lambda
void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
Matrix** B_sp, std::map<Body*, uint> bodyNumberMapping,
Vector* const a, uint nbBodies) const {
uint i;
uint indexBody1, indexBody2;
// Init the vector a with zero values
for (i=0; i<nbBodies; i++) {
a[i].initWithValue(0.0);
}
for(i=0; i<nbConstraints; i++) {
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
a[indexBody1] = a[indexBody1] + (B_sp[0][i].getVector() * lambda.getValue(i));
a[indexBody2] = a[indexBody2] + (B_sp[1][i].getVector() * lambda.getValue(i));
}
}

53
src/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.h Normal file
View File

@ -0,0 +1,53 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef LCPPROJECTEDGAUSSSEIDEL_H
#define LCPPROJECTEDGAUSSSEIDEL_H
// Libraries
#include "LCPSolver.h"
#include <map>
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class LCPProjectedGaussSeidel :
This class implements the Projected-Gauss-Seidel (PGS)
algorithm in order to solve a LCP problem. This class inherits
from the LCPSolver class.
-------------------------------------------------------------------
*/
class LCPProjectedGaussSeidel : public LCPSolver {
protected:
void computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
Matrix** B_sp, std::map<Body*, uint> bodyNumberMapping,
Vector* const a, uint nbBodies) const ; // Compute the vector a used in the solve() method
public:
LCPProjectedGaussSeidel(uint maxIterations); // Constructor
virtual ~LCPProjectedGaussSeidel(); // Destructor
virtual void solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints,
uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const; // Solve a LCP problem using Projected-Gauss-Seidel algorithm // Set the initial value for lambda
};
} // End of the ReactPhysics3D namespace
#endif

34
src/reactphysics3d/mathematics/lcp/LCPSolver.cpp Normal file
View File

@ -0,0 +1,34 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
// Libraries
#include "LCPSolver.h"
using namespace reactphysics3d;
// Constructor
LCPSolver::LCPSolver(uint maxIterations)
: maxIterations(maxIterations) {
}
// Destructor
LCPSolver::~LCPSolver() {
}

84
src/reactphysics3d/mathematics/lcp/LCPSolver.h Normal file
View File

@ -0,0 +1,84 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef LCPSOLVER_H
#define LCPSOLVER_H
// Libraries
#include <vector>
#include <map>
#include "../Vector.h"
#include "../Matrix.h"
#include "../../body/Body.h"
#include "../../typeDefinitions.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
/* -------------------------------------------------------------------
Class LCPSolver :
This abstract class represents an algorithm to solve a Linear
Complementary Problem (LCP). Given a matrix "A=J*B", a vector "b",
a vector "lowLimit" of lower limits and a vector "highLimits" of
upper limits. The goal is to find a vector "lambda" such that:
w = Ax - b
lowLimits <= lambda <= highLimits
and one of the thre following conditions holds :
lambda_i = lowLimits_i, w_i >= 0
lambda_i = highLimits_i, w_i >= 0
lowLimits_i < lambda_i < highLimits_i, w_i = 0
Note that the matrix A is givent by the two matrices J and B with A=J*B.
But only their sparse representations "J_sp" and "B_sp" are passed in
arguments to solve() to be more efficient.
-------------------------------------------------------------------
*/
class LCPSolver {
protected:
uint maxIterations; // Maximum number of iterations
Vector lambdaInit; // Initial value for lambda at the beginning of the algorithm
public:
LCPSolver(uint maxIterations); // Constructor
virtual ~LCPSolver(); // Destructor
virtual void solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints,
uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const=0; // Solve a LCP problem
void setLambdaInit(const Vector& lambdaInit); // Set the initial lambda vector
void setMaxIterations(uint maxIterations); // Set the maximum number of iterations
};
// Set the initial lambda vector
inline void LCPSolver::setLambdaInit(const Vector& lambdaInit) {
this->lambdaInit.changeSize(lambdaInit.getNbComponent());
this->lambdaInit = lambdaInit;
}
// Set the maximum number of iterations
inline void LCPSolver::setMaxIterations(uint maxIterations) {
assert(maxIterations > 0);
this->maxIterations = maxIterations;
}
} // End of the ReactPhysics3D namespace
#endif

450
src/reactphysics3d/mathematics/mathematics.h Normal file
View File

@ -0,0 +1,450 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef MATHEMATICS_H
#define MATHEMATICS_H
// Libraries
#include "Matrix.h"
#include "Matrix3x3.h"
#include "Quaternion.h"
#include "Vector.h"
#include "Vector3D.h"
#include "constants.h"
#include "exceptions.h"
#include "mathematics_functions.h"
#include <vector>
#include <cstdio>
#include <cassert>
#include <cmath>
// ReactPhysics3D namespace
namespace reactphysics3d {
// ---------- Mathematics functions ---------- //
// Rotate a vector according to a rotation quaternion.
// The function returns the vector rotated according to the quaternion in argument
inline reactphysics3d::Vector3D rotateVectorWithQuaternion(const reactphysics3d::Vector3D& vector, const reactphysics3d::Quaternion& quaternion) {
// Convert the vector into a quaternion
reactphysics3d::Quaternion vectorQuaternion(0, vector);
// Compute the quaternion rotation result
reactphysics3d::Quaternion quaternionResult = (quaternion * vectorQuaternion) * quaternion.getInverse();
// Convert the result quaternion into a vector
return quaternionResult.vectorV();
}
// Given two lines (given by the points "point1", "point2" and the vectors "d1" and "d2" that are not parallel, this method returns the values
// "alpha" and "beta" such that the two points P1 and P2 are the two closest point between the two lines and such that
// P1 = point1 + alpha * d1
// P2 = point2 + beta * d2
inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3D& point1, const reactphysics3d::Vector3D& d1, const reactphysics3d::Vector3D& point2,
const reactphysics3d::Vector3D& d2, double* alpha, double* beta) {
reactphysics3d::Vector3D r = point1 - point2;
double a = d1.scalarProduct(d1);
double b = d1.scalarProduct(d2);
double c = d1.scalarProduct(r);
double e = d2.scalarProduct(d2);
double f = d2.scalarProduct(r);
double d = a*e-b*b;
// The two lines must not be parallel
assert(!reactphysics3d::approxEqual(d, 0.0));
// Compute the "alpha" and "beta" values
*alpha = (b*f -c*e)/d;
*beta = (a*f-b*c)/d;
}
// This method returns true if the point "P" is on the segment between "segPointA" and "segPointB" and return false otherwise
inline bool isPointOnSegment(const reactphysics3d::Vector3D& segPointA, const reactphysics3d::Vector3D& segPointB, const reactphysics3d::Vector3D& P) {
// Check if the point P is on the line between "segPointA" and "segPointB"
reactphysics3d::Vector3D d = segPointB - segPointA;
reactphysics3d::Vector3D dP = P - segPointA;
if (!d.isParallelWith(dP)) {
return false;
}
// Compute the length of the segment
double segmentLength = d.length();
// Compute the distance from point "P" to points "segPointA" and "segPointB"
double distA = dP.length();
double distB = (P - segPointB).length();
// If one of the "distA" and "distB" is greather than the length of the segment, then P is not on the segment
if (distA > segmentLength || distB > segmentLength) {
return false;
}
// Otherwise, the point P is on the segment
return true;
}
// Given two lines in 3D that intersect, this method returns the intersection point between the two lines.
// The first line is given by the point "p1" and the vector "d1", the second line is given by the point "p2" and the vector "d2".
inline reactphysics3d::Vector3D computeLinesIntersection(const reactphysics3d::Vector3D& p1, const reactphysics3d::Vector3D& d1,
const reactphysics3d::Vector3D& p2, const reactphysics3d::Vector3D& d2) {
// Computes the two closest points on the lines
double alpha, beta;
closestPointsBetweenTwoLines(p1, d1, p2, d2, &alpha, &beta);
reactphysics3d::Vector3D point1 = p1 + alpha * d1;
reactphysics3d::Vector3D point2 = p2 + beta * d2;
// The two points must be very close
//assert((point1-point2).length() <= 0.1);
// Return the intersection point (halfway between "point1" and "point2")
return 0.5 * (point1 + point2);
}
// Given two segments in 3D that are not parallel and that intersect, this method computes the intersection point between the two segments.
// This method returns the intersection point.
inline reactphysics3d::Vector3D computeNonParallelSegmentsIntersection(const reactphysics3d::Vector3D& seg1PointA, const reactphysics3d::Vector3D& seg1PointB,
const reactphysics3d::Vector3D& seg2PointA, const reactphysics3d::Vector3D& seg2PointB) {
// Determine the lines of both segments
reactphysics3d::Vector3D d1 = seg1PointB - seg1PointA;
reactphysics3d::Vector3D d2 = seg2PointB - seg2PointA;
// The segments must not be parallel
assert(!d1.isParallelWith(d2));
// Compute the closet points between the two lines
double alpha, beta;
closestPointsBetweenTwoLines(seg1PointA, d1, seg2PointA, d2, &alpha, &beta);
reactphysics3d::Vector3D point1 = seg1PointA + alpha * d1;
reactphysics3d::Vector3D point2 = seg2PointA + beta * d2;
// The closest points have to be on the segments, otherwise there is no intersection between the segments
assert(isPointOnSegment(seg1PointA, seg1PointB, point1));
assert(isPointOnSegment(seg2PointA, seg2PointB, point2));
// If the two closest point aren't very close, there is no intersection between the segments
reactphysics3d::Vector3D d = point2 - point1;
assert(d.length() <= EPSILON);
// They are very close so we return the intersection point (halfway between "point1" and "point2"
return 0.5 * (point1 + point2);
}
// Move a set of points by a given vector.
// The method returns a set of points moved by the given vector.
inline std::vector<reactphysics3d::Vector3D> movePoints(const std::vector<reactphysics3d::Vector3D>& points, const reactphysics3d::Vector3D& vector) {
std::vector<reactphysics3d::Vector3D> result;
// For each point of the set
for (unsigned int i=0; i<points.size(); ++i) {
// Move the point
result.push_back(points[i] + vector);
}
// Return the result set of points
return result;
}
// Compute the projection of a set of 3D points onto a 3D plane. The set of points is given by "points" and the plane is given by
// a point "A" and a normal vector "normal". This method returns the initial set of points projected onto the plane.
inline std::vector<reactphysics3d::Vector3D> projectPointsOntoPlane(const std::vector<reactphysics3d::Vector3D>& points, const reactphysics3d::Vector3D& A,
const reactphysics3d::Vector3D& normal) {
assert(normal.length() != 0.0);
std::vector<Vector3D> projectedPoints;
reactphysics3d::Vector3D n = normal.getUnit();
// For each point of the set
for (unsigned int i=0; i<points.size(); ++i) {
// Compute the projection of the point onto the plane
projectedPoints.push_back(points[i] - (n * (points[i] - A).scalarProduct(n)));
}
// Return the projected set of points
return projectedPoints;
}
// Compute the distance between a point "P" and a line (given by a point "A" and a vector "v")
inline double computeDistanceBetweenPointAndLine(const reactphysics3d::Vector3D& P, const reactphysics3d::Vector3D& A, const reactphysics3d::Vector3D& v) {
assert(v.length() != 0);
return ((P-A).crossProduct(v).length() / (v.length()));
}
// Compute the orthogonal projection of a point "P" on a line (given by a point "A" and a vector "v")
inline reactphysics3d::Vector3D computeOrthogonalProjectionOfPointOntoALine(const reactphysics3d::Vector3D& P, const reactphysics3d::Vector3D& A, const reactphysics3d::Vector3D& v) {
return (A + ((P-A).scalarProduct(v) / (v.scalarProduct(v))) * v);
}
// Given a point P and 4 points that form a rectangle (point P and the 4 points have to be on the same plane) this method computes
// the point Q that is the nearest point to P that is inside (on a border of) the rectangle. The point P should be outside the rectangle.
// The result point Q will be in a segment of the rectangle
inline reactphysics3d::Vector3D computeNearestPointOnRectangle(const reactphysics3d::Vector3D& P, const std::vector<reactphysics3d::Vector3D> rectangle) {
assert(rectangle.size() == 4);
double distPSegment1 = computeDistanceBetweenPointAndLine(P, rectangle[0], rectangle[1] - rectangle[0]);
double distPSegment2 = computeDistanceBetweenPointAndLine(P, rectangle[1], rectangle[2] - rectangle[1]);
double distPSegment3 = computeDistanceBetweenPointAndLine(P, rectangle[2], rectangle[3] - rectangle[2]);
double distPSegment4 = computeDistanceBetweenPointAndLine(P, rectangle[3], rectangle[0] - rectangle[3]);
double distSegment1Segment3 = computeDistanceBetweenPointAndLine(rectangle[0], rectangle[3], rectangle[3] - rectangle[2]);
double distSegment2Segment4 = computeDistanceBetweenPointAndLine(rectangle[1], rectangle[3], rectangle[0] - rectangle[3]);
Vector3D resultPoint;
// Check if P is between the lines of the first pair of parallel segments of the rectangle
if (distPSegment1 <= distSegment1Segment3 && distPSegment3 <= distSegment1Segment3) {
// Find among segments 2 and 4 which one is the nearest
if (distPSegment2 <= distPSegment4) { // Segment 2 is the nearest
// We compute the projection of the point P onto the segment 2
resultPoint = computeOrthogonalProjectionOfPointOntoALine(P, rectangle[1], rectangle[2] - rectangle[1]);
}
else { // Segment 4 is the nearest
// We compute the projection of the point P onto the segment 4
resultPoint = computeOrthogonalProjectionOfPointOntoALine(P, rectangle[3], rectangle[0] - rectangle[3]);
}
}
// Check if P is between the lines of the second pair of parallel segments of the rectangle
else if (distPSegment2 <= distSegment2Segment4 && distPSegment4 <= distSegment2Segment4) {
// Find among segments 1 and 3 which one is the nearest
if (distPSegment1 <= distPSegment3) { // Segment 1 is the nearest
// We compute the projection of the point P onto the segment 1
resultPoint = computeOrthogonalProjectionOfPointOntoALine(P, rectangle[0], rectangle[1] - rectangle[0]);
}
else { // Segment 3 is the nearest
// We compute the projection of the point P onto the segment 3
resultPoint = computeOrthogonalProjectionOfPointOntoALine(P, rectangle[2], rectangle[3] - rectangle[2]);
}
}
else if (distPSegment4 <= distPSegment2) {
if (distPSegment1 <= distPSegment3) { // The point P is in the corner of point rectangle[0]
// Return the corner of the rectangle
return rectangle[0];
}
else { // The point P is in the corner of point rectangle[3]
// Return the corner of the rectangle
return rectangle[3];
}
}
else {
if (distPSegment1 <= distPSegment3) { // The point P is in the corner of point rectangle[1]
// Return the corner of the rectangle
return rectangle[1];
}
else { // The point P is in the corner of point rectangle[2]
// Return the corner of the rectangle
return rectangle[2];
}
}
// Return the result point
return resultPoint;
}
// Compute the intersection between two parallel segments (the first segment is between the points "seg1PointA" and "seg1PointB" and the second
// segment is between the points "seg2PointA" and "seg2PointB"). The result is the segment intersection (represented by the points "resultPointA"
// and "resultPointB". Because the two given segments don't have to be on the same exact line, the result intersection segment will a segment
// halway between the first and the second given segments.
inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3D& seg1PointA, const reactphysics3d::Vector3D& seg1PointB,
const reactphysics3d::Vector3D& seg2PointA, const reactphysics3d::Vector3D& seg2PointB,
reactphysics3d::Vector3D& resultPointA, reactphysics3d::Vector3D& resultPointB) {
// Compute the segment vectors
reactphysics3d::Vector3D d1 = seg1PointB - seg1PointA;
reactphysics3d::Vector3D d2 = seg2PointB - seg2PointA;
// The two segments should be parallel
assert(d1.isParallelWith(d2));
// Compute the projection of the two points of the second segment onto the vector of segment 1
double projSeg2PointA = d1.getUnit().scalarProduct(seg2PointA - seg1PointA);
double projSeg2PointB = d1.getUnit().scalarProduct(seg2PointB - seg1PointA);
// The projections intervals should intersect
assert(!(projSeg2PointA < 0.0 && projSeg2PointB < 0.0));
assert(!(projSeg2PointA > d1.length() && projSeg2PointB > d1.length()));
// Compute the vector "v" from a point on the line 1 to the orthogonal point of the line 2
reactphysics3d::Vector3D point = computeOrthogonalProjectionOfPointOntoALine(seg2PointA, seg1PointA, d1);
reactphysics3d::Vector3D v = seg2PointA - point;
// Return the segment intersection according to the configuration of two projection intervals
if (projSeg2PointA >= 0 && projSeg2PointA <= d1.length() && projSeg2PointB >= d1.length()) {
// Move the contact points halfway between the two segments
resultPointA = seg2PointA - 0.5 * v;
resultPointB = seg1PointB + 0.5 * v;
}
else if (projSeg2PointA <= 0 && projSeg2PointB >= 0 && projSeg2PointB <= d1.length()) {
// Move the contact points halfway between the two segments
resultPointA = seg1PointA + 0.5 * v;
resultPointB = seg2PointB - 0.5 * v;
}
else if (projSeg2PointA <= 0 && projSeg2PointB >= d1.length()) {
// Move the contact points halfway between the two segments
resultPointA = seg1PointA + 0.5 * v;
resultPointB = seg1PointB + 0.5 * v;
}
else if (projSeg2PointA <= d1.length() && projSeg2PointB <= d1.length()) {
// Move the contact points halfway between the two segments
resultPointA = seg2PointA - 0.5 * v;
resultPointB = seg2PointB - 0.5 * v;
}
}
// This method clip a 3D segment with 3D rectangle polygon. The segment and the rectangle are asssumed to be on the same plane. We
// also assume that the segment is not completely outside the clipping rectangle.
// The segment is given by the two vertices in "segment" and the rectangle is given by the ordered vertices in "clipRectangle".
// This method returns the clipped segment.
inline std::vector<reactphysics3d::Vector3D> clipSegmentWithRectangleInPlane(const std::vector<reactphysics3d::Vector3D>& segment, const std::vector<reactphysics3d::Vector3D> clipRectangle) {
double const epsilon = 0.01;
assert(segment.size() == 2);
assert(clipRectangle.size() == 4);
std::vector<reactphysics3d::Vector3D> inputSegment = segment;
std::vector<reactphysics3d::Vector3D> outputSegment;
// For each edge of the clip rectangle
for (unsigned int i=0; i<4; ++i) {
outputSegment.clear();
// Current clipped segment
//assert(inputSegment.size() == 2);
reactphysics3d::Vector3D S = inputSegment[0];
reactphysics3d::Vector3D P = inputSegment[1];
// Edge of the clip rectangle
reactphysics3d::Vector3D A = clipRectangle[i];
reactphysics3d::Vector3D B = clipRectangle[ (i+1) % 4];
reactphysics3d::Vector3D planeNormal = clipRectangle[(i+2) % 4] - clipRectangle[(i+1) % 4];
// If the point P is inside the clip plane
if (planeNormal.scalarProduct(P-A) >= 0.0 - epsilon) {
// If the point S is inside the clip plane
if (planeNormal.scalarProduct(S-A) >= 0.0 - epsilon) {
outputSegment.push_back(P);
outputSegment.push_back(S);
}
else { // P is inside and S is outside the clip plane
// Compute the intersection point between the segment SP and the clip plane
reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A);
outputSegment.push_back(P);
outputSegment.push_back(intersectPoint);
}
}
else if (planeNormal.scalarProduct(S-A) > 0.0 - epsilon) { // P is outside and S is inside the clip plane
// Compute the intersection point between the segment SP and the clip plane
reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A);
outputSegment.push_back(S);
outputSegment.push_back(intersectPoint);
}
inputSegment = outputSegment;
}
// Return the clipped segment
return outputSegment;
}
// This method uses the Sutherland-Hodgman clipping algorithm to clip a subject polygon (given by the ordered 3D vertices in "subjectPolygon") using
// a rectangle polygon (given by the ordered 3D vertices in "clipRectangle"). The subject polygon and the clip rectangle are in 3D but we assumed that
// they are on a same plane in 3D. The method returns the ordered 3D vertices of the subject polygon clipped using the rectangle polygon.
inline std::vector<reactphysics3d::Vector3D> clipPolygonWithRectangleInPlane(const std::vector<reactphysics3d::Vector3D>& subjectPolygon, const std::vector<reactphysics3d::Vector3D>& clipRectangle) {
double const epsilon = 0.1;
assert(clipRectangle.size() == 4);
std::vector<reactphysics3d::Vector3D> outputPolygon;
std::vector<reactphysics3d::Vector3D> inputPolygon = subjectPolygon;
// For each edge of the clip rectangle
for (unsigned int i=0; i<4; ++i) {
outputPolygon.clear();
// Each edge defines a clip plane. The clip plane is define by a point on this plane (a vertice of the current edge) and
// a plane normal (because we are using a clip rectangle, the plane normal is the next edge of the clip rectangle).
reactphysics3d::Vector3D planeNormal = clipRectangle[(i+2) % 4] - clipRectangle[(i+1) % 4];
reactphysics3d::Vector3D A = clipRectangle[i]; // Segment AB is the current segment of the "clipRectangle"
reactphysics3d::Vector3D B = clipRectangle[(i+1) % 4];
reactphysics3d::Vector3D S = inputPolygon[0];
// For each vertex of the subject polygon
for (unsigned int j=0; j<inputPolygon.size(); ++j) {
reactphysics3d::Vector3D P = inputPolygon[(j+1) % inputPolygon.size()];
// If the point P is inside the clip plane
double test = planeNormal.scalarProduct(P-A);
if (planeNormal.scalarProduct(P-A) >= 0.0 - epsilon) {
// If the point S is also inside the clip plane
if (planeNormal.scalarProduct(S-A) >= 0.0 - epsilon) {
outputPolygon.push_back(P);
}
else { // If the point S is outside the clip plane
// Compute the intersection point between the segment SP and the clip plane
reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A);
outputPolygon.push_back(intersectPoint);
outputPolygon.push_back(P);
}
}
else if (planeNormal.scalarProduct(S-A) > 0.0) {
// Compute the intersection point between the segment SP and the clip plane
reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A);
outputPolygon.push_back(intersectPoint);
}
S = P;
}
inputPolygon = outputPolygon;
}
// Return the clipped polygon
return outputPolygon;
}
// Compute the intersection point between a line and a plane in 3D space. There must be an intersection, therefore the
// the lineVector must not be orthogonal to the planeNormal.
inline reactphysics3d::Vector3D intersectLineWithPlane(const reactphysics3d::Vector3D& linePoint, const reactphysics3d::Vector3D& lineVector,
const reactphysics3d::Vector3D& planePoint, const reactphysics3d::Vector3D& planeNormal) {
assert(!approxEqual(lineVector.scalarProduct(planeNormal), 0.0));
// The plane is represented by the equation planeNormal dot X = d where X is a point of the plane
double d = planeNormal.scalarProduct(planePoint);
// Compute the parameter t
double t = (d - planeNormal.scalarProduct(linePoint)) / planeNormal.scalarProduct(lineVector);
// Compute the intersection point
return linePoint + lineVector * t;
}
} // End of the ReactPhysics3D namespace
#endif

47
src/reactphysics3d/mathematics/mathematics_functions.h Normal file
View File

@ -0,0 +1,47 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef MATHEMATICS_FUNCTIONS_H
#define MATHEMATICS_FUNCTIONS_H
// Libraries
#include "constants.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
// ---------- Mathematics functions ---------- //
// function to test if two real numbers are (almost) equal
// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
inline bool approxEqual(double a, double b) {
double difference = a - b;
return (difference < EPSILON && difference > -EPSILON);
}
} // End of ReactPhysics3D namespace
#endif

53
src/reactphysics3d/reactphysics3d.h Normal file
View File

@ -0,0 +1,53 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
/********************************************************************************
* ReactPhysics3D *
* Version 0.01 *
* http://code.google.com/p/reactphysics3d/ *
* Daniel Chappuis *
********************************************************************************/
#ifndef REACTPHYSICS3D_H
#define REACTPHYSICS3D_H
// Libraries
#include "body/RigidBody.h"
#include "engine/PhysicsWorld.h"
#include "engine/PhysicsEngine.h"
#include "body/BoundingVolume.h"
#include "body/OBB.h"
#include "body/AABB.h"
// TODO : Use using namespace std in every possible cpp files to increase readability
// TODO : Check for memory management (RigidBody must free for BoundingVolume it has, ...)
// Alias to the ReactPhysics3D namespace
namespace rp3d = reactphysics3d;
// TODO : Replace in all files of the project "unsigned int" by "uint" (see typeDefinitions.h"
#endif

61
src/reactphysics3d/testing/reactphysics3dTestSuite.cpp Normal file
View File

@ -0,0 +1,61 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
// Libraries
#include <iostream>
#include "TestSuite/Suite.h"
#include "testing_mathematics/MathematicsTest.h"
#include "testing_mathematics/VectorTest.h"
#include "testing_mathematics/Vector3DTest.h"
#include "testing_mathematics/MatrixTest.h"
#include "testing_mathematics/Matrix3x3Test.h"
#include "testing_mathematics/QuaternionTest.h"
#include "testing_physics/TimeTest.h"
#include "testing_physics/KilogramTest.h"
// Namespaces
using namespace std;
using namespace TestSuite;
// Main function
int main() {
// ReactPhysics3D TestSuite
Suite reactphysics3DTestSuite("ReactPhysics3D TestSuite");
// Mathematics tests
reactphysics3DTestSuite.addTest(new MathematicsTest);
reactphysics3DTestSuite.addTest(new VectorTest);
reactphysics3DTestSuite.addTest(new Vector3DTest);
reactphysics3DTestSuite.addTest(new MatrixTest);
reactphysics3DTestSuite.addTest(new Matrix3x3Test);
reactphysics3DTestSuite.addTest(new QuaternionTest);
// Physics tests
reactphysics3DTestSuite.addTest(new TimeTest);
reactphysics3DTestSuite.addTest(new KilogramTest);
// Run the ReactPhysics3D TestSuite and display the report
reactphysics3DTestSuite.run();
long nbFailures = reactphysics3DTestSuite.report();
reactphysics3DTestSuite.free();
return nbFailures;
double inPause;
cin >> inPause;
}

63
src/reactphysics3d/testing/testing_mathematics/MathematicsTest.h Normal file
View File

@ -0,0 +1,63 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef MATHEMATICSTEST_H
#define MATHEMATICSTEST_H
// Libraries
#include "../TestSuite/Test.h"
#include "../../mathematics/mathematics.h"
#include <stdexcept>
#include <iostream>
#include <cmath>
// Namespaces
using namespace reactphysics3d;
// Class MathematicsTest
class MathematicsTest : public TestSuite::Test {
private :
public :
// Constructor
MathematicsTest() {
}
// Run method of the Test
void run() {
testEqual();
}
// Test the equal() method
void testEqual() {
double number1 = 19.13417;
double number2 = 19.13417 + EPSILON/2.0;
double number3 = 19.13417 + 2*EPSILON;
test_(equal(number1, number2));
test_(equal(number2, number1));
test_(!equal(number1, number3));
test_(!equal(number3, number1));
}
};
#endif

417
src/reactphysics3d/testing/testing_mathematics/Matrix3x3Test.h Normal file
View File

@ -0,0 +1,417 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef MATRIX3DTEST_H
#define MATRIX3DTEST_H
// Libraries
#include "../TestSuite/Test.h"
#include "../../mathematics/Matrix3x3.h"
#include <stdexcept>
#include <iostream>
#include <cmath>
// Namespaces
using namespace reactphysics3d;
// Class MatrixTest
class Matrix3x3Test : public TestSuite::Test {
private :
Matrix3x3 matrix1;
Matrix3x3 matrix2;
public :
// Constructor
Matrix3x3Test() {
matrix1.setAllValues(3, 7, -5, 13, -1, 2, 6, 5, 9);
matrix2.setAllValues(-13, 8, 2, 5, -25, 11, -7, 6, 21);
}
// Run method of the Test
void run() {
testConstructors();
testGetValue();
testSetValue();
testSetAllValues() ;
testGetTranspose();
testGetInverse();
testGetDeterminant();
testGetTrace();
testGetQuaternion();
testIdentityMatrix();
testOperatorAddition();
testOperatorSubstraction();
testOperatorMultiplicationWithConstant();
testOperatorMultiplicationWithMatrix();
testOperatorMultiplicationWithVector();
testOperatorAssignment();
testOperatorEquality();
}
// Test the constructors
void testConstructors() {
// Constructor without argument
Matrix3x3 matrix; // This shouldn't throw an exception
test_(matrix.getValue(0,0) == 0);
test_(matrix.getValue(1,2) == 0);
test_(matrix.getValue(0,2) == 0);
// Constructor with arguments
Matrix3x3 matrix3(1, 2, 3, 4, 5, 6, 7, 8, 9);
test_(matrix3.getValue(0,0) == 1);
test_(matrix3.getValue(0,1) == 2);
test_(matrix3.getValue(0,2) == 3);
test_(matrix3.getValue(1,0) == 4);
test_(matrix3.getValue(1,1) == 5);
test_(matrix3.getValue(1,2) == 6);
test_(matrix3.getValue(2,0) == 7);
test_(matrix3.getValue(2,1) == 8);
test_(matrix3.getValue(2,2) == 9);
// Copy-constructor
Matrix3x3 matrix4 = matrix3;
test_(matrix4.getValue(0,0) == 1);
test_(matrix4.getValue(0,1) == 2);
test_(matrix4.getValue(0,2) == 3);
test_(matrix4.getValue(1,0) == 4);
test_(matrix4.getValue(1,1) == 5);
test_(matrix4.getValue(1,2) == 6);
test_(matrix4.getValue(2,0) == 7);
test_(matrix4.getValue(2,1) == 8);
test_(matrix4.getValue(2,2) == 9);
// Conversion-constructor (Quaternion --> Matrix3x3)
// Rotation matrix of a rotation of 180 degrees around x axis
Matrix3x3 rotation1(1, 0, 0, 0, -1, 0, 0, 0, -1);
Quaternion quaternion1(1, 0, 0, 0);
Matrix3x3 matrix5(quaternion1); // Convert the quaternion into a matrix
test_(matrix5 == rotation1); // Check if the matrix result and the rotation matrix are the same
// Rotation matrix of a rotation of 180 degrees around y axis
Matrix3x3 rotation2(-1, 0, 0, 0, 1, 0, 0, 0, -1);
Quaternion quaternion2(0, 1, 0, 0);
Matrix3x3 matrix6(quaternion2); // Convert the quaternion into a matrix
test_(matrix6 == rotation2); // Check if the matrix result and the rotation matrix are the same
// Rotation matrix of a rotation of 180 degrees around z axis
Matrix3x3 rotation3(-1, 0, 0, 0, -1, 0, 0, 0, 1);
Quaternion quaternion3(0, 0, 1, 0);
Matrix3x3 matrix7(quaternion3); // Convert the quaternion into a matrix
test_(matrix7 == rotation3); // Check if the matrix result and the rotation matrix are the same
}
// Test getValue()
void testGetValue() {
// Try a valid getValue()
try {
test_(matrix1.getValue(0, 0) == 3); // This shouldn't throw an exception
test_(matrix1.getValue(1, 0) == 13); // This shouldn't throw an exception
test_(matrix1.getValue(1, 2) == 2); // This shouldn't throw an exception
test_(matrix1.getValue(1, 1) == -1); // This shouldn't throw an exception
succeed_(); // Succeed if no exceptions have been thrown
}
catch(std::invalid_argument& ex) {
fail_("Valid getValue() call throws an exception"); // Failed if an exception has been thrown
}
// Try an invalid getValue() call
try {
matrix1.getValue(-1, 0); // This should throw an exception
fail_("Invalid getValue() call undetected");
}
catch(std::invalid_argument& ex) {
succeed_(); // Succeed if an exception has been thrown
}
// Try an invalid getValue() call
try {
matrix1.getValue(0, 3); // This should throw an exception
fail_("Invalid getValue() call undetected");
}
catch(std::invalid_argument& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test setValue()
void testSetValue() {
Matrix3x3 matrix;
// Try a valid setValue()
try {
matrix.setValue(0, 0, 18); // This shouldn't throw an exception
matrix.setValue(0, 2, -6); // This shouldn't throw an exception
matrix.setValue(1, 0, -44); // This shouldn't throw an exception
matrix.setValue(1, 2, 21); // This shouldn't throw an exception
matrix.setValue(1, 1, 5); // This shouldn't throw an exception
test_(matrix.getValue(0, 0) == 18);
test_(matrix.getValue(0, 2) == -6);
test_(matrix.getValue(1, 0) == -44);
test_(matrix.getValue(1, 2) == 21);
test_(matrix.getValue(1, 1) == 5);
succeed_(); // Succeed if no exceptions have been thrown
}
catch(std::invalid_argument& ex) {
fail_("Valid setValue() call throws an exception"); // Failed if an exception has been thrown
}
// Try an invalid setValue() call
try {
matrix.setValue(-1, 0, 42); // This should throw an exception
fail_("Invalid setValue() call undetected"); // Failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // Succeed if an exception has been thrown
}
// Try an invalid setValue() call
try {
matrix1.setValue(0, 3, 53); // This should throw an exception
fail_("Invalid setValue() call undetected"); // Failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test setAllValues()
void testSetAllValues() {
Matrix3x3 matrix;
matrix.setAllValues(1,2,3,4,5,6,7,8,9);
test_(matrix.getValue(0,0) == 1);
test_(matrix.getValue(0,1) == 2);
test_(matrix.getValue(0,2) == 3);
test_(matrix.getValue(1,0) == 4);
test_(matrix.getValue(1,1) == 5);
test_(matrix.getValue(1,2) == 6);
test_(matrix.getValue(2,0) == 7);
test_(matrix.getValue(2,1) == 8);
test_(matrix.getValue(2,2) == 9);
}
// Test getTranspose()
void testGetTranspose() {
// Get the transpose of matrix1
Matrix3x3 matrix = matrix1.getTranspose();
// Test the transpose matrix
test_(matrix.getValue(0, 0) == 3);
test_(matrix.getValue(0, 1) == 13);
test_(matrix.getValue(0, 2) == 6);
test_(matrix.getValue(1, 0) == 7);
test_(matrix.getValue(1, 1) == -1);
test_(matrix.getValue(1, 2) == 5);
test_(matrix.getValue(2, 0) == -5);
test_(matrix.getValue(2, 1) == 2);
test_(matrix.getValue(2, 2) == 9);
}
// Test getInverse()
void testGetInverse() {
// Construct a 3x3 matrix
Matrix3x3 matrix(0, 1, 2, 1, 0, 3, 4, -3, 8);
// Try to inverse a invertible matrix
try {
Matrix3x3 result = matrix.getInverse(); // This shouldn't thrown an exception
test_(result.getValue(0, 0) == -4.5);
test_(result.getValue(0, 1) == 7);
test_(result.getValue(0, 2) == -3.0/2.0);
test_(result.getValue(1, 0) == -2);
test_(result.getValue(1, 1) == 4);
test_(result.getValue(1, 2) == -1);
test_(result.getValue(2, 0) == 3.0/2.0);
test_(result.getValue(2, 1) == -2);
test_(result.getValue(2, 2) == 1.0/2.0);
succeed_(); // Succeed if no exceptions have been thrown
}
catch(MathematicsException& ex) {
fail_("Valid getInverse() call throws an exception"); // Failed if an exception has been thrown
}
// Try to inverse a square non-invertible matrix (determinant equal to zero)
try {
Matrix3x3 matrix4;
matrix4.getInverse(); // This should throw an exception
fail_("Invalid getInverse() call undetected (non-invertible matrix)"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test getDeterminant()
void testGetDeterminant() {
Matrix3x3 matrix;
test_(matrix.getDeterminant() == 0);
test_(matrix1.getDeterminant() == -1147);
test_(matrix2.getDeterminant() == 5937);
}
// Test getTrace()
void testGetTrace() {
Matrix3x3 matrix;
test_(matrix.getTrace() == 0);
test_(matrix1.getTrace() == 11);
test_(matrix2.getTrace() == -17);
}
// Test getQuaternion()
void testGetQuaternion() {
// Rotation matrix of a rotation of 180 degrees around x axis
Matrix3x3 rotation1(1, 0, 0, 0, -1, 0, 0, 0, -1);
// Convert the matrix into a quaternion
Quaternion quaternion1 = rotation1.getQuaternion();
test_(quaternion1.getX() == 1);
test_(quaternion1.getY() == 0);
test_(quaternion1.getZ() == 0);
test_(quaternion1.getW() == 0);
// Rotation matrix of a rotation of 180 degrees around y axis
Matrix3x3 rotation2(-1, 0, 0, 0, 1, 0, 0, 0, -1);
// Convert the matrix into a quaternion
Quaternion quaternion2 = rotation2.getQuaternion();
test_(quaternion2.getX() == 0);
test_(quaternion2.getY() == 1);
test_(quaternion2.getZ() == 0);
test_(quaternion2.getW() == 0);
// Rotation matrix of a rotation of 180 degrees around z axis
Matrix3x3 rotation3(-1, 0, 0, 0, -1, 0, 0, 0, 1);
// Convert the matrix into a quaternion
Quaternion quaternion3 = rotation3.getQuaternion();
test_(quaternion3.getX() == 0);
test_(quaternion3.getY() == 0);
test_(quaternion3.getZ() == 1);
test_(quaternion3.getW() == 0);
}
// Test identityMatrix()
void testIdentityMatrix() {
Matrix3x3 matrix = Matrix3x3::identity();
test_(matrix.getValue(0, 0) == 1);
test_(matrix.getValue(0, 1) == 0);
test_(matrix.getValue(0, 2) == 0);
test_(matrix.getValue(1, 0) == 0);
test_(matrix.getValue(1, 1) == 1);
test_(matrix.getValue(1, 2) == 0);
test_(matrix.getValue(2, 0) == 0);
test_(matrix.getValue(2, 1) == 0);
test_(matrix.getValue(2, 2) == 1);
}
// Test operator+()
void testOperatorAddition() {
Matrix3x3 result = matrix1 + matrix2;
test_(result.getValue(0,0) == -10);
test_(result.getValue(0,1) == 15);
test_(result.getValue(0,2) == -3);
test_(result.getValue(1,0) == 18);
test_(result.getValue(1,1) == -26);
test_(result.getValue(1,2) == 13);
test_(result.getValue(2,0) == -1);
test_(result.getValue(2,1) == 11);
test_(result.getValue(2,2) == 30);
}
// Test operator-()
void testOperatorSubstraction() {
Matrix3x3 result = matrix1 - matrix2;
test_(result.getValue(0,0) == 16);
test_(result.getValue(0,1) == -1);
test_(result.getValue(0,2) == -7);
test_(result.getValue(1,0) == 8);
test_(result.getValue(1,1) == 24);
test_(result.getValue(1,2) == -9);
test_(result.getValue(2,0) == 13);
test_(result.getValue(2,1) == -1);
test_(result.getValue(2,2) == -12);
}
// Test operator* (multiplication with a constant number)
void testOperatorMultiplicationWithConstant() {
Matrix3x3 result = matrix1 * 2;
test_(result.getValue(0,0) == 6);
test_(result.getValue(0,1) == 14);
test_(result.getValue(0,2) == -10);
test_(result.getValue(1,0) == 26);
test_(result.getValue(1,1) == -2);
test_(result.getValue(1,2) == 4);
test_(result.getValue(2,0) == 12);
test_(result.getValue(2,1) == 10);
test_(result.getValue(2,2) == 18);
}
// Test operator* (multiplication with matrix)
void testOperatorMultiplicationWithMatrix() {
Matrix3x3 result = matrix1 * matrix2;
test_(result.getValue(0,0) == 31);
test_(result.getValue(0,1) == -181);
test_(result.getValue(0,2) == -22);
test_(result.getValue(1,0) == -188);
test_(result.getValue(1,1) == 141);
test_(result.getValue(1,2) == 57);
test_(result.getValue(2,0) == -116);
test_(result.getValue(2,1) == -23);
test_(result.getValue(2,2) == 256);
}
void testOperatorMultiplicationWithVector() {
Vector3D vector(4,7,3);
Vector3D result = matrix1 * vector;
test_(result.getX() == 46);
test_(result.getY() == 51);
test_(result.getZ() == 86);
}
// Test operator=()
void testOperatorAssignment() {
Matrix3x3 matrix;
matrix = matrix1;
test_(matrix.getValue(0,0) == 3);
test_(matrix.getValue(0,1) == 7);
test_(matrix.getValue(0,2) == -5);
test_(matrix.getValue(1,0) == 13);
test_(matrix.getValue(1,1) == -1);
test_(matrix.getValue(1,2) == 2);
test_(matrix.getValue(2,0) == 6);
test_(matrix.getValue(2,1) == 5);
test_(matrix.getValue(2,2) == 9);
}
// Test operator==()
void testOperatorEquality() {
Matrix3x3 matrix(3, 7, -5, 13, -1, 2, 6, 5, 9);
test_(matrix == matrix1);
test_(matrix1 == matrix);
test_(matrix == matrix);
matrix.setValue(1,1, 100);
test_(!(matrix == matrix1));
test_(!(matrix1 == matrix));
}
};
#endif

648
src/reactphysics3d/testing/testing_mathematics/MatrixTest.h Normal file
View File

@ -0,0 +1,648 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef MATRIXTEST_H
#define MATRIXTEST_H
// Libraries
#include "../TestSuite/Test.h"
#include "../../mathematics/Matrix.h"
#include <stdexcept>
// Namespaces
using namespace reactphysics3d;
// Class MatrixTest
class MatrixTest : public TestSuite::Test {
private :
Matrix matrix1;
Matrix matrix2;
Matrix matrix3;
public :
// Constructor
MatrixTest() : matrix1(2,3), matrix2(2,3), matrix3(3,2) {
matrix1.setValue(0, 0, 4);
matrix1.setValue(0, 1, 5);
matrix1.setValue(0, 2, 7);
matrix1.setValue(1, 0, 2);
matrix1.setValue(1, 1, 3);
matrix1.setValue(1, 2, -4);
matrix2.setValue(0, 0, -12);
matrix2.setValue(0, 1, 3);
matrix2.setValue(0, 2, 16);
matrix2.setValue(1, 0, -7);
matrix2.setValue(1, 1, 4);
matrix2.setValue(1, 2, 6);
matrix3.setValue(0, 0, -4);
matrix3.setValue(0, 1, -2);
matrix3.setValue(1, 0, 7);
matrix3.setValue(1, 1, 9);
matrix3.setValue(2, 0, 18);
matrix3.setValue(2, 1, 33);
}
// Run method of the Test
void run() {
testConstructors();
testGetValue();
testSetValue();
testGetNbRow();
testGetNbColumn();
testGetCofactor();
testGetTranspose();
testGetInverse();
testGetDeterminant();
testGetTrace();
testIdentityMatrix();
testOperatorAddition();
testOperatorSubstraction();
testOperatorMultiplicationWithConstant();
testOperatorMultiplicationWithMatrix();
testOperatorAssignment();
testOperatorEquality();
}
// Test the constructors
void testConstructors() {
// Try a valid constructor call
try {
// Constructor
Matrix matrix(4,6); // This shouldn't throw an exception
test_(matrix.getNbRow() == 4);
test_(matrix.getNbColumn() == 6);
test_(matrix.getValue(0,0) == 0);
succeed_(); // Succeed if no exceptions have been thrown
}
catch(std::invalid_argument& ex) {
fail_("Valid constructor call throws an exception"); // Failed if an exception has been thrown
}
// Try an invalid constructor call
try {
// Constructor
Matrix matrix(-2,6); // This should throw an exception
fail_("Invalid constructor call undetected (argument -2) "); // Failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // Succeed if an exception has been thrown
}
// Try an invalid constructor call
try {
// Constructor
Matrix matrix(3,0); // This should throw an exception
fail_("Invalid constructor call undetected (argument 0)"); // Failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // Succeed if an exception has been thrown
}
// Copy-constructor
Matrix matrix4 = matrix1;
test_(matrix4.getNbRow() == 2);
test_(matrix4.getNbColumn() == 3);
test_(matrix4.getValue(0, 0) == 4);
test_(matrix4.getValue(0, 1) == 5);
test_(matrix4.getValue(0, 2) == 7);
test_(matrix4.getValue(1, 0) == 2);
test_(matrix4.getValue(1, 1) == 3);
test_(matrix4.getValue(1, 2) == -4);
}
// Test getValue()
void testGetValue() {
// Try a valid getValue()
try {
test_(matrix1.getValue(0, 0) == 4); // This shouldn't throw an exception
test_(matrix1.getValue(0, 2) == 7); // This shouldn't throw an exception
test_(matrix1.getValue(1, 0) == 2); // This shouldn't throw an exception
test_(matrix1.getValue(1, 2) == -4); // This shouldn't throw an exception
test_(matrix1.getValue(1, 1) == 3); // This shouldn't throw an exception
succeed_(); // Succeed if no exceptions have been thrown
}
catch(std::invalid_argument& ex) {
fail_("Valid getValue() call throws an exception"); // Failed if an exception has been thrown
}
// Try an invalid getValue() call
try {
matrix1.getValue(-1, 0); // This should throw an exception
fail_("Invalid getValue() call undetected");
}
catch(std::invalid_argument& ex) {
succeed_(); // Succeed if an exception has been thrown
}
// Try an invalid getValue() call
try {
matrix1.getValue(0, 3); // This should throw an exception
fail_("Invalid getValue() call undetected");
}
catch(std::invalid_argument& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test setValue()
void testSetValue() {
Matrix matrix(2,3);
// Try a valid setValue()
try {
matrix.setValue(0, 0, 18); // This shouldn't throw an exception
matrix.setValue(0, 2, -6); // This shouldn't throw an exception
matrix.setValue(1, 0, -44); // This shouldn't throw an exception
matrix.setValue(1, 2, 21); // This shouldn't throw an exception
matrix.setValue(1, 1, 5); // This shouldn't throw an exception
test_(matrix.getValue(0, 0) == 18);
test_(matrix.getValue(0, 2) == -6);
test_(matrix.getValue(1, 0) == -44);
test_(matrix.getValue(1, 2) == 21);
test_(matrix.getValue(1, 1) == 5);
succeed_(); // Succeed if no exceptions have been thrown
}
catch(std::invalid_argument& ex) {
fail_("Valid setValue() call throws an exception"); // Failed if an exception has been thrown
}
// Try an invalid setValue() call
try {
matrix.setValue(-1, 0, 42); // This should throw an exception
fail_("Invalid setValue() call undetected"); // Failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // Succeed if an exception has been thrown
}
// Try an invalid setValue() call
try {
matrix1.setValue(0, 3, 53); // This should throw an exception
fail_("Invalid setValue() call undetected"); // Failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test getNbRow()
void testGetNbRow() {
test_(matrix1.getNbRow() == 2);
test_(matrix3.getNbRow() == 3);
}
// Test getNbColumn()
void testGetNbColumn() {
test_(matrix1.getNbColumn() == 3);
test_(matrix3.getNbColumn() == 2);
}
// Test getCofactor()
void testGetCofactor() {
// Try a valid getCofactor()
try {
Matrix matrix = matrix1.getCofactor(0,1); // This shouldn't throw an exception
test_(matrix.getNbRow() == 1);
test_(matrix.getNbColumn() == 2);
test_(matrix.getValue(0, 0) == 2);
test_(matrix.getValue(0, 1) == -4);
Matrix matrix4 = matrix1.getCofactor(1,2); // This shouldn't throw an exception
test_(matrix4.getNbRow() == 1);
test_(matrix4.getNbColumn() == 2);
test_(matrix4.getValue(0, 0) == 4);
test_(matrix4.getValue(0, 1) == 5);
succeed_(); // Succeed if no exceptions have been thrown
}
catch(std::invalid_argument& ex) {
fail_("Valid getCofactor() call throws an exception"); // Failed if an exception has been thrown
}
// Try an invalid getCofactor() call
try {
matrix1.getCofactor(-1,0); // This should throw an exception
fail_("Invalid getCofactor() call undetected"); // Failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // Succeed if an exception has been thrown
}
// Try an invalid getCofactor() call
try {
matrix1.getCofactor(0,3); // This should throw an exception
fail_("Invalid getCofactor() call undetected"); // Failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test getTranspose()
void testGetTranspose() {
// Get the transpose of matrix1
Matrix matrix = matrix1.getTranspose();
// Test the transpose matrix
test_(matrix.getNbRow() == 3);
test_(matrix.getNbColumn() == 2);
test_(matrix.getValue(0, 0) == 4);
test_(matrix.getValue(0, 1) == 2);
test_(matrix.getValue(1, 0) == 5);
test_(matrix.getValue(1, 1) == 3);
test_(matrix.getValue(2, 0) == 7);
test_(matrix.getValue(2, 1) == -4);
}
// Test getInverse()
void testGetInverse() {
// Construct a 3x3 matrix
Matrix matrix(3,3);
matrix.setValue(0, 0, 0);
matrix.setValue(0, 1, 1);
matrix.setValue(0, 2, 2);
matrix.setValue(1, 0, 1);
matrix.setValue(1, 1, 0);
matrix.setValue(1, 2, 3);
matrix.setValue(2, 0, 4);
matrix.setValue(2, 1, -3);
matrix.setValue(2, 2, 8);
// Try to inverse a invertible matrix
try {
Matrix result = matrix.getInverse(); // This shouldn't thrown an exception
test_(result.getValue(0, 0) == -4.5);
test_(result.getValue(0, 1) == 7);
test_(result.getValue(0, 2) == -3.0/2.0);
test_(result.getValue(1, 0) == -2);
test_(result.getValue(1, 1) == 4);
test_(result.getValue(1, 2) == -1);
test_(result.getValue(2, 0) == 3.0/2.0);
test_(result.getValue(2, 1) == -2);
test_(result.getValue(2, 2) == 1.0/2.0);
succeed_(); // Succeed if no exceptions have been thrown
}
catch(MathematicsException& ex) {
fail_("Valid getInverse() call throws an exception"); // Failed if an exception has been thrown
}
// Try to inverse a non-square matrix
try {
matrix1.getInverse(); // This should throw an exception
fail_("Invalid getInverse() call undetected (non-square matrix)"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
// Try to inverse a square non-invertible matrix (determinant equal to zero)
try {
Matrix matrix4(2,2);
matrix4.setValue(0, 0, 3);
matrix4.setValue(0, 1, 2);
matrix4.setValue(1, 0, 3);
matrix4.setValue(1, 1, 2);
matrix4.getInverse(); // This should throw an exception
fail_("Invalid getInverse() call undetected (non-invertible matrix)"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test getDeterminant()
void testGetDeterminant() {
// Try to compute the determinant of a square matrix
try {
Matrix matrix(2,2);
test_(matrix.getDeterminant() == 0); // This shouldn't throw an exception
matrix.setValue(0,0, 4);
matrix.setValue(0,1, -9);
matrix.setValue(1,0, 0);
matrix.setValue(1,1, 5);
test_(matrix.getDeterminant() == 20); // This shouldn't throw an exception
matrix.setValue(0,0, 6);
matrix.setValue(0,1, -9);
matrix.setValue(1,0, -4);
matrix.setValue(1,1, 6);
test_(matrix.getDeterminant() == 0); // This shouldn't throw an exception
succeed_(); // Succeed if no exceptions have been thrown
}
catch(MathematicsException& ex) {
fail_("Valid getDeterminant() call throws an exception"); // Failed if an exception has been thrown
}
// Try to compute the determinant of a non-square matrix
try {
Matrix matrix5(2,8);
matrix5.setValue(0, 2, 3);
matrix5.setValue(1, 1, 2);
matrix5.getDeterminant(); // This should throw an exception
fail_("getDeterminant() call with a non-square matrix undetected"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test getTrace()
void testGetTrace() {
// Try to compute the trace of a square matrix
try {
// Construct a 3x3 matrix
Matrix matrix(3,3);
matrix.setValue(0, 0, -2);
matrix.setValue(0, 1, 1);
matrix.setValue(0, 2, 2);
matrix.setValue(1, 0, 1);
matrix.setValue(1, 1, 5);
matrix.setValue(1, 2, 3);
matrix.setValue(2, 0, 4);
matrix.setValue(2, 1, -3);
matrix.setValue(2, 2, 8);
test_(matrix.getTrace() == 11); // This shouldn't throw an exception
succeed_(); // Succeed if no exceptions have been thrown
}
catch(MathematicsException& ex) {
fail_("Valid getTrace() call throws an exception"); // Failed if an exception has been thrown
}
// Try to compute the trace of a non-square matrix
try {
Matrix matrix5(2,8);
matrix5.getTrace(); // This should throw an exception
fail_("getTrace() call with a non-square matrix undetected"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test identityMatrix()
void testIdentityMatrix() {
// Try to compute a valid identity matrix
try {
Matrix matrix = Matrix::identity(2); // This shouldn't throw an exception
test_(matrix.getNbRow() == 2);
test_(matrix.getNbColumn() == 2);
test_(matrix.getValue(0, 0) == 1);
test_(matrix.getValue(0, 1) == 0);
test_(matrix.getValue(1, 0) == 0);
test_(matrix.getValue(1, 1) == 1);
succeed_(); // Succeed if no exceptions have been thrown
}
catch(std::invalid_argument& ex) {
fail_("Valid identity() call throws an exception"); // Failed if an exception has been thrown
}
// Try to compute an invalid identity matrix
try {
Matrix matrix5 = Matrix::identity(0); // This should throw an exception
fail_("Invalid identity() call (argument 0) undetected"); // Failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // Succeed if an exception has been thrown
}
// Try to compute an invalid identity matrix
try {
Matrix matrix5 = Matrix::identity(-1); // This should throw an exception
fail_("Invalid identity() call (argument -1) undetected"); // Failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test operator+()
void testOperatorAddition() {
// Try to compute a valid addition
try {
Matrix result = matrix1 + matrix2; // This shouldn't throw an exception
test_(result.getNbRow() == 2);
test_(result.getNbColumn() == 3);
test_(result.getValue(0,0) == -8);
test_(result.getValue(0,1) == 8);
test_(result.getValue(0,2) == 23);
test_(result.getValue(1,0) == -5);
test_(result.getValue(1,1) == 7);
test_(result.getValue(1,2) == 2);
succeed_(); // Succeed if no exceptions have been thrown
}
catch(MathematicsException& ex) {
fail_("Valid matrix addition throws an exception"); // Failed if an exception has been thrown
}
// Try to compute an invalid addition
try {
Matrix matrix5(2,4);
matrix1 + matrix5; // This should throw an exception
fail_("Invalid matrix addition undetected"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
// Try to compute an invalid addition
try {
Matrix matrix5(1,3);
matrix1 + matrix5; // This should throw an exception
fail_("Invalid matrix addition undetected"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test operator-()
void testOperatorSubstraction() {
// Try to compute a valid substraction
try {
Matrix result = matrix1 - matrix2; // This shouldn't throw an exception
test_(result.getNbRow() == 2);
test_(result.getNbColumn() == 3);
test_(result.getValue(0,0) == 16);
test_(result.getValue(0,1) == 2);
test_(result.getValue(0,2) == -9);
test_(result.getValue(1,0) == 9);
test_(result.getValue(1,1) == -1);
test_(result.getValue(1,2) == -10);
succeed_(); // Succeed if no exceptions have been thrown
}
catch(MathematicsException& ex) {
fail_("Valid matrix substraction throws an exception"); // Failed if an exception has been thrown
}
// Try to compute an invalid substraction
try {
Matrix matrix5(2,4);
matrix1 - matrix5; // This should throw an exception
fail_("Invalid matrix substraction undetected"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
// Try to compute an invalid substraction
try {
Matrix matrix5(1,3);
matrix1 - matrix5; // This should throw an exception
fail_("Invalid matrix substraction undetected"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test operator* (multiplication with a constant number)
void testOperatorMultiplicationWithConstant() {
Matrix matrix = matrix1 * 2;
test_(matrix.getNbRow() == 2);
test_(matrix.getNbColumn() == 3);
test_(matrix.getValue(0,0) == 8);
test_(matrix.getValue(0,1) == 10);
test_(matrix.getValue(0,2) == 14);
test_(matrix.getValue(1,0) == 4);
test_(matrix.getValue(1,1) == 6);
test_(matrix.getValue(1,2) == -8);
}
// Test operator* (multiplication with matrix)
void testOperatorMultiplicationWithMatrix() {
// Try to compute a valid multiplication
try {
Matrix result = matrix1 * matrix3; // This shouldn't throw an exception
test_(result.getNbRow() == 2);
test_(result.getNbColumn() == 2);
test_(result.getValue(0,0) == 145);
test_(result.getValue(0,1) == 268);
test_(result.getValue(1,0) == -59);
test_(result.getValue(1,1) == -109);
succeed_(); // Succeed if no exceptions have been thrown
}
catch(MathematicsException& ex) {
fail_("Valid matrix multiplication throws an exception"); // Failed if an exception has been thrown
}
// Try to compute an invalid multiplication
try {
Matrix matrix5(1,3);
matrix1 * matrix5; // This should throw an exception
fail_("Invalid matrix substraction undetected"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
// Try to compute an invalid multiplication
try {
Matrix matrix5(2,2);
matrix1 * matrix5; // This should throw an exception
fail_("Invalid matrix substraction undetected"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test operator=()
void testOperatorAssignment() {
// Try to compute a valid assignment
try {
Matrix matrix(2,3);
matrix = matrix1; // This shouldn't throw an exception
test_(matrix.getValue(0, 0) == 4);
test_(matrix.getValue(0, 2) == 7);
test_(matrix.getValue(1, 0) == 2);
test_(matrix.getValue(1, 2) == -4);
test_(matrix.getValue(1, 1) == 3);
succeed_(); // Succeed if no exceptions have been thrown
}
catch(MathematicsException& ex) {
fail_("Valid matrix assignment throws an exception"); // Failed if an exception has been thrown
}
// Try to compute an invalid assignment
try {
Matrix matrix(2,2);
matrix = matrix1; // This should throw an exception
fail_("Invalid matrix assignment undetected"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test operator==()
void testOperatorEquality() {
// Try to test a valid equality
try {
Matrix matrix(2,3);
matrix.setValue(0, 0, 4);
matrix.setValue(0, 1, 5);
matrix.setValue(0, 2, 7);
matrix.setValue(1, 0, 2);
matrix.setValue(1, 1, 3);
matrix.setValue(1, 2, -4);
test_(matrix == matrix1); // This shouldn't throw an exception
test_(matrix1 == matrix); // This shouldn't throw an exception
test_(matrix == matrix); // This shouldn't throw an exception
matrix.setValue(1,1, 5);
test_(!(matrix == matrix1)); // This shouldn't throw an exception
test_(!(matrix1 == matrix)); // This shouldn't throw an exception
succeed_(); // Succeed if no exceptions have been thrown
}
catch(MathematicsException& ex) {
fail_("Valid matrix equality test throws an exception"); // Failed if an exception has been thrown
}
// Try to test a invalid equality
try {
Matrix matrix(2,2);
matrix == matrix1; // This should throw an exception
fail_("Invalid matrix assignment undetected"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
};
#endif

350
src/reactphysics3d/testing/testing_mathematics/QuaternionTest.h Normal file
View File

@ -0,0 +1,350 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef QUATERNIONTEST_H
#define QUATERNIONTEST_H
// Libraries
#include "../TestSuite/Test.h"
#include "../../mathematics/Quaternion.h"
#include "../../mathematics/constants.h"
#include <stdexcept>
#include <iostream>
// Namespaces
using namespace reactphysics3d;
// Class MatrixTest
class QuaternionTest : public TestSuite::Test {
private :
Quaternion quaternion1;
Quaternion quaternion2;
public :
// Constructor
QuaternionTest() : quaternion1(2,3,4,5), quaternion2(6,7,8,9) {
}
// Run method of the Test
void run() {
testConstructors();
testVectorV();
testLength();
testGetUnit();
testGetConjugate();
testGetInverse();
testScalarProduct();
testgetRotationAngleAxis();
testSlerp();
testOperatorAddition();
testOperatorSubstraction();
testOperatorMultiplicationWithConstant();
testOperatorMultiplicationWithQuaternion();
testOperatorAssignment();
testOperatorEquality();
}
// Test the constructors
void testConstructors() {
// Constructor without argument
Quaternion quaternion;
test_(quaternion.getX() == 0);
test_(quaternion.getY() == 0);
test_(quaternion.getZ() == 0);
test_(quaternion.getW() == 0);
// Constructor with argument
Quaternion quaternion3(1,2,3,4);
test_(quaternion3.getX() == 1);
test_(quaternion3.getY() == 2);
test_(quaternion3.getZ() == 3);
test_(quaternion3.getW() == 4);
// Constructor with vector
Vector3D vector(2,3,4);
Quaternion quaternion4(5, vector);
test_(quaternion4.getX() == 2);
test_(quaternion4.getY() == 3);
test_(quaternion4.getZ() == 4);
test_(quaternion4.getW() == 5);
// Copy-constructor
Quaternion quaternion5 = quaternion3;
test_(quaternion5.getX() == 1);
test_(quaternion5.getY() == 2);
test_(quaternion5.getZ() == 3);
test_(quaternion5.getW() == 4);
}
// Test getX()
void testGetX() {
test_(quaternion1.getX() == 2);
test_(quaternion2.getX() == 6);
}
// Test getY()
void testGetY() {
test_(quaternion1.getY() == 3);
test_(quaternion2.getY() == 7);
}
// Test getZ()
void testGetZ() {
test_(quaternion1.getZ() == 4);
test_(quaternion2.getZ() == 8);
}
// Test getW()
void testGetW() {
test_(quaternion1.getW() == 5);
test_(quaternion2.getW() == 9);
}
// Test setX()
void testSetX() {
Quaternion quaternion;
quaternion.setX(3);
test_(quaternion.getX() == 3);
test_(quaternion.getY() == 0);
test_(quaternion.getZ() == 0);
test_(quaternion.getW() == 0);
}
// Test setY()
void testSetY() {
Quaternion quaternion;
quaternion.setY(3);
test_(quaternion.getX() == 0);
test_(quaternion.getY() == 3);
test_(quaternion.getZ() == 0);
test_(quaternion.getW() == 0);
}
// Test setZ()
void testSetZ() {
Quaternion quaternion;
quaternion.setZ(3);
test_(quaternion.getX() == 0);
test_(quaternion.getY() == 0);
test_(quaternion.getZ() == 3);
test_(quaternion.getW() == 0);
}
// Test setW()
void testSetW() {
Quaternion quaternion;
quaternion.setW(3);
test_(quaternion.getX() == 0);
test_(quaternion.getY() == 0);
test_(quaternion.getZ() == 0);
test_(quaternion.getW() == 3);
}
// Test vectorV()
void testVectorV() {
Vector3D vector1(2,3,4);
Vector3D vector2(6,7,8);
Vector3D vectorTest1 = quaternion1.vectorV();
Vector3D vectorTest2 = quaternion2.vectorV();
test_(vectorTest1 == vector1);
test_(vectorTest2 == vector2);
}
// Test length()
void testLength() {
Quaternion quaternion;
test_(quaternion.length() == 0);
Quaternion quaternion3(3, 4, 0, 0);
test_(quaternion3.length() == 5);
Quaternion quaternion4(0, 4, 3, 0);
test_(quaternion4.length() == 5);
Quaternion quaternion5(0, 0, 3, 4);
test_(quaternion5.length() == 5);
}
// Test getUnit()
void testGetUnit() {
// Try to compute a valid unit quaternion
try {
Quaternion quaternion(3, 4, 0, 0);
Quaternion unit = quaternion.getUnit(); // This shouldn't throw an exception
test_(unit.getX() == 3.0/5.0);
test_(unit.getY() == 4.0/5.0);
test_(unit.getZ() == 0);
test_(unit.getW() == 0);
Quaternion quaternion3(0, 0, 4, 3);
Quaternion unit2 = quaternion3.getUnit(); // This shouldn't throw an exception
test_(unit2.getX() == 0);
test_(unit2.getY() == 0);
test_(unit2.getZ() == 4.0/5.0);
test_(unit2.getW() == 3.0/5.0);
succeed_(); // Succeed if no exception have been thrown
}
catch(MathematicsException& ex) {
fail_("Valid getUnit() call throw an exception"); // Failed if an exception has been thrown
}
// Try to compute an invalid unit quaternion
try {
Quaternion quaternion(0, 0, 0, 0);
quaternion.getUnit(); // This should throw an exception
fail_("Invalid getUnit() call undetected"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test getConjugate()
void testGetConjugate() {
Quaternion conjugate1 = quaternion1.getConjugate();
test_(conjugate1.getX() == -2);
test_(conjugate1.getY() == -3);
test_(conjugate1.getZ() == -4);
test_(conjugate1.getW() == 5);
Quaternion conjugate2 = quaternion2.getConjugate();
test_(conjugate2.getX() == -6);
test_(conjugate2.getY() == -7);
test_(conjugate2.getZ() == -8);
test_(conjugate2.getW() == 9);
}
// Test getInverse()
void testGetInverse() {
// Try to compute a valid inverse quaternion
try {
Quaternion quaternion(3, 4, 0, 0);
Quaternion inverse = quaternion.getInverse(); // This shouldn't throw an exception
test_(inverse.getX() == -3.0/25.0);
test_(inverse.getY() == -4.0/25.0);
test_(inverse.getZ() == 0);
test_(inverse.getW() == 0);
Quaternion quaternion3(0, 0, 4, 3);
Quaternion inverse2 = quaternion3.getInverse(); // This shouldn't throw an exception
test_(inverse2.getX() == 0);
test_(inverse2.getY() == 0);
test_(inverse2.getZ() == -4.0/25.0);
test_(inverse2.getW() == 3.0/25.0);
succeed_(); // Succeed if no exception have been thrown
}
catch(MathematicsException& ex) {
fail_("Valid getInverse() call throw an exception"); // Failed if an exception has been thrown
}
// Try to compute an invalid unit quaternion
try {
Quaternion quaternion(0, 0, 0, 0);
quaternion.getInverse(); // This should throw an exception
fail_("Invalid getInverse() call undetected"); // Failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // Succeed if an exception has been thrown
}
}
// Test the scalarProduct() method
void testScalarProduct() {
double result = quaternion1.scalarProduct(quaternion2);
test_(result == 110.0);
}
// Test the getRotationAngleAxis() method
void testgetRotationAngleAxis() {
Quaternion quaternion(1.0, 2.0, 3.0, 0.0);
double invAxisLength = 1.0/sqrt(1.0 + 2.0*2.0 + 3.0*3.0);
double angle;
Vector3D axis;
quaternion.getRotationAngleAxis(angle, axis);
test_(equal(angle, PI));
test_(equal(axis.getX(), 1.0*invAxisLength));
test_(equal(axis.getY(), 2.0*invAxisLength));
test_(equal(axis.getZ(), 3.0*invAxisLength));
}
// Test the slerp() method
void testSlerp() {
// TODO : Test the Quaternion::slerp() method
}
// Test operator+()
void testOperatorAddition() {
Quaternion result = quaternion1 + quaternion2;
test_(result.getX() == 8);
test_(result.getY() == 10);
test_(result.getZ() == 12);
test_(result.getW() == 14);
}
// Test operator-()
void testOperatorSubstraction() {
Quaternion result = quaternion1 - quaternion2;
test_(result.getX() == -4);
test_(result.getY() == -4);
test_(result.getZ() == -4);
test_(result.getW() == -4);
}
// Test operator* (multiplication with a constant number)
void testOperatorMultiplicationWithConstant() {
Quaternion result = quaternion1 * 3;
test_(result.getX() == 6);
test_(result.getY() == 9);
test_(result.getZ() == 12);
test_(result.getW() == 15);
}
// Test operator* (multiplication with quaternion)
void testOperatorMultiplicationWithQuaternion() {
Quaternion result = quaternion1 * quaternion2;
test_(result.getX() == 44);
test_(result.getY() == 70);
test_(result.getZ() == 72);
test_(result.getW() == -20);
}
// Test operator=()
void testOperatorAssignment() {
Quaternion quaternion;
quaternion = quaternion1;
test_(quaternion.getX() == 2);
test_(quaternion.getY() == 3);
test_(quaternion.getZ() == 4);
test_(quaternion.getW() == 5);
}
// Test operator==()
void testOperatorEquality() {
Quaternion quaternion(2,3,4,5);
test_(quaternion == quaternion1);
test_(quaternion1 == quaternion);
test_(!(quaternion2 == quaternion1));
test_(!(quaternion1 == quaternion2));
}
};
#endif

272
src/reactphysics3d/testing/testing_mathematics/Vector3DTest.h Normal file
View File

@ -0,0 +1,272 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef VECTOR3DTEST_H
#define VECTOR3DTEST_H
// Libraries
#include "../TestSuite/Test.h"
#include "../../mathematics/Vector3D.h"
#include <stdexcept>
// Namespaces
using namespace reactphysics3d;
// Class Vector3DTest
class Vector3DTest : public TestSuite::Test {
private :
Vector3D vector1;
Vector3D vector2;
public :
// Constructor
Vector3DTest() : vector1(1,2,3), vector2(-3,5,7) {
}
// Run method of the Test
void run() {
testConstructors();
testGetX();
testGetY();
testGetZ();
testSetX();
testSetY();
testSetZ();
testSetAllValues();
testLength();
testGetUnit();
testScalarProduct();
testCrossProduct();
testOperatorAddition();
testOperatorSubstraction();
testOperatorConstantMultiplications();
testOperatorAssignment();
}
// Test the constructors
void testConstructors() {
// Constructor without arguments
Vector3D vector;
test_(vector.getX() == 0);
test_(vector.getY() == 0);
test_(vector.getZ() == 0);
// Constructor with arguments
Vector3D vector3(4, -67, 21);
test_(vector3.getX() == 4);
test_(vector3.getY() == -67);
test_(vector3.getZ() == 21);
// Copy-constructor
Vector3D vector4 = vector1;
test_(vector4.getX() == 1);
test_(vector4.getY() == 2);
test_(vector4.getZ() == 3);
}
// Test getX()
void testGetX() {
test_(vector1.getX() == 1);
}
// Test getY()
void testGetY() {
test_(vector1.getY() == 2);
}
// Test getZ()
void testGetZ() {
test_(vector1.getZ() == 3);
}
// Test setX()
void testSetX() {
Vector3D vector(5, 6, 7);
vector.setX(8);
test_(vector.getX() == 8);
test_(vector.getY() == 6);
test_(vector.getZ() == 7);
}
// Test setY()
void testSetY() {
Vector3D vector(5, 6, 7);
vector.setY(8);
test_(vector.getX() == 5);
test_(vector.getY() == 8);
test_(vector.getZ() == 7);
}
// Test setZ()
void testSetZ() {
Vector3D vector(5, 6, 7);
vector.setZ(8);
test_(vector.getX() == 5);
test_(vector.getY() == 6);
test_(vector.getZ() == 8);
}
// Test setAllValues()
void testSetAllValues() {
Vector3D vector(5, 6, 7);
vector1.setAllValues(4,3,9);
test_(vector1.getX() == 4);
test_(vector1.getY() == 3);
test_(vector1.getZ() == 9);
}
// Test length()
void testLength() {
Vector3D vector3;
test_(vector3.length() == 0);
vector3.setAllValues(3, 4, 0);
test_(vector3.length() == 5);
vector3.setAllValues(0, -3, 4);
test_(vector3.length() == 5);
}
// Test getUnit()
void testGetUnit() {
Vector3D vector3(-23, 0, 0);
test_(vector3.getUnit().length() == 1);
test_(vector3.getUnit().getX() == -1);
test_(vector3.getUnit().getY() == 0);
test_(vector3.getUnit().getZ() == 0);
vector3.setAllValues(0, 6, 0);
test_(vector3.getUnit().length() == 1);
test_(vector3.getUnit().getX() == 0);
test_(vector3.getUnit().getY() == 1);
test_(vector3.getUnit().getZ() == 0);
vector3.setAllValues(0, 0, 13);
test_(vector3.getUnit().length() == 1);
test_(vector3.getUnit().getX() == 0);
test_(vector3.getUnit().getY() == 0);
test_(vector3.getUnit().getZ() == 1);
vector3.setAllValues(0, 0, 0); // Vector of length equal to zero
try {
vector3.getUnit(); // This should throw an exception
fail_("getUnit() with a vector of length equals to zero undetected"); // The test failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // The test succeed if an exception has been thrown
}
}
// Test scalarProduct()
void testScalarProduct() {
Vector3D vector3(2, -3, 5);
Vector3D vector4(7, 4, 6);
// Test the scalar product result
test_(vector3.scalarProduct(vector4) == 32);
}
// Test crossProduct()
void testCrossProduct() {
Vector3D vector3(4, -5, 2);
Vector3D vector4(3, 2, 6);
// Compute the cross product
Vector3D result = vector3.crossProduct(vector4);
// Test the result
test_(result.getX() == -34);
test_(result.getY() == -18);
test_(result.getZ() == 23);
}
// Test operator+()
void testOperatorAddition() {
Vector3D vector3(4, -5, 2);
Vector3D vector4(3, 2, 6);
// Compute the sum
Vector3D result = vector3 + vector4;
// Test the result
test_(result.getX() == 7);
test_(result.getY() == -3);
test_(result.getZ() == 8);
}
// Test operator-()
void testOperatorSubstraction() {
Vector3D vector3(4, -5, 2);
Vector3D vector4(3, 2, 6);
// Compute the substraction
Vector3D result = vector3 - vector4;
// Test the result
test_(result.getX() == 1);
test_(result.getY() == -7);
test_(result.getZ() == -4);
}
// Test operator*() (with a constant number)
void testOperatorConstantMultiplications() {
Vector3D vector3(4, -5, 2);
// Compute the multiplication
Vector3D result = vector3 * 5;
// Test the result
test_(result.getX() == 20);
test_(result.getY() == -25);
test_(result.getZ() == 10);
}
// Test operator=()
void testOperatorAssignment() {
Vector3D vector3(4, -5, 2);
// Assignment
Vector3D result;
result = vector3;
// Test the result
test_(result.getX() == 4);
test_(result.getY() == -5);
test_(result.getZ() == 2);
}
// Test operator==()
void testOperatorEquality() {
Vector3D vector3(4, -5, 2);
Vector3D vector4(4, -5, 2);
Vector3D vector5(3, -5, -2);
// Test the equality
test_(vector3 == vector4);
test_(vector4 == vector3);
test_(vector3 == vector3);
test_(!(vector3 == vector5));
test_(!(vector5 == vector3));
}
};
#endif

450
src/reactphysics3d/testing/testing_mathematics/VectorTest.h Normal file
View File

@ -0,0 +1,450 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef VECTORTEST_H
#define VECTORTEST_H
// Libraries
#include "../TestSuite/Test.h"
#include "../../mathematics/Vector.h"
#include <stdexcept>
// Namespaces
using namespace reactphysics3d;
// Class VectorTest
class VectorTest : public TestSuite::Test {
private :
Vector vect2; // Vector of dimension 2
Vector vect4; // Vector of dimension 4
public :
// Constructor
VectorTest() : vect2(2), vect4(4) {
vect2.setValue(0, 3);
vect2.setValue(1, 5);
vect4.setValue(0, 1);
vect4.setValue(1, -5);
vect4.setValue(2, 10);
vect4.setValue(3, 3);
}
// Run method of the Test
void run() {
testConstructors();
testGetValue();
testSetValue();
testGetNbComponent();
testLength();
testGetUnit();
testScalarProduct();
testCrossProduct();
testOperatorAddition();
testOperatorSubstraction();
testOperatorConstantMultiplications();
testOperatorAssignment();
}
// Test the constructors
void testConstructors() {
// Try a valid constructor call
try {
// Constructor
Vector vector(3); // This shouldn't throw an exception
test_(vector.getValue(0) == 0);
test_(vector.getValue(1) == 0);
test_(vector.getValue(2) == 0);
succeed_(); // Test succeed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
fail_("Valid constructor call throws an exception"); // Test failed if an exception has been thrown
}
// Try a invalid constructor call
try {
// Constructor
Vector vector3(-1); // This should throw an exception
fail_("Invalid constructor (argument -1) call undetected"); // Test failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // Test succeed if an exception has been thrown
}
// Try a invalid constructor call
try {
// Constructor
Vector vector4(0); // This should throw an exception
fail_("Invalid constructor call (argument 0) undetected"); // Test failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // Test succeed if an exception has been thrown
}
// Copy-Constructor
Vector vector5 = vect2;
test_(vector5.getValue(0) == 3);
test_(vector5.getValue(1) == 5);
}
// Test getValue()
void testGetValue() {
test_(vect2.getValue(0) == 3);
test_(vect2.getValue(1) == 5);
// Try to get an invalid value
try {
vect2.getValue(-1); // This should throw an exception
fail_("Invalid getValue(-1) undetected"); // The test failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // The test succeed if an exception has been thrown
}
// Try to get an invalid value
try {
vect2.getValue(2); // This should throw an exception
fail_("Invalid getValue(2) undetected"); // The test failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // The test succeed if an exception has been thrown
}
}
// Test setValue()
void testSetValue() {
Vector vector(5);
// Try to set valid values
try {
vector.setValue(0, 5);
vector.setValue(3, -1);
vector.setValue(4, 14);
test_(vector.getValue(0) == 5);
test_(vector.getValue(3) == -1);
test_(vector.getValue(4) == 14);
succeed_(); // The test succeed if an exception have been thrown
}
catch(std::invalid_argument& ex) {
fail_("Valid setValue() throws an exception"); // The failed if an exception has been thrown
}
// Try to set an invalid value
try {
vector.setValue(-1, 4); // This should throw an exception
fail_("Invalid setValue(-1,4) undetected"); // The test failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // The test succeed if an exception has been thrown
}
// Try to set an invalid value
try {
vector.setValue(5, 2); // This should throw an exception
fail_("Invalid setValue(5,2) undetected"); // The test failed if no exception have been thrown
}
catch(std::invalid_argument& ex) {
succeed_(); // The test succeed if an exception has been thrown
}
}
// Test getNbComponent()
void testGetNbComponent() {
test_(vect2.getNbComponent() == 2);
test_(vect4.getNbComponent() == 4);
}
// Test length()
void testLength() {
Vector vector1(2);
test_(vector1.length() == 0);
vector1.setValue(0, 4);
test_(vector1.length() == 4);
vector1.setValue(1, -3);
test_(vector1.length() == 5);
}
// Test getUnit()
void testGetUnit() {
Vector vector1(3);
vector1.setValue(0, 3);
test_(vector1.getUnit().length() == 1);
test_(vector1.getUnit().getValue(0) == 1);
test_(vector1.getUnit().getValue(1) == 0);
test_(vector1.getUnit().getValue(2) == 0);
Vector vector2(8);
vector2.setValue(2, 54);
test_(vector2.getUnit().length() == 1);
test_(vector2.getUnit().getValue(0) == 0);
test_(vector2.getUnit().getValue(1) == 0);
test_(vector2.getUnit().getValue(2) == 1);
Vector vector3(7); // Vector of length equal to zero
try {
vector3.getUnit(); // This should throw an exception
fail_("getUnit() with a vector of length equals to zero undetected"); // The test failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // The test succeed if an exception has been thrown
}
}
// Test scalarProduct()
void testScalarProduct() {
Vector vector1(2);
vector1.setValue(0, 4);
vector1.setValue(1, -5);
Vector vector2(2);
vector2.setValue(0, 3);
vector2.setValue(1, 2);
// Try to compute a valid scalar product
try {
test_(vector1.scalarProduct(vector2) == 2);
succeed_(); // The test succeed if no exception have been thrown
}
catch(MathematicsException& ex) {
fail_("scalarProduct() thrown an exception during a valid scalar product computation"); // The test failed if an exception has been thrown
}
// Try to compute a invalid scalar product
Vector vector3(5);
try {
vector1.scalarProduct(vector3); // This should throw an exception
fail_("Invalid dimensions in scalarProduct() undetected"); // The test failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // The test succeed if an exception has been thrown
}
}
// Test crossProduct()
void testCrossProduct() {
Vector vector1(3);
vector1.setValue(0, 4);
vector1.setValue(1, -5);
vector1.setValue(2, 2);
Vector vector2(3);
vector2.setValue(0, 3);
vector2.setValue(1, 2);
vector2.setValue(2, 6);
// Try to compute a valid scalar product
try {
Vector result = vector1.crossProduct(vector2);
test_(result.getValue(0) == -34);
test_(result.getValue(1) == -18);
test_(result.getValue(2) == 23);
succeed_(); // The test succeed if no exception have been thrown
}
catch(MathematicsException& ex) {
fail_("crossProduct() thrown an exception during a valid cross product computation"); // The test failed if an exception has been thrown
}
// Try to compute a invalid cross product
Vector vector3(5);
try {
vector1.crossProduct(vector3); // This should throw an exception
fail_("Invalid dimensions in crossProduct() undetected"); // The test failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // The test succeed if an exception has been thrown
}
}
// Test operator+()
void testOperatorAddition() {
Vector vector1(4);
vector1.setValue(0, 3);
vector1.setValue(3, -2);
Vector vector2(4);
vector2.setValue(0, 9);
vector2.setValue(2, 6);
vector2.setValue(3, 9);
// Try to compute a valid sum (two vectors with the same dimensions)
try {
// Compute the sum
Vector sum = vector1 + vector2; // This shouldn't throw an exception
test_(sum.getValue(0) == 12);
test_(sum.getValue(1) == 0);
test_(sum.getValue(2) == 6);
test_(sum.getValue(3) == 7);
succeed_(); // The test succeed if no exception have been thrown
}
catch(MathematicsException& ex) {
fail_("A valid sum of two vectors throws an excception"); // The test failed if an exception has been thrown
}
// Try to compute an invalid sum (vectors with different dimensions)
Vector vector3(3);
Vector vector4(5);
try {
// Compute the sum
Vector sum = vector3 + vector4; // This should throw an exception
fail_("An invalid sum of two vectors undetected"); // The test failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // The test succeed if an exception has been thrown
}
}
// Test operator-()
void testOperatorSubstraction() {
Vector vector1(4);
vector1.setValue(0, 3);
vector1.setValue(3, -2);
Vector vector2(4);
vector2.setValue(0, 9);
vector2.setValue(2, 6);
vector2.setValue(3, 9);
// Try to compute a valid subtraction (two vectors with the same dimensions)
try {
// Compute the substraction
Vector sub = vector1 - vector2; // This shouldn't throw an exception
test_(sub.getValue(0) == -6);
test_(sub.getValue(1) == 0);
test_(sub.getValue(2) == -6);
test_(sub.getValue(3) == -11);
succeed_(); // The test succeed if no exception have been thrown
}
catch(MathematicsException& ex) {
fail_("A valid subtraction of two vectors throws an excception"); // The test failed if an exception has been thrown
}
// Try to compute an invalid substraction (vectors with different dimensions)
Vector vector3(3);
Vector vector4(5);
try {
// Compute the substraction
Vector sub = vector3 - vector4; // This should throw an exception
fail_("An invalid substraction of two vectors undetected"); // The test failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // The test succeed if an exception has been thrown
}
}
// Test operator*() (with a constant number)
void testOperatorConstantMultiplications() {
Vector vector(4);
vector.setValue(0, 3);
vector.setValue(3, -2);
// Compute the multiplication
Vector sum = vector * 3.0;
test_(sum.getValue(0) == 9);
test_(sum.getValue(1) == 0);
test_(sum.getValue(2) == 0);
test_(sum.getValue(3) == -6);
}
// Test operator=()
void testOperatorAssignment() {
Vector vector1(2);
vector1.setValue(0, 4);
vector1.setValue(1, 7);
Vector vector2(8);
// Try to compute a valid assignment (two vectors with the same dimensions)
try {
Vector vector(2);
vector = vector1; // This shouldn't throw an exception
test_(vector == vector1);
vector = vector; // This shouldn't throw an exception
succeed_(); // The test succeed if no exception have been thrown
}
catch(MathematicsException& ex) {
fail_("A valid vector assignment throws an excception"); // The test failed if an exception has been thrown
}
// Try to compute an invalid assignment (vectors with different dimensions)
try {
Vector vector3(2);
vector3 = vector2; // This should throw an exception
fail_("An invalid vector assignment undetected"); // The test failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // The test succeed if an exception has been thrown
}
// Try to compute an invalid assignment (vectors with different dimensions)
try {
Vector vector3(2);
vector2 = vector3; // This should throw an exception
fail_("An invalid vector assignment undetected"); // The test failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // The test succeed if an exception has been thrown
}
}
// Test operator==()
void testOperatorEquality() {
Vector vector1(2);
vector1.setValue(0, 4);
vector1.setValue(1, 7);
Vector vector2(2);
vector2.setValue(0, 4);
vector2.setValue(1, 7);
Vector vector3(2);
vector3.setValue(0, 5);
vector3.setValue(1, 7);
Vector vector4(8);
// Try to test a valid equality (two vectors with the same dimensions)
try {
test_(vector1 == vector2); // This shouldn't throw an exception
test_(vector2 == vector1); // This shouldn't throw an exception
test_(vector1 == vector1); // This shouldn't throw an exception
test_(!(vector1 == vector3)); // This shouldn't throw an exception
test_(!(vector3 == vector1)); // This shouldn't throw an exception
succeed_(); // The test succeed if no exception have been thrown
}
catch(MathematicsException& ex) {
fail_("A valid vector equality test throws an excception"); // The test failed if an exception has been thrown
}
// Try to test an invalid equality (vectors with different dimensions)
try {
vector4 == vector1; // This should throw an exception
fail_("An invalid equality test of two vectors undetected"); // The test failed if no exception have been thrown
}
catch(MathematicsException& ex) {
succeed_(); // The test succeed if an exception has been thrown
}
}
};
#endif

130
src/reactphysics3d/testing/testing_physics/KilogramTest.h Executable file
View File

@ -0,0 +1,130 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef KILOGRAMTEST_H
#define KILOGRAMTEST_H
// Libraries
#include "../TestSuite/Test.h"
#include "../../physics/Kilogram.h"
#include <stdexcept>
// Namespaces
using namespace reactphysics3d;
// Class KilogramTest
class KilogramTest : public TestSuite::Test {
private :
Kilogram mass1;
Kilogram mass2;
public :
// Constructor
KilogramTest() : mass1(5.0), mass2(10.0) {
}
// Run method of the Test
void run() {
testConstructors();
testGetValue();
testSetValue();
}
// Test the constructors
void testConstructors() {
// Try valid constructors calls
try {
Kilogram mass(30.4); // This should'n throw an exception
Kilogram mass3(mass); // This should'n throw an exception
Kilogram mass4(0.0); // This should'n throw an exception
Kilogram mass5;
test_(mass.getValue() == 30.4);
test_(mass3.getValue() == 30.4);
test_(mass4.getValue() == 0.0);
test_(mass5.getValue() == 0.0);
succeed_();
}
catch(std::invalid_argument& ex) {
fail_("Valid constructor call throws an exception");
}
// Try an invalid constructor call
try{
Kilogram mass4(-0.1); // This should throw an exception
fail_("Invalid constructors calls didn't throw an exception");
}
catch(std::invalid_argument& ex) {
succeed_();
}
// Try an invalid constructor call
try{
Kilogram mass4(-10.); // This should throw an exception
fail_("Invalid constructors calls didn't throw an exception");
}
catch(std::invalid_argument& ex) {
succeed_();
}
}
// Test the method getValue()
void testGetValue() {
test_(mass1.getValue() == 5.0);
test_(mass2.getValue() == 10.0);
}
// Test the method setValue()
void testSetValue() {
Kilogram mass(10.0);
// Try a valid setValue() call
try {
mass.setValue(0.0); // This should'n throw an exception
test_(mass.getValue() == 0.0);
mass.setValue(43.0); // This should'n throw an exception
test_(mass.getValue() == 43.0);
succeed_();
}
catch(std::invalid_argument& ex) {
fail_("Valid setValue() call throw an exception");
}
// Try an invalid setValue() call
try {
mass.setValue(-0.1); // This should throw an exception
fail_("Invalid setValue() call didn't throw an exception");
}
catch(std::invalid_argument& ex) {
succeed_();
}
// Try an invalid setValue() call
try {
mass.setValue(-40.0); // This should throw an exception
fail_("Invalid setValue() call didn't throw an exception");
}
catch(std::invalid_argument& ex) {
succeed_();
}
}
};
#endif

189
src/reactphysics3d/testing/testing_physics/TimeTest.h Executable file
View File

@ -0,0 +1,189 @@
/****************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef TIMETEST_H
#define TIMETEST_H
// Libraries
#include "../TestSuite/Test.h"
#include "../../physics/Time.h"
#include <stdexcept>
// Namespaces
using namespace reactphysics3d;
// Class TimeTest
class TimeTest : public TestSuite::Test {
private :
Time time1;
Time time2;
public :
// Constructor
TimeTest() : time1(5.0), time2(10.0) {
}
// Run method of the Test
void run() {
testConstructors();
testGetValue();
testSetValue();
testOperatorAddition();
testOperatorSubstraction();
testOperatorMultiplication();
}
// Test the constructors
void testConstructors() {
// Try valid constructors calls
try {
Time time(30.4); // This should'n throw an exception
Time time3(time); // This should'n throw an exception
Time time4(0.0); // This should'n throw an exception
Time time5;
test_(time.getValue() == 30.4);
test_(time3.getValue() == 30.4);
test_(time4.getValue() == 0.0);
test_(time5.getValue() == 0.0);
succeed_();
}
catch(std::invalid_argument& ex) {
fail_("Valid constructor call throws an exception");
}
// Try an invalid constructor call
try{
Time time4(-0.1); // This should throw an exception
fail_("Invalid constructors calls didn't throw an exception");
}
catch(std::invalid_argument& ex) {
succeed_();
}
// Try an invalid constructor call
try{
Time time4(-10.); // This should throw an exception
fail_("Invalid constructors calls didn't throw an exception");
}
catch(std::invalid_argument& ex) {
succeed_();
}
}
// Test the method getValue()
void testGetValue() {
test_(time1.getValue() == 5.0);
test_(time2.getValue() == 10.0);
}
// Test the method setValue()
void testSetValue() {
Time time(10.0);
// Try a valid setValue() call
try {
time.setValue(0.0); // This should'n throw an exception
test_(time.getValue() == 0.0);
time.setValue(43.0); // This should'n throw an exception
test_(time.getValue() == 43.0);
succeed_();
}
catch(std::invalid_argument& ex) {
fail_("Valid setValue() call throw an exception");
}
// Try an invalid setValue() call
try {
time.setValue(-0.1); // This should throw an exception
fail_("Invalid setValue() call didn't throw an exception");
}
catch(std::invalid_argument& ex) {
succeed_();
}
// Try an invalid setValue() call
try {
time.setValue(-40.0); // This should throw an exception
fail_("Invalid setValue() call didn't throw an exception");
}
catch(std::invalid_argument& ex) {
succeed_();
}
}
// Test the overloaded addition operator
void testOperatorAddition() {
Time result;
result = time1 + time2;
test_(result.getValue() == 15.0);
}
// Test the overloaded substraction operator
void testOperatorSubstraction() {
Time result;
// Try a valid substraction
try {
result = time2 - time1; // This should'n throw an exception
test_(result.getValue() == 5.0);
succeed_();
}
catch(std::invalid_argument& ex) {
fail_("Valid call to substraction operator throw an exception");
}
// try an invalid substraction
try {
result = time1 - time2;
fail_("Invalid call to substraction didn't throw an exception");
}
catch(std::invalid_argument& ex) {
succeed_();
}
}
// Test the overloaded multiplication operator
void testOperatorMultiplication() {
Time result;
// Try a valid substraction
try {
result = time1 * 3.0; // This should'n throw an exception
test_(result.getValue() == 15.0);
succeed_();
}
catch(std::invalid_argument& ex) {
fail_("Valid call to multiplication operator throw an exception");
}
// try an invalid substraction
try {
result = time1 * (-3.0);
fail_("Invalid call to multiplication didn't throw an exception");
}
catch(std::invalid_argument& ex) {
succeed_();
}
}
};
#endif

32
src/reactphysics3d/typeDefinitions.h Normal file
View File

@ -0,0 +1,32 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis *
*********************************************************************************
* *
* Permission is hereby granted, free of charge, to any person obtaining a copy *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell *
* copies of the Software, and to permit persons to whom the Software is *
* furnished to do so, subject to the following conditions: *
* *
* The above copyright notice and this permission notice shall be included in *
* all copies or substantial portions of the Software. *
* *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN *
* THE SOFTWARE. *
********************************************************************************/
#ifndef TYPEDEFINITIONS_H
#define TYPEDEFINITIONS_H
// Type definitions
typedef unsigned int uint;
#endif

27
src/typeDefinitions.h Normal file
View File

@ -0,0 +1,27 @@
/***************************************************************************
* Copyright (C) 2009 Daniel Chappuis *
****************************************************************************
* This file is part of ReactPhysics3D. *
* *
* ReactPhysics3D is free software: you can redistribute it and/or modify *
* it under the terms of the GNU Lesser General Public License as published *
* by the Free Software Foundation, either version 3 of the License, or *
* (at your option) any later version. *
* *
* ReactPhysics3D is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU Lesser General Public License for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#ifndef TYPEDEFINITIONS_H
#define TYPEDEFINITIONS_H
// Type definitions
typedef unsigned int uint;
#endif