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

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@332 92aac97c-a6ce-11dd-a772-7fcde58d38e6

Browse Source
This commit is contained in:
chappuis.daniel 2010-06-11 21:23:47 +00:00
parent 6ff9026fd5
commit 9695ab5556
10 changed files with 109 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
sources/demo/Context.cpp
View File

@ -29,17 +29,19 @@ using namespace reactphysics3d;
// Constructor of the class Context
Context::Context() {
Cube* cube1 = new Cube(Vector3D(5.0,14.0, 4.0), Quaternion(1.0, 1.0, 0.0, 0.0), 4.0, Kilogram(3.0));
cube1->getRigidBody()->setLinearVelocity(Vector3D(0.0, -5.0, 0.0));
Cube* cube1 = new Cube(Vector3D(5.0, 13.0, 1), Quaternion(1.0, 1.0, 0.0, 0.0), 4.0, Kilogram(3.0));
Cube* cube2 = new Cube(Vector3D(5.0, 13.0, 9), Quaternion(0.5, 0.5, 0.5, 0.0), 4.0, Kilogram(3.0));
cube1->getRigidBody()->setLinearVelocity(Vector3D(0.0, 0.0, 0.5));
cube2->getRigidBody()->setLinearVelocity(Vector3D(0.0, 0.0, -0.5));
//Cube* cube2 = new Cube(Vector3D(0.0, 17, 8.0), Quaternion(0.0, 1.0, 0.0, 0.0), 3.0, Kilogram(2.0));
//Cube* cube3 = new Cube(Vector3D(4.0, 17, -2.0), Quaternion(0.0, 1.0, 0.0, 0.0), 2.0, Kilogram(11.0));
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(10.0));
//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(10.0));
addObject(cube1);
//addObject(cube2);
addObject(cube2);
//addObject(cube3);
addObject(plane1);
//addObject(plane1);
}

2
sources/demo/Simulation.cpp
View File

