78 lines
2.9 KiB
C++
78 lines
2.9 KiB
C++
/** @file
|
|
* Original ReactPhysics3D C++ library by Daniel Chappuis <http://www.reactphysics3d.com/> This code is re-licensed with permission from ReactPhysics3D author.
|
|
* @author Daniel CHAPPUIS
|
|
* @author Edouard DUPIN
|
|
* @copyright 2010-2016, Daniel Chappuis
|
|
* @copyright 2017, Edouard DUPIN
|
|
* @license MPL v2.0 (see license file)
|
|
*/
|
|
#include <ephysics/engine/ConstraintSolver.hpp>
|
|
#include <ephysics/engine/Profiler.hpp>
|
|
|
|
using namespace ephysics;
|
|
|
|
ConstraintSolver::ConstraintSolver( etk::Map<RigidBody*, int> mapBodyToVelocityIndex):
|
|
this.mapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
|
|
this.isWarmStartingActive(true),
|
|
this.raintSolverData(mapBodyToVelocityIndex) {
|
|
|
|
}
|
|
|
|
void ConstraintSolver::initializeForIsland(float dt, Island* island) {
|
|
PROFILE("ConstraintSolver::initializeForIsland()");
|
|
assert(island != null);
|
|
assert(island->getNbBodies() > 0);
|
|
assert(island->getNbJoints() > 0);
|
|
// Set the current time step
|
|
this.timeStep = dt;
|
|
// Initialize the raint solver data used to initialize and solve the raints
|
|
this.raintSolverData.timeStep = this.timeStep;
|
|
this.raintSolverData.isWarmStartingActive = this.isWarmStartingActive;
|
|
// For each joint of the island
|
|
Joint** joints = island->getJoints();
|
|
for (int iii=0; iii<island->getNbJoints(); ++iii) {
|
|
// Initialize the raint before solving it
|
|
joints[iii]->initBeforeSolve(this.raintSolverData);
|
|
// Warm-start the raint if warm-starting is enabled
|
|
if (this.isWarmStartingActive) {
|
|
joints[iii]->warmstart(this.raintSolverData);
|
|
}
|
|
}
|
|
}
|
|
|
|
void ConstraintSolver::solveVelocityConstraints(Island* island) {
|
|
PROFILE("ConstraintSolver::solveVelocityConstraints()");
|
|
assert(island != null);
|
|
assert(island->getNbJoints() > 0);
|
|
// For each joint of the island
|
|
Joint** joints = island->getJoints();
|
|
for (int iii=0; iii<island->getNbJoints(); ++iii) {
|
|
joints[iii]->solveVelocityConstraint(this.raintSolverData);
|
|
}
|
|
}
|
|
|
|
void ConstraintSolver::solvePositionConstraints(Island* island) {
|
|
PROFILE("ConstraintSolver::solvePositionConstraints()");
|
|
assert(island != null);
|
|
assert(island->getNbJoints() > 0);
|
|
Joint** joints = island->getJoints();
|
|
for (int iii=0; iii < island->getNbJoints(); ++iii) {
|
|
joints[iii]->solvePositionConstraint(this.raintSolverData);
|
|
}
|
|
}
|
|
|
|
void ConstraintSolver::setConstrainedVelocitiesArrays(vec3* rainedLinearVelocities,
|
|
vec3* rainedAngularVelocities) {
|
|
assert(rainedLinearVelocities != null);
|
|
assert(rainedAngularVelocities != null);
|
|
this.raintSolverData.linearVelocities = rainedLinearVelocities;
|
|
this.raintSolverData.angularVelocities = rainedAngularVelocities;
|
|
}
|
|
|
|
void ConstraintSolver::setConstrainedPositionsArrays(vec3* rainedPositions,
|
|
etk::Quaternion* rainedOrientations) {
|
|
assert(rainedPositions != null);
|
|
assert(rainedOrientations != null);
|
|
this.raintSolverData.positions = rainedPositions;
|
|
this.raintSolverData.orientations = rainedOrientations;
|
|
} |