void Simulate(int pause) {
if ( showGraphics )
envs->Draw();
if ( envs->recordingVideo )
envs->Video_Record();
if ( !pause ) {
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,STEP_SIZE);
dJointGroupEmpty(contactgroup);
if ( envs->In_Evolution_Mode() )
envs->Evolve( world,
space);
else if ( envs->In_Champ_Mode() )
envs->Show_Champ( world,
space);
}
}
static void simLoop (int pause)
{
if (!pause) {
// add random forces and torques to all bodies
int i;
const dReal scale1 = 5;
const dReal scale2 = 5;
for (i=0; i<NUM; i++) {
dBodyAddForce (body[i],
scale1*(dRandReal()*2-1),
scale1*(dRandReal()*2-1),
scale1*(dRandReal()*2-1));
dBodyAddTorque (body[i],
scale2*(dRandReal()*2-1),
scale2*(dRandReal()*2-1),
scale2*(dRandReal()*2-1));
}
dWorldStep (world,0.05);
createTest();
}
// float sides[3] = {SIDE,SIDE,SIDE};
dsSetColor (1,1,0);
for (int i=0; i<NUM; i++)
dsDrawSphere (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),RADIUS);
}
static int SimulationThread(void *unused)
{
dAllocateODEDataForThread(dAllocateFlagCollisionData);
if (SDL_SetThreadPriority(SDL_THREAD_PRIORITY_HIGH))
{
//SDL_perror("SDL_SetThreadPriority");
}
while (!Quit)
{
if (SDL_LockMutex(Mutex))
{
SDL_perror("SDL_LockMutex");
break;
}
if (SDL_CondWait(Cond, Mutex))
{
SDL_perror("SDL_CondWait");
break;
}
PushEvent(UPDATE);
dSpaceCollide(Space, 0, &Near);
dWorldStep(World, Step);
dJointGroupEmpty(Group);
SDL_UnlockMutex(Mutex);
}
dCleanupODEAllDataForThread();
return 0;
}
bool Simulator::SimulateOneStep(const double speed, const double steer)
{
//set desired speed
dJointSetHinge2Param(m_robotJoints[0], dParamVel2, -speed); // hinge-2 velocity = -speed
dJointSetHinge2Param(m_robotJoints[0], dParamFMax2, 10); // maximum torque = 0.1
//set desired steering angle
dReal v = steer - dJointGetHinge2Angle1 (m_robotJoints[0]);
//if (v > 0.1) v = 0.1;
//if (v < -0.1) v = -0.1;
v *= 10.0;
dJointSetHinge2Param(m_robotJoints[0], dParamVel, v);
dJointSetHinge2Param(m_robotJoints[0], dParamFMax, 0.2);
dJointSetHinge2Param(m_robotJoints[0], dParamLoStop, -0.75);
dJointSetHinge2Param(m_robotJoints[0], dParamHiStop, 0.75);
dJointSetHinge2Param(m_robotJoints[0], dParamFudgeFactor, 0.1);
//simulate the dynamics for one time step (set to 0.05)
m_collision = false;
dSpaceCollide(m_space, reinterpret_cast<void *>(this), &CollisionCheckingCallbackFn);
dWorldStep(m_world, 0.2); // setting time step for integral
dJointGroupEmpty(m_contacts);
return !m_collision;
}
// シミュレーションループ
static void simLoop(int pause) {
control();
dSpaceCollide(space,0,&nearCallback); // 衝突検出計算
dWorldStep(world,0.0001); // 1ステップ進める
dJointGroupEmpty(contactgroup); // 衝突変数をリセット
draw(); // ロボットの描画
}
//"Pause" key pauses this loop, and any other key resumes it
static void simLoop (int pause)
{
static int numSimSteps = 0; // Number of steps
static int jointAngleStep = 0; //
/* control(): Assign the angle to the joint: target_angleR/L[i] to jointR/L[i] */
control();
if (!pause) // If “pause” key is not pressed
{
dSpaceCollide(space,0,&nearCallback); // Detect colision: nearCallback
dWorldStep(world,SIM_STEP); // Step of simulation
dJointGroupEmpty(contactgroup); // Clear container of collision points
numSimSteps++; // Increases number of steps
//feedback = dJointGetFeedback(joint1); // Get joint1 feedback
// Assign the precomputed angles to the joints
if(numSimSteps%50 == 0 && jointAngleStep < TLENG)
{
for(int i=0;i<MAX_JOINTS;i++)
{
target_angleR[i] = PreComputedAngleR[jointAngleStep][i];
target_angleL[i] = PreComputedAngleL[jointAngleStep][i];
}
jointAngleStep++;
}
}
Model::rDrawRobot(); // Draw robot with previously specified joint angles (in control() )
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldStep (world,0.05);
//if (!pause) dWorldStepFast (world,0.05, 1);
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (i==selected) {
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
/*{
for (int i = 1; i < IndexCount; i++) {
dsDrawLine(Vertices[Indices[i - 1]], Vertices[Indices[i]]);
}
}*/
{const dReal* Pos = dGeomGetPosition(TriMesh);
const dReal* Rot = dGeomGetRotation(TriMesh);
{for (int i = 0; i < IndexCount / 3; i++){
const dVector3& v0 = Vertices[Indices[i * 3 + 0]];
const dVector3& v1 = Vertices[Indices[i * 3 + 1]];
const dVector3& v2 = Vertices[Indices[i * 3 + 2]];
dsDrawTriangle(Pos, Rot, (dReal*)&v0, (dReal*)&v1, (dReal*)&v2, 0);
}}}
if (Ray){
dVector3 Origin, Direction;
dGeomRayGet(Ray, Origin, Direction);
dReal Length = dGeomRayGetLength(Ray);
dVector3 End;
End[0] = Origin[0] + (Direction[0] * Length);
End[1] = Origin[1] + (Direction[1] * Length);
End[2] = Origin[2] + (Direction[2] * Length);
End[3] = Origin[3] + (Direction[3] * Length);
dsDrawLine(Origin, End);
}
}
static void simLoop (int pause)
{
// stop after a given number of iterations, as long as we are not in
// interactive mode
if (cmd_graphics && !cmd_interactive &&
(iteration >= max_iterations)) {
dsStop();
return;
}
iteration++;
if (!pause) {
// do stuff for this test and check to see if the joint is behaving well
dReal error = doStuffAndGetError (test_num);
if (error > max_error) max_error = error;
if (cmd_interactive && error < dInfinity) {
printf ("scaled error = %.4e\n",error);
}
// take a step
dWorldStep (world,STEPSIZE);
// occasionally re-orient the first body to create a deliberate error.
if (cmd_occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
}
if (cmd_graphics) {
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE*0.99f,SIDE*0.99f,SIDE*0.99f};
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
if (body[1]) {
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
}
}
// Simulation loop
void simLoop(int pause) {
control();
dWorldStep(world, 0.02);
const double* var;
// Draw a robot
dsSetColor(1.0,1.0,1.0); // Set color (r, g, b), In this case white is set
for (int i = 0; i < NUM; i++ ){// Draw capsules for links
dsDrawCapsuleD(var, var , l[i], r[i]);
}
}
void CPHIsland:: Step(dReal step)
{
if(!m_flags.is_active()) return;
//dWorldStepFast1 (DWorld(), fixed_step, phIterations/*+Random.randI(0,phIterationCycle)*/);
if(m_flags.is_exact_integration_prefeared() && nj < max_joint_allowed_for_exeact_integration)
dWorldStep(DWorld(),fixed_step);
else
dWorldQuickStep (DWorld(), fixed_step);
//dWorldStep(DWorld(),fixed_step);
}
static void simLoop (int pause)
{
const dReal kd = -0.3; // angular damping constant
const dReal ks = 0.5; // spring constant
if (!pause) {
// add an oscillating torque to body 0, and also damp its rotational motion
static dReal a=0;
const dReal *w = dBodyGetAngularVel (body[0]);
dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
a += 0.01;
// add a spring force to keep the bodies together, otherwise they will
// fly apart along the slider axis.
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),
ks*(p2[2]-p1[2]));
dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),
ks*(p1[2]-p2[2]));
// occasionally re-orient one of the bodies to create a deliberate error.
if (occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
dWorldStep (world,0.05);
}
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE*0.8f,SIDE*0.8f,SIDE*2.0f};
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
void PWorld::step(dReal dt)
{
try {
dSpaceCollide (space,this,&nearCallback);
dWorldStep (world,(dt<0) ? delta_time : dt);
dJointGroupEmpty (contactgroup);
}
catch (...)
{
//qDebug() << "Some Error Happened;";
}
}
static void simLoop (int pause)
{
const dReal *pos1,*R1;
dsSetColorAlpha (1,1,1,1);
dSpaceCollide (space,0,&nearCallback);
dJointGroupEmpty (contactgroup);
dWorldStep(world,0.01);
pos1 = dBodyGetPosition(capsule);
R1 = dBodyGetRotation(capsule);
dsDrawCapsule(pos1,R1,length,radius); // draw a capsule
}
void simLoop(int pause) {
//control();
dWorldStep(world, 0.0001);
for ( std::map<size_t, DrawBody *>::const_iterator it = DrawBody::getDrawMap().begin(); it != DrawBody::getDrawMap().end(); ++it){
DrawBody * drawBody = it->second;
if (drawBody) {
drawBody->doDraw();
}
}
}
/*** Simulation loop ***/
void simLoop(int pause)
{
if (!pause and !pauseGlobal) {
if(shouldIKeepRunningTheSimulation()){
walk(); // control of walking
dSpaceCollide(space,0,&nearCallback); // collision detection
#ifndef USE_NLEG_ROBOT
dWorldStep(world, 0.001); // jmc: original was 0.01 but it was crashing
#else
dWorldStep(world, 0.001);
// dWorldQuickStep(world, 0.001); // jmc: original was 0.01 but it was crashing
#endif
dJointGroupEmpty(contactgroup); // Point of contact group sky
}
#ifndef NOVIZ
else dsStop(); //end the simulation
#endif
}
#ifdef USE_NLEG_ROBOT
drawRobot_Nleg();
#else
drawRobot();
#endif
}
//executes one step of simulation
void simulationStep(bool bMoviePlay)
{
double timestep=0.01;
if (!bMoviePlay)
{
dSpaceCollide (space,0,&nearCallback);
dWorldStep(world,timestep);
}
for (int x=0; x<creatures.size(); x++)
creatures[x]->Update(timestep);
if (!bMoviePlay)
dJointGroupEmpty (contactgroup);
}
/* ------------------------
* シミュレーションループ
------------------------ */
static void simLoop(int pause)
{
/* ローカル変数の定義 */
double vx, vy, vx1, vy1, vx2, vy2, x, y;
double sup_x[2], sup_y[2];
t2 = clock();
dSpaceCollide(space, 0, &nearCallback);
GetOmniPotision(&x, &y, 0);
printf("Position(x, y) = (%lf, %lf) \r", x, y);
VectorRoute(x, y, routeX, routeY, lineRoute-2, &vx1, &vy1);
VectorObst(x, y, obstX, obstY, lineObst-1, 1.0, 1.0, 0.0, &vx2, &vy2);
vx = vx1+vx2;
vy = vy1+vy2;
/* 停留点にはまった場合(障害物回避ベクトルを切る) */
if(abs(vx)<0.2 || abs(vy)<0.2)
{
vx = vx1*2;
vy = vy1*2;
}
//ControlOmni(vx, vy, 0);
dWorldStep(world, 0.01);
dJointGroupEmpty(contactgroup);
DrawOmni();
/* 経路の表示 */
//DrawLine(routeX, routeY, 0.01, lineRoute, 1.0, 0.2, 0.2);
DrawBox();
//SetCamera(x, y-0.15, 0.45, 90.0, 0.0, 0.0);
dt = t2-t1;
/* 1000[ms]毎にロボットが移動した経路を更新 */
if(dt >1000)
{
t1 = clock();
omniX[line] = x;
omniY[line] = y;
line++;
/* ロボットの移動経路をtxtファイルに記録 */
SaveTxt("omni.txt", omniX, omniY, omniZ, line);
}
/* 自律制御による移動経路の表示 */
//DrawLine(omniX, omniY, 0.01, line, 0.2, 0.8, 0.2);
/* 補助線の表示 */
sup_x[0] = x;
sup_x[1] = x+vx1;
sup_y[0] = y;
sup_y[1] = y+vy1;
DrawLine(sup_x, sup_y, 0.4, 3, 1.0, 0.1, 0.0);
}
static void simLoop (int pause)
{
int i;
if (!pause) {
// motor
dJointSetHinge2Param (joint[0],dParamVel2,-speed);
dJointSetHinge2Param (joint[0],dParamFMax2,0.1);
// steering
dReal v = steer - dJointGetHinge2Angle1 (joint[0]);
if (v > 0.1) v = 0.1;
if (v < -0.1) v = -0.1;
v *= 10.0;
dJointSetHinge2Param (joint[0],dParamVel,v);
dJointSetHinge2Param (joint[0],dParamFMax,0.2);
dJointSetHinge2Param (joint[0],dParamLoStop,-0.75);
dJointSetHinge2Param (joint[0],dParamHiStop,0.75);
dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
// remove all contact joints
dJointGroupEmpty (contactgroup);
}
dsSetColor (0,1,1);
dsSetTexture (DS_WOOD);
dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
dsSetColor (1,1,1);
for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]),
dBodyGetRotation(body[i]),0.02f,RADIUS);
dVector3 ss;
dGeomBoxGetLengths (ground_box,ss);
dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
/*
printf ("%.10f %.10f %.10f %.10f\n",
dJointGetHingeAngle (joint[1]),
dJointGetHingeAngle (joint[2]),
dJointGetHingeAngleRate (joint[1]),
dJointGetHingeAngleRate (joint[2]));
*/
}
void CDynamics3DEngine::Update() {
/* Update the physics state from the entities */
for(TDynamics3DEntityMap::iterator it = m_tPhysicsEntities.begin();
it != m_tPhysicsEntities.end(); ++it) {
it->second->UpdateFromEntityStatus();
}
/* Remove contact joints to be ready to calculate new ones */
dJointGroupEmpty(m_tContactJointGroupID);
/* Check collisions inside the space */
SGeomCheckData sGeomCheckData(*this);
dSpaceCollide(m_tSpaceID, &sGeomCheckData, &ManageCloseGeomsAddingContactJointsCallback);
/* Perform the physics step */
dWorldStep(m_tWorldID, m_fSimulationClockTick);
/* Update the simulated space */
for(TDynamics3DEntityMap::iterator it = m_tPhysicsEntities.begin();
it != m_tPhysicsEntities.end(); ++it) {
it->second->UpdateEntityStatus();
}
}
/*** シミュレーションループ ***/
static void simLoop(int pause)
{
if (!pause)
{
dSpaceCollide(space,0,&nearCallback); // add
control();
dWorldStep(world, 0.01);
dJointGroupEmpty(contactgroup); // add
}
const dReal *linear_vel = dBodyGetLinearVel(base.body);
const dReal *angular_vel = dBodyGetAngularVel(base.body);
printf("linear : %.3f %.3f %.3f\n", linear_vel[0], linear_vel[1], linear_vel[2]);
printf("angular: %.3f %.3f %.3f\n", angular_vel[0], angular_vel[1], angular_vel[2]);
drawBase();
drawWheel(); // add
drawBall(); //add
drawGoal();
}
static void simLoop (int pause)
{
int i;
if (!pause) {
static double angle = 0;
angle += 0.05;
dBodyAddForce (body[NUM-1],0,0,1.5*(sin(angle)+1.0));
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
/* remove all contact joints */
dJointGroupEmpty (contactgroup);
}
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (i=0; i<NUM; i++) dsDrawSphere (dBodyGetPosition(body[i]),
dBodyGetRotation(body[i]),RADIUS);
}
void PhysicsServer::StepSolver( float dt )
{
// process collision
dJointGroupEmpty( m_ContactGroupID );
m_NContacts = 0;
dSpaceCollide( m_DefaultSpaceID, this, OnCollide );
// perform integration
if (m_Mode == smStepFast)
{
dWorldStepFast1( m_WorldID, dt, m_MaxIter );
}
else if (m_Mode == smNormal)
{
dWorldStep( m_WorldID, dt );
}
else if (m_Mode == smQuickStep)
{
dWorldQuickStep( m_WorldID, dt );
}
} // PhysicsServer::StepSolver
void Simulation::doSimulationStep()
{
++simulationStep;
simulatedTime += scene->stepLength;
scene->updateActuators();
collisions = contactPoints = 0;
dSpaceCollide2((dGeomID)staticSpace, (dGeomID)movableSpace, this, (dNearCallback*)&staticCollisionWithSpaceCallback);
if(scene->detectBodyCollisions)
dSpaceCollide(movableSpace, this, (dNearCallback*)&staticCollisionSpaceWithSpaceCallback);
if(scene->useQuickSolver && (simulationStep % scene->quickSolverSkip) == 0)
dWorldQuickStep(physicalWorld, scene->stepLength);
else
dWorldStep(physicalWorld, scene->stepLength);
dJointGroupEmpty(contactGroup);
updateFrameRate();
}
static void simLoop(int pause)
{
const dReal *pos1, *R1, *pos2, *R2;
dSpaceCollide(space, 0, &nearCallback); // add, Write this first
dWorldStep(world, 0.01);
dJointGroupEmpty(contactgroup); // add
dsSetColor(1.0, 0.0, 0.0);
// draw a ball
pos1 = dBodyGetPosition(ball.body);
R1 = dBodyGetRotation(ball.body);
dsDrawSphere(pos1, R1, ball.radius);
// draw a ccylinder
pos2 = dBodyGetPosition(pole.body);
R2 = dBodyGetRotation(pole.body);
dsDrawCapsule(pos2, R2, pole.length, pole.radius);
}
//Fonction de boucle de simulation
void simLoop (int pause){
int i;
if (!pause) {
//Affichage du pas de simulation
//printf("%d\n",nb_pas_simulation);
nb_pas_simulation++;
//pas de simulation pour les agents
for( i=0 ; i<NB_AGENTS ; i++ ){
//on tue les agents qui sont à terre !!!
//noyade( &(agents[i]) );
//si un agent arrive au bord il s'arrete
//arret_au_bord( &(agents[i]) );
//Chaque agent accompli ses actions
step( &(agents[i]) );
}
//collisions eventuelles
dSpaceCollide (space, 0, &nearCallback );
//un pas de simulation
dWorldStep (world, PAS_SIMU );
//dWorldStepFast1 ((world, PAS_SIMU, 1 );
//nettoyage apres collision
dJointGroupEmpty (contactgroup);
}
//dessin de la scène
//if ( nb_pas_simulation % 1 == 0 ) {
dessin_env( &bloc_depart, &bloc_arrive );
for( i=0 ; i < NB_AGENTS ; i++ ){ dessinAgent( &(agents[i]) ); }
dessin_attractant( &attract );
//}
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldStep (world,0.05);
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
int color_changed = 0;
if (i==selected) {
dsSetColor (0,0.7,1);
color_changed = 1;
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
color_changed = 1;
}
for (int j=0; j < GPB; j++) drawGeom (obj[i].geom[j],0,0);
if (color_changed) dsSetColor (1,1,0);
}
{for (int i = 0; i < RayCount; i++){
dVector3 Origin, Direction;
dGeomRayGet(Rays[i], Origin, Direction);
dReal Length = dGeomRayGetLength(Rays[i]);
dVector3 End;
End[0] = Origin[0] + (Direction[0] * Length);
End[1] = Origin[1] + (Direction[1] * Length);
End[2] = Origin[2] + (Direction[2] * Length);
End[3] = Origin[3] + (Direction[3] * Length);
dsDrawLine(Origin, End);
}}
}
void WorldPhysics::mainLoop ()
{
for (int i=0;i<4;i++) {
if (i%2) {
dJointSetHinge2Param (wheelJoints[i],dParamVel2,-speed+dspeed);
dJointSetHinge2Param (wheelJoints[i],dParamFMax2,10);
} else
{
dJointSetHinge2Param (wheelJoints[i],dParamVel2,-speed-dspeed);
dJointSetHinge2Param (wheelJoints[i],dParamFMax2,10);
}
if (i>=2) {
dJointSetHinge2Param (wheelJoints[i],dParamVel,0);
dJointSetHinge2Param (wheelJoints[i],dParamFMax,100);
dJointSetHinge2Param (wheelJoints[i],dParamLoStop,0);
dJointSetHinge2Param (wheelJoints[i],dParamHiStop,0);
dJointSetHinge2Param (wheelJoints[i],dParamFudgeFactor,0.1);
}
if (i<2) {
// steering
dReal v = steer - dJointGetHinge2Angle1 (wheelJoints[i]);
if (v > 0.1) v = 0.1;
if (v < -0.1) v = -0.1;
v *= 10.0;
dJointSetHinge2Param (wheelJoints[i],dParamVel,v);
dJointSetHinge2Param (wheelJoints[i],dParamFMax,5);
dJointSetHinge2Param (wheelJoints[i],dParamLoStop,-1);
dJointSetHinge2Param (wheelJoints[i],dParamHiStop,1);
dJointSetHinge2Param (wheelJoints[i],dParamFudgeFactor,0.1);
}
}
dSpaceCollide (space,this,&nearCallback);
dWorldStep (world,0.05);
// remove all contact joints
dJointGroupEmpty (contactgroup);
}
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
}
// update simulation state
bool PhysicsServer::update(double step)
{
if (_world == 0 || _space == 0)
return false;
// handle collisions
dSpaceCollide (_space, this, &collisionCallback);
// update all clients
list<PhysicsClient*>::const_iterator iter = _clientList.begin();
while(iter != _clientList.end() )
{
(*iter)->update(step);
iter++;
}
// step world
dWorldStep (_world, step);
// remove all contact joints
dJointGroupEmpty (_contactgroup);
return true;
}
void ODEWorld::nextStep(double stepsize, bool quick)
{
s_chash.clear();
s_collisions.clear();
dWorldID w = world();
dSpaceID s = space();
dSpaceCollide(s, 0, &nearCB);
// Quick Mode
if (quick) {
dWorldQuickStep(w, stepsize);
}
else {
dWorldStep(w, stepsize);
}
//dJointGroupID jgroup = jointGroup();
dJointGroupEmpty(m_jgroup);
m_itrCnt++;
m_nowtime += stepsize;
}