diff --git a/sources/reactphysics3d/engine/ConstraintSolver.cpp b/sources/reactphysics3d/engine/ConstraintSolver.cpp index 7bd824c7..0403d86c 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.cpp +++ b/sources/reactphysics3d/engine/ConstraintSolver.cpp @@ -162,6 +162,16 @@ void ConstraintSolver::freeMemory() { delete[] bodyMapping; } +// Compute the vector b +void ConstraintSolver::computeVectorB() { + // TODO : Implement this method ... +} + +// Compute the matrix B_sp +void ConstraintSolver::computeMatrixB_sp() { + // TODO : Implement this method ... +} + // Solve the current LCP problem void ConstraintSolver::solve(double dt) { // Allocate memory for the matrices @@ -170,6 +180,12 @@ void ConstraintSolver::solve(double dt) { // Fill-in all the matrices needed to solve the LCP problem fillInMatrices(); + // Compute the vector b + computeVectorB(); + + // Compute the matrix B + computeMatrixB_sp(); + // Solve the LCP problem (computation of lambda) lcpSolver.solve(A, b, lowLimits, highLimits, lambda); diff --git a/sources/reactphysics3d/engine/ConstraintSolver.h b/sources/reactphysics3d/engine/ConstraintSolver.h index 223bd08d..9d53c244 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.h +++ b/sources/reactphysics3d/engine/ConstraintSolver.h @@ -59,9 +59,12 @@ class ConstraintSolver { 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 + 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 + void computeVectorB(); // Compute the vector b + void computeMatrixB_sp(); // Compute the matrix B_sp public: ConstraintSolver(); // Constructor