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();
}
//Auxiliary function to print info on a space
void OdeInit::printInfoOnSpace(dSpaceID my_space,const std::string & my_space_name)
{
int num_geoms = 0;
dGeomID geom_temp;
int geom_class_temp = 0;
dReal aabb[6];
dBodyID body_temp;
num_geoms = dSpaceGetNumGeoms(my_space);
printf("\nSpace: %s: ID: %p. sublevel: %d, nr. geoms: %d. \n",my_space_name.c_str(),my_space,dSpaceGetSublevel(my_space),num_geoms);
for (int i=0;i<=(num_geoms-1);i++){
geom_temp = dSpaceGetGeom (my_space, i);
geom_class_temp = dGeomGetClass(geom_temp);
printGeomClassAndNr(geom_class_temp,i);
if (!dGeomIsSpace(geom_temp)){
if (dGeomGetBody(geom_temp)!=NULL){ // i.e. a placeable geom
printf(" ID: %p, Coordinates:",geom_temp);
ICubSim::printPositionOfGeom(geom_temp);
dGeomGetAABB(geom_temp,aabb);
printf(" Bounding box coordinates: %f-%f,%f-%f,%f-%f\n:",aabb[0],aabb[1],aabb[2],aabb[3],aabb[4],aabb[5]);
if (geom_class_temp==8){ // trimesh
body_temp = dGeomGetBody(geom_temp);
printf(" Coordinates of associated body are:");
ICubSim::printPositionOfBody(body_temp);
}
} else {
dGeomGetAABB(geom_temp,aabb);
printf(" ID: %p; bounding box coordinates: %f-%f,%f-%f,%f-%f\n:",geom_temp,aabb[0],aabb[1],aabb[2],aabb[3],aabb[4],aabb[5]);
}
}
}
}
/**
* Returns a dBody id from its DEF name.
* @param def DEF name of teh desired dBody
* @return a pointer to the dBody if found NULL otherwise
*/
dBodyID getBodyFromDEF(const char *def) {
//std::cout<<"Try to get body "<<def<<std::endl;
dGeomID geom = dWebotsGetGeomFromDEF(def);
if (! geom) {
printf("Fatal error: did not find dGeom for DEF %s", def);
s_data->disablePhysics();
return NULL;
}
dBodyID body;
if (dGeomIsSpace(geom)){
body = dGeomGetBody(dSpaceGetGeom((dSpaceID)geom, 0));
std::cerr<<"I don't know why I have to do this"<<std::endl;
s_data->disablePhysics();
}else
body = dGeomGetBody(geom);
if (! body) {
printf("Fatal error: did not find dBody for DEF %s", def);
s_data->disablePhysics();
return NULL;
}
return body;
}
void Update(dSpaceID space)
{
int n = dSpaceGetNumGeoms(space);
for (int item = 0; item < n; ++item)
{
Update(dSpaceGetGeom(space, item));
}
}
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 DisplayOpenDESpaces::displaySpace(dSpaceID space)
{
int ngeoms = dSpaceGetNumGeoms(space);
for (int i = 0 ; i < ngeoms ; ++i)
{
dGeomID geom = dSpaceGetGeom(space, i);
std::map<dGeomID, Color>::const_iterator it = m_gcolors.find(geom);
if (it != m_gcolors.end())
dsSetColor(it->second.r,it->second.g,it->second.b);
else
dsSetColor(m_activeColor.r, m_activeColor.g, m_activeColor.b);
drawGeom(geom, nullptr, nullptr, 0);
}
}
void PhysicsVisualization::draw(int pause)
{
UNUSED(pause);
if (nullptr != g_drawingThread && nullptr != g_envToDraw)
{
g_envToDraw->pauseSimulation();
// now we draw everything
dSpaceID collisionSpace = g_envToDraw->getCollisionSpaceID();
dSpaceID visualsSpace = g_envToDraw->getVisualsSpaceID();
uint ngeoms = dSpaceGetNumGeoms(collisionSpace);
for (uint i = 0; i < ngeoms; ++i) {
dGeomID g = dSpaceGetGeom(collisionSpace, i);
drawGeom(g);
}
ngeoms = dSpaceGetNumGeoms(visualsSpace);
for (uint i = 0; i < ngeoms; ++i) {
dGeomID g = dSpaceGetGeom(visualsSpace, i);
drawGeom(g);
}
g_envToDraw->unPauseSimulation();
}
}
void insertGeomInMonitored(dGeomID geom, amarsi::Limbs limb){
if(!dGeomIsSpace(geom)){
s_data->addMonitoredBody(geom,limb);
//std::cerr<<" Directly found geom of type "<<dGeomGetClass(toe)<<std::endl;
return;
}
//std::cerr<<"IS a space, explore childrens"<<std::endl;
dSpaceID geomSpace=(dSpaceID)(geom);
for (int ii=0;ii<dSpaceGetNumGeoms(geomSpace);++ii){
dGeomID g=dSpaceGetGeom(geomSpace,ii);
if(g){
//std::cerr<<"store a geom of type "<<dGeomGetClassData(g)<<std::endl;
s_data->addMonitoredBody(geom,limb);
}
}
}
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 );
}
}
}
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
#if 1
// What is this for??? - Bram
if (!pause)
{
for (int i=0; i<num; i++)
for (int j=0; j < GPB; j++)
if (obj[i].geom[j])
if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass)
setCurrentTransform(obj[i].geom[j]);
setCurrentTransform(TriMesh1);
setCurrentTransform(TriMesh2);
}
#endif
//if (!pause) dWorldStep (world,0.05);
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);
}
if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass) {
dTriIndex* Indices = (dTriIndex*)::Indices;
// assume all trimeshes are drawn as bunnies
const dReal* Pos = dGeomGetPosition(obj[i].geom[j]);
const dReal* Rot = dGeomGetRotation(obj[i].geom[j]);
for (int ii = 0; ii < IndexCount / 3; ii++) {
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[ii * 3 + 0] * 3 + 0],
Vertices[Indices[ii * 3 + 0] * 3 + 1],
Vertices[Indices[ii * 3 + 0] * 3 + 2],
Vertices[Indices[ii * 3 + 1] * 3 + 0],
Vertices[Indices[ii * 3 + 1] * 3 + 1],
Vertices[Indices[ii * 3 + 1] * 3 + 2],
Vertices[Indices[ii * 3 + 2] * 3 + 0],
Vertices[Indices[ii * 3 + 2] * 3 + 1],
Vertices[Indices[ii * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
}
// tell the tri-tri collider the current transform of the trimesh --
// this is fairly important for good results.
// Fill in the (4x4) matrix.
dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 );
p_matrix[ 0 ] = Rot[ 0 ]; p_matrix[ 1 ] = Rot[ 1 ]; p_matrix[ 2 ] = Rot[ 2 ]; p_matrix[ 3 ] = 0;
p_matrix[ 4 ] = Rot[ 4 ]; p_matrix[ 5 ] = Rot[ 5 ]; p_matrix[ 6 ] = Rot[ 6 ]; p_matrix[ 7 ] = 0;
p_matrix[ 8 ] = Rot[ 8 ]; p_matrix[ 9 ] = Rot[ 9 ]; p_matrix[10 ] = Rot[10 ]; p_matrix[11 ] = 0;
p_matrix[12 ] = Pos[ 0 ]; p_matrix[13 ] = Pos[ 1 ]; p_matrix[14 ] = Pos[ 2 ]; p_matrix[15 ] = 1;
// Flip to other matrix.
obj[i].last_matrix_index = !obj[i].last_matrix_index;
dGeomTriMeshSetLastTransform( obj[i].geom[j],
*(dMatrix4*)( obj[i].matrix_dblbuff + obj[i].last_matrix_index * 16 ) );
} else {
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
}
dTriIndex* Indices = (dTriIndex*)::Indices;
{const dReal* Pos = dGeomGetPosition(TriMesh1);
const dReal* Rot = dGeomGetRotation(TriMesh1);
{for (int i = 0; i < IndexCount / 3; i++){
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[i * 3 + 0] * 3 + 0],
Vertices[Indices[i * 3 + 0] * 3 + 1],
Vertices[Indices[i * 3 + 0] * 3 + 2],
Vertices[Indices[i * 3 + 1] * 3 + 0],
Vertices[Indices[i * 3 + 1] * 3 + 1],
Vertices[Indices[i * 3 + 1] * 3 + 2],
Vertices[Indices[i * 3 + 2] * 3 + 0],
Vertices[Indices[i * 3 + 2] * 3 + 1],
Vertices[Indices[i * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 0);
}}}
{const dReal* Pos = dGeomGetPosition(TriMesh2);
const dReal* Rot = dGeomGetRotation(TriMesh2);
{for (int i = 0; i < IndexCount / 3; i++){
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[i * 3 + 0] * 3 + 0],
Vertices[Indices[i * 3 + 0] * 3 + 1],
Vertices[Indices[i * 3 + 0] * 3 + 2],
Vertices[Indices[i * 3 + 1] * 3 + 0],
Vertices[Indices[i * 3 + 1] * 3 + 1],
Vertices[Indices[i * 3 + 1] * 3 + 2],
Vertices[Indices[i * 3 + 2] * 3 + 0],
Vertices[Indices[i * 3 + 2] * 3 + 1],
Vertices[Indices[i * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
}}}
}
/**
* \brief This function handles the calculation of a step in the world.
*
* pre:
* - world_init = true
* - step_size > 0
*
* post:
* - handled the collisions
* - step the world for step_size seconds
* - the contactgroup should be empty
*/
void WorldPhysics::stepTheWorld(void) {
MutexLocker locker(&iMutex);
std::vector<dJointFeedback*>::iterator iter;
geom_data* data;
int i;
// if world_init = false or step_size <= 0 debug something
if(world_init && step_size > 0) {
if(old_gravity != world_gravity) {
old_gravity = world_gravity;
dWorldSetGravity(world, world_gravity.x(),
world_gravity.y(), world_gravity.z());
}
if(old_cfm != world_cfm) {
old_cfm = world_cfm;
dWorldSetCFM(world, (dReal)world_cfm);
}
if(old_erp != world_erp) {
old_erp = world_erp;
dWorldSetERP(world, (dReal)world_erp);
}
// printf("now WorldPhysics.cpp..stepTheWorld(void)....1 : dSpaceGetNumGeoms: %d\n",dSpaceGetNumGeoms(space));
/// first clear the collision counters of all geoms
for(i=0; i<dSpaceGetNumGeoms(space); i++) {
data = (geom_data*)dGeomGetData(dSpaceGetGeom(space, i));
data->num_ground_collisions = 0;
data->contact_ids.clear();
data->contact_points.clear();
data->ground_feedbacks.clear();
}
for(iter = contact_feedback_list.begin();
iter != contact_feedback_list.end(); iter++) {
free((*iter));
}
contact_feedback_list.clear();
draw_intern.clear();
/// then we have to clear the contacts
dJointGroupEmpty(contactgroup);
/// first check for collisions
num_contacts = log_contacts = 0;
create_contacts = 1;
dSpaceCollide(space,this, &WorldPhysics::callbackForward);
drawLock.lock();
draw_extern.swap(draw_intern);
drawLock.unlock();
// then calculate the next state for a time of step_size seconds
try {
if(fast_step) dWorldQuickStep(world, step_size);
else dWorldStep(world, step_size);
} catch (...) {
control->sim->handleError(PHYSICS_UNKNOWN);
}
if(WorldPhysics::error) {
control->sim->handleError(WorldPhysics::error);
WorldPhysics::error = PHYSICS_NO_ERROR;
}
}
}
void ompl::control::OpenDEStateSpace::setDefaultBounds()
{
// limit all velocities to 1 m/s, 1 rad/s, respectively
base::RealVectorBounds bounds1(3);
bounds1.setLow(-1);
bounds1.setHigh(1);
setLinearVelocityBounds(bounds1);
setAngularVelocityBounds(bounds1);
// find the bounding box that contains all geoms included in the collision spaces
double mX, mY, mZ, MX, MY, MZ;
mX = mY = mZ = std::numeric_limits<double>::infinity();
MX = MY = MZ = -std::numeric_limits<double>::infinity();
bool found = false;
std::queue<dSpaceID> spaces;
for (auto &collisionSpace : env_->collisionSpaces_)
spaces.push(collisionSpace);
while (!spaces.empty())
{
dSpaceID space = spaces.front();
spaces.pop();
int n = dSpaceGetNumGeoms(space);
for (int j = 0; j < n; ++j)
{
dGeomID geom = dSpaceGetGeom(space, j);
if (dGeomIsSpace(geom) != 0)
spaces.push((dSpaceID)geom);
else
{
bool valid = true;
dReal aabb[6];
dGeomGetAABB(geom, aabb);
// things like planes are infinite; we want to ignore those
for (double k : aabb)
if (fabs(k) >= std::numeric_limits<dReal>::max())
{
valid = false;
break;
}
if (valid)
{
found = true;
if (aabb[0] < mX)
mX = aabb[0];
if (aabb[1] > MX)
MX = aabb[1];
if (aabb[2] < mY)
mY = aabb[2];
if (aabb[3] > MY)
MY = aabb[3];
if (aabb[4] < mZ)
mZ = aabb[4];
if (aabb[5] > MZ)
MZ = aabb[5];
}
}
}
}
if (found)
{
double dx = MX - mX;
double dy = MY - mY;
double dz = MZ - mZ;
double dM = std::max(dx, std::max(dy, dz));
// add 10% in each dimension + 1% of the max dimension
dx = dx / 10.0 + dM / 100.0;
dy = dy / 10.0 + dM / 100.0;
dz = dz / 10.0 + dM / 100.0;
bounds1.low[0] = mX - dx;
bounds1.high[0] = MX + dx;
bounds1.low[1] = mY - dy;
bounds1.high[1] = MY + dy;
bounds1.low[2] = mZ - dz;
bounds1.high[2] = MZ + dz;
setVolumeBounds(bounds1);
}
}
dGeomID PhysicsSpace::GetGeom( Int32 i )
{
return dSpaceGetGeom(_SpaceID, i);
}
void draw_all_objects (dSpaceID space)
{
int i, j;
draw_all_objects_called = 1;
if (!graphical_test) return;
int n = dSpaceGetNumGeoms (space);
// draw all contact points
dsSetColor (0,1,1);
dSpaceCollide (space,0,&nearCallback);
// draw all rays
for (i=0; i<n; i++) {
dGeomID g = dSpaceGetGeom (space,i);
if (dGeomGetClass (g) == dRayClass) {
dsSetColor (1,1,1);
dVector3 origin,dir;
dGeomRayGet (g,origin,dir);
origin[2] += Z_OFFSET;
dReal length = dGeomRayGetLength (g);
for (j=0; j<3; j++) dir[j] = dir[j]*length + origin[j];
dsDrawLine (origin,dir);
dsSetColor (0,0,1);
dsDrawSphere (origin,dGeomGetRotation(g),0.01);
}
}
// draw all other objects
for (i=0; i<n; i++) {
dGeomID g = dSpaceGetGeom (space,i);
dVector3 pos;
if (dGeomGetClass (g) != dPlaneClass) {
memcpy (pos,dGeomGetPosition(g),sizeof(pos));
pos[2] += Z_OFFSET;
}
switch (dGeomGetClass (g)) {
case dSphereClass: {
dsSetColorAlpha (1,0,0,0.8);
dReal radius = dGeomSphereGetRadius (g);
dsDrawSphere (pos,dGeomGetRotation(g),radius);
break;
}
case dBoxClass: {
dsSetColorAlpha (1,1,0,0.8);
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,dGeomGetRotation(g),sides);
break;
}
case dCapsuleClass: {
dsSetColorAlpha (0,1,0,0.8);
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,dGeomGetRotation(g),length,radius);
break;
}
case dCylinderClass: {
dsSetColorAlpha (0,1,0,0.8);
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,dGeomGetRotation(g),length,radius);
break;
}
case dPlaneClass: {
dVector4 n;
dMatrix3 R,sides;
dVector3 pos2;
dGeomPlaneGetParams (g,n);
dRFromZAxis (R,n[0],n[1],n[2]);
for (j=0; j<3; j++) pos[j] = n[j]*n[3];
pos[2] += Z_OFFSET;
sides[0] = 2;
sides[1] = 2;
sides[2] = 0.001;
dsSetColor (1,0,1);
for (j=0; j<3; j++) pos2[j] = pos[j] + 0.1*n[j];
dsDrawLine (pos,pos2);
dsSetColorAlpha (1,0,1,0.8);
dsDrawBox (pos,R,sides);
break;
}
}
}
}