@ -27,7 +27,7 @@ using namespace reactphysics3d;
// Constructor of the class Simulation
Simulation::Simulation()
:world(new PhysicsWorld(Vector3D(0.0, -0.6, 0.0))), engine(world, Time(0.01)), scene(this->world) {
:world(new PhysicsWorld(Vector3D(0.0, 0.0, 0.0))), engine(world, Time(0.01)), scene(this->world) {
simRunning = false;
mouseButtonPressed = false;
nbFrame = 0;

10
sources/reactphysics3d/body/BodyState.h
View File

@ -70,7 +70,9 @@ class BodyState {
Vector3D getLinearVelocity() const; // Return the linear velocity
void setLinearVelocity(const Vector3D& linearVelocity); // TODO : Delete this
Vector3D getAngularVelocity() const; // Return the angular velocity
void setAngularVelocity(const Vector3D& angularVelocity);
Quaternion getSpin() const; // Return the spin of the body
void setSpin(const Quaternion& spin);
void setMassInverse(Kilogram massInverse); // Set the inverse of the mass
Matrix3x3 getInertiaTensorInverse() const; // Get the inverse of the inertia tensor
void setInertiaTensorInverse(const Matrix3x3& inertiaTensorInverse); // Set the inverse of the inertia tensor
@ -143,11 +145,19 @@ inline Vector3D BodyState::getAngularVelocity() const {
return angularVelocity;
}
inline void BodyState::setAngularVelocity(const Vector3D& angularVelocity) {
this->angularVelocity = angularVelocity;
}
// Return the spin of the body
inline Quaternion BodyState::getSpin() const {
return spin;
}
inline void BodyState::setSpin(const Quaternion& spin) {
this->spin = spin;
}
// Set the inverse of the mass
inline void BodyState::setMassInverse(Kilogram massInverse) {
this->massInverse = massInverse;

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

@ -69,8 +69,8 @@ bool CollisionDetection::computeCollisionDetection() {
void CollisionDetection::computeBroadPhase() {
// For each pair of bodies in the physics world
for(std::vector<Body*>::const_iterator it1 = world->getBodyListStartIterator(); it1 != world->getBodyListEndIterator(); ++it1) {
for(std::vector<Body*>::const_iterator it2 = it1; it2 != world->getBodyListEndIterator(); ++it2) {
for(std::vector<Body*>::const_iterator it1 = world->getBodiesBeginIterator(); it1 != world->getBodiesEndIterator(); ++it1) {
for(std::vector<Body*>::const_iterator it2 = it1; it2 != world->getBodiesEndIterator(); ++it2) {
// If both bodies are RigidBody and are different
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(*it1);
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(*it2);

4
sources/reactphysics3d/constraint/Contact.h
View File

@ -62,6 +62,7 @@ class Contact : public Constraint {
virtual void evaluate(); // Evaluate the constraint
uint getNbAuxConstraints() const; // Return the number of auxiliary constraints
double getPenetrationDepth() const; // TODO : Delete this
void draw() const; // TODO : Delete this (Used to debug collision detection)
};
@ -89,6 +90,9 @@ inline Vector3D Contact::getPoint() const {
return point;
}
inline double Contact::getPenetrationDepth() const {
return penetrationDepth;
}
} // End of the ReactPhysics3D namespace

58
sources/reactphysics3d/engine/ConstraintSolver.cpp
View File

@ -21,18 +21,19 @@
#include "ConstraintSolver.h"
#include "../mathematics/lcp/LCPProjectedGaussSeidel.h"
#include "../body/RigidBody.h"
#include "../integration/SemiImplicitEuler.h" // TODO : Delete this
using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver(PhysicsWorld* world)
:physicsWorld(world), bodyMapping(0), nbConstraints(0), lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
integrationAlgorithm = new SemiImplicitEuler();
}
// Destructor
ConstraintSolver::~ConstraintSolver() {
delete integrationAlgorithm;
}
// Allocate all the matrices needed to solve the LCP problem
@ -42,7 +43,7 @@ void ConstraintSolver::allocate() {
// For each constraint
std::vector<Constraint*>::iterator it;
for (it = physicsWorld->getConstraintsBeginIterator(); it <physicsWorld->getConstraintsEndIterator(); it++) {
for (it = physicsWorld->getConstraintsBeginIterator(); it != physicsWorld->getConstraintsEndIterator(); it++) {
constraint = *it;
// Evaluate the constraint
@ -288,5 +289,56 @@ void ConstraintSolver::solve(double dt) {
//std::cout << "Velocity Y after : " << V[bodyNumberMapping[constraintBodies.at(i)]].getValue(1) << std::endl;
}
for (std::vector<Body*>::iterator it = physicsWorld->getBodiesBeginIterator(); it != physicsWorld->getBodiesEndIterator(); it++) {
// If this is a not constrained body
if (bodyNumberMapping.find(*it) == bodyNumberMapping.end()) {
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
// The current body state of the body becomes the previous body state
rigidBody->updatePreviousBodyState();
// Integrate the current body state at time t to get the next state at time t + dt
integrationAlgorithm->integrate(rigidBody->getCurrentBodyState(), dt, dt);
// If the body state has changed, we have to update some informations in the rigid body
rigidBody->update();
}
else {
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
// If the gravity force is on
/*
if(physicsWorld->getIsGravityOn()) {
// Apply the current gravity force to the body
rigidBody->getCurrentBodyState().setExternalForce(physicsWorld->getGravity());
}
*/
// The current body state of the body becomes the previous body state
rigidBody->updatePreviousBodyState();
const Vector newLinVelocity = V[bodyNumberMapping[*it]].getSubVector(0, 3);
const Vector newAngVelocity = V[bodyNumberMapping[*it]].getSubVector(3, 3);
const Vector3D linVel(newLinVelocity.getValue(0), newLinVelocity.getValue(1), newLinVelocity.getValue(2));
const Vector3D angVel(newAngVelocity.getValue(0), newAngVelocity.getValue(1), newAngVelocity.getValue(2));
BodyState& bodyState = rigidBody->getCurrentBodyState();
rigidBody->getCurrentBodyState().setLinearVelocity(linVel);
rigidBody->getCurrentBodyState().setAngularVelocity(angVel);
// Normalize the orientation quaternion
rigidBody->getCurrentBodyState().setOrientation(rigidBody->getCurrentBodyState().getOrientation().getUnit());
// Compute the spin quaternion
const Vector3D angularVelocity = rigidBody->getCurrentBodyState().getAngularVelocity();
rigidBody->getCurrentBodyState().setSpin(Quaternion(angularVelocity.getX(), angularVelocity.getY(), angularVelocity.getZ(), 0) * rigidBody->getCurrentBodyState().getOrientation() * 0.5);
bodyState.setPosition(bodyState.getPosition() + bodyState.getLinearVelocity() * dt);
bodyState.setOrientation(bodyState.getOrientation() + bodyState.getSpin() * dt);
// If the body state has changed, we have to update some informations in the rigid body
rigidBody->update();
}
}
freeMemory();
}

2
sources/reactphysics3d/engine/ConstraintSolver.h
View File

@ -24,6 +24,7 @@
#include "../typeDefinitions.h"
#include "../constraint/Constraint.h"
#include "../mathematics/lcp/LCPSolver.h"
#include "../integration/IntegrationAlgorithm.h" // TODO : Delete this
#include "PhysicsWorld.h"
#include <map>
@ -41,6 +42,7 @@ const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when sol
*/
class ConstraintSolver {
protected:
IntegrationAlgorithm* integrationAlgorithm; // TODO : Delete this
PhysicsWorld* physicsWorld; // Reference to the physics world
LCPSolver* lcpSolver; // LCP Solver
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world

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

@ -54,7 +54,7 @@ void PhysicsEngine::updateDynamic() {
// While the time accumulator is not empty
while(timer.getAccumulator() >= timer.getTimeStep().getValue()) {
// For each body in the dynamic world
for(std::vector<Body*>::const_iterator it = world->getBodyListStartIterator(); it != world->getBodyListEndIterator(); ++it) {
for(std::vector<Body*>::const_iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) {
// If the body is a RigidBody and if the rigid body motion is enabled
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
if (rigidBody && rigidBody->getIsMotionEnabled()) {
@ -68,7 +68,7 @@ void PhysicsEngine::updateDynamic() {
}
// For each body in the dynamic world
for(std::vector<Body*>::const_iterator it = world->getBodyListStartIterator(); it != world->getBodyListEndIterator(); ++it) {
for(std::vector<Body*>::const_iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) {
// If the body is a RigidBody and if the rigid body motion is enabled
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
if (rigidBody && rigidBody->getIsMotionEnabled()) {
@ -89,6 +89,10 @@ void PhysicsEngine::updateCollision() {
// Compute the collision detection
if (collisionDetection.computeCollisionDetection()) {
// TODO : Delete this ----------------------------------------------------------
for (std::vector<Constraint*>::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); it++) {
Contact* contact = dynamic_cast<Contact*>(*it);
std::cout << "Const : " << contact << "pDepth before: " << contact->getPenetrationDepth() << std::endl;
}
/*
for (std::vector<Constraint*>::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) {
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>((*it)->getBody1());
@ -102,9 +106,8 @@ void PhysicsEngine::updateCollision() {
// Solve constraints
constraintSolver.solve(timer.getTimeStep().getValue());
}
// For each body in the dynamic world
for(std::vector<Body*>::const_iterator it = world->getBodyListStartIterator(); it != world->getBodyListEndIterator(); ++it) {
else {
for(std::vector<Body*>::const_iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) {
// If the body is a RigidBody and if the rigid body motion is enabled
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
if (rigidBody && rigidBody->getIsMotionEnabled()) {
@ -112,16 +115,28 @@ void PhysicsEngine::updateCollision() {
updateBodyState(rigidBody, timer.getTimeStep());
}
}
}
// TODO : Delete this
collisionDetection.computeCollisionDetection();
for (std::vector<Constraint*>::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); it++) {
Contact* contact = dynamic_cast<Contact*>(*it);
std::cout << "Const : " << contact << "pDepth after: " << contact->getPenetrationDepth() << std::endl;
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(contact->getBody1());
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(contact->getBody2());
rigidBody1->getCurrentBodyState().setPosition(rigidBody1->getCurrentBodyState().getPosition() - contact->getNormal().getUnit() * contact->getPenetrationDepth() * 1.4);
rigidBody2->getCurrentBodyState().setPosition(rigidBody2->getCurrentBodyState().getPosition() + contact->getNormal().getUnit() * contact->getPenetrationDepth() * 1.4);
}
// Update the timer
timer.update();
}
// For each body in the the dynamic world
for(std::vector<Body*>::const_iterator it = world->getBodyListStartIterator(); it != world->getBodyListEndIterator(); ++it) {
for(std::vector<Body*>::const_iterator it = world->getBodiesBeginIterator(); it != world->getBodiesEndIterator(); ++it) {
// If the body is a RigidBody and if the rigid body motion is enabled
RigidBody* rigidBody = dynamic_cast<RigidBody*>(*it);
if (rigidBody && rigidBody->getIsMotionEnabled()) {
if (rigidBody) {
// Update the interpolation factor of the rigid body
// This one will be used to compute the interpolated state
rigidBody->setInterpolationFactor(timer.getInterpolationFactor());

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

@ -54,8 +54,6 @@ class PhysicsWorld {
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
std::vector<Body*>::const_iterator getBodyListStartIterator() const; // Return a start iterator on the body list
std::vector<Body*>::const_iterator getBodyListEndIterator() const; // Return a end iterator on the body list
void addConstraint(Constraint* constraint) throw(std::invalid_argument); // Add a constraint
void removeConstraint(Constraint* constraint) throw(std::invalid_argument); // Remove a constraint
void removeAllContactConstraints(); // Remove all collision contacts constraints
@ -72,18 +70,6 @@ inline Vector3D PhysicsWorld::getGravity() const {
return gravity;
}
// Return a start iterator on the body list
inline std::vector<Body*>::const_iterator PhysicsWorld::getBodyListStartIterator() const {
// Return an iterator on the start of the body list
return bodies.begin();
}
// Return a end iterator on the body list
inline std::vector<Body*>::const_iterator PhysicsWorld::getBodyListEndIterator() const {
// Return an iterator on the end of the body list
return bodies.end();
}
// Return if the gravity is on
inline bool PhysicsWorld::getIsGravityOn() const {
return isGravityOn;

2
sources/reactphysics3d/mathematics/Vector.h
View File

@ -139,7 +139,7 @@ inline void Vector::fillInSubVector(uint rowIndex, const Vector& subVector) {
// 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) {
if (index < 0 || index+nbElements > nbComponent) {
throw std::invalid_argument("Error : arguments are out of bounds");
}