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

Add the initWithValue() method

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@321 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2010-05-19 21:43:27 +00:00
parent 06f7b0552d
commit 2f6cde24ee
5 changed files with 122 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -25,8 +25,8 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
ConstraintSolver::ConstraintSolver(PhysicsWorld& physicsWorld) ConstraintSolver::ConstraintSolver(PhysicsWorld& world)
:physicsWorld(physicsWorld), bodyMapping(0) , lcpSolver(LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) { :physicsWorld(world), bodyMapping(0) , lcpSolver(LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
} }
@ -67,21 +67,22 @@ void ConstraintSolver::allocate() {
nbBodies = bodyNumberMapping.size(); nbBodies = bodyNumberMapping.size();
bodyMapping = new Body**[nbConstraints]; bodyMapping = new Body**[nbConstraints];
J_sp = new Matrix*[nbConstraints];
B_sp = new Matrix*[2];
for (uint i=0; i<nbConstraints; i++) { for (uint i=0; i<nbConstraints; i++) {
bodyMapping[i] = new Body*[2]; bodyMapping[i] = new Body*[2];
J_sp[i] = new Matrix[2];
B_sp[i] = new Matrix[nbConstraints];
} }
J_sp = Matrix(nbConstraints, 12);
errorValues = Vector(nbConstraints); errorValues = Vector(nbConstraints);
B_sp = Matrix(12, nbConstraints);
b = Vector(nbConstraints); b = Vector(nbConstraints);
lambda = Vector(nbConstraints); lambda = Vector(nbConstraints);
lowerBounds = Vector(nbConstraints); lowerBounds = Vector(nbConstraints);
upperBounds = Vector(nbConstraints); upperBounds = Vector(nbConstraints);
Minv_sp = Matrix(6*nbBodies, 6); Minv_sp = new Matrix[nbBodies];
Minv_sp.initWithValue(0.0); V = new Vector[nbBodies];
V = Vector(6*nbBodies); Fext = new Vector[nbBodies];
Fext = Vector(6*nbBodies);
} }
// Fill in all the matrices needed to solve the LCP problem // Fill in all the matrices needed to solve the LCP problem
@ -93,8 +94,9 @@ void ConstraintSolver::fillInMatrices() {
Constraint* constraint = activeConstraints.at(c); Constraint* constraint = activeConstraints.at(c);
// Fill in the J_sp matrix // Fill in the J_sp matrix
J_sp.fillInSubMatrix(c, 0, constraint->getBody1Jacobian()); J_sp[c][0] = constraint->getBody1Jacobian();
J_sp.fillInSubMatrix(c, 6, constraint->getBody2Jacobian()); J_sp[c][1] = constraint->getBody2Jacobian();
// Fill in the body mapping matrix // Fill in the body mapping matrix
bodyMapping[c][0] = constraint->getBody1(); bodyMapping[c][0] = constraint->getBody1();
@ -111,11 +113,13 @@ void ConstraintSolver::fillInMatrices() {
// If the current constraint has auxiliary constraints // If the current constraint has auxiliary constraints
if (nbAuxConstraints > 0) { if (nbAuxConstraints > 0) {
// Fill in the J_sp matrix
J_sp.fillInSubMatrix(c+1, 0, constraint->getAuxJacobian());
// For each auxiliary constraints // For each auxiliary constraints
for (uint i=1; i<nbAuxConstraints; i++) { for (uint i=1; i<=nbAuxConstraints; i++) {
// Fill in the J_sp matrix
J_sp[c+i][0] = constraint->getAuxJacobian().getSubMatrix(i-1, 0, 1, 6);
J_sp[c+i][1] = constraint->getAuxJacobian().getSubMatrix(i-1, 6, 1, 6);
// Fill in the body mapping matrix // Fill in the body mapping matrix
bodyMapping[c+i][0] = constraint->getBody1(); bodyMapping[c+i][0] = constraint->getBody1();
bodyMapping[c+i][1] = constraint->getBody2(); bodyMapping[c+i][1] = constraint->getBody2();
@ -128,25 +132,34 @@ void ConstraintSolver::fillInMatrices() {
} }
// For each current body that is implied in some constraint // For each current body that is implied in some constraint
RigidBody* rigidBody;
Body* body;
Vector v(6);
Vector f(6);
for (uint b=0; b<nbBodies; b++) { for (uint b=0; b<nbBodies; b++) {
Body* body = constraintBodies.at(b); body = constraintBodies.at(b);
uint bodyNumber = bodyNumberMapping.at(body); uint bodyNumber = bodyNumberMapping.at(body);
// TODO : Use polymorphism and remove this casting // TODO : Use polymorphism and remove this downcasting
RigidBody* rigidBody = dynamic_cast<RigidBody*>(body); rigidBody = dynamic_cast<RigidBody*>(body);
assert(rigidBody != 0); assert(rigidBody != 0);
// Compute the vector with velocities values // Compute the vector with velocities values
V.fillInSubVector(bodyNumber*6, rigidBody->getCurrentBodyState().getLinearVelocity()); v.fillInSubVector(0, rigidBody->getCurrentBodyState().getLinearVelocity());
V.fillInSubVector(bodyNumber*6+3, rigidBody->getCurrentBodyState().getAngularVelocity()); v.fillInSubVector(3, rigidBody->getCurrentBodyState().getAngularVelocity());
V[bodyNumber] = v;
// Compute the vector with forces and torques values // Compute the vector with forces and torques values
Fext.fillInSubVector(bodyNumber*6, rigidBody->getCurrentBodyState().getExternalForce()); f.fillInSubVector(0, rigidBody->getCurrentBodyState().getExternalForce());
Fext.fillInSubVector(bodyNumber*6+3, rigidBody->getCurrentBodyState().getExternalTorque()); f.fillInSubVector(3, rigidBody->getCurrentBodyState().getExternalTorque());
Fext[bodyNumber] = f;
// Compute the inverse sparse mass matrix // Compute the inverse sparse mass matrix
Minv_sp.fillInSubMatrix(b*6, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3)); Matrix mInv(6,6);
Minv_sp.fillInSubMatrix(b*6+3, 3, rigidBody->getCurrentBodyState().getInertiaTensorInverse()); mInv.initWithValue(0.0);
mInv.fillInSubMatrix(0, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3));
mInv.fillInSubMatrix(3, 3, rigidBody->getCurrentBodyState().getInertiaTensorInverse());
Minv_sp[bodyNumber] = mInv;
} }
} }
@ -161,6 +174,11 @@ void ConstraintSolver::freeMemory() {
delete[] bodyMapping[i]; delete[] bodyMapping[i];
} }
delete[] bodyMapping; delete[] bodyMapping;
delete[] J_sp;
delete[] B_sp;
delete[] Minv_sp;
delete[] V;
delete[] Fext;
} }
// Compute the vector b // Compute the vector b
@ -174,12 +192,12 @@ void ConstraintSolver::computeVectorB(double dt) {
// Substract 1.0/dt*J*V to the vector b // Substract 1.0/dt*J*V to the vector b
indexBody1 = bodyNumberMapping[bodyMapping[c][0]]; indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]]; indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
b.setValue(c, b.getValue(c) - oneOverDT * (J_sp.getSubMatrix(c, 0, 1, 6) * V.getSubVector(indexBody1*6, 6)).getValue(0,0)); b.setValue(c, b.getValue(c) - oneOverDT * (J_sp[c][0] * V[indexBody1]).getValue(0,0));
b.setValue(c, b.getValue(c) - oneOverDT * (J_sp.getSubMatrix(c, 6, 1, 6) * V.getSubVector(indexBody2*6, 6)).getValue(0,0)); b.setValue(c, b.getValue(c) - oneOverDT * (J_sp[c][1] * V[indexBody2]).getValue(0,0));
// Substract J*M^-1*F_ext to the vector b // Substract J*M^-1*F_ext to the vector b
b.setValue(c, b.getValue(c) - ((J_sp.getSubMatrix(c, 0, 1, 6) * Minv_sp.getSubMatrix(indexBody1*6, 0, 6, 6))*Fext.getSubVector(indexBody1*6, 6) b.setValue(c, b.getValue(c) - ((J_sp[c][0] * Minv_sp[indexBody1]) * Fext[indexBody1]
+ (J_sp.getSubMatrix(c, 6, 1, 6) * Minv_sp.getSubMatrix(indexBody2*6, 0, 6, 6))*Fext.getSubVector(indexBody2*6, 6))).getValue(0,0); + (J_sp[c][1] * Minv_sp[indexBody2])*Fext[indexBody2]).getValue(0,0));
} }
} }
@ -192,10 +210,8 @@ void ConstraintSolver::computeMatrixB_sp() {
for (uint c = 0; c<activeConstraints.size(); c++) { for (uint c = 0; c<activeConstraints.size(); c++) {
indexBody1 = bodyNumberMapping[bodyMapping[c][0]]; indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]]; indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
Matrix b1 = Minv_sp.getSubMatrix(indexBody1*6, 0, 6, 6) * J_sp.getSubMatrix(c, 0, 1, 6).getTranspose(); B_sp[0][c] = Minv_sp[indexBody1] * J_sp[c][0].getTranspose();
Matrix b2 = Minv_sp.getSubMatrix(indexBody2*6, 0, 6, 6) * J_sp.getSubMatrix(c, 6, 1, 6).getTranspose(); B_sp[1][c] = Minv_sp[indexBody2] * J_sp[c][1].getTranspose();
B_sp.fillInSubMatrix(0, c, b1);
B_sp.fillInSubMatrix(6, c, b2);
} }
} }
@ -208,13 +224,13 @@ void ConstraintSolver::solve(double dt) {
fillInMatrices(); fillInMatrices();
// Compute the vector b // Compute the vector b
computeVectorB(double dt); computeVectorB(dt);
// Compute the matrix B // Compute the matrix B
computeMatrixB_sp(); computeMatrixB_sp();
// Solve the LCP problem (computation of lambda) // Solve the LCP problem (computation of lambda)
lcpSolver.solve(A, b, lowLimits, highLimits, lambda); //lcpSolver.solve(A, b, lowLimits, highLimits, lambda);
// TODO : Implement this method ... // TODO : Implement this method ...

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

@ -23,7 +23,7 @@
// Libraries // Libraries
#include "../typeDefinitions.h" #include "../typeDefinitions.h"
#include "../constraint/Constraint.h" #include "../constraint/Constraint.h"
#include "../mathematics/lcp/LCPProjectedGaussSeidel.h" #include "../mathematics/lcp/LCPSolver.h"
#include "PhysicsWorld.h" #include "PhysicsWorld.h"
#include <map> #include <map>
@ -51,17 +51,23 @@ class ConstraintSolver {
// in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains // 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 // 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 // J_sp matrix. A integer body index refers to its index in the "bodies" std::vector
Matrix J_sp; // Sparse representation of the jacobian matrix of all constraints Matrix** J_sp; // 2-dimensional array thar correspond to the sparse representation of the jacobian matrix of all constraints
Matrix B_sp; // Useful matrix in sparse representation // 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 b; // Vector "b" of the LCP problem
Vector lambda; // Lambda vector of the LCP problem Vector lambda; // Lambda vector of the LCP problem
Vector errorValues; // Error vector of all constraints Vector errorValues; // Error vector of all constraints
Vector lowerBounds; // Vector that contains the low limits for the variables of the LCP problem 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 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 Matrix* Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body
Vector V; // Vector that contains linear and angular velocities of each body // This is an array of size nbBodies that contains in each cell a 6x6 matrix
Vector Fext; // Vector that contains external forces and torques of each body Vector* V; // 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* 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 allocate(); // Allocate all the matrices needed to solve the LCP problem void allocate(); // Allocate all the matrices needed to solve the LCP problem
void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem
void freeMemory(); // Free the memory that was allocated in the allocate() method void freeMemory(); // Free the memory that was allocated in the allocate() method
@ -69,7 +75,7 @@ class ConstraintSolver {
void computeMatrixB_sp(); // Compute the matrix B_sp void computeMatrixB_sp(); // Compute the matrix B_sp
public: public:
ConstraintSolver(); // Constructor ConstraintSolver(const PhysicsWorld& world); // Constructor
virtual ~ConstraintSolver(); // Destructor virtual ~ConstraintSolver(); // Destructor
void solve(double dt); // Solve the current LCP problem void solve(double dt); // Solve the current LCP problem
}; };

66
sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.cpp
View File

@ -23,8 +23,8 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
LCPProjectedGaussSeidel::LCPProjectedGaussSeidel(unsigned int maxIterations) LCPProjectedGaussSeidel::LCPProjectedGaussSeidel(uint maxIterations)
:maxIterations(maxIterations) { :LCPSolver(maxIterations) {
} }
@ -34,28 +34,50 @@ LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() {
} }
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm // Solve a LCP problem using the Projected-Gauss-Seidel algorithm
void LCPProjectedGaussSeidel::solve(const Matrix& A, const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& x) { // This method outputs the result in the lambda vector
assert(A.getNbRow() == A.getNbColumn()); void LCPProjectedGaussSeidel::solve(const Matrix** const J_sp, const Matrix** const B_sp, uint nbConstraints,
assert(b.getNbComponent() == A.getNbColumn()); uint nbBodies, const Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
assert(lowLimits.getNbComponent() == A.getNbColumn()); const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const {
assert(highLimits.getNbComponent() == A.getNbColumn()); lambda = lambdaInit;
double d[] = new double[nbConstraints];
Body* indexBody1, indexBody2;
double deltaLambda;
uint i, iter;
Vector a(6*nbBodies);
double delta; // 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 (unsigned int k=1; k<=maxIterations; k++) { for(iter=0; iter<maxIterations; iter++) {
for (unsigned int i=0; i<A.getNbRow(); i++) { for (i=0; i<nbConstraints; i++) {
delta = 0.0; indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
for (unsigned int j=0; j<i; j++) { indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
delta += A.getValue(i,j) * x.getValue(j); //deltaLambda = ...
}
for (unsigned int j=i+1; j<A.getNbRow(); j++) {
delta += A.getValue(i,j)*x.getValue(j);
}
x.setValue(i, (b.getValue(i) - delta)/A.getValue(i,i));
// Clamping according to the limits
if (x.getValue(i) > highLimits.getValue(i)) x.setValue(i, highLimits.getValue(i));
if (x.getValue(i) < lowLimits.getValue(i)) x.setValue(i, lowLimits.getValue(i));
} }
} }
// TODO : Implement this method ...
// Clean
delete[] d;
}
// 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,
const Matrix** const B_sp, std::map<Body*, uint> bodyNumberMappingVector& a) {
uint i;
Body* indexBody1, indexBody2;
// Init the vector a with zero values
a.initWithValue(0.0);
for(i=0; i<nbConstraints; i++) {
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
// TODO : Implement this ...
}
} }

