void CPhysicManager::Update(float tms){
calc_time+=tms;
// printf("update %f\n",calc_time);
// float full_time = tms;
// float dt=1.0/30.0f;
//if ( dt > tms ) dt = tms;
if (calc_time>10) {
//fast!!
dSpaceCollide (space,0,&nearCallback);
dWorldQuickStep(world, tms );
dJointGroupEmpty(contactgroup);
calc_time=0.0f;
}
else
while (calc_time>1.0/30.0f) {
dSpaceCollide (space,0,&nearCallback);
dWorldQuickStep(world, (1.0/30.0f) );
dJointGroupEmpty(contactgroup);
calc_time-=1.0/30.0f;
};
};
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldQuickStep (world,0.02);
if (write_world) {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (world,f,"X");
fclose (f);
}
write_world = 0;
}
if (doFeedback)
{
if (fbnum>MAX_FEEDBACKNUM)
printf("joint feedback buffer overflow!\n");
else
{
dVector3 sum = {0, 0, 0};
printf("\n");
for (int i=0; i<fbnum; i++) {
dReal* f = feedbacks[i].first?feedbacks[i].fb.f1:feedbacks[i].fb.f2;
printf("%f %f %f\n", f[0], f[1], f[2]);
sum[0] += f[0];
sum[1] += f[1];
sum[2] += f[2];
}
printf("Sum: %f %f %f\n", sum[0], sum[1], sum[2]);
dMass m;
dBodyGetMass(obj[selected].body, &m);
printf("Object G=%f\n", GRAVITY*m.mass);
}
doFeedback = 0;
fbnum = 0;
}
// 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.8,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
void Level::step()
{
if (--stepct_ == 0) {
manager->sweep();
stepct_ = 5*int(1/STEP);
}
if (!frozen_) {
manager->step();
collide();
dWorldQuickStep(world, STEP);
dJointGroupEmpty(contact_joints);
if ((freeze_timer_ -= STEP) <= 0) {
freeze();
}
}
else if (!player) {
if ((wait_timer_ -= STEP) <= 0) {
player = next_player_;
MOUSE_FOCUS = next_player_;
wait_timer_ = 0;
}
}
}
void avancarPasso(dReal dt)
{
dWorldSetQuickStepNumIterations (world, 20);
dSpaceCollide(space,0,&nearCallback);
dWorldQuickStep(world, dt); // Esse valor deve ser sincronizado com as chamadas da função.
dJointGroupEmpty(contactgroup);
}
// simulation loop
static void simLoop (int pause)
{
const dReal *pos;
const dReal *R;
// force for the spheres
// find collisions and add contact joints
dSpaceCollide (space,0,&nearCallback);
// step the simulation
dWorldQuickStep (world,0.01);
// remove all contact joints
dJointGroupEmpty (contactgroup);
// redraw sphere at new location
pos = dGeomGetPosition (sphere0_geom);
R = dGeomGetRotation (sphere0_geom);
dsDrawSphere (pos,R,dGeomSphereGetRadius (sphere0_geom));
pos = dGeomGetPosition (sphere1_geom);
R = dGeomGetRotation (sphere1_geom);
dsDrawSphere (pos,R,dGeomSphereGetRadius (sphere1_geom));
pos = dGeomGetPosition (sphere2_geom);
R = dGeomGetRotation (sphere2_geom);
dsDrawSphere (pos,R,dGeomSphereGetRadius (sphere2_geom));
}
void ompl::control::OpenDEStatePropagator::propagate(const base::State *state, const Control* control, const double duration, base::State *result) const
{
env_->mutex_.lock();
// place the OpenDE world at the start state
si_->getStateSpace()->as<OpenDEStateSpace>()->writeState(state);
// apply the controls
env_->applyControl(control->as<RealVectorControlSpace::ControlType>()->values);
// created contacts as needed
CallbackParam cp = { env_.get(), false };
for (unsigned int i = 0 ; i < env_->collisionSpaces_.size() ; ++i)
dSpaceCollide(env_->collisionSpaces_[i], &cp, &nearCallback);
// propagate one step forward
dWorldQuickStep(env_->world_, (const dReal)duration);
// remove created contacts
dJointGroupEmpty(env_->contactGroup_);
// read the final state from the OpenDE world
si_->getStateSpace()->as<OpenDEStateSpace>()->readState(result);
env_->mutex_.unlock();
// update the collision flag for the start state, if needed
if (!(state->as<OpenDEStateSpace::StateType>()->collision & (1 << OpenDEStateSpace::STATE_COLLISION_KNOWN_BIT)))
{
if (cp.collision)
state->as<OpenDEStateSpace::StateType>()->collision &= (1 << OpenDEStateSpace::STATE_COLLISION_VALUE_BIT);
state->as<OpenDEStateSpace::StateType>()->collision &= (1 << OpenDEStateSpace::STATE_COLLISION_KNOWN_BIT);
}
}
void Scene::step( void )
{
//Our internal units are pixels, and at 160dpi there are 6300 pixels per meter
float gravity[3];
gravity[0] = (m_motion_lopass[0]*1 + m_motion_hipass[0]*10) * 6300;
gravity[1] = (m_motion_lopass[1]*1 + m_motion_hipass[1]*10) * 6300;
gravity[2] = (m_motion_lopass[2]*1 + m_motion_hipass[2]*10) * 6300;
float hisq = m_motion_hipass[0]*m_motion_hipass[0] + m_motion_hipass[1]*m_motion_hipass[1] + m_motion_hipass[2]*m_motion_hipass[2];
if( hisq > 1.0f )
{
int num_woken = 0;
for( SceneObj *obj_p=m_obj_p; obj_p; obj_p=obj_p->m_next_p )
{
if( !dBodyIsEnabled(obj_p->m_dbody) )
{
dBodyEnable( obj_p->m_dbody );
num_woken++;
if( num_woken == 3 )
break;
}
}
}
dWorldSetGravity( m_dworld, gravity[0], gravity[1], gravity[2] );
do_collision();
dWorldQuickStep( m_dworld, 1.0f/60.0f );
dJointGroupEmpty( m_colljoints );
}
void simLoop (int pause)
{
if (!pause) {
const dReal step = 0.02;
const unsigned nsteps = 1;
for (unsigned i=0; i<nsteps; ++i) {
dSpaceCollide(space, 0, nearCallback);
dWorldQuickStep(world, step);
dJointGroupEmpty(contact_group);
}
} else {
dSpaceCollide(space, 0, nearCallback);
dJointGroupEmpty(contact_group);
}
// now we draw everything
unsigned ngeoms = dSpaceGetNumGeoms(space);
for (unsigned i=0; i<ngeoms; ++i) {
dGeomID g = dSpaceGetGeom(space, i);
if (g == ground)
continue; // drawstuff is already drawing it for us
drawGeom(g);
}
if (dBodyGetPosition(ball1_body)[0] < -track_len)
resetSim();
}
int main(int argc, char** argv)
{
init_gfx();
init_2d();
// create world and set gravity
world = dWorldCreate();
dWorldSetGravity(world, 0, -5, 0);
// create a body for our ball
dBodyID body = dBodyCreate(world);
dBodySetPosition(body, 0, 0, 0);
for (;;) { // infinite loop
// get the position and move there in OpenGL
const dReal* pos = dBodyGetPosition(body);
glTranslatef(pos[0], pos[1], pos[2]);
draw_circle();
// integrate all bodies
dWorldQuickStep(world, 0.01);
// redraw screen
step();
}
}
int main(int argc, char** argv)
{
init_gfx();
init_2d();
world = dWorldCreate();
dBodyID ball = dBodyCreate(world);
dBodySetPosition(ball, 0, 0, 0);
for (;;) {
Uint8* chars = SDL_GetKeyState(NULL);
if (chars[SDLK_LEFT]) {
dBodyAddForce(ball, -force, 0, 0);
}
if (chars[SDLK_RIGHT]) {
dBodyAddForce(ball, +force, 0, 0);
}
if (chars[SDLK_UP]) {
dBodyAddForce(ball, 0, +force, 0);
}
if (chars[SDLK_DOWN]) {
dBodyAddForce(ball, 0, -force, 0);
}
const dReal* pos = dBodyGetPosition(ball);
glTranslatef(pos[0], pos[1], pos[2]);
draw_circle();
dWorldQuickStep(world, 0.01);
step();
}
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldQuickStep (world,0.05);
if (write_world) {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (world,f,"X");
fclose (f);
}
write_world = 0;
}
// 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.8,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
static void simLoop (int pause)
{
dSpaceCollide (space,0,&nearCallback);
if (!pause)
{
dWorldQuickStep (world, 0.01); // 100 Hz
}
dJointGroupEmpty (contactgroup);
dsSetColorAlpha (1,1,0,0.5);
const dReal *CPos = dBodyGetPosition(cylbody);
const dReal *CRot = dBodyGetRotation(cylbody);
float cpos[3] = {CPos[0], CPos[1], CPos[2]};
float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] };
dsDrawCylinder
(
cpos,
crot,
CYLLENGTH,
CYLRADIUS
); // single precision
const dReal *SPos = dBodyGetPosition(sphbody);
const dReal *SRot = dBodyGetRotation(sphbody);
float spos[3] = {SPos[0], SPos[1], SPos[2]};
float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] };
dsDrawSphere
(
spos,
srot,
SPHERERADIUS
); // single precision
}
/***********************************************************
Process function
***********************************************************/
void ODEPhysicHandler::Process()
{
double currentime = SynchronizedTimeHandler::getInstance()->GetCurrentTimeDouble();
float tdiff = (float)(currentime - _lasttime);
static float nbSecondsByStep = 0.001f;
// Find the time elapsed between last time
float nbSecsElapsed = tdiff/1000.0f;
// Find the corresponding number of steps that must be taken
int nbStepsToPerform = static_cast<int>(nbSecsElapsed/nbSecondsByStep);
// Make these steps to advance world time
for (int i=0;i<nbStepsToPerform;++i)
{
// Detect collision
dSpaceCollide(_space, this, &nearCallback);
// Step world
dWorldQuickStep(_world, nbSecondsByStep);
// Remove all temporary collision joints now that the world has been stepped
dJointGroupEmpty(_contactgroup);
}
_lasttime = currentime;
}
int physics_step(float dt)
{
dSpaceCollide(space, 0, callback);
dWorldQuickStep(world, dt);
dJointGroupEmpty(group);
return 1;
}
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)
{
double simstep = 0.001; // 1ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf(dt/simstep);
// fprintf(stderr, "dt=%f, nr of steps = %d\n", dt, nrofsteps);
for (int i=0; i<nrofsteps && !pause; i++)
{
dSpaceCollide (space,0,&nearCallback);
dWorldQuickStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
dsSetColor (1,1,1);
const dReal *SPos = dBodyGetPosition(sphbody);
const dReal *SRot = dBodyGetRotation(sphbody);
float spos[3] = {SPos[0], SPos[1], SPos[2]};
float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] };
dsDrawSphere
(
spos,
srot,
RADIUS
);
// draw world trimesh
dsSetColor(0.4,0.7,0.9);
dsSetTexture (DS_NONE);
const dReal* Pos = dGeomGetPosition(world_mesh);
//dIASSERT(dVALIDVEC3(Pos));
float pos[3] = { Pos[0], Pos[1], Pos[2] };
const dReal* Rot = dGeomGetRotation(world_mesh);
//dIASSERT(dVALIDMAT3(Rot));
float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] };
int numi = sizeof(world_indices) / sizeof(int);
for (int i=0; i<numi/3; i++)
{
int i0 = world_indices[i*3+0];
int i1 = world_indices[i*3+1];
int i2 = world_indices[i*3+2];
float *v0 = world_vertices+i0*3;
float *v1 = world_vertices+i1*3;
float *v2 = world_vertices+i2*3;
dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw
}
}
void ode::drawBall()
{
dsSetTexture (DS_WOOD);
dWorldQuickStep (world,0.05);
dsSetColor(0.4,0,0);
for (int i=0; i< spheres; i++)
{
dsDrawSphere (dGeomGetPosition(sphere[i]),dGeomGetRotation(sphere[i]),RADIUS);
}
}
void simLoop(int pause)
{
if (!pause) {
dSpaceCollide (space, 0, &nearCallback);
dWorldQuickStep(world, 0.01);
dJointGroupEmpty(contactgroup);
}
dsSetColor (1,1,0);
for (int i=0; i<ncards; ++i) {
dsSetColor (1, dReal(i)/ncards, 0);
cards[i]->draw();
}
}
static void simLoop (int pause)
{
const dReal stepsize = 0.02;
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) {
if (mov_type == 1)
moveplat_1(stepsize);
else
moveplat_2(stepsize);
dGeomSetPosition(platform, platpos[0], platpos[1], platpos[2]);
updatecam();
dWorldQuickStep (world,stepsize);
//dWorldStep (world,stepsize);
}
if (write_world) {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (world,f,"X");
fclose (f);
}
write_world = 0;
}
// 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 (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0.8,0);
}
else {
dsSetColor (1,1,0);
}
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
dsSetColor (1,0,0);
drawGeom (platform,0,0,show_aabb);
//usleep(5000);
}
int physics_loop (void *d)
{
printlog(1, "Starting physics loop");
Uint32 simtime = SDL_GetTicks(); //set simulated time to realtime
Uint32 realtime; //real time (with possible delay since last update)
Uint32 stepsize_ms = internal.stepsize*1000+0.0001;
while (runlevel == running)
{
//technically, collision detection doesn't need this, but this is easier
SDL_mutexP(ode_mutex);
Car::Physics_Step(); //control, antigrav...
Body::Physics_Step(); //drag (air/liquid "friction")
dSpaceCollide (space, 0, &Geom::Collision_Callback);
dWorldQuickStep (world, internal.stepsize);
dJointGroupEmpty (contactgroup);
Collision_Feedback::Physics_Step(); //forces from collisions
Joint::Physics_Step(); //joint forces
Geom::Physics_Step(); //sensor/radar handling
camera.Physics_Step(); //move camera to wanted postion
//done with ode
SDL_mutexV(ode_mutex);
Graphic_List_Update(); //make copy of position/rotation for rendering
//broadcast to wake up sleeping threads
if (internal.sync_events || internal.sync_graphics)
{
SDL_mutexP(sync_mutex);
SDL_CondBroadcast (sync_cond);
SDL_mutexV(sync_mutex);
}
simtime += stepsize_ms;
realtime = SDL_GetTicks();
if (simtime > realtime)
SDL_Delay (simtime - realtime);
else
++stepsize_warnings;
}
return 0;
}
static void simLoop (int pause)
{
if (!pause) {
dSpaceCollide(space,0,&nearCallback);
dWorldQuickStep (world,0.05);
dJointGroupEmpty(contactgroup);
}
dReal sides1[3];
dGeomBoxGetLengths(geom[0], sides1);
dReal sides2[3];
dGeomBoxGetLengths(geom[1], sides2);
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 update(State* state) {
float speed;
if(state->ekey)
speed = 0.5;
else
speed = 0.3;
if(state->wkey) {
state->posz -= cos(state->roty*3.1415/180) * speed;
state->posx += sin(state->roty*3.1415/180) * speed;
}
if(state->akey) {
state->posz += cos((state->roty + 90)*3.1415/180) * speed;
state->posx -= sin((state->roty + 90)*3.1415/180) * speed;
}
if(state->skey) {
state->posz += cos(state->roty*3.1415/180) * speed;
state->posx -= sin(state->roty*3.1415/180) * speed;
}
if(state->dkey) {
state->posz -= cos((state->roty + 90)*3.1415/180) * speed;
state->posx += sin((state->roty + 90)*3.1415/180) * speed;
}
if(state->shiftkey) {
state->posy -= speed;
}
if(state->spacekey) {
state->posy += speed;
}
if(state->gkey) {
for(int i = 0; i < state->simSpeed; i++) {
dSpaceCollide(state->worldSpace, state, &nearCallback);
dWorldQuickStep(state->world, 0.015/60);
dJointGroupEmpty(state->physicsContactgroup);
// cout << state->var[0] << " " << state->var[1] << " " << state->var[2] << endl;
}
// state->gkey = false;
}
}
//Simulation Loop to test for collisions on every frame
void simLoop()
{
//dSpaceCollide determines whih pairs of geoms are potentially interecting,
//the callback function det's actual collisions before adding all the contact joints to groupcontact
//This allows us control over the effect in the contact joints
dSpaceCollide(spacePhy, 0, &nearCallback);
// Now we advance the simulation by calling dWorldQuickStep. This is a faster version of dWorldStep but it is also
// slightly less accurate. As well as the World object ID we also pass a step size value. In each step the simulation
// is updated by a certain number of smaller steps or iterations. The default number of iterations is 20 but you can
// change this by calling dWorldSetQuickStepNumIterations.
dWorldQuickStep(world, 0.3);
//Remove all temporary collision joints now that the world has been stepped
dJointGroupEmpty(contactgroup);
//And we finish by calling DrawGeom which renders the objects according to their type or class
// DrawGeom(object.Geom[0], 0, 0, 0);
}
static void simLoop( int pause )
{
dsSetColor( 0,0,2 );
dSpaceCollide( space,0,&nearCallback );
if ( !pause ) dWorldQuickStep( world,0.05 );
for ( int j = 0; j < dSpaceGetNumGeoms( space ); j++ )
{
dSpaceGetGeom( space, j );
}
// 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 ( obj[i].geom[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 );
}
}
}
}
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();
}
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
int main(int argc, char** argv)
{
init_gfx();
init_2d();
world = dWorldCreate();
dBodyID ball = dBodyCreate(world);
dBodySetPosition(ball, 0, 0, 0);
for (;;) {
Uint8* chars = SDL_GetKeyState(NULL);
const dReal* pos = dBodyGetPosition(ball);
const dReal* q = dBodyGetQuaternion(ball);
double angle = get_q_angle(q);
const dReal* axis = get_q_axis(q);
if (chars[SDLK_LEFT]) {
dBodyAddForceAtPos(ball, -force, 0, 0,
pos[0], pos[1]+1, pos[2]);
}
if (chars[SDLK_RIGHT]) {
dBodyAddForceAtPos(ball, force, 0, 0,
pos[0], pos[1]+1, pos[2]);
}
if (chars[SDLK_UP]) {
dBodyAddForce(ball, 0, +force, 0);
}
if (chars[SDLK_DOWN]) {
dBodyAddForce(ball, 0, -force, 0);
}
glTranslatef(pos[0], pos[1], pos[2]);
glRotatef(angle * 180 / M_PI, axis[0], axis[1], axis[2]);
draw_circle();
dWorldQuickStep(world, 0.01);
step();
}
}
static void simLoop (int pause)
{
int i;
double simstep = 0.002; // 2ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf(dt/simstep);
for (i=0; i<nrofsteps && !pause; i++)
{
dSpaceCollide (space,0,&nearCallback);
dWorldQuickStep (world, simstep);
dJointGroupEmpty (contactgroup);
inspectJoints();
}
for (i=0; i<SEGMCNT; i++)
{
float r=0,g=0,b=0.2;
float v = colours[i];
if (v>1.0) v=1.0;
if (v<0.5)
{
r=2*v;
g=1.0;
}
else
{
r=1.0;
g=2*(1.0-v);
}
dsSetColor (r,g,b);
drawGeom(seggeoms[i]);
}
dsSetColor (1,1,1);
for (i=0; i<STACKCNT; i++)
drawGeom(stackgeoms[i]);
}
void ODEWorld::nextStep()
{
s_chash.clear();
s_collisions.clear();
dWorldID w = world();
dSpaceID s = space();
dSpaceCollide(s, 0, &nearCB);
// DO NOT use dWorldQuickStep, which causes some problems. 080113
if (m_quickStep) {
dWorldQuickStep(w, m_timeStep);
}
else {
dWorldStep(w, m_timeStep);
}
//dJointGroupID jgroup = jointGroup();
dJointGroupEmpty(m_jgroup);
m_itrCnt++;
m_nowtime += m_timeStep;
}
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;
}