/**
* @brief Desenhar o campo da simulacao
*
* Desenha toda a parte grafica da simulacao, como
* as paredes, as linhas, os robos e a bola.
*
*/
void desenharCampo()
{
dVector3 ss;
dsSetColor (0,0,0);
for(int i=0; i < 6; i++) //Paredes
{
dGeomBoxGetLengths (wall[i],ss);
dsDrawBox (dGeomGetPosition(wall[i]), dGeomGetRotation(wall[i]), ss);
}
for(int i=0; i < 3;i++) //Gols
{
dGeomBoxGetLengths (goalL[i],ss);
dsDrawBox (dGeomGetPosition(goalL[i]), dGeomGetRotation(goalL[i]), ss);
dGeomBoxGetLengths (goalR[i],ss);
dsDrawBox (dGeomGetPosition(goalR[i]), dGeomGetRotation(goalR[i]), ss);
}
dsSetColor (1,1,1); //Círculo Central
dsDrawCylinder(dGeomGetPosition(circle),dGeomGetRotation(circle),0.0001,CIRCLE_RADIUS);
for (int i=0; i < 4; i++) //Quinas
{
dGeomBoxGetLengths (triangle[i],ss);
dsDrawBox (dGeomGetPosition(triangle[i]), dGeomGetRotation(triangle[i]), ss);
}
}
int dcTriListCollider::CollideBox(dxGeom* Box, int Flags, dContactGeom* Contacts, int Stride)
{
Fvector AABB;
dVector3 BoxSides;
dGeomBoxGetLengths(Box,BoxSides);
dReal* R=const_cast<dReal*>(dGeomGetRotation(Box));
AABB.x=(dFabs(BoxSides[0]*R[0])+dFabs(BoxSides[1]*R[1])+dFabs(BoxSides[2]*R[2]))/2.f +10.f*EPS_L;
AABB.y=(dFabs(BoxSides[0]*R[4])+dFabs(BoxSides[1]*R[5])+dFabs(BoxSides[2]*R[6]))/2.f +10.f*EPS_L;
AABB.z=(dFabs(BoxSides[0]*R[8])+dFabs(BoxSides[1]*R[9])+dFabs(BoxSides[2]*R[10]))/2.f+10.f*EPS_L;
dBodyID box_body=dGeomGetBody(Box);
if(box_body) {
const dReal*velocity=dBodyGetLinearVel(box_body);
AABB.x+=dFabs(velocity[0])*0.04f;
AABB.y+=dFabs(velocity[1])*0.04f;
AABB.z+=dFabs(velocity[2])*0.04f;
}
BoxTri bt(*this);
return dSortTriPrimitiveCollide
(bt,
Box,
Geometry,
Flags,
Contacts,
Stride,
AABB
);
}
IoObject *IoODEBox_lengths(IoODEBox *self, IoObject *locals, IoMessage *m)
{
dVector3 lengths;
IoODEBox_assertHasBoxId(self, locals, m);
dGeomBoxGetLengths(GEOMID, lengths);
return IoSeq_newWithODEPoint(IOSTATE, lengths);
}
void drawGeom (dGeomID g, const dReal *pos, const dReal *R)
{
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 == dCCylinderClass) {
dReal radius,length;
dGeomCCylinderGetParams (g,&radius,&length);
dsDrawCappedCylinder (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);
}
}
void DrawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
// If the geom ID is missing then return immediately.
if (!g)
{
return;
}
// If there was no position vector supplied then get the existing position.
if (!pos)
{
pos = dGeomGetPosition (g);
}
// If there was no rotation matrix given then get the existing rotation.
if (!R)
{
R = dGeomGetRotation (g);
}
// Get the geom's class type.
int type = dGeomGetClass (g);
if (type == dBoxClass)
{
// Create a temporary array of floats to hold the box dimensions.
dReal sides[3];
dGeomBoxGetLengths(g, sides);
// Now to actually render the box we make a call to DrawBox, passing the geoms dimensions, position vector and
// rotation matrix. And this function is the subject of our next discussion.
// DrawBox(sides, pos, R);
}
}
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);
}
}
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);
}
}
static void printBox (PrintingContext &c, dxGeom *g)
{
dVector3 sides;
dGeomBoxGetLengths (g,sides);
c.print ("type","box");
c.print ("sides",sides);
}
void TSRODERigidBody::DebugRender()
{
Graphics()->SetRasterizerState( Graphics()->m_FillWireFrameState );
TSRMatrix4 bodyTransform;
TSRMatrix4 geomTransform;
const float* pBodyPosition = dBodyGetPosition( m_BodyID );
const float* pBodyRotation = dBodyGetRotation( m_BodyID );
ODEToMatrix4( bodyTransform, pBodyPosition, pBodyRotation );
TSRGlobalConstants.PushMatrix();
TSRGlobalConstants.MultMatrix( bodyTransform.d );
TSRDebugDraw::RenderAxis( 1.0f );
TSRDebugDraw::RenderSphere( 0.25f );
for ( unsigned int i = 0; i < m_GeomIDs.size(); i++ )
{
dGeomID currGeomTransformID = m_GeomIDs[ i ];
dGeomID geomID = dGeomTransformGetGeom( currGeomTransformID );
const float* pGeomPosition = dGeomGetPosition( geomID );
const float* pGeomRotation = dGeomGetRotation( geomID );
ODEToMatrix4( bodyTransform, pGeomPosition, pGeomRotation );
TSRGlobalConstants.PushMatrix();
TSRGlobalConstants.MultMatrix( bodyTransform.d );
switch( dGeomGetClass( geomID ) )
{
case dBoxClass:
{
dVector3 extents;
dGeomBoxGetLengths( geomID, extents );
TSRVector3 aabbMin( -extents[ 0 ], -extents[ 1 ], -extents[ 2 ] );
TSRVector3 aabbMax( +extents[ 0 ], +extents[ 1 ], +extents[ 2 ] );
aabbMin *= 0.5f;
aabbMax *= 0.5f;
TSRDebugDraw::RenderAABB( aabbMin, aabbMax );
}
break;
case dSphereClass:
{
float radius;
radius = dGeomSphereGetRadius( geomID );
TSRDebugDraw::RenderSphere( radius );
}
break;
case dCylinderClass:
{
float radius,length;
dGeomCylinderGetParams( geomID, &radius, &length );
TSRDebugDraw::RenderCylinder( length, radius, TSRVector3( 0.0f, 0.0f, 1.0f ) );
}
break;
}
TSRGlobalConstants.PopMatrix();
}
TSRGlobalConstants.PopMatrix();
Graphics()->SetRasterizerState( Graphics()->m_FillSolidState );
}
static void draw_phys_box(dGeomID geom)
{
dVector3 v;
dGeomBoxGetLengths(geom, v);
opengl_draw_box((float) v[0],
(float) v[1],
(float) v[2]);
}
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);
}
static void ccdGeomToBox(const dGeomID g, ccd_box_t *box)
{
dVector3 dim;
ccdGeomToObj(g, (ccd_obj_t *)box);
dGeomBoxGetLengths(g, dim);
box->dim[0] = dim[0] / 2.;
box->dim[1] = dim[1] / 2.;
box->dim[2] = dim[2] / 2.;
}
void CRigidBox::draw() const {
const dReal* v = dGeomGetPosition(mGeomID);
CVector3 o(TO_WORLD(v[0]), TO_WORLD(v[1]), TO_WORLD(v[2]));
const dReal* r = dGeomGetRotation(mGeomID);
F32 sides[3];
dGeomBoxGetLengths(mGeomID, sides);
sides[0] = TO_WORLD(sides[0]);
sides[1] = TO_WORLD(sides[1]);
sides[2] = TO_WORLD(sides[2]);
drawBox(&o.x, r, sides);
}
/*******************************************************************************
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 ODE_Particle::getLengths(dVector3 &sides)
{
if (getShapeType() == dBoxClass)
{
dGeomBoxGetLengths (geom,sides);
}
else
{
printf("ERROR in ODE_Particle.cpp: Requesting dimensions for non box object");
exit(0);
}
}
void TrackedVehicle::draw() {
{
dsSetColor(0, 0, 1);
const dReal *pos = dGeomGetPosition(this->vehicleGeom);
const dReal *R = dGeomGetRotation(this->vehicleGeom);
dReal sides[3];
dGeomBoxGetLengths(this->vehicleGeom, sides);
dsDrawBoxD(pos, R, sides);
}
this->leftTrack->draw();
this->rightTrack->draw();
}
/*** ロボットアームの描画 ***/
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);
}
static void drawBox (dGeomID id, int R, int G, int B)
{
if (!id)
return;
const dReal *pos = dGeomGetPosition (id);
const dReal *rot = dGeomGetRotation (id);
dsSetColor (R,G,B);
dVector3 l;
dGeomBoxGetLengths (id, l);
dsDrawBox (pos, rot, l);
}
void CManipulator::drawBox (dGeomID id, float R, float G, float B)
{
if (!id)
return;
const dReal *pos = dGeomGetPosition (id);
const dReal *rot = dGeomGetRotation (id);
dsSetColor (R,G,B);
dVector3 l;
dGeomBoxGetLengths (id, l);
dsDrawBoxD (pos, rot, l);
}
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 GetBoxExtensions(dGeomID box,const dReal* axis,
const dReal *pos, const dReal *rot,
float center_prg,dReal* lo_ext,dReal* hi_ext)
{
R_ASSERT2(dGeomGetClass(box)==dBoxClass,"is not a box");
dVector3 length;
dGeomBoxGetLengths(box,length);
dReal dif=dDOT(pos,axis)-center_prg;
dReal ful_ext=dFabs(dDOT14(axis,rot+0))*length[0]
+dFabs(dDOT14(axis,rot+1))*length[1]
+dFabs(dDOT14(axis,rot+2))*length[2];
ful_ext/=2.f;
*lo_ext=-ful_ext+dif;
*hi_ext=ful_ext+dif;
}
void ODE_Particle::getLengths(dReal &r0,dReal &r1,dReal &r2)
{
if (getShapeType() == dBoxClass)
{
dVector3 sides;
dGeomBoxGetLengths (geom,sides);
r0=sides[0];
r1=sides[1];
r2=sides[2];
}
else
{
printf("ERROR in ODE_Particle.cpp: Requesting dimensions for non box object");
exit(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 drawGeom(dGeomID g)
{
int gclass = dGeomGetClass(g);
const dReal *pos = dGeomGetPosition(g);
const dReal *rot = dGeomGetRotation(g);
switch (gclass) {
case dSphereClass:
dsSetColorAlpha(0, 0.75, 0.5, 0.5);
dsSetTexture (DS_CHECKERED);
dsDrawSphere(pos, rot, dGeomSphereGetRadius(g));
break;
case dBoxClass:
{
dVector3 lengths;
dsSetColorAlpha(1, 1, 0, 0.5);
dsSetTexture (DS_WOOD);
dGeomBoxGetLengths(g, lengths);
dsDrawBox(pos, rot, lengths);
break;
}
case dTriMeshClass:
{
int numi = dGeomTriMeshGetTriangleCount(g);
for (int i=0; i<numi; ++i) {
dVector3 v0, v1, v2;
dGeomTriMeshGetTriangle(g, i, &v0, &v1, &v2);
dsSetTexture (DS_WOOD);
dsSetDrawMode(DS_WIREFRAME);
dsSetColorAlpha(0, 0, 0, 1.0);
dsDrawTriangle(pos, rot, v0, v1, v2, true);
dsSetDrawMode(DS_POLYFILL);
dsSetColorAlpha(1, 1, 0, 0.5);
dsDrawTriangle(pos, rot, v0, v1, v2, true);
}
break;
}
default:
{}
}
}
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 drawGeom (dGeomID g)
{
const dReal *pos = dGeomGetPosition(g);
const dReal *R = dGeomGetRotation(g);
int type = dGeomGetClass (g);
if (type == dBoxClass)
{
dVector3 sides;
dGeomBoxGetLengths (g, sides);
dsDrawBox (pos,R,sides);
}
if (type == dCylinderClass)
{
dReal r,l;
dGeomCylinderGetParams(g, &r, &l);
dsDrawCylinder (pos, R, l, r);
}
}
void CBoxGeom::get_max_area_dir_bt(Fvector& dir)
{
dVector3 length,ddir;
dGeomBoxGetLengths (geometry(),length);
dReal S1=length[0]*length[1],S2=length[0]*length[2],S3=length[1]*length[2];
const dReal* R= dGeomGetRotation(geometry());
if(S1>S2)
{
if(S1>S3)
{
ddir[0]=R[2];ddir[1]=R[6];ddir[2]=R[10];//S1
}
else
{
ddir[0]=R[0];ddir[1]=R[4];ddir[2]=R[8];//S3
}
}
else
{
if(S2>S3)
{
ddir[0]=R[1];ddir[1]=R[5];ddir[2]=R[9];//S2
}
else
{
ddir[0]=R[0];ddir[1]=R[4];ddir[2]=R[8];//S3
}
}
if(geom())
{
const dReal* TR=dGeomGetRotation(geometry_transform());
dir.x=dDOT(ddir,TR);
dir.y=dDOT(ddir,TR+4);
dir.z=dDOT(ddir,TR+8);
}
else
{
dir.x=ddir[0];
dir.y=ddir[1];
dir.z=ddir[2];
}
}
void SkidSteeringVehicle::draw() {
{
dsSetColor(0, 0, 1);
const dReal *pos = dGeomGetPosition(this->vehicleGeom);
const dReal *R = dGeomGetRotation(this->vehicleGeom);
dReal sides[3];
dGeomBoxGetLengths(this->vehicleGeom, sides);
dsDrawBoxD(pos, R, sides);
}
dsSetColor(1, 1, 0);
for(int fr = 0; fr < 2; fr++) {
for(int lr = 0; lr < 2; lr++) {
const dReal *pos = dGeomGetPosition(this->wheelGeom[fr][lr]);
const dReal *R = dGeomGetRotation(this->wheelGeom[fr][lr]);
dReal radius, length;
dGeomCylinderGetParams(this->wheelGeom[fr][lr], &radius, &length);
dsDrawCylinderD(pos, R, length, radius);
}
}
}
void SceneObj::render( void )
{
GLenum err;
if( (err=glGetError()) != 0 )
{
fprintf( stderr, "GLERR %x\n", err );
}
glMatrixMode( GL_MODELVIEW );
glPushMatrix();
const dReal *pos = dBodyGetPosition( m_dbody );
const dReal *rot = dBodyGetRotation( m_dbody );
GLfloat mat[16];
mat[ 0] = rot[ 0]; mat[ 1] = rot[ 4]; mat[ 2] = rot[ 8]; mat[ 3] = 0;
mat[ 4] = rot[ 1]; mat[ 5] = rot[ 5]; mat[ 6] = rot[ 9]; mat[ 7] = 0;
mat[ 8] = rot[ 2]; mat[ 9] = rot[ 6]; mat[10] = rot[10]; mat[11] = 0;
mat[12] = pos[ 0]; mat[13] = pos[ 1]; mat[14] = pos[ 2]; mat[15] = 1;
glMultMatrixf( mat );
dVector3 dims;
dGeomBoxGetLengths( m_dgeom, dims );
glScalef( dims[0]*0.5f, dims[1]*0.5f, dims[2]*0.5f );
glVertexPointer( 3, GL_FLOAT, 0, box_xyz );
glEnableClientState( GL_VERTEX_ARRAY );
glTexCoordPointer( 2, GL_FLOAT, 0, box_st );
glEnableClientState( GL_TEXTURE_COORD_ARRAY );
glEnable( GL_TEXTURE_2D );
glBindTexture( GL_TEXTURE_2D, m_scene_p->m_textures[m_texid] );
glDrawElements( GL_TRIANGLES, 36, GL_UNSIGNED_BYTE, box_idx );
glDisable( GL_TEXTURE_2D );
glPopMatrix();
}
// simulation loop
static void simLoop (int pause)
{
static bool todo = false;
if ( todo ) { // DEBUG
static int cnt = 0;
++cnt;
if (cnt == 5)
command ( 'q' );
if (cnt == 10)
dsStop();
}
if (!pause) {
double simstep = 0.01; // 10ms 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[W], 1,1,0);
drawBox (geom[EXT], 0,1,0);
dVector3 anchorPos;
dReal ang1 = 0;
dReal ang2 = 0;
dVector3 axisP, axisR1, axisR2;
if ( dJointTypePU == type ) {
dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
ang1 = pu->getAngle1();
ang2 = pu->getAngle2();
pu->getAxis1 (axisR1);
pu->getAxis2 (axisR2);
pu->getAxisP (axisP);
dJointGetPUAnchor (pu->id(), anchorPos);
}
else if ( dJointTypePR == type ) {
dPRJoint *pr = dynamic_cast<dPRJoint *> (joint);
pr->getAxis1 (axisP);
pr->getAxis2 (axisR1);
dJointGetPRAnchor (pr->id(), anchorPos);
}
// Draw the axisR
if ( geom[INT] ) {
dsSetColor (1,0,1);
dVector3 l;
dGeomBoxGetLengths (geom[INT], l);
const dReal *rotBox = dGeomGetRotation (geom[W]);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i];
dsDrawBox (pos, rotBox, l);
}
dsSetTexture (DS_CHECKERED);
if ( geom[AXIS1] ) {
dQuaternion q, qAng;
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dGeomGetQuaternion (geom[AXIS1], q);
dQuaternion qq;
dQMultiply1 (qq, qAng, q);
dMatrix3 R;
dQtoR (qq,R);
dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS1]), &radius, &length);
dsSetColor (1,0,0);
dsDrawCylinder (anchorPos, R, length, radius);
}
if ( dJointTypePU == type && geom[AXIS2] ) {
//dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
dQuaternion q, qAng, qq, qq1;
dGeomGetQuaternion (geom[AXIS2], q);
dQFromAxisAndAngle (qAng, 0, 1, 0, ang2);
dQMultiply1 (qq, qAng, q);
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dQMultiply1 (qq1, qAng, qq);
dMatrix3 R;
dQtoR (qq1,R);
dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS2]), &radius, &length);
dsSetColor (0,0,1);
dsDrawCylinder (anchorPos, R, length, radius);
}
dsSetTexture (DS_WOOD);
// Draw the anchor
if ( geom[ANCHOR] ) {
dsSetColor (1,1,1);
dVector3 l;
dGeomBoxGetLengths (geom[ANCHOR], l);
const dReal *rotBox = dGeomGetRotation (geom[D]);
const dReal *posBox = dGeomGetPosition (geom[D]);
dVector3 e;
for (int i=0; i<3; ++i)
e[i] = posBox[i] - anchorPos[i];
dNormalize3 (e);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i];
dsDrawBox (pos, rotBox, l);
}
drawBox (geom[D], 1,1,0);
}
}