10
sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.h
View File

@ -34,13 +34,13 @@ namespace reactphysics3d {
*/ */
class LCPProjectedGaussSeidel : public LCPSolver { class LCPProjectedGaussSeidel : public LCPSolver {
protected: protected:
unsigned int maxIterations; // Maximum number of iterations
public: public:
LCPProjectedGaussSeidel(unsigned int maxIterations); // Constructor LCPProjectedGaussSeidel(uint maxIterations); // Constructor
virtual ~LCPProjectedGaussSeidel(); // Destructor virtual ~LCPProjectedGaussSeidel(); // Destructor
virtual void solve(const Matrix& A, const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& x); // Solve a LCP problem using Projected-Gauss-Seidel algorithm 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
}; };
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace

14
sources/reactphysics3d/mathematics/lcp/LCPSolver.h
View File

@ -58,13 +58,13 @@ class LCPSolver {
Vector lambdaInit; // Initial value for lambda at the beginning of the algorithm Vector lambdaInit; // Initial value for lambda at the beginning of the algorithm
public: public:
LCPSolver(uint maxIterations); // Constructor LCPSolver(uint maxIterations); // Constructor
virtual ~LCPSolver(); // Destructor virtual ~LCPSolver(); // Destructor
virtual void solve(const Matrix& J_sp, const Matrix& B_sp, Body*** bodyMapping, 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, std::map<Body*, uint> bodyNumberMapping, const Vector& b, const Vector& lowLimits,
const Vector& highLimits, Vector& lambda) const=0; // Solve a LCP problem const Vector& highLimits, Vector& lambda) const=0; // Solve a LCP problem
void setLambdaInit(const Vector& lambdaInit); // Set the initial lambda vector void setLambdaInit(const Vector& lambdaInit); // Set the initial lambda vector
void setMaxIterations(uint maxIterations); // Set the maximum number of iterations void setMaxIterations(uint maxIterations); // Set the maximum number of iterations
}; };
// Set the initial lambda vector // Set the initial lambda vector