git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@305 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
parent
6ed69219c7
commit
5293491b3b
|
@ -49,6 +49,10 @@ void ConstraintSolver::allocate() {
|
||||||
if (constraint->isActive()) {
|
if (constraint->isActive()) {
|
||||||
activeConstraints.push_back(constraint);
|
activeConstraints.push_back(constraint);
|
||||||
|
|
||||||
|
// Add the two bodies of the constraint in the constraintBodies list
|
||||||
|
constraintBodies.push_back(constraint->getBody1());
|
||||||
|
constraintBodies.push_back(constraint->getBody2());
|
||||||
|
|
||||||
// Fill in the body number maping
|
// Fill in the body number maping
|
||||||
bodyNumberMapping.insert(std::pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
|
bodyNumberMapping.insert(std::pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
|
||||||
bodyNumberMapping.insert(std::pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
|
bodyNumberMapping.insert(std::pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
|
||||||
|
@ -74,6 +78,7 @@ void ConstraintSolver::allocate() {
|
||||||
lowerBounds = Vector(nbConstraints);
|
lowerBounds = Vector(nbConstraints);
|
||||||
upperBounds = Vector(nbConstraints);
|
upperBounds = Vector(nbConstraints);
|
||||||
Minv_sp = Matrix(6*nbBodies, 6);
|
Minv_sp = Matrix(6*nbBodies, 6);
|
||||||
|
Minv_sp.initWithValue(0.0);
|
||||||
V = Vector(6*nbBodies);
|
V = Vector(6*nbBodies);
|
||||||
Fext = Vector(6*nbBodies);
|
Fext = Vector(6*nbBodies);
|
||||||
}
|
}
|
||||||
|
@ -121,19 +126,26 @@ void ConstraintSolver::fillInMatrices() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// For each current body of the physics world
|
// For each current body that is implied in some constraint
|
||||||
for (unsigned int b=0; b<nbBodies; b++) {
|
for (unsigned int b=0; b<nbBodies; b++) {
|
||||||
|
Body* body = constraintBodies.at(b);
|
||||||
|
unsigned int bodyNumber = bodyNumberMapping.at(body);
|
||||||
|
|
||||||
|
// TODO : Use polymorphism and remove this casting
|
||||||
|
RigidBody* rigidBody = dynamic_cast<RigidBody*>(body);
|
||||||
|
assert(rigidBody != 0);
|
||||||
|
|
||||||
// Compute the vector with velocities values
|
// Compute the vector with velocities values
|
||||||
V.fillInSubVector(b*3, )
|
V.fillInSubVector(bodyNumber*6, rigidBody->getCurrentBodyState()->getLinearVelocity());
|
||||||
Minv.fillInSubMatrix(i, 0, bodies.at(b)->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3));
|
V.fillInSubVector(bodyNumber*6+3, rigidBody->getCurrentBodyState()->getAngularVelocity());
|
||||||
Minv.fillInSubMatrix(i+3, 3, bodies.at(b)->getCurrentBodyState().getInertiaTensorInverse());
|
|
||||||
u.fillInSubVector(i, bodies.at(b)->getCurrentBodyState().getLinearVelocity());
|
// Compute the vector with forces and torques values
|
||||||
u.fillInSubVector(i+3, bodies.at(b)->getCurrentBodyState().getAngularVelocity());
|
Fext.fillInSubVector(bodyNumber*6, rigidBody->getCurrentBodyState()->getExternalForce());
|
||||||
fExt.fillInSubVector(i, bodies.at(b)->getCurrentBodyState().getExternalForce());
|
Fext.fillInSubVector(bodyNumber*6+3, rigidBody->getCurrentBodyState()->getExternalTorque());
|
||||||
Vector3D externalTorque = bodies.at(b)->getCurrentBodyState().getExternalTorque();
|
|
||||||
Matrix3x3 inertia = bodies.at(b)->getInertiaTensor();
|
// Compute the inverse sparse mass matrix
|
||||||
Vector3D angularVelocity = bodies.at(b)->getCurrentBodyState().getAngularVelocity();
|
Minv_sp.fillInSubMatrix(b*6, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3));
|
||||||
fExt.fillInSubVector(i+3, externalTorque - (angularVelocity.crossProduct(inertia*angularVelocity)));
|
Minv_sp.fillInSubMatrix(b*6+3, 3, rigidBody->getCurrentBodyState().getInertiaTensorInverse());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -42,6 +42,7 @@ class ConstraintSolver {
|
||||||
LCPSolver& lcpSolver; // LCP Solver
|
LCPSolver& lcpSolver; // LCP Solver
|
||||||
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
|
||||||
unsigned int nbBodies; // Current number of bodies in the physics world
|
unsigned int 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*, unsigned int> 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
|
||||||
|
|
Loading…
Reference in New Issue
Block a user