// 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
}
void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
int i,j,n;
const int N = 100;
dContactGeom contact[N];
if (dGeomGetClass (o2) == dRayClass) {
n = dCollide (o2,o1,N,&contact[0],sizeof(dContactGeom));
}
else {
n = dCollide (o1,o2,N,&contact[0],sizeof(dContactGeom));
}
if (n > 0) {
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.01,0.01,0.01};
for (i=0; i<n; i++) {
contact[i].pos[2] += Z_OFFSET;
dsDrawBox (contact[i].pos,RI,ss);
dVector3 n;
for (j=0; j<3; j++) n[j] = contact[i].pos[j] + 0.1*contact[i].normal[j];
dsDrawLine (contact[i].pos,n);
}
}
}
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]);
}
}
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 nearCallback (void *data, dGeomID o1, dGeomID o2)
{
int i;
// if (o1->body && o2->body) return;
// exit without doing anything if the two bodies are connected by a joint
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;
dContact contact[MAX_CONTACTS]; // up to MAX_CONTACTS contacts per box-box
for (i=0; i<MAX_CONTACTS; 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;
}
if (int numc = dCollide (o1,o2,MAX_CONTACTS,&contact[0].geom,
sizeof(dContact))) {
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.02,0.02,0.02};
for (i=0; i<numc; i++) {
if (dGeomGetClass(o1) == dRayClass || dGeomGetClass(o2) == dRayClass){
dMatrix3 Rotation;
dRSetIdentity(Rotation);
dsDrawSphere(contact[i].geom.pos, Rotation, REAL(0.01));
dVector3 End;
End[0] = contact[i].geom.pos[0] + (contact[i].geom.normal[0] * contact[i].geom.depth);
End[1] = contact[i].geom.pos[1] + (contact[i].geom.normal[1] * contact[i].geom.depth);
End[2] = contact[i].geom.pos[2] + (contact[i].geom.normal[2] * contact[i].geom.depth);
End[3] = contact[i].geom.pos[3] + (contact[i].geom.normal[3] * contact[i].geom.depth);
dsDrawLine(contact[i].geom.pos, End);
continue;
}
dJointID c = dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,b1,b2);
if (show_contacts) dsDrawBox (contact[i].geom.pos,RI,ss);
}
}
}
IoObject *IoDrawStuff_dsDrawLine(IoDrawStuff *self, IoObject *locals, IoMessage *m)
{
float *pos1;
float *pos2;
//IoSeq_assertIsVector(self, locals, m);
{
IoSeq *other = IoMessage_locals_vectorArgAt_(m, locals, 0);
pos1 = IoSeq_floatPointerOfLength_(other, 3);
other = IoMessage_locals_vectorArgAt_(m, locals, 1);
pos2 = IoSeq_floatPointerOfLength_(other, 3);
}
dsDrawLine(pos1, pos2);
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);
}
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
assert(o1);
assert(o2);
if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
{
fprintf(stderr,"testing space %p %p\n", (void*)o1, (void*)o2);
// colliding a space with something
dSpaceCollide2(o1,o2,data,&nearCallback);
// Note we do not want to test intersections within a space,
// only between spaces.
return;
}
const int N = 32;
dContact contact[N];
int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
if (n > 0)
{
for (int i=0; i<n; i++)
{
contact[i].surface.mode = 0;
contact[i].surface.mu = 50.0; // was: dInfinity
dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
dJointAttach (c, dGeomGetBody(contact[i].geom.g1), dGeomGetBody(contact[i].geom.g2));
if (show_contacts)
{
dMatrix3 RI;
dRSetIdentity (RI);
const dReal ss[3] = {0.12,0.12,0.12};
dsSetColorAlpha (0,0,1,0.5);
dsDrawBox (contact[i].geom.pos,RI,ss);
dReal *pos = contact[i].geom.pos;
dReal depth = contact[i].geom.depth;
dReal *norm = contact[i].geom.normal;
dReal endp[3] = {pos[0]+depth*norm[0], pos[1]+depth*norm[1], pos[2]+depth*norm[2]};
dsSetColorAlpha (1,1,1,1);
dsDrawLine (contact[i].geom.pos, endp);
}
}
}
}
void InRoFField::drawObjects()
{
for (size_t i = 0; i < LENGTH(field); ++i) {
field[i]->drawInStuff();
}
dVector3 slopeLines[][2] = {
{{0.9 , -0.9, Z_LINE_OFFSET}, {0.9, -0.4, Z_LINE_OFFSET}},
{{0.75, -0.6, Z_LINE_OFFSET}, {-0.6, -0.6, Z_LINE_OFFSET}},
{{0.9, 0.44, Z_LINE_OFFSET}, {-0.6, 0.44, Z_LINE_OFFSET}},
{{-0.75, -0.45, Z_LINE_OFFSET}, {-0.75, 0.29, Z_LINE_OFFSET}},
{{0.75, -0.75, Z_LINE_OFFSET}, {0.75, -0.45, Z_LINE_OFFSET}},
{{0, -0.75, Z_LINE_OFFSET}, {0, -0.45, Z_LINE_OFFSET}},
};
dsSetColor(0,0,0);
for (size_t i = 0; i < LENGTH(slopeLines); ++i) {
dsDrawLine(slopeLines[i][0], slopeLines[i][1]);
}
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldStep (world,0.05);
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
int color_changed = 0;
if (i==selected) {
dsSetColor (0,0.7,1);
color_changed = 1;
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
color_changed = 1;
}
for (int j=0; j < GPB; j++) drawGeom (obj[i].geom[j],0,0);
if (color_changed) dsSetColor (1,1,0);
}
{for (int i = 0; i < RayCount; i++){
dVector3 Origin, Direction;
dGeomRayGet(Rays[i], Origin, Direction);
dReal Length = dGeomRayGetLength(Rays[i]);
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 DrawBlock(Block* Block){
dVector3 v[8];
v[0][AXIS0] = Block->MinX;
v[0][UP] = REAL(-1.0);
v[0][AXIS1] = Block->MinZ;
v[1][AXIS0] = Block->MinX;
v[1][UP] = REAL(-1.0);
v[1][AXIS1] = Block->MaxZ;
v[2][AXIS0] = Block->MaxX;
v[2][UP] = REAL(-1.0);
v[2][AXIS1] = Block->MinZ;
v[3][AXIS0] = Block->MaxX;
v[3][UP] = REAL(-1.0);
v[3][AXIS1] = Block->MaxZ;
v[4][AXIS0] = Block->MinX;
v[4][UP] = REAL(1.0);
v[4][AXIS1] = Block->MinZ;
v[5][AXIS0] = Block->MinX;
v[5][UP] = REAL(1.0);
v[5][AXIS1] = Block->MaxZ;
v[6][AXIS0] = Block->MaxX;
v[6][UP] = REAL(1.0);
v[6][AXIS1] = Block->MinZ;
v[7][AXIS0] = Block->MaxX;
v[7][UP] = REAL(1.0);
v[7][AXIS1] = Block->MaxZ;
// Bottom
dsDrawLine(v[0], v[1]);
dsDrawLine(v[1], v[3]);
dsDrawLine(v[3], v[2]);
dsDrawLine(v[2], v[0]);
// Top
dsDrawLine(v[4], v[5]);
dsDrawLine(v[5], v[7]);
dsDrawLine(v[7], v[6]);
dsDrawLine(v[6], v[4]);
// Sides
dsDrawLine(v[0], v[4]);
dsDrawLine(v[1], v[5]);
dsDrawLine(v[2], v[6]);
dsDrawLine(v[3], v[7]);
}
void nearCallback(void *, dGeomID a, dGeomID b)
{
const unsigned max_contacts = 8;
dContact contacts[max_contacts];
if (!dGeomGetBody(a) && !dGeomGetBody(b))
return; // don't handle static geom collisions
int n = dCollide(a, b, max_contacts, &contacts[0].geom, sizeof(dContact));
//clog << "got " << n << " contacts" << endl;
/* Simple contact merging:
* If we have contacts that are too close with the same normal, keep only
* the one with maximum depth.
* The epsilon that defines what "too close" means can be a heuristic.
*/
int new_n = 0;
dReal epsilon = 1e-1; // default
/* If we know one of the geoms is a sphere, we can base the epsilon on the
* sphere's radius.
*/
dGeomID s = 0;
if ((dGeomGetClass(a) == dSphereClass && (s = a)) ||
(dGeomGetClass(b) == dSphereClass && (s = b))) {
epsilon = dGeomSphereGetRadius(s) * 0.3;
}
for (int i=0; i<n; ++i) {
// this block draws the contact points before merging, in red
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(1, 0, 0);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.008);
// let's offset the line a bit to avoid drawing overlap issues
float xyzf[3], hprf[3];
dsGetViewpoint(xyzf, hprf);
dVector3 xyz = {dReal(xyzf[0]), dReal(xyzf[1]), dReal(xyzf[2])};
dVector3 v;
dSubtractVectors3(v, contacts[i].geom.pos, xyz);
dVector3 c;
dCalcVectorCross3(c, v, contacts[i].geom.pos);
dSafeNormalize3(c);
dVector3 pos1;
dAddScaledVectors3(pos1, contacts[i].geom.pos, c, 1, 0.005);
dVector3 pos2;
dAddScaledVectors3(pos2, pos1, contacts[i].geom.normal, 1, 0.05);
dsDrawLine(pos1, pos2);
// end of contacts drawing code
int closest_point = i;
for (int j=0; j<new_n; ++j) {
dReal alignment = dCalcVectorDot3(contacts[i].geom.normal, contacts[j].geom.normal);
if (alignment > 0.99 // about 8 degrees of difference
&&
dCalcPointsDistance3(contacts[i].geom.pos, contacts[j].geom.pos) < epsilon) {
// they are too close
closest_point = j;
//clog << "found close points: " << j << " and " << i << endl;
break;
}
}
if (closest_point != i) {
// we discard one of the points
if (contacts[i].geom.depth > contacts[closest_point].geom.depth)
// the new point is deeper, copy it over closest_point
contacts[closest_point] = contacts[i];
} else
contacts[new_n++] = contacts[i]; // the point is preserved
}
//clog << "reduced from " << n << " to " << new_n << endl;
n = new_n;
for (int i=0; i<n; ++i) {
contacts[i].surface.mode = dContactBounce | dContactApprox1 | dContactSoftERP;
contacts[i].surface.mu = 10;
contacts[i].surface.bounce = 0.2;
contacts[i].surface.bounce_vel = 0;
contacts[i].surface.soft_erp = 1e-3;
//clog << "depth: " << contacts[i].geom.depth << endl;
dJointID contact = dJointCreateContact(world, contact_group, &contacts[i]);
dJointAttach(contact, dGeomGetBody(a), dGeomGetBody(b));
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(0, 0, 1);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.01);
dsSetColor(0, 1, 0);
dVector3 pos2;
dAddScaledVectors3(pos2, contacts[i].geom.pos, contacts[i].geom.normal, 1, 0.1);
dsDrawLine(contacts[i].geom.pos, pos2);
}
//clog << "----" << endl;
}
static void drawGeom(dGeomID geomID)
{
// printf("1%lu", (uint64_t)geomID);
int gclass = dGeomGetClass(geomID);
// printf("2\n");
const dReal *pos = NULL;
const dReal *rot = NULL;
bool canDrawJoints = false;
ODEUserObject* userObj = (ODEUserObject*)dGeomGetData(geomID);
if (nullptr != userObj) {
dsSetColorAlpha(1, 1, 1, 1);
// printf("%f %f %f %f\n", userObj->colorVec[0], userObj->colorVec[1], userObj->colorVec[2], userObj->colorVec[3]);
dsSetTexture (userObj->textureNum);
dsSetColorAlpha(userObj->m_colorVec[0], userObj->m_colorVec[1], userObj->m_colorVec[2], userObj->m_colorVec[3]);
} else {
dsSetColorAlpha(1, 1, 0, 1);
dsSetTexture (DS_WOOD);
}
switch (gclass) {
case dSphereClass:
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dsDrawSphere(pos, rot, dGeomSphereGetRadius(geomID));
}
canDrawJoints = true;
break;
case dBoxClass:
{
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dVector3 lengths;
dGeomBoxGetLengths(geomID, lengths);
dsDrawBox(pos, rot, lengths);
}
canDrawJoints = true;
break;
}
case dCylinderClass:
{
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dReal length;
dReal radius;
dGeomCylinderGetParams(geomID, &radius, &length);
dsDrawCylinder(pos, rot, length, radius);
}
canDrawJoints = true;
break;
}
default:
break;
}
// printf("class: %d\n", gclass);
if (canDrawJoints) {
#ifdef DRAW_JOINTS_TOO
dBodyID body = dGeomGetBody(geomID);
int numJoints = dBodyGetNumJoints(body);
for (int i = 0; i < numJoints; ++i)
{
dJointID joint = dBodyGetJoint(body, i);
int jointClass = dJointGetType(joint);
switch (jointClass)
{
case dJointTypeHinge:
{
dVector3 a11;
dBodyID body1 = dJointGetBody(joint, 0);
dBodyID body2 = dJointGetBody(joint, 1);
if (body1 && body2) {
const dReal* bodyPos1 = dBodyGetPosition(body1);
const dReal* bodyPos2 = dBodyGetPosition(body2);
dJointGetHingeAnchor(joint, a11);
dsSetColor(1, 0, 0);
dsDrawLine(a11, bodyPos1);
dsDrawLine(a11, bodyPos2);
dsSetColor(0, 1, 0);
dsDrawLine(bodyPos1, bodyPos2);
}
}
}
}
#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;
}
}
}
}
