void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!draw_geom){
return;
}
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
/*
// cylinder option not yet implemented
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
*/
else if (type == dGeomTransformClass) {
dGeomID g2 = dGeomTransformGetGeom (g);
const dReal *pos2 = dGeomGetPosition (g2);
const dReal *R2 = dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMULTIPLY0_331 (actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMULTIPLY0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R,0);
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
void drawGeom( dGeomID g, int colored = 0 )
{
if( !g ) //If the geometry object is missing, end the function.
return;
const dReal *position; //Define pointers to internal positions and orientations.
const dReal *orientation; //Pointers to constant objects (so the objects will not change).
int type = dGeomGetClass( g ); //Get the type of geometry.
position = dGeomGetPosition( g ); //Then, get the geometry position.
orientation = dGeomGetRotation( g ); //And get existing geometry orientation.
if( type == dBoxClass ) //Is it a box?
{
dReal sides[3];
dGeomBoxGetLengths( g, sides ); //Get length of sides.
renderBox( sides, position, orientation, colored ); //Render the actual box in environment.
}
else if (type == dCylinderClass)
{
dReal radius, length;
dGeomCylinderGetParams(g, &radius, &length);
renderCylinder(radius, length, position, orientation);
}
else if (type == dCapsuleClass)
{
dReal radius, length;
dGeomCapsuleGetParams(g, &radius, &length);
renderCapsule(radius, length, position, orientation);
}
}
static void draw_phys_ccylinder(dGeomID geom)
{
dReal r;
dReal l;
dGeomCapsuleGetParams(geom, &r, &l);
opengl_draw_cap((float) r, (float) l);
}
static void printCapsule (PrintingContext &c, dxGeom *g)
{
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
c.print ("type","capsule");
c.print ("radius",radius);
c.print ("length",length);
}
static void ccdGeomToCap(const dGeomID g, ccd_cap_t *cap)
{
dReal r, h;
ccdGeomToObj(g, (ccd_obj_t *)cap);
dGeomCapsuleGetParams(g, &r, &h);
cap->radius = r;
cap->height = h / 2.;
}
/*** ロボットアームの描画 ***/
void drawArm()
{
dReal r,length;
for (int i = 0; i < NUM; i++ ) { // カプセルの描画
dGeomCapsuleGetParams(rlink[i].geom, &r,&length);
dsDrawCapsule(dBodyGetPosition(rlink[i].body),
dBodyGetRotation(rlink[i].body),length,r);
}
}
/*******************************************************************************
Function to draw a geometry object.
*******************************************************************************/
void GOdeObject::drawGeom( dGeomID g, const dReal *position, const dReal *orientation ) const
{
if( !g ) //If the geometry object is missing, end the function.
return;
if( !position ) //Position was not passed?
position = dGeomGetPosition( g ); //Then, get the geometry position.
if( !orientation ) //Orientation was not given?
orientation = dGeomGetRotation( g ); //And get existing geometry orientation.
int type = dGeomGetClass( g ); //Get the type of geometry.
if( type == dBoxClass ) //Is it a box?
{
dReal sides[3];
dGeomBoxGetLengths( g, sides ); //Get length of sides.
renderBox( sides, position, orientation ); //Render the actual box in environment.
}
if( type == dSphereClass ) //Is it a sphere?
{
dReal radius;
radius = dGeomSphereGetRadius( g ); //Get the radius.
renderSphere( radius, position, orientation ); //Render sphere in environment.
}
if( type == dCapsuleClass )
{
dReal radius;
dReal length;
dGeomCapsuleGetParams( g, &radius, &length ); //Get both radius and length.
renderCapsule( radius, length, position, orientation ); //Render capsule in environment.
}
if( type == dGeomTransformClass ) //Is it an embeded geom in a composite body.
{
dGeomID g2 = dGeomTransformGetGeom( g ); //Get the actual geometry inside the wrapper.
const dReal *position2 = dGeomGetPosition( g2 ); //Get position and orientation of wrapped geometry.
const dReal *orientation2 = dGeomGetRotation( g2 );
dVector3 actualPosition; //Real world coordinated position and orientation
dMatrix3 actualOrientation; //of the wrapped geometry.
dMultiply0_331( actualPosition, orientation, position2 ); //Get world coordinates of geometry position.
actualPosition[0] += position[0];
actualPosition[1] += position[1];
actualPosition[2] += position[2];
dMultiply0_333( actualOrientation, orientation, orientation2 ); //Get world coordinates of geom orientation.
drawGeom( g2, actualPosition, actualOrientation ); //Draw embeded geometry.
}
}
/*** ロボットアームの描画 ***/
void drawArm()
{
dReal r,length;
dReal box_length[3] = {0.4, 0.4, 0.4};
for (int i = 0; i < NUM-1; i++ ) { // カプセルの描画
dGeomCapsuleGetParams(rlink[i].geom, &r,&length);
dsDrawCapsule(dBodyGetPosition(rlink[i].body),
dBodyGetRotation(rlink[i].body),length,r);
}
dGeomBoxGetLengths(rlink[NUM-1].geom, box_length);
dsDrawBox(dBodyGetPosition(rlink[NUM-1].body), dBodyGetRotation(rlink[NUM-1].body), box_length);
}
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
} else if (type == dConvexClass) {
//dVector3 sides={0.50,0.50,0.50};
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
}
/*
// cylinder option not yet implemented
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
*/
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
void dmDraw(dmObject *obj) /*** 物体の描画 ***/
{
if (!obj->geom) return;
obj->p = (double *) dGeomGetPosition(obj->geom);
obj->R = (double *) dGeomGetRotation(obj->geom);
dsSetColor(obj->color[0],obj->color[1],obj->color[2]); // 色の設定(r,g,b)
// printf("color=%.1f %.1f %.1f\n",obj->color[0],obj->color[1],obj->color[2]);
int type = dGeomGetClass(obj->geom);
switch (type)
{
case dBoxClass:
{
dVector3 sides;
dGeomBoxGetLengths(obj->geom,sides);
dsDrawBox(obj->p,obj->R,sides);
}
break;
case dSphereClass:
dsDrawSphere(obj->p,obj->R,dGeomSphereGetRadius(obj->geom));
break;
case dCapsuleClass:
{
dReal radius,length;
dGeomCapsuleGetParams(obj->geom,&radius,&length);
dsDrawCapsule(obj->p,obj->R,length,radius);
}
break;
case dCylinderClass:
{
dReal radius,length;
dGeomCylinderGetParams(obj->geom,&radius,&length);
dsDrawCylinder(obj->p,obj->R,length,radius);
}
break;
default:
printf("Bad geometry type \n");
}
}
void sTrimeshCapsuleColliderData::SetupInitialContext(dxTriMesh *TriMesh, dxGeom *Capsule,
int flags, int skip)
{
const dMatrix3* pRot = (const dMatrix3*)dGeomGetRotation(Capsule);
memcpy(m_mCapsuleRotation, pRot, sizeof(dMatrix3));
const dVector3* pDst = (const dVector3*)dGeomGetPosition(Capsule);
memcpy(m_vCapsulePosition, pDst, sizeof(dVector3));
m_vCapsuleAxis[0] = m_mCapsuleRotation[0*4 + nCAPSULE_AXIS];
m_vCapsuleAxis[1] = m_mCapsuleRotation[1*4 + nCAPSULE_AXIS];
m_vCapsuleAxis[2] = m_mCapsuleRotation[2*4 + nCAPSULE_AXIS];
// Get size of Capsule
dGeomCapsuleGetParams(Capsule, &m_vCapsuleRadius, &m_fCapsuleSize);
m_fCapsuleSize += 2*m_vCapsuleRadius;
const dMatrix3* pTriRot = (const dMatrix3*)dGeomGetRotation(TriMesh);
memcpy(m_mTriMeshRot, pTriRot, sizeof(dMatrix3));
const dVector3* pTriPos = (const dVector3*)dGeomGetPosition(TriMesh);
memcpy(m_mTriMeshPos, pTriPos, sizeof(dVector3));
// global info for contact creation
m_iStride =skip;
m_iFlags =flags;
// reset contact counter
m_ctContacts = 0;
// reset best depth
m_fBestDepth = - MAX_REAL;
m_fBestCenter = 0;
m_fBestrt = 0;
// reset collision normal
m_vNormal[0] = REAL(0.0);
m_vNormal[1] = REAL(0.0);
m_vNormal[2] = REAL(0.0);
}
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
int i;
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
//<---- Convex Object
else if (type == dConvexClass)
{
#if 0
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
#else
dsDrawConvex(pos,R,
Sphere_planes,
Sphere_planecount,
Sphere_points,
Sphere_pointcount,
Sphere_polygons);
#endif
}
//----> Convex Object
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
else if (type == dGeomTransformClass) {
dGeomID g2 = dGeomTransformGetGeom (g);
const dReal *pos2 = dGeomGetPosition (g2);
const dReal *R2 = dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMULTIPLY0_331 (actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMULTIPLY0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R,0);
}
if (show_body) {
dBodyID body = dGeomGetBody(g);
if (body) {
const dReal *bodypos = dBodyGetPosition (body);
const dReal *bodyr = dBodyGetRotation (body);
dReal bodySides[3] = { 0.1, 0.1, 0.1 };
dsSetColorAlpha(0,1,0,1);
dsDrawBox(bodypos,bodyr,bodySides);
}
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
int i;
if (!g) return;
if (dGeomIsSpace(g))
{
displaySpace((dSpaceID)g);
return;
}
int type = dGeomGetClass (g);
if (type == dBoxClass)
{
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass)
{
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass)
{
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
else if (type == dCylinderClass)
{
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
else
show_aabb = 0;
if (show_aabb)
{
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
void add_phys_mass(dMass *mass, dGeomID geom, const float p[3],
const float r[16])
{
dVector3 v;
dMatrix3 M;
dReal rad;
dReal len;
dMass add;
if (r) set_rotation(M, r);
if (dGeomGetClass(geom) != dPlaneClass)
{
dReal m = get_data(geom)->mass;
/* Create a new mass for the given geom. */
switch (dGeomGetClass(geom))
{
case dBoxClass:
dGeomBoxGetLengths(geom, v);
dMassSetBoxTotal(&add, m, v[0], v[1], v[2]);
break;
case dSphereClass:
rad = dGeomSphereGetRadius(geom);
dMassSetSphereTotal(&add, m, rad);
break;
case dCapsuleClass:
dGeomCapsuleGetParams(geom, &rad, &len);
dMassSetCapsuleTotal(&add, m, 3, rad, len);
break;
default:
dMassSetZero(&add);
break;
}
/* Transform the geom and mass to the given position and rotation. */
if(dGeomGetBody(geom))
{
if (p)
{
dGeomSetOffsetPosition(geom, p[0], p[1], p[2]);
dMassTranslate (&add, p[0], p[1], p[2]);
}
if (r)
{
dGeomSetOffsetRotation(geom, M);
dMassRotate (&add, M);
}
}
else
{
if (p) dGeomSetPosition(geom, p[0], p[1], p[2]);
if (r) dGeomSetRotation(geom, M);
}
/* Accumulate the new mass with the body's existing mass. */
dMassAdd(mass, &add);
}
}
// simulation loop
static void simLoop (int pause)
{
const dReal *rot;
dVector3 ax;
dReal l=0;
switch (joint->getType() )
{
case dJointTypeSlider :
( (dSliderJoint *) joint)->getAxis (ax);
l = ( (dSliderJoint *) joint)->getPosition();
break;
case dJointTypePiston :
( (dPistonJoint *) joint)->getAxis (ax);
l = ( (dPistonJoint *) joint)->getPosition();
break;
default:
{} // keep the compiler happy
}
if (!pause)
{
double simstep = 0.01; // 1ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf (dt/simstep);
if (!nrofsteps)
nrofsteps = 1;
for (int i=0; i<nrofsteps && !pause; i++)
{
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
update();
dReal radius, length;
dsSetTexture (DS_WOOD);
drawBox (geom[BODY2], 1,1,0);
drawBox (geom[RECT], 0,0,1);
if ( geom[BODY1] )
{
const dReal *pos = dGeomGetPosition (geom[BODY1]);
rot = dGeomGetRotation (geom[BODY1]);
dsSetColor (0,0,1);
dGeomCapsuleGetParams (geom[BODY1], &radius, &length);
dsDrawCapsule (pos, rot, length, radius);
}
drawBox (geom[OBS], 1,0,1);
// Draw the prismatic axis
if ( geom[BODY1] )
{
const dReal *pos = dGeomGetPosition (geom[BODY1]);
rot = dGeomGetRotation (geom[BODY2]);
dVector3 p;
p[X] = pos[X] - l*ax[X];
p[Y] = pos[Y] - l*ax[Y];
p[Z] = pos[Z] - l*ax[Z];
dsSetColor (1,0,0);
dsDrawCylinder (p, rot, 3.75, 1.05*AXIS_RADIUS);
}
if (joint->getType() == dJointTypePiston )
{
dVector3 anchor;
dJointGetPistonAnchor(joint->id(), anchor);
// Draw the rotoide axis
rot = dGeomGetRotation (geom[BODY2]);
dsSetColor (1,0.5,0);
dsDrawCylinder (anchor, rot, 4, AXIS_RADIUS);
dsSetColor (0,1,1);
rot = dGeomGetRotation (geom[BODY1]);
dsDrawSphere (anchor, rot, 1.5*RADIUS);
}
}
}
void drawRobot_Nleg()
{
#ifndef NOVIZ
if(visualize){
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
dReal r,length;
dVector3 sides;
// Drawing of body
// Drawing of body
dsSetTexture(1);
dsSetColor(2.5,2.5,2.5);
dGeomBoxGetLengths(torso[segment].geom,sides);
dsDrawBox(dBodyGetPosition(torso[segment].body),
dBodyGetRotation(torso[segment].body),sides);
// Drawing oflegs
dsSetTexture(0);
dsSetColor(1.3,0.0,0.0);
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++ ) {
dGeomCapsuleGetParams(leg[segment][i][j].geom, &r,&length);
if (j== 0) dsDrawCapsule(dBodyGetPosition(leg[segment][i][j].body),
dBodyGetRotation(leg[segment][i][j].body),0.5*length,1.2*r);
else dsDrawCapsule(dBodyGetPosition(leg[segment][i][j].body),
dBodyGetRotation(leg[segment][i][j].body),length,r);
}
}
}
// int middleBodySegment = BODY_SEGMENTS/2;
// cerr <<dBodyGetPosition(torso.body)[0]<<" " <<dBodyGetPosition(torso.body)[1]<< " " << dBodyGetPosition(torso.body)[2]<<endl;
//FROM SIDE
// hpr[0] = 90.0f;
// hpr[1] = 0.0f;//-1.2f;
// hpr[2] = 0.0f; // point of view[m]
// xyz[0] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[0];
// xyz[1] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[1]-ceil(log2(BODY_SEGMENTS));
// xyz[2] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[2]+0.2f; // gaze[°]
//FROM BACK
// hpr[0] = 1.0f;
// hpr[1] = 0.0f;//-1.2f;
// hpr[2] = 0.0f; // point of view[m]
// xyz[0] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[0]-ceil(log2(BODY_SEGMENTS));
// xyz[1] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[1];
// xyz[2] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[2]+0.2f; // gaze[°]
//FROM BELOW and BACK
hpr[0] = 1.0f;
hpr[1] = -1.2f;
hpr[2] = 0.5f; // point of view[m]
xyz[0] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[0]-BODY_SEGMENTS/2.0f;
xyz[1] = dBodyGetPosition(torso[MIDDLE_BODY_SEGMENT].body)[1];
xyz[2] = 0.1; // gaze[°]
dsSetViewpoint(xyz,hpr);
}
#endif
}
/*** Drawing of robot ***/
void drawRobot()
{
#ifndef NOVIZ
if(visualize){
dReal r,length;
dVector3 sides;
// Drawing of body
dsSetTexture(1);
dsSetColor(2.5,2.5,2.5);
dGeomBoxGetLengths(torso[0].geom,sides);
dsDrawBox(dBodyGetPosition(torso[0].body),
dBodyGetRotation(torso[0].body),sides);
// Drawing oflegs
dsSetTexture(0);
dsSetColor(1.3,0.0,0.0);
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++ ) {
dGeomCapsuleGetParams(leg[0][i][j].geom, &r,&length);
if (j== 0) dsDrawCapsule(dBodyGetPosition(leg[0][i][j].body),
dBodyGetRotation(leg[0][i][j].body),0.5*length,1.2*r);
else dsDrawCapsule(dBodyGetPosition(leg[0][i][j].body),
dBodyGetRotation(leg[0][i][j].body),length,r);
}
}
// hpr[0] = 0.0f;
// hpr[1] = -20.0f;
// hpr[2] = 0.0f; // point of view[m]
// //cerr <<dBodyGetPosition(torso[0].body)[0]<<" " <<dBodyGetPosition(torso[0].body)[1]<< " " << dBodyGetPosition(torso[0].body)[2]<<endl;
// xyz[0] = dBodyGetPosition(torso[0].body)[0]-.75f;
// xyz[1] = dBodyGetPosition(torso[0].body)[1];
// xyz[2] = dBodyGetPosition(torso[0].body)[2]+0.05f; // gaze[°]
hpr[0] = 1.0f;
hpr[1] = -1.2f;
hpr[2] = 0.5f; // point of view[m]
// hpr[0] = dBodyGetRotation(torso[0].body)[0];
// hpr[1] = dBodyGetRotation(torso[0].body)[1];
// hpr[2] = dBodyGetRotation(torso[0].body)[2];
//cerr <<dBodyGetPosition(torso[0].body)[0]<<" " <<dBodyGetPosition(torso[0].body)[1]<< " " << dBodyGetPosition(torso[0].body)[2]<<endl;
xyz[0] = dBodyGetPosition(torso[0].body)[0]-.75f;
xyz[1] = dBodyGetPosition(torso[0].body)[1];
xyz[2] = 0.1; //dBodyGetPosition(torso[0].body)[2]-0.15f; // gaze[°]
//overrides to look from the other side of the robot
//hpr[0] = -180.0f; //slides view to the right
//xyz[0] = dBodyGetPosition(torso[0].body)[0]+.75f;
// hpr[1] = -30.2f; //slides down/up
// hpr[2] = 20.5f; // point of view[m]
//different override for other corner
xyz[0] = dBodyGetPosition(torso[0].body)[0]+.75f;
xyz[1] = dBodyGetPosition(torso[0].body)[1]-.50;
hpr[0] = -220.0f; //slides view to the right
dsSetViewpoint(xyz,hpr);
}
#endif
}
// copied from an OpenDE demo program
//todo:pass trimesh as argument to this function
void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb, Tmesh tm)
{
int i;
if (!g) return;
if (dGeomIsSpace(g))
{
displaySpace((dSpaceID)g);
return;
}
int type = dGeomGetClass (g);
if (type == dBoxClass)
{
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass)
{
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass)
{
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
else if (type == dCylinderClass)
{
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
else if (type == dTriMeshClass)
{
//dTriIndex* Indices = DISP.tmd[i].indices;
const dReal* Pos = dGeomGetPosition(g);
const dReal* Rot = dGeomGetRotation(g);
for (int ii = 0; ii < (tm.indexSize/3); ii++)
{
const dReal v[9] = { // explicit conversion from float to dReal
tm.vertices[tm.indices[ii * 3 + 0] * 3 + 0],
tm.vertices[tm.indices[ii * 3 + 0] * 3 + 1],
tm.vertices[tm.indices[ii * 3 + 0] * 3 + 2],
tm.vertices[tm.indices[ii * 3 + 1] * 3 + 0],
tm.vertices[tm.indices[ii * 3 + 1] * 3 + 1],
tm.vertices[tm.indices[ii * 3 + 1] * 3 + 2],
tm.vertices[tm.indices[ii * 3 + 2] * 3 + 0],
tm.vertices[tm.indices[ii * 3 + 2] * 3 + 1],
tm.vertices[tm.indices[ii * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
}
//std::cout<<"done once"<<std::endl;
}
else
if (type == dRayClass)
{
dVector3 Origin, Direction;
dGeomRayGet(g, Origin, Direction);
dReal Length = dGeomRayGetLength(g);
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);
double *ori = new double[3];
double *end = new double[4];
ori[0]=Origin[0];
ori[1]=Origin[1];
ori[2]=Origin[2];
end[0]=End[0];
end[1]=End[1];
end[2]=End[2];
end[3]=End[3];
dsDrawLine(ori, end);
}
else
show_aabb = 0;
if (show_aabb)
{
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
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;
}
}
}
}
// copied from an OpenDE demo program
void DisplayOpenDESpaces::drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
int i;
if (g == nullptr) return;
if (dGeomIsSpace(g) != 0)
{
displaySpace((dSpaceID)g);
return;
}
int type = dGeomGetClass (g);
if (type == dBoxClass)
{
if (pos == nullptr) pos = dGeomGetPosition (g);
if (R == nullptr) R = dGeomGetRotation (g);
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass)
{
if (pos == nullptr) pos = dGeomGetPosition (g);
if (R == nullptr) R = dGeomGetRotation (g);
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass)
{
if (pos == nullptr) pos = dGeomGetPosition (g);
if (R == nullptr) R = dGeomGetRotation (g);
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
else if (type == dCylinderClass)
{
if (pos == nullptr) pos = dGeomGetPosition (g);
if (R == nullptr) R = dGeomGetRotation (g);
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
else if (type == dGeomTransformClass)
{
if (pos == nullptr) pos = dGeomGetPosition (g);
if (R == nullptr) R = dGeomGetRotation (g);
dGeomID g2 = dGeomTransformGetGeom (g);
const dReal *pos2 = dGeomGetPosition (g2);
const dReal *R2 = dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMULTIPLY0_331 (actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMULTIPLY0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R,0);
}
else
show_aabb = 0;
if (show_aabb != 0)
{
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
