Add the initWithValue() method
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@321 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel
parent
06f7b0552d
commit
2f6cde24ee
|
|
@ -25,8 +25,8 @@
|
|||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
ConstraintSolver::ConstraintSolver(PhysicsWorld& physicsWorld)
|
||||
:physicsWorld(physicsWorld), bodyMapping(0) , lcpSolver(LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
|
||||
ConstraintSolver::ConstraintSolver(PhysicsWorld& world)
|
||||
:physicsWorld(world), bodyMapping(0) , lcpSolver(LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -67,21 +67,22 @@ void ConstraintSolver::allocate() {
|
|||
nbBodies = bodyNumberMapping.size();
|
||||
|
||||
bodyMapping = new Body**[nbConstraints];
|
||||
J_sp = new Matrix*[nbConstraints];
|
||||
B_sp = new Matrix*[2];
|
||||
for (uint i=0; i<nbConstraints; i++) {
|
||||
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);
|
||||
B_sp = Matrix(12, nbConstraints);
|
||||
b = Vector(nbConstraints);
|
||||
lambda = Vector(nbConstraints);
|
||||
lowerBounds = Vector(nbConstraints);
|
||||
upperBounds = Vector(nbConstraints);
|
||||
Minv_sp = Matrix(6*nbBodies, 6);
|
||||
Minv_sp.initWithValue(0.0);
|
||||
V = Vector(6*nbBodies);
|
||||
Fext = Vector(6*nbBodies);
|
||||
Minv_sp = new Matrix[nbBodies];
|
||||
V = new Vector[nbBodies];
|
||||
Fext = new Vector[nbBodies];
|
||||
}
|
||||
|
||||
// Fill in all the matrices needed to solve the LCP problem
|
||||
|
|
@ -93,8 +94,9 @@ void ConstraintSolver::fillInMatrices() {
|
|||
Constraint* constraint = activeConstraints.at(c);
|
||||
|
||||
// Fill in the J_sp matrix
|
||||
J_sp.fillInSubMatrix(c, 0, constraint->getBody1Jacobian());
|
||||
J_sp.fillInSubMatrix(c, 6, constraint->getBody2Jacobian());
|
||||
J_sp[c][0] = constraint->getBody1Jacobian();
|
||||
J_sp[c][1] = constraint->getBody2Jacobian();
|
||||
|
||||
|
||||
// Fill in the body mapping matrix
|
||||
bodyMapping[c][0] = constraint->getBody1();
|
||||
|
|
@ -111,11 +113,13 @@ void ConstraintSolver::fillInMatrices() {
|
|||
|
||||
// If the current constraint has auxiliary constraints
|
||||
if (nbAuxConstraints > 0) {
|
||||
// Fill in the J_sp matrix
|
||||
J_sp.fillInSubMatrix(c+1, 0, constraint->getAuxJacobian());
|
||||
|
||||
// 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
|
||||
bodyMapping[c+i][0] = constraint->getBody1();
|
||||
bodyMapping[c+i][1] = constraint->getBody2();
|
||||
|
|
@ -128,25 +132,34 @@ void ConstraintSolver::fillInMatrices() {
|
|||
}
|
||||
|
||||
// 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++) {
|
||||
Body* body = constraintBodies.at(b);
|
||||
body = constraintBodies.at(b);
|
||||
uint bodyNumber = bodyNumberMapping.at(body);
|
||||
|
||||
// TODO : Use polymorphism and remove this casting
|
||||
RigidBody* rigidBody = dynamic_cast<RigidBody*>(body);
|
||||
// TODO : Use polymorphism and remove this downcasting
|
||||
rigidBody = dynamic_cast<RigidBody*>(body);
|
||||
assert(rigidBody != 0);
|
||||
|
||||
// Compute the vector with velocities values
|
||||
V.fillInSubVector(bodyNumber*6, rigidBody->getCurrentBodyState().getLinearVelocity());
|
||||
V.fillInSubVector(bodyNumber*6+3, rigidBody->getCurrentBodyState().getAngularVelocity());
|
||||
v.fillInSubVector(0, rigidBody->getCurrentBodyState().getLinearVelocity());
|
||||
v.fillInSubVector(3, rigidBody->getCurrentBodyState().getAngularVelocity());
|
||||
V[bodyNumber] = v;
|
||||
|
||||
// Compute the vector with forces and torques values
|
||||
Fext.fillInSubVector(bodyNumber*6, rigidBody->getCurrentBodyState().getExternalForce());
|
||||
Fext.fillInSubVector(bodyNumber*6+3, rigidBody->getCurrentBodyState().getExternalTorque());
|
||||
f.fillInSubVector(0, rigidBody->getCurrentBodyState().getExternalForce());
|
||||
f.fillInSubVector(3, rigidBody->getCurrentBodyState().getExternalTorque());
|
||||
Fext[bodyNumber] = f;
|
||||
|
||||
// Compute the inverse sparse mass matrix
|
||||
Minv_sp.fillInSubMatrix(b*6, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3));
|
||||
Minv_sp.fillInSubMatrix(b*6+3, 3, rigidBody->getCurrentBodyState().getInertiaTensorInverse());
|
||||
Matrix mInv(6,6);
|
||||
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;
|
||||
delete[] J_sp;
|
||||
delete[] B_sp;
|
||||
delete[] Minv_sp;
|
||||
delete[] V;
|
||||
delete[] Fext;
|
||||
}
|
||||
|
||||
// Compute the vector b
|
||||
|
|
@ -174,12 +192,12 @@ void ConstraintSolver::computeVectorB(double dt) {
|
|||
// Substract 1.0/dt*J*V to the vector b
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
|
||||
b.setValue(c, b.getValue(c) - 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.getSubMatrix(c, 6, 1, 6) * V.getSubVector(indexBody2*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[c][1] * V[indexBody2]).getValue(0,0));
|
||||
|
||||
// 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)
|
||||
+ (J_sp.getSubMatrix(c, 6, 1, 6) * Minv_sp.getSubMatrix(indexBody2*6, 0, 6, 6))*Fext.getSubVector(indexBody2*6, 6))).getValue(0,0);
|
||||
b.setValue(c, b.getValue(c) - ((J_sp[c][0] * Minv_sp[indexBody1]) * Fext[indexBody1]
|
||||
+ (J_sp[c][1] * Minv_sp[indexBody2])*Fext[indexBody2]).getValue(0,0));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -192,10 +210,8 @@ void ConstraintSolver::computeMatrixB_sp() {
|
|||
for (uint c = 0; c<activeConstraints.size(); c++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
|
||||
Matrix b1 = Minv_sp.getSubMatrix(indexBody1*6, 0, 6, 6) * J_sp.getSubMatrix(c, 0, 1, 6).getTranspose();
|
||||
Matrix b2 = Minv_sp.getSubMatrix(indexBody2*6, 0, 6, 6) * J_sp.getSubMatrix(c, 6, 1, 6).getTranspose();
|
||||
B_sp.fillInSubMatrix(0, c, b1);
|
||||
B_sp.fillInSubMatrix(6, c, b2);
|
||||
B_sp[0][c] = Minv_sp[indexBody1] * J_sp[c][0].getTranspose();
|
||||
B_sp[1][c] = Minv_sp[indexBody2] * J_sp[c][1].getTranspose();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -208,13 +224,13 @@ void ConstraintSolver::solve(double dt) {
|
|||
fillInMatrices();
|
||||
|
||||
// Compute the vector b
|
||||
computeVectorB(double dt);
|
||||
computeVectorB(dt);
|
||||
|
||||
// Compute the matrix B
|
||||
computeMatrixB_sp();
|
||||
|
||||
// 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 ...
|
||||
|
||||
|
|
|
|||
|
|
@ -23,7 +23,7 @@
|
|||
// Libraries
|
||||
#include "../typeDefinitions.h"
|
||||
#include "../constraint/Constraint.h"
|
||||
#include "../mathematics/lcp/LCPProjectedGaussSeidel.h"
|
||||
#include "../mathematics/lcp/LCPSolver.h"
|
||||
#include "PhysicsWorld.h"
|
||||
#include <map>
|
||||
|
||||
|
|
@ -51,17 +51,23 @@ 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
|
||||
Matrix J_sp; // Sparse representation of the jacobian matrix of all constraints
|
||||
Matrix B_sp; // Useful matrix in sparse representation
|
||||
Matrix** J_sp; // 2-dimensional array thar correspond to the sparse representation of the jacobian matrix of all constraints
|
||||
// The dimension of this array is nbConstraints times 2. Each cell will contain
|
||||
// a 1x6 matrix
|
||||
Matrix** B_sp; // 2-dimensional array that correspond to a useful matrix in sparse representation
|
||||
// The dimension of this array is 2 times nbConstraints. Each cell will contain
|
||||
// a 6x1 matrix
|
||||
Vector b; // Vector "b" of the LCP problem
|
||||
Vector lambda; // Lambda vector of the LCP problem
|
||||
Vector errorValues; // Error vector of all constraints
|
||||
Vector lowerBounds; // Vector that contains the low limits for the variables of the LCP problem
|
||||
Vector upperBounds; // Vector that contains the high limits for the variables of the LCP problem
|
||||
Matrix Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body
|
||||
Vector V; // Vector that contains linear and angular velocities of each body
|
||||
Vector Fext; // Vector that contains external forces and torques of each body
|
||||
|
||||
Matrix* Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body
|
||||
// This is an array of size nbBodies that contains in each cell a 6x6 matrix
|
||||
Vector* 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 fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem
|
||||
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
|
||||
|
||||
public:
|
||||
ConstraintSolver(); // Constructor
|
||||
ConstraintSolver(const PhysicsWorld& world); // Constructor
|
||||
virtual ~ConstraintSolver(); // Destructor
|
||||
void solve(double dt); // Solve the current LCP problem
|
||||
};
|
||||
|
|
|
|||
|
|
@ -23,8 +23,8 @@
|
|||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
LCPProjectedGaussSeidel::LCPProjectedGaussSeidel(unsigned int maxIterations)
|
||||
:maxIterations(maxIterations) {
|
||||
LCPProjectedGaussSeidel::LCPProjectedGaussSeidel(uint maxIterations)
|
||||
:LCPSolver(maxIterations) {
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -34,28 +34,50 @@ LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() {
|
|||
}
|
||||
|
||||
// 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) {
|
||||
assert(A.getNbRow() == A.getNbColumn());
|
||||
assert(b.getNbComponent() == A.getNbColumn());
|
||||
assert(lowLimits.getNbComponent() == A.getNbColumn());
|
||||
assert(highLimits.getNbComponent() == A.getNbColumn());
|
||||
// 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,
|
||||
const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const {
|
||||
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 (unsigned int i=0; i<A.getNbRow(); i++) {
|
||||
delta = 0.0;
|
||||
for (unsigned int j=0; j<i; j++) {
|
||||
delta += A.getValue(i,j) * x.getValue(j);
|
||||
}
|
||||
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));
|
||||
for(iter=0; iter<maxIterations; iter++) {
|
||||
for (i=0; i<nbConstraints; i++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
|
||||
//deltaLambda = ...
|
||||
}
|
||||
}
|
||||
|
||||
// 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 ...
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -34,13 +34,13 @@ namespace reactphysics3d {
|
|||
*/
|
||||
class LCPProjectedGaussSeidel : public LCPSolver {
|
||||
protected:
|
||||
unsigned int maxIterations; // Maximum number of iterations
|
||||
|
||||
public:
|
||||
LCPProjectedGaussSeidel(unsigned int maxIterations); // Constructor
|
||||
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
|
||||
|
||||
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
|
||||
};
|
||||
|
||||
} // End of the ReactPhysics3D namespace
|
||||
|
|
|
|||
|
|
@ -58,13 +58,13 @@ class LCPSolver {
|
|||
Vector lambdaInit; // Initial value for lambda at the beginning of the algorithm
|
||||
|
||||
public:
|
||||
LCPSolver(uint maxIterations); // Constructor
|
||||
virtual ~LCPSolver(); // Destructor
|
||||
virtual void solve(const Matrix& J_sp, const Matrix& B_sp, Body*** 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
|
||||
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
|
||||
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
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user