void nearCallback(void *data, dGeomID o1, dGeomID o2) {
State* state = (State*)data;
if(dGeomIsSpace(o1) || dGeomIsSpace(o2)) {
dSpaceCollide2(o1, o2, data, &nearCallback);
if(dGeomIsSpace(o1))
dSpaceCollide((dSpaceID)o1, data, &nearCallback);
if(dGeomIsSpace(o2))
dSpaceCollide((dSpaceID)o2, data, &nearCallback);
} else {
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
const int MAX_CONTACTS = 18;
dContact contact[MAX_CONTACTS];
for(int i = 0; i < MAX_CONTACTS; i++) {
contact[i].surface.mode = dContactBounce;
contact[i].surface.mu = 2000;
contact[i].surface.bounce = 0.1;
contact[i].surface.bounce_vel = 0.15;
}
if(int numc = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact))) {
for(int i = 0; i < numc; i++) {
dJointID c = dJointCreateContact(state->world, state->physicsContactgroup, &contact[i]);
dJointAttach(c, b1, b2);
}
}
}
}
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();
}
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)
{
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
}
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)
{
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);
}
}
}
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);
}
}
}
// 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));
}
//"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() )
}
/***********************************************************
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;
}
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);
}
}
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(); // ロボットの描画
}
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);
}
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)
{
int i,j;
for (i=0; i < NUM; i++) {
for (j=0; j < NUM; j++) test_matrix[i][j] = 0;
}
dSpaceCollide (space,0,&nearCallback);
for (i=0; i < NUM; i++) {
for (j=i+1; j < NUM; j++) {
if (good_matrix[i][j] && !test_matrix[i][j]) {
printf ("failed to report collision (%d,%d) (seed=%ld)\n",i,j,seed);
}
}
}
seed++;
init_test();
for (i=0; i<NUM; i++) {
dVector3 pos,side;
dMatrix3 R;
dRSetIdentity (R);
for (j=0; j<3; j++) pos[j] = (bounds[i][j*2+1] + bounds[i][j*2]) * 0.5;
for (j=0; j<3; j++) side[j] = bounds[i][j*2+1] - bounds[i][j*2];
if (hits[i] > 0) dsSetColor (1,0,0);
else dsSetColor (1,1,0);
dsDrawBox (pos,R,side);
}
}
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;
}
int physics_step(float dt)
{
dSpaceCollide(space, 0, callback);
dWorldQuickStep(world, dt);
dJointGroupEmpty(group);
return 1;
}
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);
}
}
void PhysicsSpace::Collide( PhysicsWorld* const w )
{
_CollideWorldID = w->getWorldID();
//free contact Joints
dJointGroupEmpty(_ColJointGroupId);
dSpaceCollide(_SpaceID, reinterpret_cast<void *>(this), &PhysicsSpace::collisionCallback);
}
void simStep()
{
if (loading == 0)
{
dSpaceCollide (space,0,&nearCallback);
dWorldStepFast1 (ode_world,0.05, 5);
for (int j = 0; j < dSpaceGetNumGeoms(space); j++){
dSpaceGetGeom(space, j);
}
dJointGroupEmpty (contactgroup);
}
}
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)
{
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
}
}
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
}
static void Near(void *unused, dGeomID g1, dGeomID g2)
{
bool s1 = dGeomIsSpace(g1);
bool s2 = dGeomIsSpace(g2);
if (s1 || s2)
{
dSpaceCollide2(g1, g2, unused, &Near);
if (s1)
dSpaceCollide(dSpaceID(g1), unused, &Near);
if (s2)
dSpaceCollide(dSpaceID(g2), unused, &Near);
}
else
{
dBodyID b1 = dGeomGetBody(g1);
dBodyID b2 = dGeomGetBody(g2);
const int N = 42;
dContact contact[N];
for (int i = 0; i < N; ++i)
{
contact[i].surface.mode = dContactBounce|dContactSoftCFM;
contact[i].surface.mu = dInfinity;
contact[i].surface.mu2 = 0;
contact[i].surface.bounce = 0.1;
contact[i].surface.bounce_vel = 0.1;
contact[i].surface.soft_cfm = 0.01;
}
int n = dCollide(g1, g2, N, &contact->geom, sizeof(dContact));
for (int i = 0; i < n; ++i)
{
dJointID joint = dJointCreateContact(World, Group, contact+i);
dJointAttach(joint, b1, b2);
}
}
}
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();
}
}
bool ompl::control::OpenDEStateSpace::evaluateCollision(const base::State *state) const
{
if ((state->as<StateType>()->collision & (1 << STATE_COLLISION_KNOWN_BIT)) != 0)
return (state->as<StateType>()->collision & (1 << STATE_COLLISION_VALUE_BIT)) != 0;
env_->mutex_.lock();
writeState(state);
CallbackParam cp = {env_.get(), false};
for (unsigned int i = 0; !cp.collision && i < env_->collisionSpaces_.size(); ++i)
dSpaceCollide(env_->collisionSpaces_[i], &cp, &nearCallback);
env_->mutex_.unlock();
if (cp.collision)
state->as<StateType>()->collision &= (1 << STATE_COLLISION_VALUE_BIT);
state->as<StateType>()->collision &= (1 << STATE_COLLISION_KNOWN_BIT);
return cp.collision;
}
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);
}
void PhysicsSim::step()
{
// Add extra forces to objects
// Grabbed object attraction
if (m_pGrabbedObject)
{
cVector3d grab_force(m_pGrabbedObject->getPosition()
- m_pCursor->m_position);
grab_force.mul(-0.01);
grab_force.add(m_pGrabbedODEObject->getVelocity()*(-0.0003));
dBodyAddForce(m_pGrabbedODEObject->body(),
grab_force.x, grab_force.y, grab_force.z);
}
// Perform simulation step
dSpaceCollide (m_odeSpace, this, &ode_nearCallback);
dWorldStepFast1 (m_odeWorld, m_fTimestep, 5);
dJointGroupEmpty (m_odeContactGroup);
/* Update positions of each object in the other simulations */
std::map<std::string,OscObject*>::iterator it;
for (it=world_objects.begin(); it!=world_objects.end(); it++)
{
ODEObject *o = static_cast<ODEObject*>(it->second->special());
if (o) {
o->update();
cVector3d pos(o->getPosition());
cMatrix3d rot(o->getRotation());
send(true, (it->second->path()+"/position").c_str(), "fff",
pos.x,pos.y,pos.z);
send(true, (it->second->path()+"/rotation").c_str(), "fffffffff",
rot.m[0][0], rot.m[0][1], rot.m[0][2],
rot.m[1][0], rot.m[1][1], rot.m[1][2],
rot.m[2][0], rot.m[2][1], rot.m[2][2]);
}
}
/* Update the responses of each constraint. */
std::map<std::string,OscConstraint*>::iterator cit;
for (cit=world_constraints.begin(); cit!=world_constraints.end(); cit++)
{
cit->second->simulationCallback();
}
m_counter++;
}
//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);
}