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

Several optimizations and cosmetic modifications

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@408 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2010-09-16 20:56:09 +00:00
parent 7762d3daca
commit c57651a789
28 changed files with 240 additions and 260 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/body/AABB.cpp
View File

@ -24,9 +24,11 @@
// 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>
#ifdef VISUAL_DEBUG
#include <GL/freeglut.h> // TODO : Remove this in the final version
#include <GL/gl.h> // TODO : Remove this in the final version
#endif
using namespace reactphysics3d;
using namespace std;
@ -49,6 +51,7 @@ AABB::~AABB() {
}
#ifdef VISUAL_DEBUG
// Draw the OBB (only for testing purpose)
void AABB::draw() const {
@ -104,11 +107,12 @@ void AABB::draw() const {
glEnd();
}
#endif
// Static method that computes an AABB from a set of vertices. The "center" argument corresponds to the center of the AABB
// This method allocates a new AABB object and return a pointer to the new allocated AABB object
AABB* AABB::computeFromVertices(const vector<Vector3D>& vertices, const Vector3D& center) {
// TODO : Implement this method;
return 0;
return NULL;
}

4
src/body/AABB.h
View File

@ -59,7 +59,9 @@ class AABB : public BroadBoundingVolume {
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)
#ifdef VISUAL_DEBUG
virtual void draw() const; // Draw the AABB (only for testing purpose)
#endif
static AABB* computeFromVertices(const std::vector<Vector3D>& vertices, const Vector3D& center); // Compute an AABB from a set of vertices
};

2
src/body/BoundingVolume.cpp
View File

@ -30,7 +30,7 @@ using namespace reactphysics3d;
// Constructor
BoundingVolume::BoundingVolume() {
this->body = 0;
this->body = NULL;
}
// Destructor

6
src/body/BoundingVolume.h
View File

@ -53,12 +53,14 @@ class BoundingVolume {
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)
#ifdef VISUAL_DEBUG
virtual void draw() const=0; // Display the bounding volume (only for testing purpose)
#endif
};
// Return the body pointer
inline Body* BoundingVolume::getBodyPointer() const {
assert(body != 0);
assert(body != NULL);
return body;
}

11
src/body/OBB.cpp
View File

@ -25,10 +25,13 @@
// 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>
#ifdef VISUAL_DEBUG
#include <GL/freeglut.h> // TODO : Remove this in the final version
#include <GL/gl.h> // TODO : Remove this in the final version
#endif
using namespace reactphysics3d;
using namespace std;
@ -55,6 +58,7 @@ OBB::~OBB() {
}
#ifdef VISUAL_DEBUG
// Draw the OBB (only for testing purpose)
void OBB::draw() const {
double e0 = extent[0];
@ -116,6 +120,7 @@ void OBB::draw() const {
glEnd();
}
#endif
// Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis.
// If the extreme vertices are part of a face of the OBB, the returned vertices will be ordered according to the face.
@ -185,7 +190,7 @@ vector<Vector3D> OBB::getExtremeVertices(const Vector3D& directionAxis) const {
// 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 NULL;
}
// Return the corresponding AABB

4
src/body/OBB.h
View File

@ -66,7 +66,9 @@ class OBB : public NarrowBoundingVolume {
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)
#ifdef VISUAL_DEBUG
virtual void draw() const; // Draw the OBB (only for testing purpose)
#endif
static OBB* computeFromVertices(const std::vector<Vector3D>& vertices, const Vector3D& center); // Compute an OBB from a set of vertices
};

6
src/collision/CollisionDetection.cpp
View File

