Correction of some errors
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@325 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel
parent
2f9a12f887
commit
639e91b21b
|
|
@ -26,7 +26,7 @@ using namespace reactphysics3d;
|
|||
|
||||
// Constructor
|
||||
ConstraintSolver::ConstraintSolver(PhysicsWorld& world)
|
||||
:physicsWorld(world), bodyMapping(0) , lcpSolver(LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
|
||||
:physicsWorld(world), bodyMapping(0), lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -38,10 +38,12 @@ ConstraintSolver::~ConstraintSolver() {
|
|||
// Allocate all the matrices needed to solve the LCP problem
|
||||
void ConstraintSolver::allocate() {
|
||||
uint nbConstraints = 0;
|
||||
Constraint* constraint;
|
||||
|
||||
// For each constraint
|
||||
for (uint c=0; c<physicsWorld.getConstraints().size(); c++) {
|
||||
Constraint* constraint = physicsWorld.getConstraints().at(c);
|
||||
std::vector<Constraint*>::iterator it;
|
||||
for (it = physicsWorld.getConstraintsBeginIterator(); it <physicsWorld.getConstraintsEndIterator(); it++) {
|
||||
constraint = *it;
|
||||
|
||||
// Evaluate the constraint
|
||||
constraint->evaluate();
|
||||
|
|
@ -230,7 +232,7 @@ void ConstraintSolver::solve(double dt) {
|
|||
computeMatrixB_sp();
|
||||
|
||||
// Solve the LCP problem (computation of lambda)
|
||||
//lcpSolver.solve(A, b, lowLimits, highLimits, lambda);
|
||||
lcpSolver->solve(J_sp, B_sp, activeConstraints.size(), nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
|
||||
|
||||
// TODO : Implement this method ...
|
||||
|
||||
|
|
|
|||
|
|
@ -41,8 +41,8 @@ const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when sol
|
|||
*/
|
||||
class ConstraintSolver {
|
||||
protected:
|
||||
LCPSolver& lcpSolver; // LCP Solver
|
||||
PhysicsWorld& physicsWorld; // Reference to the physics world
|
||||
LCPSolver* lcpSolver; // LCP Solver
|
||||
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
|
||||
std::vector<Body*> constraintBodies; // Bodies that are implied in some constraint
|
||||
uint nbBodies; // Current number of bodies in the physics world
|
||||
|
|
@ -75,7 +75,7 @@ class ConstraintSolver {
|
|||
void computeMatrixB_sp(); // Compute the matrix B_sp
|
||||
|
||||
public:
|
||||
ConstraintSolver(const PhysicsWorld& world); // Constructor
|
||||
ConstraintSolver(PhysicsWorld& world); // Constructor
|
||||
virtual ~ConstraintSolver(); // Destructor
|
||||
void solve(double dt); // Solve the current LCP problem
|
||||
};
|
||||
|
|
|
|||
|
|
@ -91,7 +91,7 @@ void PhysicsEngine::updateCollision() {
|
|||
if (collisionDetection.computeCollisionDetection()) {
|
||||
|
||||
// TODO : Delete this ----------------------------------------------------------
|
||||
for (std::vector<Constraint*>::const_iterator it = world->getConstraintListStartIterator(); it != world->getConstraintListEndIterator(); ++it) {
|
||||
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);
|
||||
|
|
|
|||
|
|
@ -59,10 +59,10 @@ class PhysicsWorld {
|
|||
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
|
||||
std::vector<Constraint*>::const_iterator getConstraintListStartIterator() const; // Return a start iterator on the constraint list
|
||||
std::vector<Constraint*>::const_iterator getConstraintListEndIterator() const; // Return a end iterator on the constraint list
|
||||
std::vector<Body*>& getBodies(); // Return a reference to the bodies of the physics world
|
||||
std::vector<Constraint*>& getConstraints(); // Return a reference to the constraints of the physics world
|
||||
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
|
||||
};
|
||||
|
||||
// --- Inline functions --- //
|
||||
|
|
@ -95,23 +95,23 @@ inline void PhysicsWorld::setIsGratityOn(bool isGravityOn) {
|
|||
}
|
||||
|
||||
// Return a start iterator on the constraint list
|
||||
inline std::vector<Constraint*>::const_iterator PhysicsWorld::getConstraintListStartIterator() const {
|
||||
inline std::vector<Constraint*>::iterator PhysicsWorld::getConstraintsBeginIterator() {
|
||||
return constraints.begin();
|
||||
}
|
||||
|
||||
// Return a end iterator on the constraint list
|
||||
inline std::vector<Constraint*>::const_iterator PhysicsWorld::getConstraintListEndIterator() const {
|
||||
inline std::vector<Constraint*>::iterator PhysicsWorld::getConstraintsEndIterator() {
|
||||
return constraints.end();
|
||||
}
|
||||
|
||||
// Return a reference to the bodies of the physics world
|
||||
std::vector<Body*>& PhysicsWorld::getBodies() {
|
||||
return bodies;
|
||||
// Return an iterator to the beginning of the bodies of the physics world
|
||||
inline std::vector<Body*>::iterator PhysicsWorld::getBodiesBeginIterator() {
|
||||
return bodies.begin();
|
||||
}
|
||||
|
||||
// Return a reference to the constraints of the physics world
|
||||
std::vector<Constraint*>& PhysicsWorld::getConstraints() {
|
||||
return constraints;
|
||||
// Return an iterator to the end of the bodies of the physics world
|
||||
inline std::vector<Body*>::iterator PhysicsWorld::getBodiesEndIterator() {
|
||||
return bodies.end();
|
||||
}
|
||||
|
||||
} // End of the ReactPhysics3D namespace
|
||||
|
|
|
|||
|
|
@ -276,7 +276,6 @@ double Matrix::getTrace() const throw(MathematicsException) {
|
|||
// 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
|
||||
|
|
@ -353,6 +352,16 @@ void Matrix::initWithValue(double 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) {
|
||||
|
|
@ -440,6 +449,27 @@ Matrix Matrix::operator*(const Matrix& matrix2) const throw(MathematicsException
|
|||
}
|
||||
}
|
||||
|
||||
// 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(nbColumn, 1);
|
||||
for (int i=0; i<nbColumn; i++) {
|
||||
double sum = 0.0;
|
||||
for (int j=0; j<vector.getNbComponent(); 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) {
|
||||
|
||||
|
|
|
|||
|
|
@ -61,6 +61,7 @@ class 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
|
||||
|
|
@ -71,6 +72,7 @@ class Matrix {
|
|||
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
|
||||
};
|
||||
|
|
|
|||
|
|
@ -100,6 +100,14 @@ Vector Vector::getUnit() const throw(MathematicsException) {
|
|||
}
|
||||
}
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -59,14 +59,16 @@ class Vector {
|
|||
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
|
||||
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
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -162,6 +164,17 @@ inline Vector operator*(double number, const Vector& vector) {
|
|||
return vector * number;
|
||||
}
|
||||
|
||||
void Vector::changeSize(uint newSize) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
} // End of the ReactPhysics3D namespace
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -36,16 +36,19 @@ LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() {
|
|||
|
||||
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
|
||||
// This method outputs the result in the lambda vector
|
||||
void LCPProjectedGaussSeidel::solve(const Matrix** const J_sp, const Matrix** const B_sp, uint nbConstraints,
|
||||
uint nbBodies, const Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
|
||||
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)
|
||||
Body* indexBody1, indexBody2;
|
||||
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(6)[nbBodies]; // Array that contains nbBodies vector of dimension 6x1
|
||||
Vector* a = new Vector[nbBodies]; // Array that contains nbBodies vector of dimension 6x1
|
||||
for (i=0; i<nbBodies; i++) {
|
||||
a[i].changeSize(6);
|
||||
}
|
||||
|
||||
// For each constraint
|
||||
for (i=0; i<nbConstraints; i++) {
|
||||
|
|
@ -56,12 +59,12 @@ void LCPProjectedGaussSeidel::solve(const Matrix** const J_sp, const Matrix** co
|
|||
for (i=0; i<nbConstraints; i++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
|
||||
deltaLambda = (b(i) - J_sp[i][0]*a[indexBody1] - J_sp[i][1]*a[indexBody2]) / d[i];
|
||||
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] + deltaLambda * B_sp[0][i];
|
||||
a[indexBody2] = a[indexBody2] + deltaLambda * B_sp[1][i];
|
||||
a[indexBody1] = a[indexBody1] + (B_sp[0][i] * deltaLambda).getVector();
|
||||
a[indexBody2] = a[indexBody2] + (B_sp[1][i] * deltaLambda).getVector();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -72,11 +75,11 @@ void LCPProjectedGaussSeidel::solve(const Matrix** const J_sp, const Matrix** co
|
|||
|
||||
// Compute the vector a used in the solve() method
|
||||
// Note that a = B * lambda
|
||||
void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstraints, const Body*** const bodyMapping,
|
||||
void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
|
||||
const Matrix** const B_sp, std::map<Body*, uint> bodyNumberMapping,
|
||||
Vector* const a, uint nbBodies) const {
|
||||
uint i;
|
||||
Body* indexBody1, indexBody2;
|
||||
uint indexBody1, indexBody2;
|
||||
|
||||
// Init the vector a with zero values
|
||||
for (i=0; i<nbBodies; i++) {
|
||||
|
|
@ -86,8 +89,8 @@ void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstr
|
|||
for(i=0; i<nbConstraints; i++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
|
||||
a[indexBody1] = a[indexBody1] + B_sp[0][i] * lambda(i);
|
||||
a[indexBody2] = a[indexBody2] + B_sp[1][i] * lambda(i);
|
||||
a[indexBody1] = a[indexBody1] + (B_sp[0][i].getVector() * lambda.getValue(i));
|
||||
a[indexBody2] = a[indexBody2] + (B_sp[1][i].getVector() * lambda.getValue(i));
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -36,16 +36,16 @@ namespace reactphysics3d {
|
|||
class LCPProjectedGaussSeidel : public LCPSolver {
|
||||
protected:
|
||||
|
||||
void computeVectorA(const Vector& lambda, uint nbConstraints, const Body*** const bodyMapping,
|
||||
void computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
|
||||
const Matrix** const 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(const Matrix** const J_sp, const Matrix** const B_sp, const 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
|
||||
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
|
||||
|
|
|
|||
|
|
@ -23,7 +23,8 @@
|
|||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
LCPSolver::LCPSolver() {
|
||||
LCPSolver::LCPSolver(uint maxIterations)
|
||||
: maxIterations(maxIterations) {
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -60,9 +60,9 @@ class LCPSolver {
|
|||
public:
|
||||
LCPSolver(uint maxIterations); // Constructor
|
||||
virtual ~LCPSolver(); // Destructor
|
||||
virtual void solve(const Matrix** const J_sp, const Matrix** const B_sp, const 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
|
||||
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
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user