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);
}
}
}
/**
* @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);
}
}
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);
}
}
}
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 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)
{
// stop after a given number of iterations, as long as we are not in
// interactive mode
if (cmd_graphics && !cmd_interactive &&
(iteration >= max_iterations)) {
dsStop();
return;
}
iteration++;
if (!pause) {
// do stuff for this test and check to see if the joint is behaving well
dReal error = doStuffAndGetError (test_num);
if (error > max_error) max_error = error;
if (cmd_interactive && error < dInfinity) {
printf ("scaled error = %.4e\n",error);
}
// take a step
dWorldStep (world,STEPSIZE);
// occasionally re-orient the first body to create a deliberate error.
if (cmd_occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
}
if (cmd_graphics) {
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE*0.99f,SIDE*0.99f,SIDE*0.99f};
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
if (body[1]) {
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
}
}
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);
}
}
static void simLoop (int pause)
{
const dReal kd = -0.3; // angular damping constant
const dReal ks = 0.5; // spring constant
if (!pause) {
// add an oscillating torque to body 0, and also damp its rotational motion
static dReal a=0;
const dReal *w = dBodyGetAngularVel (body[0]);
dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
a += 0.01;
// add a spring force to keep the bodies together, otherwise they will
// fly apart along the slider axis.
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),
ks*(p2[2]-p1[2]));
dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),
ks*(p1[2]-p2[2]));
// occasionally re-orient one of the bodies to create a deliberate error.
if (occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
dWorldStep (world,0.05);
}
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE*0.8f,SIDE*0.8f,SIDE*2.0f};
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 doDraw(){
if (!leg) return;
dsSetColor(0.0,0.0,1.0);
dsDrawCapsuleD(dBodyGetPosition(leg->lower_part.body), dBodyGetRotation(leg->lower_part.body), leg->lower_part.length, leg->lower_part.radius);
dsSetColor(1.0,0.0,0.0);
dsDrawCapsuleD(dBodyGetPosition(leg->upper_part.body), dBodyGetRotation(leg->upper_part.body), leg->upper_part.length, leg->upper_part.radius);
//TEST
double k1 = 1.0, fMax = 10.0; // k1: proportional gain, fMax：Max torque[Nm]
//dJointSetHingeParam(jointHinge, dParamVel, k1); // Set angular velocity[m/s]
//dJointSetHingeParam(jointHinge, dParamFMax, fMax); // Set max torque[N/m]
printf("\r %6d:",dJointGetHingeAngle(leg->jointHinge));
}
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 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 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);
}
void simLoop (int pause)
{
static bool DumpInfo=true;
const dReal ss[3] = {0.02,0.02,0.02};
dContactGeom contacts[8];
int contactcount = dCollideConvexConvex(geoms[0],geoms[1],8,contacts,sizeof(dContactGeom));
//fprintf(stdout,"Contact Count %d\n",contactcount);
const dReal* pos;
const dReal* R;
dsSetTexture (DS_WOOD);
pos = dGeomGetPosition (geoms[0]);
R = dGeomGetRotation (geoms[0]);
dsSetColor (0.6f,0.6f,1);
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
pos = dGeomGetPosition (geoms[1]);
R = dGeomGetRotation (geoms[1]);
dsSetColor (0.4f,1,1);
dsDrawConvex(pos,R,planes,
planecount,
points,
pointcount,
polygons);
/*if (show_contacts) */
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColor (1.0f,0,0);
for(int i=0;i<contactcount;++i)
{
if(DumpInfo)
{
//DumpInfo=false;
fprintf(stdout,"Contact %d Normal %f,%f,%f Depth %f\n",
i,
contacts[i].normal[0],
contacts[i].normal[1],
contacts[i].normal[2],
contacts[i].depth);
}
dsDrawBox (contacts[i].pos,RI,ss);
}
if(DumpInfo)
DumpInfo=false;
}
static void simLoop (int pause)
{
if (!pause) {
// add random forces and torques to all bodies
int i;
const dReal scale1 = 5;
const dReal scale2 = 5;
for (i=0; i<NUM; i++) {
dBodyAddForce (body[i],
scale1*(dRandReal()*2-1),
scale1*(dRandReal()*2-1),
scale1*(dRandReal()*2-1));
dBodyAddTorque (body[i],
scale2*(dRandReal()*2-1),
scale2*(dRandReal()*2-1),
scale2*(dRandReal()*2-1));
}
dWorldStep (world,0.05);
createTest();
}
// float sides[3] = {SIDE,SIDE,SIDE};
dsSetColor (1,1,0);
for (int i=0; i<NUM; i++)
dsDrawSphere (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),RADIUS);
}
void SlopeField::drawObjects()
{
for (size_t i = 0; i < LENGTH(slopes); ++i) {
slopes[i]->drawInStuff();
}
#define LINE_DRAW_OFFSET 0.005
dVector3 slopeLines[][2] = {
{{1 , 0.985, LINE_DRAW_OFFSET}, {1 , -0.985, LINE_DRAW_OFFSET}},
{{1.5, 0.985, 0.15 + LINE_DRAW_OFFSET}, {1.5, -0.985, 0.15 + LINE_DRAW_OFFSET}},
{{2 , 0.985, 0.3 + LINE_DRAW_OFFSET}, {2 , -0.985, 0.3 + LINE_DRAW_OFFSET}},
{{2.5, 0.985, 0.3 + LINE_DRAW_OFFSET}, {2.5, -0.985, 0.3 + LINE_DRAW_OFFSET}},
{{3 , 0.985, 0.3 + LINE_DRAW_OFFSET}, {3 , -0.985, 0.3 + LINE_DRAW_OFFSET}},
{{3.5, 0.985, 0.15 + LINE_DRAW_OFFSET}, {3.5, -0.985, 0.15 + LINE_DRAW_OFFSET}},
{{4 , 0.985, LINE_DRAW_OFFSET}, {4 , -0.985, LINE_DRAW_OFFSET}},
{{1 , 0 , 0.00 + LINE_DRAW_OFFSET}, {2 , 0 , 0.3 + LINE_DRAW_OFFSET}},
{{2 , 0 , 0.30 + LINE_DRAW_OFFSET}, {3 , 0 , 0.3 + LINE_DRAW_OFFSET}},
{{3 , 0 , 0.30 + LINE_DRAW_OFFSET}, {4 , 0 , 0.0 + LINE_DRAW_OFFSET}},
{{1 , 0.5 , 0.00 + LINE_DRAW_OFFSET}, {2 , 0.5 , 0.3 + LINE_DRAW_OFFSET}},
{{2 , 0.5 , 0.30 + LINE_DRAW_OFFSET}, {3 , 0.5 , 0.3 + LINE_DRAW_OFFSET}},
{{3 , 0.5 , 0.30 + LINE_DRAW_OFFSET}, {4 , 0.5 , 0.0 + LINE_DRAW_OFFSET}},
{{1 , -0.5 , 0.00 + LINE_DRAW_OFFSET}, {2 , -0.5 , 0.3 + LINE_DRAW_OFFSET}},
{{2 , -0.5 , 0.30 + LINE_DRAW_OFFSET}, {3 , -0.5 , 0.3 + LINE_DRAW_OFFSET}},
{{3 , -0.5 , 0.30 + LINE_DRAW_OFFSET}, {4 , -0.5 , 0.0 + LINE_DRAW_OFFSET}},
};
dsSetColor(1,1,1);
for (size_t i = 0; i < LENGTH(slopeLines); ++i) {
dsDrawLine(slopeLines[i][0], slopeLines[i][1]);
}
}
void DrawOmni(){
/* 車輪の描画 */
dReal radius, length;
dsSetColor(0.3, 0.3, 0.3);
for (int i=0; i<OMNI_NUM; i++)
{
for (int j=0; j< WHEEL_NUM; j++)
{
dGeomCylinderGetParams(wheel[i][j].geom, &radius, &length);
dsDrawCylinder(dGeomGetPosition(wheel[i][j].geom), dGeomGetRotation(wheel[i][j].geom),length, radius);
}
/* 土台の描画 */
dsSetColor(0.3, 0.1, 0.1);
dsDrawBox(dBodyGetPosition(base[i].body),dBodyGetRotation(base[i].body),baseSize[i]);
}
}
/*** ゴールの描画 ***/
static void drawGoal()
{
dsSetTexture(DS_NONE);
for (int i = 0; i < GOAL_PARTS_NUM; i++)
{
if (i < 4) dsSetColor(1.3, 1.3, 1.3);
else dsSetColor(1.3, 1.3, 0.0);
dsDrawBox(dGeomGetPosition(goal_parts[i]),
dGeomGetRotation(goal_parts[i]),GOAL_SIDES[i]);
if (i < 4) dsSetColor(1.3, 1.3, 1.3);
else dsSetColor(0.0, 0.0, 1.3);
dsDrawBox(dGeomGetPosition(goal_parts2[i]),
dGeomGetRotation(goal_parts2[i]),GOAL_SIDES[i]);
}
}
/*** 台車の描画 ***/
static void drawBase()
{
// dsSetColor(1.3, 1.3, 0.0); // 黄色
// dsSetColor(0.0, 0.0, 1.3); // 青
dsSetColor(0.1, 0.1, 0.1); // 黒
dsDrawBox(dBodyGetPosition(base.body),dBodyGetRotation(base.body),SIDE);
}
// simulation loop
static void simLoop (int pause)
{
const dReal *pos1,*R1,*pos2,*R2,*pos3,*R3;
//@a sphere
dsSetColor(1,0,0); // set color (red, green, blue�j a value is between 0 and 1
dsSetSphereQuality(3); // set sphere quality. 3 is enough
pos1 = dBodyGetPosition(sphere.body); // get a body position
R1 = dBodyGetRotation(sphere.body); // get a body rotation matrix
dsDrawSphere(pos1,R1,radius); // draw a sphere
// a cylinder
dsSetColorAlpha (0,1,0,1);
pos2 = dBodyGetPosition(cylinder.body);
R2 = dBodyGetRotation(cylinder.body);
dsDrawCylinder(pos2,R2,length,radius); // draw a cylinder
// a capsule
dsSetColorAlpha (1,1,1,1);
pos2 = dBodyGetPosition(capsule.body);
R2 = dBodyGetRotation(capsule.body);
dsDrawCapsule(pos2,R2,length,radius); // draw a capsule
// a box
dsSetColorAlpha (0,0,1,1);
pos3 = dBodyGetPosition(box.body);
R3 = dBodyGetRotation(box.body);
dsDrawBox(pos3,R3,sides); // draw a box
// a ray
dReal posA[3] = {0, 5, 0}, posB[3]= {0, 5, 1.9};
dsDrawLine(posA,posB); // draw a ray
}
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]));
*/
}
//Fonction de dessin avec drawstuff
void dessin_env( Bloc *bloc1, Bloc *bloc2 ){
dReal sides[3] = {LARGEUR, LONGUEUR, EPAISSEUR };
dsSetColor (COULEUR_BLOC);
dsSetTexture (DS_WOOD);
dsDrawBox ( dBodyGetPosition( bloc1->bodyID ) , dBodyGetRotation( bloc1->bodyID ), sides);
dsDrawBox ( dBodyGetPosition( bloc2->bodyID ) , dBodyGetRotation( bloc2->bodyID ), sides);
}
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);
}
IoObject *IoDrawStuff_dsSetColor(IoDrawStuff *self, IoObject *locals, IoMessage *m)
{
float red = IoMessage_locals_intArgAt_(m, locals, 0);
float green = IoMessage_locals_intArgAt_(m, locals, 1);
float blue = IoMessage_locals_intArgAt_(m, locals, 2);
dsSetColor(red, green, blue);
return self;
}
// ロボットの描画
static void draw() {
const dReal *pos, *rot;
dReal side[3];
// SHIELDの描画
pos = dBodyGetPosition(rod[0].body);
rot = dBodyGetRotation(rod[0].body);
dsSetColor(1.0 ,0.0 ,0.0);
dsDrawCylinder(pos, rot, SHIELD_LENGTH, SHIELD_RADIUS);
// RODの描画
pos = dBodyGetPosition(rod[1].body);
rot = dBodyGetRotation(rod[1].body);
side[0] = ROD_WIDTH;
side[1] = ROD_WIDTH;
side[2] = ROD_LENGTH;
dsSetColor(0.9 ,0.7 ,0.13);
dsDrawBox(pos, rot, side);
// BODYの描画
pos = dBodyGetPosition(rod[2].body);
rot = dBodyGetRotation(rod[2].body);
side[0] = BODY_WIDTH;
side[1] = BODY_LENGTH;
side[2] = BODY_HEIGHT;
dsSetColor(0.0 ,0.0 ,1.0);
dsDrawBox(pos, rot, side);
// BULLETの描画
pos = dBodyGetPosition(bullet.body);
rot = dBodyGetRotation(bullet.body);
dsSetColor(0 ,0 ,0);
dsDrawSphere(pos, rot, BULLET_RADIUS+0.1);
// RAYの描画
dVector3 pos_s,pos_e;
pos_s[0] = CANNON_X; pos_s[1]=CANNON_Y; pos_s[2]=CANNON_Z;
pos_e[0] = CANNON_X + CANNON_Y * tan(cannon_q_d[0]);
pos_e[1] = 0.0;
pos_e[2] = CANNON_Z + CANNON_Y * tan(- 1.0 * cannon_q_d[1]) / cos(cannon_q_d[0]);
dsSetColor(1 ,0 ,0);
dsDrawLine(pos_s, pos_e);
}
void desenharMundo()
{
desenharRobos();
//desenhar bolas
dsSetColor (1,0.5,0.0);
dsDrawSphere (dBodyGetPosition(bola.corpo),dBodyGetRotation(bola.corpo), BOLA_RAIO);
desenharCampo();
}
// Simulation loop
void simLoop(int pause) {
control();
dWorldStep(world, 0.02);
const double* var;
// Draw a robot
dsSetColor(1.0,1.0,1.0); // Set color (r, g, b), In this case white is set
for (int i = 0; i < NUM; i++ ){// Draw capsules for links
dsDrawCapsuleD(var, var , l[i], r[i]);
}
}
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 DisplayOpenDESpaces::displaySpace(std::vector<dGeomID> g, std::vector<Tmesh> *tm)
{
int ngeoms = g.size();
for (int i = 0 ; i < ngeoms-1 ; ++i)
{
dGeomID geom = g[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);
if(tm==NULL)
drawGeom(geom, NULL, NULL, 0);
else
drawGeom(geom, NULL, NULL, 0,tm->at(i));
}
drawGeom(g[ngeoms-1], NULL, NULL, 0);
}
/*** 車輪の描画 ***/
void drawWheel()
{
dReal radius, length;
dsSetColor(1.1,1.1,1.1);
for (int i=0; i< WHEEL_NUM; i++)
{
dGeomCylinderGetParams(wheel[i].geom, &radius, &length);
dsDrawCylinder(dGeomGetPosition(wheel[i].geom),
dGeomGetRotation(wheel[i].geom),length, radius);
}
}
