Continue to implement this class
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@313 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
parent
4018a12c56
commit
9677d4fbae
|
@ -36,10 +36,10 @@ ConstraintSolver::~ConstraintSolver() {
|
||||||
|
|
||||||
// Allocate all the matrices needed to solve the LCP problem
|
// Allocate all the matrices needed to solve the LCP problem
|
||||||
void ConstraintSolver::allocate() {
|
void ConstraintSolver::allocate() {
|
||||||
unsigned int nbConstraints = 0;
|
uint nbConstraints = 0;
|
||||||
|
|
||||||
// For each constraint
|
// For each constraint
|
||||||
for (unsigned int c=0; c<physicsWorld.getConstraints().size(); c++) {
|
for (uint c=0; c<physicsWorld.getConstraints().size(); c++) {
|
||||||
Constraint* constraint = physicsWorld.getConstraints().at(c);
|
Constraint* constraint = physicsWorld.getConstraints().at(c);
|
||||||
|
|
||||||
// Evaluate the constraint
|
// Evaluate the constraint
|
||||||
|
@ -66,7 +66,7 @@ void ConstraintSolver::allocate() {
|
||||||
nbBodies = bodyNumberMapping.size();
|
nbBodies = bodyNumberMapping.size();
|
||||||
|
|
||||||
bodyMapping = new Body**[nbConstraints];
|
bodyMapping = new Body**[nbConstraints];
|
||||||
for (unsigned int i=0; i<nbConstraints; i++) {
|
for (uint i=0; i<nbConstraints; i++) {
|
||||||
bodyMapping[i] = new Body*[2];
|
bodyMapping[i] = new Body*[2];
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -88,7 +88,7 @@ void ConstraintSolver::allocate() {
|
||||||
void ConstraintSolver::fillInMatrices() {
|
void ConstraintSolver::fillInMatrices() {
|
||||||
|
|
||||||
// For each active constraint
|
// For each active constraint
|
||||||
for (unsigned int c=0; c<activeConstraints.size(); c++) {
|
for (uint c=0; c<activeConstraints.size(); c++) {
|
||||||
Constraint* constraint = activeConstraints.at(c);
|
Constraint* constraint = activeConstraints.at(c);
|
||||||
|
|
||||||
// Fill in the J_sp matrix
|
// Fill in the J_sp matrix
|
||||||
|
@ -106,7 +106,7 @@ void ConstraintSolver::fillInMatrices() {
|
||||||
// Fill in the error vector
|
// Fill in the error vector
|
||||||
errorValues.fillInSubVector(c, constraint->getErrorValue());
|
errorValues.fillInSubVector(c, constraint->getErrorValue());
|
||||||
|
|
||||||
unsigned int nbAuxConstraints = constraint->getNbAuxConstraints();
|
uint nbAuxConstraints = constraint->getNbAuxConstraints();
|
||||||
|
|
||||||
// If the current constraint has auxiliary constraints
|
// If the current constraint has auxiliary constraints
|
||||||
if (nbAuxConstraints > 0) {
|
if (nbAuxConstraints > 0) {
|
||||||
|
@ -114,7 +114,7 @@ void ConstraintSolver::fillInMatrices() {
|
||||||
J_sp.fillInSubMatrix(c+1, 0, constraint->getAuxJacobian());
|
J_sp.fillInSubMatrix(c+1, 0, constraint->getAuxJacobian());
|
||||||
|
|
||||||
// For each auxiliary constraints
|
// For each auxiliary constraints
|
||||||
for (unsigned int i=1; i<nbAuxConstraints; i++) {
|
for (uint i=1; i<nbAuxConstraints; i++) {
|
||||||
// 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();
|
||||||
|
@ -127,9 +127,9 @@ void ConstraintSolver::fillInMatrices() {
|
||||||
}
|
}
|
||||||
|
|
||||||
// For each current body that is implied in some constraint
|
// For each current body that is implied in some constraint
|
||||||
for (unsigned int b=0; b<nbBodies; b++) {
|
for (uint b=0; b<nbBodies; b++) {
|
||||||
Body* body = constraintBodies.at(b);
|
Body* body = constraintBodies.at(b);
|
||||||
unsigned int bodyNumber = bodyNumberMapping.at(body);
|
uint bodyNumber = bodyNumberMapping.at(body);
|
||||||
|
|
||||||
// TODO : Use polymorphism and remove this casting
|
// TODO : Use polymorphism and remove this casting
|
||||||
RigidBody* rigidBody = dynamic_cast<RigidBody*>(body);
|
RigidBody* rigidBody = dynamic_cast<RigidBody*>(body);
|
||||||
|
@ -156,30 +156,43 @@ void ConstraintSolver::freeMemory() {
|
||||||
bodyNumberMapping.clear();
|
bodyNumberMapping.clear();
|
||||||
|
|
||||||
// Free the bodyMaping array
|
// Free the bodyMaping array
|
||||||
for (unsigned int i=0; i<nbBodies; i++) {
|
for (uint i=0; i<nbBodies; i++) {
|
||||||
delete[] bodyMapping[i];
|
delete[] bodyMapping[i];
|
||||||
}
|
}
|
||||||
delete[] bodyMapping;
|
delete[] bodyMapping;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute the vector b
|
// Compute the vector b
|
||||||
void ConstraintSolver::computeVectorB() {
|
void ConstraintSolver::computeVectorB(double dt) {
|
||||||
|
uint indexBody1, indexBody2;
|
||||||
|
double oneOverDT = 1.0/dt;
|
||||||
|
|
||||||
|
b = errorValues * oneOverDT;
|
||||||
|
|
||||||
|
// Substract 1.0/dt*J*V to the vector b
|
||||||
|
for (uint c = 0; c<activeConstraints.size(); c++) {
|
||||||
|
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
|
||||||
|
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
|
||||||
|
b -= oneOverDT * (J_sp(c, 0) * V.getSubVector(indexBody1, 6));
|
||||||
|
b -= oneOverDT * (J_sp(c, 1) * V.getSubVector(indexBody2, 6));
|
||||||
|
}
|
||||||
|
|
||||||
|
// TODO : Continue to implement this method ... compute and remove J*Minv*Fext from b
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute the matrix B_sp
|
// Compute the matrix B_sp
|
||||||
void ConstraintSolver::computeMatrixB_sp() {
|
void ConstraintSolver::computeMatrixB_sp() {
|
||||||
unsigned int indexBody1;
|
uint indexBody1;
|
||||||
unsigned int indexBody2;
|
uint indexBody2;
|
||||||
|
|
||||||
// For each constraint
|
// For each constraint
|
||||||
for (unsigned int j = 0; j<activeConstraints.size(); j++) {
|
for (uint c = 0; c<activeConstraints.size(); c++) {
|
||||||
indexBody1 = bodyNumberMapping[bodyMapping[j][0]];
|
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
|
||||||
indexBody2 = bodyNumberMapping[bodyMapping[j][1]];
|
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
|
||||||
Matrix b1 = Minv_sp.getSubMatrix(indexBody1*6, 0, 6, 6) * J_sp.getSubMatrix(j, 0, 1, 6).getTranspose();
|
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(j, 6, 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, j, b1);
|
B_sp.fillInSubMatrix(0, c, b1);
|
||||||
B_sp.fillInSubMatrix(6, j, b2);
|
B_sp.fillInSubMatrix(6, c, b2);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -192,7 +205,7 @@ void ConstraintSolver::solve(double dt) {
|
||||||
fillInMatrices();
|
fillInMatrices();
|
||||||
|
|
||||||
// Compute the vector b
|
// Compute the vector b
|
||||||
computeVectorB();
|
computeVectorB(double dt);
|
||||||
|
|
||||||
// Compute the matrix B
|
// Compute the matrix B
|
||||||
computeMatrixB_sp();
|
computeMatrixB_sp();
|
||||||
|
|
|
@ -21,6 +21,7 @@
|
||||||
#define CONSTRAINTSOLVER_H
|
#define CONSTRAINTSOLVER_H
|
||||||
|
|
||||||
// Libraries
|
// Libraries
|
||||||
|
#include "../typeDefinitions.h"
|
||||||
#include "../constraint/Constraint.h"
|
#include "../constraint/Constraint.h"
|
||||||
#include "PhysicsWorld.h"
|
#include "PhysicsWorld.h"
|
||||||
#include <map>
|
#include <map>
|
||||||
|
@ -29,7 +30,7 @@
|
||||||
namespace reactphysics3d {
|
namespace reactphysics3d {
|
||||||
|
|
||||||
// Constants
|
// Constants
|
||||||
const unsigned int MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem
|
const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem
|
||||||
|
|
||||||
/* -------------------------------------------------------------------
|
/* -------------------------------------------------------------------
|
||||||
Class ConstrainSolver :
|
Class ConstrainSolver :
|
||||||
|
@ -43,11 +44,11 @@ class ConstraintSolver {
|
||||||
PhysicsWorld& physicsWorld; // Reference to the physics world
|
PhysicsWorld& physicsWorld; // Reference to the physics world
|
||||||
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
|
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
|
||||||
std::vector<Body*> constraintBodies; // Bodies that are implied in some constraint
|
std::vector<Body*> constraintBodies; // Bodies that are implied in some constraint
|
||||||
unsigned int nbBodies; // Current number of bodies in the physics world
|
uint nbBodies; // Current number of bodies in the physics world
|
||||||
std::map<Body*, unsigned int> bodyNumberMapping; // Map a body pointer with its index number
|
std::map<Body*, uint> bodyNumberMapping; // Map a body pointer with its index number
|
||||||
Body*** bodyMapping; // 2-dimensional array that contains the mapping of body reference
|
Body*** bodyMapping; // 2-dimensional array that contains the mapping of body reference
|
||||||
// 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 integer index of 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; // Sparse representation of the jacobian matrix of all constraints
|
||||||
Matrix B_sp; // Useful matrix in sparse representation
|
Matrix B_sp; // Useful matrix in sparse representation
|
||||||
|
@ -63,7 +64,7 @@ class ConstraintSolver {
|
||||||
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
|
||||||
void computeVectorB(); // Compute the vector b
|
void computeVectorB(double dt); // Compute the vector b
|
||||||
void computeMatrixB_sp(); // Compute the matrix B_sp
|
void computeMatrixB_sp(); // Compute the matrix B_sp
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
Loading…
Reference in New Issue
Block a user