void HarrierSim::simLoop()
{
//set the trusters
float thVar = 5.0;
dBodyAddRelForceAtRelPos(itsHarrierBody,0,itsUpThruster+randomDoubleFromNormal(thVar),0, 0,0,itsHarrierLength/4);
dBodyAddRelForceAtRelPos(itsHarrierBody,0,itsUpThruster+randomDoubleFromNormal(thVar),0, 0,0,-itsHarrierLength/4);
dBodyAddRelForceAtRelPos(itsHarrierBody,0,itsUpThruster+randomDoubleFromNormal(thVar),0, itsHarrierLength/4,0,0);
dBodyAddRelForceAtRelPos(itsHarrierBody,0,itsUpThruster+randomDoubleFromNormal(thVar),0, -itsHarrierLength/4,0,0);
dBodyAddRelForceAtRelPos(itsHarrierBody,itsPanThruster,0,0, 0,0,itsHarrierLength/2);
dBodyAddRelForceAtRelPos(itsHarrierBody,0,itsPitchThruster,0, 0,0,itsHarrierLength/2);
dBodyAddRelForceAtRelPos(itsHarrierBody,0,-itsPitchThruster,0, 0,0,-1*itsHarrierLength/2);
dBodyAddRelForceAtRelPos(itsHarrierBody,0,itsRollThruster,0, itsHarrierWidth*2,0,0);
dBodyAddRelForceAtRelPos(itsHarrierBody,0,-itsRollThruster,0, -1*itsHarrierWidth*2,0,0);
//Apply a viscosity water force
//applyHydrodynamicForces(0.5);
//Folow the sub with the camera
// const double *bodyPos = dBodyGetPosition(itsSubBody);
//const double *bodyR = dBodyGetRotation(itsSubBody);
const dReal *bodyPos = dBodyGetPosition(itsHarrierBody);
const dReal *bodyR = dBodyGetRotation(itsHarrierBody);
updateSensors(bodyPos, bodyR);
dSpaceCollide (space,this,&nearCallback); //check for collisions
dWorldStep(world,0.1);
dJointGroupEmpty (contactgroup); //delete the contact joints
}
void RigidBody::addRelativeForceAtRelativePosition(const ngl::Vec3 &_f,const ngl::Vec3 &_p)
{
dBodyAddRelForceAtRelPos(m_id,_f.m_x,_f.m_y,_f.m_z,_p.m_x,_p.m_y,_p.m_z);
}
void SParts::addRelForceAtRelPos(dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz)
{
dBodyAddRelForceAtRelPos(m_odeobj->body(), fx, fy, fz, px, py, pz);
}
void PhysicsBody::addRelForceAtRelPos(const Vec3f &v, const Vec3f &p)
{
dBodyAddRelForceAtRelPos(_BodyID, v.x(), v.y(), v.z(), p.x(), p.y(), p.z());
}
