diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 72999a3109..82941af9b4 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -114,6 +114,8 @@ typedef enum mjtGeom_ { // type of geometric shape mjGEOM_SKIN, // skin mjGEOM_LABEL, // text label mjGEOM_TRIANGLE, // triangle + mjGEOM_CONE, // cone + mjGEOM_PYRAMID, // pyramid mjGEOM_NONE = 1001 // missing geom type } mjtGeom; @@ -469,6 +471,7 @@ struct mjVisual_ { // visualization options int offwidth; // width of offscreen buffer int offheight; // height of offscreen buffer int ellipsoidinertia; // geom for inertia visualization (0: box, 1: ellipsoid) + // int conecontact; // geom for contact visualization (0: cylinder, 1: cone) int bvactive; // visualize active bounding volumes (0: no, 1: yes) } global; diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 66177c4f70..9b45bdd27f 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -14,6 +14,7 @@ #include "engine/engine_vis_visualize.h" +#include #include #include @@ -99,33 +100,69 @@ static void islandColor(float rgba[4], int islanddofadr) { static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, const mjvOption* vopt, mjvScene* scn) { int objtype = mjOBJ_UNKNOWN, category = mjCAT_DECOR; - mjtNum mat[9], tmp[9], vec[3], frc[3], confrc[6], axis[3]; - mjtNum framewidth, framelength, scl = m->stat.meansize; - mjContact* con; mjvGeom* thisgeom; - mjtByte split; // fast return if all relevant features are disabled if (!flags[mjVIS_CONTACTPOINT] && !flags[mjVIS_CONTACTFORCE] && vopt->frame != mjFRAME_CONTACT) { return; } + // visual sizes + mjtNum scl = m->stat.meansize; + float conwidth = m->vis.scale.contactwidth * scl; + float conheight = m->vis.scale.contactheight * scl; + float hypot = sqrtf(conwidth*conwidth + conheight*conheight); + mjtNum framelength = m->vis.scale.framelength * scl / 2; + mjtNum framewidth = m->vis.scale.framewidth * scl / 2; + // loop over contacts for (int i=0; i < d->ncon; i++) { // get pointer - con = d->contact + i; + mjContact* con = d->contact + i; // mat = contact rotation matrix (normal along z) + mjtNum tmp[9], mat[9]; mju_copy(tmp, con->frame+3, 6); mju_copy(tmp+6, con->frame, 3); mju_transpose(mat, tmp, 3, 3); + // get bodyflex ids + int bf[2]; + bf[0] = (con->geom[0] >= 0) ? m->geom_bodyid[con->geom[0]] : m->nbody + con->flex[0]; + bf[1] = (con->geom[1] >= 0) ? m->geom_bodyid[con->geom[1]] : m->nbody + con->flex[1]; + // contact point if (flags[mjVIS_CONTACTPOINT]) { START - thisgeom->type = mjGEOM_CYLINDER; - thisgeom->size[0] = thisgeom->size[1] = m->vis.scale.contactwidth * scl; - thisgeom->size[2] = m->vis.scale.contactheight * scl; + + // non-world contact, use cylinder + if (bf[0] > 0 && bf[1] > 0) { + thisgeom->type = mjGEOM_CYLINDER; + thisgeom->size[0] = thisgeom->size[1] = conwidth; + thisgeom->size[2] = conheight; + } + + // world contact, use pyramid or cone + else { + // geom type according to cone + thisgeom->type = m->opt.cone == mjCONE_PYRAMIDAL ? mjGEOM_PYRAMID : mjGEOM_CONE; + + // get tangential friction coefs, convert tangent to sines + float f0 = con->friction[0]; + float f1 = con->friction[1]; + float sin0 = sqrtf(f0*f0 / (1 + f0*f0)); + float sin1 = sqrtf(f1*f1 / (1 + f1*f1)); + + // get cosine, hypotenuse + float sinmax = mjMAX(sin0, sin1); + float cos = sqrtf(1 - sinmax*sinmax); + + // set sizes + thisgeom->size[0] = hypot * sin0; + thisgeom->size[1] = hypot * sin1; + thisgeom->size[2] = hypot * cos; + } + mju_n2f(thisgeom->pos, con->pos, 3); mju_n2f(thisgeom->mat, mat, 9); @@ -191,18 +228,16 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, // contact frame if (vopt->frame == mjFRAME_CONTACT) { - // set length and width of axis cylinders using half regular frame scaling - framelength = m->vis.scale.framelength * scl / 2; - framewidth = m->vis.scale.framewidth * scl / 2; - // draw the three axes (separate geoms) for (int j=0; j < 3; j++) { START // prepare axis + mjtNum axis[3]; for (int k=0; k < 3; k++) { axis[k] = (j == k ? framelength : 0); } + mjtNum vec[3]; mju_mulMatVec(vec, mat, axis, 3, 3); // create a cylinder @@ -230,11 +265,13 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, mju_transpose(mat, con->frame, 3, 3); // get contact force:torque in contact frame + mjtNum confrc[6]; mj_contactForce(m, d, i, confrc); // contact force if (flags[mjVIS_CONTACTFORCE]) { // get force, fill zeros if only normal + mjtNum frc[3]; mju_zero3(frc); mju_copy(frc, confrc, mjMIN(3, con->dim)); if (mju_norm3(frc) < mjMINVAL) { @@ -242,9 +279,10 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, } // render combined or split - split = (flags[mjVIS_CONTACTSPLIT] && con->dim > 1); + mjtByte split = (flags[mjVIS_CONTACTSPLIT] && con->dim > 1); for (int j = (split ? 1 : 0); j < (split ? 3 : 1); j++) { // set vec to combined, normal or friction force, in world frame + mjtNum vec[3]; switch (j) { case 0: // combined mju_mulMatVec(vec, mat, frc, 3, 3); @@ -264,13 +302,6 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, // scale vector mju_scl3(vec, vec, m->vis.map.force/m->stat.meanmass); - // get bodyflex ids - int bf[2]; - for (int k=0; k < 2; k++) { - bf[k] = (con->geom[k] >= 0) ? m->geom_bodyid[con->geom[k]] : - m->nbody + con->flex[k]; - } - // make sure arrow points towards bodyflex with higher id if (bf[0] > bf[1]) { mju_scl3(vec, vec, -1); diff --git a/src/render/render_context.c b/src/render/render_context.c index ec349b2a13..056f8c806d 100644 --- a/src/render/render_context.c +++ b/src/render/render_context.c @@ -755,23 +755,33 @@ static void disk(int sign, int nSlice, int nStack) { // set vertex and normal on cone, given az, r -static void setVertexCone(float* v, float* n, float az, float r) { +// apex 0: cone open to +z, 1: cone open to -z +static void setVertexCone(float v[3], float n[3], float az, float r, int apex) { const float scale = 1.0f/sqrtf(2.0f); // vertex v[0] = cosf(az) * r; v[1] = sinf(az) * r; - v[2] = 1 - r; + v[2] = apex ? 1 - r : r; // normal - n[0] = cosf(az) * scale; - n[1] = sinf(az) * scale; + n[0] = cosf(az) * scale * (apex ? 1 : -1); + n[1] = sinf(az) * scale * (apex ? 1 : -1); n[2] = scale; } -// make open cone -static void cone(int nSlice, int nStack) { + +static void flip(float vec[3]) { + vec[0] = -vec[0]; + vec[1] = -vec[1]; + vec[2] = -vec[2]; +} + + + +// make open cone, apex at either 0 or 1, one-sided or two sided surface +static void cone(int nSlice, int nStack, int apex, int twosided) { float az1, az2, r1, r2; float v1[3], v2[3], v3[3], v4[3]; float n1[3], n2[3], n3[3], n4[3]; @@ -784,10 +794,10 @@ static void cone(int nSlice, int nStack) { az2 = (2.0f*mjPI * (j+1)) / (float)nSlice; // compute triangle vertices - setVertexCone(v1, n1, az1, r1); - setVertexCone(v2, n2, az2, r1); + setVertexCone(v1, n1, az1, r1, apex); + setVertexCone(v2, n2, az2, r1, apex); v3[0] = v3[1] = 0; - v3[2] = 1; + v3[2] = apex; n3[0] = n1[0]+n2[0]; n3[1] = n1[1]+n2[1]; n3[2] = n1[2]+n2[2]; @@ -800,6 +810,19 @@ static void cone(int nSlice, int nStack) { glVertex3fv(v2); glNormal3fv(n3); glVertex3fv(v3); + + // flip normals, make reverse triangle + if (twosided) { + flip(n3); + flip(n2); + flip(n1); + glNormal3fv(n3); + glVertex3fv(v3); + glNormal3fv(n2); + glVertex3fv(v2); + glNormal3fv(n1); + glVertex3fv(v1); + } } glEnd(); @@ -814,10 +837,10 @@ static void cone(int nSlice, int nStack) { az2 = (2.0f*mjPI * (j+1)) / (float)nSlice; // compute quad vertices - setVertexCone(v1, n1, az1, r2); - setVertexCone(v2, n2, az2, r2); - setVertexCone(v3, n3, az2, r1); - setVertexCone(v4, n4, az1, r1); + setVertexCone(v1, n1, az1, r2, apex); + setVertexCone(v2, n2, az2, r2, apex); + setVertexCone(v3, n3, az2, r1, apex); + setVertexCone(v4, n4, az1, r1, apex); // make quad glNormal3fv(n1); @@ -828,13 +851,28 @@ static void cone(int nSlice, int nStack) { glVertex3fv(v3); glNormal3fv(n4); glVertex3fv(v4); + + // flip normals, make reverse quad + if (twosided) { + flip(n4); + flip(n3); + flip(n2); + flip(n1); + glNormal3fv(n4); + glVertex3fv(v4); + glNormal3fv(n3); + glVertex3fv(v3); + glNormal3fv(n2); + glVertex3fv(v2); + glNormal3fv(n1); + glVertex3fv(v1); + } } } glEnd(); } - // set one vertex and normal on cylinder, given az and h static void setVertexCylinder(float* v, float* n, float az, float h) { v[0] = cosf(az); @@ -1027,11 +1065,21 @@ static void makeBuiltin(const mjModel* m, mjrContext* con) { glEnd(); glEndList(); - // cone, bottom at z=0 + // cone, apex at z=1 glNewList(con->baseBuiltin + mjrCONE, GL_COMPILE); - cone(numslices, numstacks); + cone(numslices, numstacks, 1, 0); disk(0, numslices, numstacks); glEndList(); + + // cone, apex at z=0 + glNewList(con->baseBuiltin + mjrOPENCONE, GL_COMPILE); + cone(numslices, numstacks, 0, 1); + glEndList(); + + // pyramid, apex at z=0 + glNewList(con->baseBuiltin + mjrOPENPYRAMID, GL_COMPILE); + cone(4, numstacks, 0, 1); + glEndList(); } diff --git a/src/render/render_context.h b/src/render/render_context.h index 6e29c0968e..733f1533ab 100644 --- a/src/render/render_context.h +++ b/src/render/render_context.h @@ -34,6 +34,8 @@ enum { mjrHAZE, mjrBOX, mjrCONE, + mjrOPENCONE, + mjrOPENPYRAMID, mjrNUM }; diff --git a/src/render/render_gl3.c b/src/render/render_gl3.c index 4591c4bb13..3bba432411 100644 --- a/src/render/render_gl3.c +++ b/src/render/render_gl3.c @@ -447,6 +447,16 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos, glEnd(); break; + case mjGEOM_CONE: // open cone + glScalef(size[0], size[1], size[2]); + glCallList(con->baseBuiltin + mjrOPENCONE); + break; + + case mjGEOM_PYRAMID: // open pyramid + glScalef(size[0], size[1], size[2]); + glCallList(con->baseBuiltin + mjrOPENPYRAMID); + break; + case mjGEOM_FLEX: // flex { // no texture for vertices and edges @@ -655,7 +665,7 @@ static void initLights(mjvScene* scn) { // init light model glLightModelfv(GL_LIGHT_MODEL_AMBIENT, rgba_global); - glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 0); + glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 1); glLightModeli(GL_LIGHT_MODEL_LOCAL_VIEWER, 1); // set light properties