package sim.physics2D.integrator;
import sim.physics2D.*;
import sim.physics2D.constraint.ConstraintEngine;
import sim.physics2D.forceGenerator.ForceEngine;
/** Implements an Euler ordinary differential equation solver. The Euler integrator
* solves with these equations:
*
* x = x + x';
* x' = x' + x''
*/
public class ODEEulerSolver implements ODESolver
{
private ConstraintEngine objCE;
private ForceEngine objFE;
private PhysicsState physicsState;
public ODEEulerSolver()
{
physicsState = PhysicsState.getInstance();
this.objCE = ConstraintEngine.getInstance();
this.objFE = ForceEngine.getInstance();
}
private sim.util.matrix.Vector ODEFunction(sim.util.matrix.Vector state)
{
int stateRowDimension = state.m;
int halfStateRowDimension = stateRowDimension / 2;
// Update the physics engine so it thinks this is the current state
physicsState.setStateVector(state);
objFE.addForces();
sim.util.matrix.Vector externalForcesVector = physicsState.getExternalForcesVector();
sim.util.matrix.Vector totalForces;
sim.util.matrix.Vector constraintForces = objCE.calculateConstraintForces(externalForcesVector);
totalForces = constraintForces.plus(externalForcesVector);
sim.util.matrix.Vector acc = physicsState.getMassInverseMatrix().times(totalForces);
// Set the first half of the stateDot matrix to velocity
// (second half of state matrix) and the second half of the
// stateDot matrix to the accelerations just calculated
sim.util.matrix.Vector stateDot = new sim.util.matrix.Vector(stateRowDimension);
for (int i = 0; i < halfStateRowDimension; i++)
{
stateDot.vals[i] = state.vals[i + halfStateRowDimension];
stateDot.vals[i + halfStateRowDimension] = acc.vals[i];
}
return stateDot;
}
public void solve(double stepSize)
{
// Make a copy of the state at the start of the current step
sim.util.matrix.Vector stepState = physicsState.getStateVectorCopy();
sim.util.matrix.Vector stateDot = ODEFunction(stepState).times(stepSize);
// Update the step to be the new state at the end of this step
physicsState.setStateVector(stepState.plus(stateDot));
}
}