@ -87,11 +87,11 @@ void CollisionDetection::computeNarrowPhase() {
// For each possible collision pair of bodies
for (unsigned int i=0; i<possibleCollisionPairs.size(); i++) {
ContactInfo* contactInfo = 0;
ContactInfo* contactInfo = NULL;
// 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);
assert(contactInfo);
// Add the contact info the current list of collision informations
contactInfos.push_back(contactInfo);
@ -104,7 +104,7 @@ 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);
assert(contactInfo);
// Compute one or several new contacts and add them into the physics world
computeContact(contactInfo);

2
src/collision/CollisionDetection.h
View File

@ -56,7 +56,7 @@ class CollisionDetection {
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
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

8
src/collision/SAPAlgorithm.cpp
View File

@ -49,7 +49,7 @@ void SAPAlgorithm::removeBodiesAABB(vector<Body*> bodies) {
const AABB* aabb;
// Removed the AABB of the bodies that have been removed
for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); it++) {
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);
@ -62,7 +62,7 @@ void SAPAlgorithm::removeBodiesAABB(vector<Body*> bodies) {
void SAPAlgorithm::addBodiesAABB(vector<Body*> bodies) {
const AABB* aabb;
for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); it++) {
for (vector<Body*>::iterator it = bodies.begin(); it != bodies.end(); ++it) {
aabb = 0;
aabb = dynamic_cast<const AABB*>((*it)->getBroadBoundingVolume());
assert(aabb);
@ -100,7 +100,7 @@ void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vect
sort(sortedAABBs.begin(), sortedAABBs.end(), compareAABBs);
// Sweep the sorted set of AABBs
for (vector<const AABB*>::iterator it = sortedAABBs.begin(); it != sortedAABBs.end(); it++) {
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()) {
@ -121,7 +121,7 @@ void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> addedBodies, vect
}
// Test collision against all possible overlapping AABBs following the current one
for (it2 = it + 1; it2 < sortedAABBs.end(); it2++) {
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;

3
src/constants.h
View File

@ -37,6 +37,9 @@ const double ONE_MINUS_EPSILON = 0.99999; // 1
const double INFINITY_CONST = std::numeric_limits<double>::infinity(); // Infinity constant
const double PI = 3.14159265; // Pi constant
// Physics Engine constants
const double DEFAULT_TIMESTEP = 0.002;
// Contact constants
const double FRICTION_COEFFICIENT = 0.4; // Friction coefficient
const double PENETRATION_FACTOR = 0.2; // Penetration factor (between 0 and 1) which specify the importance of the

4
src/constraint/Constraint.h
View File

@ -45,7 +45,7 @@ class Constraint {
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 nbConstraints; // Number mathematical constraints associated with this Constraint
uint nbConstraints; // Number mathematical constraints associated with this Constraint
public :
Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active); // Constructor // Constructor
@ -53,7 +53,7 @@ class Constraint {
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 noConstraint, Matrix1x6**& J_sp) const=0; // Compute the jacobian matrix for all mathematical constraints
virtual void computeJacobian(int noConstraint, Matrix1x6**& J_sp) const=0; // Compute the jacobian matrix for all mathematical constraints
virtual void computeLowerBound(int noConstraint, Vector& lowerBounds) const=0; // Compute the lowerbounds values for all the mathematical constraints
virtual void computeUpperBound(int noConstraint, Vector& upperBounds) const=0; // Compute the upperbounds values for all the mathematical constraints
virtual void computeErrorValue(int noConstraint, Vector& errorValues) const=0; // Compute the error values for all the mathematical constraints

4
src/constraint/Contact.cpp
View File

@ -66,8 +66,8 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
Vector3D body2Position = rigidBody2->getPosition();
int currentIndex = noConstraint; // Current constraint index
assert(rigidBody1 != 0);
assert(rigidBody2 != 0);
assert(rigidBody1);
assert(rigidBody2);
// For each point in the contact
for (int i=0; i<nbPoints; i++) {

14
src/constraint/Contact.h
View File

@ -30,8 +30,10 @@
#include "../body/RigidBody.h"
#include "../constants.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
#ifdef VISUAL_DEBUG
#include <GL/freeglut.h>
#include <GL/gl.h>
#endif
// ReactPhysics3D namespace
namespace reactphysics3d {
@ -64,12 +66,14 @@ class Contact : public Constraint {
Vector3D getNormal() const; // Return the normal vector of the contact
Vector3D getPoint(int index) const; // Return a contact point
int getNbPoints() const; // Return the number of contact points
virtual void computeJacobian(int noConstraint, Matrix1x6**& J_SP) const; // Compute the jacobian matrix for all mathematical constraints
virtual void computeJacobian(int noConstraint, Matrix1x6**& J_SP) const; // Compute the jacobian matrix for all mathematical constraints
virtual void computeLowerBound(int noConstraint, Vector& lowerBounds) const; // Compute the lowerbounds values for all the mathematical constraints
virtual void computeUpperBound(int noConstraint, Vector& upperBounds) const; // Compute the upperbounds values for all the mathematical constraints
virtual void computeErrorValue(int noConstraint, Vector& errorValues) const; // Compute the error values for all the mathematical constraints
double getPenetrationDepth() const; // Return the penetration depth
void draw() const; // TODO : Delete this (Used to debug collision detection)
#ifdef VISUAL_DEBUG
void draw() const; // Draw the contact (for debugging)
#endif
};
// Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane
@ -107,11 +111,13 @@ inline double Contact::getPenetrationDepth() const {
return penetrationDepth;
}
#ifdef VISUAL_DEBUG
// 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);
}
#endif
} // End of the ReactPhysics3D namespace

10
src/engine/ConstraintSolver.cpp
View File

@ -30,6 +30,8 @@
using namespace reactphysics3d;
using namespace std;
// TODO : Maybe we could use std::vector instead of Vector to store vector in the constraint solver
// Constructor
ConstraintSolver::ConstraintSolver(PhysicsWorld* world)
:physicsWorld(world), bodyMapping(0), nbConstraints(0), constraintsCapacity(0),
@ -52,7 +54,7 @@ void ConstraintSolver::initialize() {
// For each constraint
vector<Constraint*>::iterator it;
for (it = physicsWorld->getConstraintsBeginIterator(); it != physicsWorld->getConstraintsEndIterator(); it++) {
for (it = physicsWorld->getConstraintsBeginIterator(); it != physicsWorld->getConstraintsEndIterator(); ++it) {
constraint = *it;
// If the constraint is active
@ -211,7 +213,7 @@ void ConstraintSolver::fillInMatrices() {
// Set the init lambda values
contact = dynamic_cast<Contact*>(constraint);
contactInfo = 0;
contactInfo = NULL;
if (contact) {
// Get the lambda init value from the cache if exists
contactInfo = contactCache.getContactCachingInfo(contact);
@ -232,13 +234,13 @@ void ConstraintSolver::fillInMatrices() {
RigidBody* rigidBody;
Body* body;
uint b=0;
for (set<Body*>::iterator it = constraintBodies.begin(); it != constraintBodies.end(); it++, b++) {
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);
assert(rigidBody);
// Compute the vector V1 with initial velocities values
V1[bodyNumber].setValue(0, rigidBody->getLinearVelocity().getValue(0));

12
src/engine/ConstraintSolver.h
View File

@ -65,10 +65,10 @@ class ConstraintSolver {
// 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
Matrix1x6** J_sp; // 2-dimensional array thar correspond to the sparse representation of the jacobian matrix of all constraints
Matrix1x6** 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
Vector6D** B_sp; // 2-dimensional array that correspond to a useful matrix in sparse representation
Vector6D** 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
@ -77,12 +77,12 @@ class ConstraintSolver {
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
Matrix6x6* Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body
Matrix6x6* 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
Vector6D* V1; // Array that contains for each body the Vector that contains linear and angular velocities
Vector6D* 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
Vector6D* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities
Vector6D* Fext; // Array that contains for each body the vector that contains external forces and torques
Vector6D* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities
Vector6D* 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

6
src/engine/ContactCache.cpp
View File

@ -68,7 +68,7 @@ ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const
// If the new contact and the contact caching info doesn't have the same number of contact points
if (contact->getNbPoints() != contactInfo->positions.size()) {
// We return NULL because, the contact doesn't match
return 0;
return NULL;
}
for (int i=0; i<contactInfo->positions.size(); i++) {
@ -87,7 +87,7 @@ ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const
posZ > contactPos.getZ() + POSITION_TOLERANCE || posZ < contactPos.getZ() - POSITION_TOLERANCE) {
// Return NULL
return 0;
return NULL;
}
}
@ -95,7 +95,7 @@ ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const
return contactInfo;
}
else {
return 0;
return NULL;
}
}

2
src/engine/ContactCache.h
View File

@ -54,7 +54,7 @@ class ContactCache {
ContactCache(); // Constructor
~ContactCache(); // Destructor
void clear(); // Remove all the contact caching info of the cache
ContactCachingInfo* getContactCachingInfo(Contact* contact) const;
ContactCachingInfo* getContactCachingInfo(Contact* contact) const; // Return the ContactCachingInfo corresponding to a contact if it exists
void addContactCachingInfo(ContactCachingInfo* contactCachingInfo); // Add a new contact caching info in the cache
};

8
src/engine/PhysicsEngine.cpp
View File

@ -30,7 +30,7 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
PhysicsEngine::PhysicsEngine(PhysicsWorld* world, double timeStep) throw (invalid_argument)
PhysicsEngine::PhysicsEngine(PhysicsWorld* world, double timeStep = DEFAULT_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) {
@ -114,7 +114,7 @@ void PhysicsEngine::updateAllBodiesMotion() {
Vector3D newAngularVelocity;
// For each body of thephysics world
for (vector<Body*>::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) {
for (vector<Body*>::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) {
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
assert(rigidBody);
@ -180,7 +180,7 @@ void PhysicsEngine::setInterpolationFactorToAllBodies() {
assert(factor >= 0.0 && factor <= 1.0);
// Set the factor to all bodies
for (vector<Body*>::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) {
for (vector<Body*>::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) {
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
assert(rigidBody);
@ -193,7 +193,7 @@ void PhysicsEngine::setInterpolationFactorToAllBodies() {
void PhysicsEngine::applyGravity() {
// For each body of the physics world
for (vector<Body*>::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); it++) {
for (vector<Body*>::iterator it=world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) {
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
assert(rigidBody);

11
src/engine/PhysicsEngine.h
View File

@ -31,6 +31,7 @@
#include "ConstraintSolver.h"
#include "../body/RigidBody.h"
#include "Timer.h"
#include "../constants.h"
// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -54,12 +55,12 @@ class PhysicsEngine {
void applyGravity(); // Apply the gravity force to all bodies
public :
PhysicsEngine(PhysicsWorld* world, double timeStep) throw (std::invalid_argument); // Constructor
~PhysicsEngine(); // Destructor
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
void start(); // Start the physics simulation
void stop(); // Stop the physics simulation
void update() throw (std::logic_error); // Update the physics simulation
};
// --- Inline functions --- //

4
src/engine/PhysicsWorld.cpp
View File

@ -51,7 +51,7 @@ void PhysicsWorld::removeAllContactConstraints() {
Contact* contact = dynamic_cast<Contact*>(*it);
// If the constraint is a contact
if (contact != 0) {
if (contact) {
// Delete the contact
delete (*it);
it = constraints.erase(it);
@ -64,7 +64,7 @@ void PhysicsWorld::removeAllContactConstraints() {
// 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++) {
for (vector<Constraint*>::iterator it = constraints.begin(); it != constraints.end(); ++it) {
delete *it;
}
constraints.clear();

70
src/mathematics/Matrix3x3.cpp
View File

@ -35,79 +35,39 @@ Matrix3x3::Matrix3x3() {
setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
}
// Constructor
Matrix3x3::Matrix3x3(double value) {
setAllValues(value, value, value, value, value, value, value, value, value);
}
// 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) {
Matrix3x3 Matrix3x3::getInverse() const {
// 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;
assert(determinant != 0.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]));
// 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);
// Return the inverse matrix
return (invDeterminant * tempMatrix.getTranspose());
}
// Overloaded operator for multiplication with a matrix

85
src/mathematics/Matrix3x3.h
View File

@ -27,7 +27,7 @@
#define MATRIX3X3_H
// Libraries
#include "exceptions.h"
#include <cassert>
#include "Vector3D.h"
// ReactPhysics3D namespace
@ -44,20 +44,20 @@ class Matrix3x3 {
double array[3][3]; // Array with the values of the matrix
public :
Matrix3x3(); // Constructor of the class Matrix3x3
Matrix3x3(); // Constructor
Matrix3x3(double value); // Constructor
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
double c1, double c2, double c3); // 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
double getValue(int i, int j) const; // Get a value in the matrix
void setValue(int i, int j, double value); // 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
Matrix3x3 getInverse() const; // Return the inverse matrix
static Matrix3x3 identity(); // Return the 3x3 identity matrix
// --- Overloaded operators --- //
@ -72,46 +72,23 @@ class Matrix3x3 {
// 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");
}
inline double Matrix3x3::getValue(int i, int j) const {
assert(i>=0 && i<3 && j>=0 && j<3);
return array[i][j];
}
// 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");
}
inline void Matrix3x3::setValue(int i, int j, double value) {
assert(i>=0 && i<3 && j>=0 && j<3);
array[i][j] = value;
}
// 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;
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
@ -156,6 +133,36 @@ inline bool Matrix3x3::operator==(const Matrix3x3& matrix2) const {
array[2][0] == matrix2.array[2][0] && array[2][1] == matrix2.array[2][1] && array[2][2] == matrix2.array[2][2]);
}
// Return the 3x3 identity matrix
inline 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
inline 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
inline 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
inline 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);
}
} // End of the ReactPhysics3D namespace
#endif

31
src/mathematics/constants.h
View File

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

46
src/mathematics/lcp/LCPProjectedGaussSeidel.cpp
View File

@ -1,27 +1,33 @@
/****************************************************************************
* 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/>. *
***************************************************************************/
/********************************************************************************
* 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 "LCPProjectedGaussSeidel.h"
#include <cmath>
using namespace reactphysics3d;
using namespace std;
// Constructor
LCPProjectedGaussSeidel::LCPProjectedGaussSeidel(uint maxIterations)
@ -37,7 +43,7 @@ LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() {
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
// This method outputs the result in the lambda vector
void LCPProjectedGaussSeidel::solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbConstraints,
uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
uint nbBodies, Body*** const bodyMapping, map<Body*, uint> bodyNumberMapping,
const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const {
lambda = lambdaInit;
@ -78,7 +84,7 @@ void LCPProjectedGaussSeidel::solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbCo
// 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,
Vector6D** B_sp, std::map<Body*, uint> bodyNumberMapping,
Vector6D** B_sp, map<Body*, uint> bodyNumberMapping,
Vector6D* const a, uint nbBodies) const {
uint i;
uint indexBody1, indexBody2;

41
src/mathematics/lcp/LCPProjectedGaussSeidel.h
View File

@ -1,21 +1,26 @@
/****************************************************************************
* 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/>. *
***************************************************************************/
/********************************************************************************
* 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 LCPPROJECTEDGAUSSSEIDEL_H
#define LCPPROJECTEDGAUSSSEIDEL_H

41
src/mathematics/lcp/LCPSolver.cpp
View File

@ -1,21 +1,26 @@
/****************************************************************************
* 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/>. *
***************************************************************************/
/********************************************************************************
* 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 "LCPSolver.h"

51
src/mathematics/lcp/LCPSolver.h
View File

@ -1,21 +1,26 @@
/****************************************************************************
* 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/>. *
***************************************************************************/
/********************************************************************************
* 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 LCPSOLVER_H
#define LCPSOLVER_H
@ -58,13 +63,13 @@ class LCPSolver {
Vector lambdaInit; // Initial value for lambda at the beginning of the algorithm
public:
LCPSolver(uint maxIterations); // Constructor
virtual ~LCPSolver(); // Destructor
LCPSolver(uint maxIterations); // Constructor
virtual ~LCPSolver(); // Destructor
virtual void solve(Matrix1x6** J_sp, Vector6D** 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
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

4
src/reactphysics3d.h
View File

@ -43,11 +43,7 @@
// 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