void PhysicsEngine::calcForcesAndReactionsForRigidBodies(float timeDelta) {
for( int index = 0; index < physicsBodies.getCount(); index++ ) {
SharedPointer<PhysicsBody> currentBody = physicsBodies[index];
currentBody->angularVelocityDelta = Eigen::Matrix<double, 3, 1>(0.0, 0.0, 0.0);
// TODO< put into method >
for( int attachedForceIterator = 0; attachedForceIterator < currentBody->attachedForces.getCount(); attachedForceIterator++ ) {
SharedPointer<AttachedForce> currentAttachedForce = currentBody->attachedForces[attachedForceIterator];
// TODO< split each force into linear and angular components >
Eigen::Matrix<double, 3, 1> angularComponentForce = currentAttachedForce->forceInNewton;
Eigen::Matrix<double, 3, 1> appliedTorque = calculateTorque(currentAttachedForce->objectLocalPosition, angularComponentForce);
Eigen::Matrix<double, 3, 1> rotationalAcceleration = calculateRotationalAcceleration(currentBody->inertiaTensor, appliedTorque);
currentBody->angularVelocityDelta += (rotationalAcceleration * timeDelta);
}
static_cast<AccelerationImplementation*>(rungeKutta4.acceleration)->setCurrentBodyMass(currentBody->mass);
rungeKutta4.integrate(currentBody->rungeKuttaState, 0.0, static_cast<double>(timeDelta));
}
// TODO< put into own method >
for( int index = 0; index < fastParticles.getCount(); index++ ) {
SharedPointer<FastParticle> currentParticle = fastParticles[index];
currentParticle->nextCurrentPosition = currentParticle->currentPosition + (currentParticle->velocity * timeDelta);
}
}
void ODE_LinearSpringModel::updateInputValues() {
if(mOwnerSpringAdapter == 0 || mOwnerSpringAdapter->getCurrentTargetJoint() == 0) {
return;
}
//The first time an update is run the mJoint is located.
//This has to be done here instead of setup() to ensure that the joint is
//really available, which can not be guaranteed in setup().
if(mJoint == 0) {
SimJoint *simJoint = mOwnerSpringAdapter->getCurrentTargetJoint();
ODE_Joint *odeJoint = dynamic_cast<ODE_Joint*>(simJoint);
if(odeJoint == 0) {
MotorAdapter *adapter = dynamic_cast<MotorAdapter*>(simJoint);
if(adapter != 0) {
odeJoint = dynamic_cast<ODE_Joint*>(adapter->getActiveMotorModel());
}
}
if(simJoint != 0 && odeJoint != 0) {
if(getType() == MotorModel::HINGE_JOINT) {
if(dynamic_cast<HingeJoint*>(simJoint) != 0) {
mJoint = odeJoint->getJoint();
}
}
else if(getType() == MotorModel::SLIDER_JOINT) {
if(dynamic_cast<SliderJoint*>(simJoint) != 0) {
mJoint = odeJoint->getJoint();
}
}
}
}
if(mJoint != 0) {
if(getType() == MotorModel::HINGE_JOINT) {
double torque = calculateTorque(dJointGetHingeAngle(mJoint));
dJointAddHingeTorque(mJoint, torque);
mOwnerSpringAdapter->getCurrentTorqueValue()->set(torque);
}
else if(getType() == MotorModel::SLIDER_JOINT) {
double force = calculateForce(dJointGetSliderPosition(mJoint));
dJointAddSliderForce(mJoint, force);
mOwnerSpringAdapter->getCurrentTorqueValue()->set(force);
}
}
}