{

int i, n;

// set figures to default

mjv_defaultFigure(&figconstraint);

mjv_defaultFigure(&figcost);

mjv_defaultFigure(&figtimer);

mjv_defaultFigure(&figsize);

// titles

strcpy(figconstraint.title, "Counts");

strcpy(figcost.title, "Convergence (log 10)");

strcpy(figsize.title, "Dimensions");

strcpy(figtimer.title, "CPU time (msec)");

// x-labels

strcpy(figconstraint.xlabel, "Solver iteration");

strcpy(figcost.xlabel, "Solver iteration");

strcpy(figsize.xlabel, "Video frame");

strcpy(figtimer.xlabel, "Video frame");

// y-tick nubmer formats

strcpy(figconstraint.yformat, "%.0f");

strcpy(figcost.yformat, "%.1f");

strcpy(figsize.yformat, "%.0f");

strcpy(figtimer.yformat, "%.2f");

// colors

figconstraint.figurergba[0] = 0.1f;

figcost.figurergba[2] = 0.2f;

figsize.figurergba[0] = 0.1f;

figtimer.figurergba[2] = 0.2f;

// legends

strcpy(figconstraint.linename[0], "total");

strcpy(figconstraint.linename[1], "active");

strcpy(figconstraint.linename[2], "changed");

strcpy(figconstraint.linename[3], "evals");

strcpy(figconstraint.linename[4], "updates");

strcpy(figcost.linename[0], "improvement");

strcpy(figcost.linename[1], "gradient");

strcpy(figcost.linename[2], "lineslope");

strcpy(figsize.linename[0], "dof");

strcpy(figsize.linename[1], "body");

strcpy(figsize.linename[2], "constraint");

strcpy(figsize.linename[3], "sqrt(nnz)");

strcpy(figsize.linename[4], "contact");

strcpy(figsize.linename[5], "iteration");

strcpy(figtimer.linename[0], "total");

strcpy(figtimer.linename[1], "collision");

strcpy(figtimer.linename[2], "prepare");

strcpy(figtimer.linename[3], "solve");

strcpy(figtimer.linename[4], "other");

// grid sizes

figconstraint.gridsize[0] = 5;

figconstraint.gridsize[1] = 5;

figcost.gridsize[0] = 5;

figcost.gridsize[1] = 5;

figsize.gridsize[0] = 3;

figsize.gridsize[1] = 5;

figtimer.gridsize[0] = 3;

figtimer.gridsize[1] = 5;

// minimum ranges

figconstraint.range[0][0] = 0;

figconstraint.range[0][1] = 20;

figconstraint.range[1][0] = 0;

figconstraint.range[1][1] = 80;

figcost.range[0][0] = 0;

figcost.range[0][1] = 20;

figcost.range[1][0] = -15;

figcost.range[1][1] = 5;

figsize.range[0][0] = -200;

figsize.range[0][1] = 0;

figsize.range[1][0] = 0;

figsize.range[1][1] = 100;

figtimer.range[0][0] = -200;

figtimer.range[0][1] = 0;

figtimer.range[1][0] = 0;

figtimer.range[1][1] = 0.4f;

// init x axis on history figures (do not show yet)

for( n=0; n<6; n++ )

for( i=0; i<mjMAXLINEPNT; i++ )

{

figtimer.linedata[n][2*i] = (float)-i;

figsize.linedata[n][2*i] = (float)-i;

}

{

int i, n;

// update constraint figure

figconstraint.linepnt[0] = mjMIN(mjMIN(d->solver_iter, mjNSOLVER), mjMAXLINEPNT);

for( i=1; i<5; i++ )

figconstraint.linepnt[i] = figconstraint.linepnt[0];

if( m->opt.solver==mjSOL_PGS )

{

figconstraint.linepnt[3] = 0;

figconstraint.linepnt[4] = 0;

}

if( m->opt.solver==mjSOL_CG )

figconstraint.linepnt[4] = 0;

for( i=0; i<figconstraint.linepnt[0]; i++ )

{

// x

figconstraint.linedata[0][2*i] = (float)i;

figconstraint.linedata[1][2*i] = (float)i;

figconstraint.linedata[2][2*i] = (float)i;

figconstraint.linedata[3][2*i] = (float)i;

figconstraint.linedata[4][2*i] = (float)i;

// y

figconstraint.linedata[0][2*i+1] = (float)d->nefc;

figconstraint.linedata[1][2*i+1] = (float)d->solver[i].nactive;

figconstraint.linedata[2][2*i+1] = (float)d->solver[i].nchange;

figconstraint.linedata[3][2*i+1] = (float)d->solver[i].neval;

figconstraint.linedata[4][2*i+1] = (float)d->solver[i].nupdate;

}

// update cost figure

figcost.linepnt[0] = mjMIN(mjMIN(d->solver_iter, mjNSOLVER), mjMAXLINEPNT);

for( i=1; i<3; i++ )

figcost.linepnt[i] = figcost.linepnt[0];

if( m->opt.solver==mjSOL_PGS )

{

figcost.linepnt[1] = 0;

figcost.linepnt[2] = 0;

}

for( i=0; i<figcost.linepnt[0]; i++ )

{

// x

figcost.linedata[0][2*i] = (float)i;

figcost.linedata[1][2*i] = (float)i;

figcost.linedata[2][2*i] = (float)i;

// y

figcost.linedata[0][2*i+1] = (float)mju_log10(mju_max(mjMINVAL, d->solver[i].improvement));

figcost.linedata[1][2*i+1] = (float)mju_log10(mju_max(mjMINVAL, d->solver[i].gradient));

figcost.linedata[2][2*i+1] = (float)mju_log10(mju_max(mjMINVAL, d->solver[i].lineslope));

}

// get timers: total, collision, prepare, solve, other

int itotal = (d->timer[mjTIMER_STEP].duration > d->timer[mjTIMER_FORWARD].duration ?

mjTIMER_STEP : mjTIMER_FORWARD);

float tdata[5] = {

(float)(d->timer[itotal].duration/mjMAX(1,d->timer[itotal].number)),

(float)(d->timer[mjTIMER_POS_COLLISION].duration/mjMAX(1,d->timer[mjTIMER_POS_COLLISION].number)),

(float)(d->timer[mjTIMER_POS_MAKE].duration/mjMAX(1,d->timer[mjTIMER_POS_MAKE].number)) +

(float)(d->timer[mjTIMER_POS_PROJECT].duration/mjMAX(1,d->timer[mjTIMER_POS_PROJECT].number)),

(float)(d->timer[mjTIMER_CONSTRAINT].duration/mjMAX(1,d->timer[mjTIMER_CONSTRAINT].number)),

0

};

tdata[4] = tdata[0] - tdata[1] - tdata[2] - tdata[3];

// update figtimer

int pnt = mjMIN(201, figtimer.linepnt[0]+1);

for( n=0; n<5; n++ )

{

// shift data

for( i=pnt-1; i>0; i-- )

figtimer.linedata[n][2*i+1] = figtimer.linedata[n][2*i-1];

// assign new

figtimer.linepnt[n] = pnt;

figtimer.linedata[n][1] = tdata[n];

}

// get sizes: nv, nbody, nefc, sqrt(nnz), ncont, iter

float sdata[6] = {

(float)m->nv,

(float)m->nbody,

(float)d->nefc,

(float)mju_sqrt((mjtNum)d->solver_nnz),

(float)d->ncon,

(float)d->solver_iter

};

// update figsize

pnt = mjMIN(201, figsize.linepnt[0]+1);

for( n=0; n<6; n++ )

{

// shift data

for( i=pnt-1; i>0; i-- )

figsize.linedata[n][2*i+1] = figsize.linedata[n][2*i-1];

// assign new

figsize.linepnt[n] = pnt;

figsize.linedata[n][1] = sdata[n];

}

{

mjrRect viewport = {rect.width - rect.width/5, rect.bottom, rect.width/5, rect.height/4};

mjr_figure(viewport, &figtimer, &con);

viewport.bottom += rect.height/4;

mjr_figure(viewport, &figsize, &con);

viewport.bottom += rect.height/4;

mjr_figure(viewport, &figcost, &con);

viewport.bottom += rect.height/4;

mjr_figure(viewport, &figconstraint, &con);

{

// set figure to default

mjv_defaultFigure(&figsensor);

// set flags

figsensor.flg_extend = 1;

figsensor.flg_barplot = 1;

// title

strcpy(figsensor.title, "Sensor data");

// y-tick nubmer format

strcpy(figsensor.yformat, "%.0f");

// grid size

figsensor.gridsize[0] = 2;

figsensor.gridsize[1] = 3;

// minimum range

figsensor.range[0][0] = 0;

figsensor.range[0][1] = 0;

figsensor.range[1][0] = -1;

figsensor.range[1][1] = 1;

{

static const int maxline = 10;

// clear linepnt

for( int i=0; i<maxline; i++ )

figsensor.linepnt[i] = 0;

// start with line 0

int lineid = 0;

// loop over sensors

for( int n=0; n<m->nsensor; n++ )

{

// go to next line if type is different

if( n>0 && m->sensor_type[n]!=m->sensor_type[n-1] )

lineid = mjMIN(lineid+1, maxline-1);

// get info about this sensor

mjtNum cutoff = (m->sensor_cutoff[n]>0 ? m->sensor_cutoff[n] : 1);

int adr = m->sensor_adr[n];

int dim = m->sensor_dim[n];

// data pointer in line

int p = figsensor.linepnt[lineid];

// fill in data for this sensor

for( int i=0; i<dim; i++ )

{

// check size

if( (p+2*i)>=mjMAXLINEPNT/2 )

break;

// x

figsensor.linedata[lineid][2*p+4*i] = (float)(adr+i);

figsensor.linedata[lineid][2*p+4*i+2] = (float)(adr+i);

// y

figsensor.linedata[lineid][2*p+4*i+1] = 0;

figsensor.linedata[lineid][2*p+4*i+3] = (float)(d->sensordata[adr+i]/cutoff);

}

// update linepnt

figsensor.linepnt[lineid] = mjMIN(mjMAXLINEPNT-1,

figsensor.linepnt[lineid]+2*dim);

}

{

// render figure on the right

mjrRect viewport = {rect.width - rect.width/4, rect.bottom, rect.width/4, rect.height/3};

mjr_figure(viewport, &figsensor, &con);

{

// autoscale

cam.lookat[0] = m->stat.center[0];

cam.lookat[1] = m->stat.center[1];

cam.lookat[2] = m->stat.center[2];

cam.distance = 1.5 * m->stat.extent;

// set to free camera

cam.type = mjCAMERA_FREE;

{

// make sure filename is given

if( !filename )

return;

// load and compile

char error[1000] = "could not load binary model";

mjModel* mnew = 0;

if( strlen(filename)>4 && !strcmp(filename+strlen(filename)-4, ".mjb") )

mnew = mj_loadModel(filename, 0);

else

mnew = mj_loadXML(filename, 0, error, 1000);

if( !mnew )

{

printf("%s\n", error);

return;

}

// delete old model, assign new

mj_deleteData(d);

mj_deleteModel(m);

m = mnew;

d = mj_makeData(m);

mj_forward(m, d);

// save filename for reload

strcpy(lastfile, filename);

// re-create custom context

mjr_makeContext(m, &con, fontscale);

// clear perturbation state and keyreset

pert.active = 0;

pert.select = 0;

keyreset = -1;

// center and scale view, update scene

autoscale(window);

mjv_updateScene(m, d, &vopt, &pert, &cam, mjCAT_ALL, &scn);

// set window title to mode name

//if( window && m->names )

//glfwSetWindowTitle(window, m->names);

{

// save start time

static double starttm = 0;

if( starttm==0 )

starttm = glfwGetTime();

// return time since start

return (mjtNum)(1000 * (glfwGetTime() - starttm));

{

for( int i=0; i<mjNTIMER; i++ )

{

d->timer[i].duration = 0;

d->timer[i].number = 0;

}

{

*y = 0.425 * cos(p1) + 0.39225 * cos(p1 + p2);

*z = -0.425 * sin(p1) - 0.39225 * sin(p1 + p2);

{

double r = sqrt(y*y + z*z), r2, angle_1, angle_2, angle_3;

if (r < 0.001) r = 0.001;

if (r > 0.817)

{

y = y / r * 0.817;

z = z / r * 0.817;

r = 0.817;

}

if (r < 0.033)

{

y = y / r * 0.033;

z = z / r * 0.033;

r = 0.033;

}

r2 = r*r;

if (y < 0.0001 && y > -0.0001)

angle_1 = 1.570796327;

else

angle_1 = atan(z / y);

angle_2 = acos((r2 + 0.0267649375) / 0.85 / r);

angle_3 = acos((0.3344850625 - r2) / 0.3334125);

*p1 = -angle_1 - angle_2;

*p2 = 3.1415926535 - angle_3;

{

double p1, p2, y, z;

p1 = d->qpos[1];

p2 = d->qpos[2];

pos_to_yz(p1, p2, &y, &z);

y = y + leftRight * 0.01;

z = z + upDown * 0.01;

yz_to_pos(y, z, &p1, &p2);

d->qpos[1] = p1;

d->qpos[2] = p2;

d->qpos[3] = 1.570796327 - p1 - p2;

mj_forward(m, d);

{

int n;

// require model

if( !m )

return;

// do not act on release

if( act==GLFW_RELEASE )

{

for (int i = 6; i < 12; ++i)

d->ctrl[i] = 0;

return;

}

switch( key )

{

case GLFW_KEY_F1: // help

showhelp++;

if( showhelp>2 )

showhelp = 0;

break;

case GLFW_KEY_F2: // option

showoption = !showoption;

break;

case GLFW_KEY_F3: // info

showinfo = !showinfo;

break;

case GLFW_KEY_F4: // depth

showdepth = !showdepth;

break;

case GLFW_KEY_F5: // toggle full screen

showfullscreen = !showfullscreen;

if( showfullscreen )

glfwMaximizeWindow(window);

else

glfwRestoreWindow(window);

break;

case GLFW_KEY_F6: // stereo

scn.stereo = (scn.stereo==mjSTEREO_NONE ? mjSTEREO_QUADBUFFERED : mjSTEREO_NONE);

break;

case GLFW_KEY_F7: // sensor figure

showsensor = !showsensor;

break;

case GLFW_KEY_F8: // profiler

showprofiler = !showprofiler;

break;

case GLFW_KEY_ENTER: // slow motion

slowmotion = !slowmotion;

break;

case GLFW_KEY_SPACE: // pause

paused = !paused;

break;

case GLFW_KEY_PAGE_UP: // previous keyreset

case GLFW_KEY_PAGE_DOWN: // next keyreset

if( key==GLFW_KEY_PAGE_UP )

keyreset = mjMAX(-1, keyreset-1);

else

keyreset = mjMIN(m->nkey-1, keyreset+1);

// continue with reset

case GLFW_KEY_BACKSPACE: // reset

mj_resetData(m, d);

if( keyreset>=0 && keyreset<m->nkey )

{

d->time = m->key_time[keyreset];

mju_copy(d->qpos, m->key_qpos+keyreset*m->nq, m->nq);

mju_copy(d->qvel, m->key_qvel+keyreset*m->nv, m->nv);

mju_copy(d->act, m->key_act+keyreset*m->na, m->na);

}

init();

mj_forward(m, d);

profilerupdate();

sensorupdate();

break;

case GLFW_KEY_RIGHT: // step forward

if( paused )

{

mj_step(m, d);

profilerupdate();

sensorupdate();

}

break;

case GLFW_KEY_LEFT: // step back

if( paused )

{

m->opt.timestep = -m->opt.timestep;

cleartimers(d);

mj_step(m, d);

m->opt.timestep = -m->opt.timestep;

profilerupdate();

sensorupdate();

}

break;

case GLFW_KEY_DOWN: // step forward 100

if( paused )

{

cleartimers(d);

for( n=0; n<100; n++ )

mj_step(m,d);

profilerupdate();

sensorupdate();

}

break;

case GLFW_KEY_UP: // step back 100

if( paused )

{

m->opt.timestep = -m->opt.timestep;

cleartimers(d);

for( n=0; n<100; n++ )

mj_step(m,d);

m->opt.timestep = -m->opt.timestep;

profilerupdate();

sensorupdate();

}

break;

case GLFW_KEY_ESCAPE: // free camera

cam.type = mjCAMERA_FREE;

break;

case '=': // bigger font

if( fontscale<200 )

{

fontscale += 50;

mjr_makeContext(m, &con, fontscale);

}

break;

case '-': // smaller font

if( fontscale>100 )

{

fontscale -= 50;

mjr_makeContext(m, &con, fontscale);

}

break;

case '[': // previous fixed camera or free

if( m->ncam && cam.type==mjCAMERA_FIXED )

{

if( cam.fixedcamid>0 )

cam.fixedcamid--;

else

cam.type = mjCAMERA_FREE;

}

break;

case ']': // next fixed camera

if( m->ncam )

{

if( cam.type!=mjCAMERA_FIXED )

{

cam.type = mjCAMERA_FIXED;

cam.fixedcamid = 0;

}

else if( cam.fixedcamid<m->ncam-1 )

cam.fixedcamid++;

}

break;

case ';': // cycle over frame rendering modes

vopt.frame = mjMAX(0, vopt.frame-1);

break;

case '\'': // cycle over frame rendering modes

vopt.frame = mjMIN(mjNFRAME-1, vopt.frame+1);

break;

case '.': // cycle over label rendering modes

vopt.label = mjMAX(0, vopt.label-1);

break;

case '/': // cycle over label rendering modes

vopt.label = mjMIN(mjNLABEL-1, vopt.label+1);

break;

case GLFW_KEY_Z:

d->ctrl[4] = 1;

d->ctrl[5] = 1;

mj_forward(m, d);

break;

case GLFW_KEY_A:

d->ctrl[4] = 0;

d->ctrl[5] = 0;

for (int i = 6; i < 14; ++i)

d->qpos[i] = 0;

mj_forward(m, d);

break;

case GLFW_KEY_X:

d->ctrl[6] = 1;

mj_forward(m, d);

break;

case GLFW_KEY_S:

d->ctrl[6] = -1;

mj_forward(m, d);

break;

case GLFW_KEY_C:

d->ctrl[7] = 1;

mj_forward(m, d);

break;

case GLFW_KEY_D:

d->ctrl[7] = -1;

mj_forward(m, d);

break;

case GLFW_KEY_V:

d->ctrl[8] = 1;

mj_forward(m, d);

break;

case GLFW_KEY_F:

d->ctrl[8] = -1;

mj_forward(m, d);

break;

case GLFW_KEY_B:

d->ctrl[9] = 1;

mj_forward(m, d);

break;

case GLFW_KEY_G:

d->ctrl[9] = -1;

mj_forward(m, d);

break;

case GLFW_KEY_N:

d->ctrl[10] = 1;

mj_forward(m, d);

break;

case GLFW_KEY_H:

d->ctrl[10] = -1;

mj_forward(m, d);

break;

case GLFW_KEY_M:

d->ctrl[11] = 1;

mj_forward(m, d);

break;

case GLFW_KEY_J:

d->ctrl[11] = -1;

mj_forward(m, d);

break;

case GLFW_KEY_K:

d->qpos[0] = 0;

d->qpos[1] = -1.11905318;

d->qpos[2] = 1.72568888;

d->qpos[3] = 0.95673764;

d->qpos[4] = 1.570796327;

d->qpos[5] = 0;

d->qpos[6] = -0.02550968;

d->qpos[7] = -0.02550968;

mj_forward(m, d);

break;

case GLFW_KEY_Y:

d->qpos[0] = 3.89 / 180 * 3.1415926;

d->qpos[1] = -58.54 / 180 * 3.1415926;

d->qpos[2] = 80.70 / 180 * 3.1415926;

d->qpos[3] = -134.68 / 180 * 3.1415926;

d->qpos[4] = -99.40 / 180 * 3.1415926;

d->qpos[5] = 15.30 / 180 * 3.1415926;

mj_forward(m, d);

break;

case GLFW_KEY_U:

d->qpos[0] = -4.79 / 180 * 3.1415926;

d->qpos[1] = -36.74 / 180 * 3.1415926;

d->qpos[2] = 61.03 / 180 * 3.1415926;

d->qpos[3] = -175.48 / 180 * 3.1415926;

d->qpos[4] = -98.52 / 180 * 3.1415926;

d->qpos[5] = 20.34 / 180 * 3.1415926;

mj_forward(m, d);

break;

case GLFW_KEY_I:

d->qpos[0] = 4.14 / 180 * 3.1415926;

d->qpos[1] = -64.22 / 180 * 3.1415926;

d->qpos[2] = 105.83 / 180 * 3.1415926;

d->qpos[3] = -140.31 / 180 * 3.1415926;

d->qpos[4] = -38.61 / 180 * 3.1415926;

d->qpos[5] = 20.32 / 180 * 3.1415926;

mj_forward(m, d);

break;

case GLFW_KEY_O:

d->qpos[0] = 22.90 / 180 * 3.1415926;

d->qpos[1] = -37.06 / 180 * 3.1415926;

d->qpos[2] = 73.24 / 180 * 3.1415926;

d->qpos[3] = -192.74 / 180 * 3.1415926;

d->qpos[4] = -160.40 / 180 * 3.1415926;

d->qpos[5] = -13.33 / 180 * 3.1415926;

mj_forward(m, d);

break;

case GLFW_KEY_P:

d->qpos[0] = -5.71 / 180 * 3.1415926;

d->qpos[1] = -77.41 / 180 * 3.1415926;

d->qpos[2] = 144.98 / 180 * 3.1415926;

d->qpos[3] = -197.17 / 180 * 3.1415926;

d->qpos[4] = -50.28 / 180 * 3.1415926;

d->qpos[5] = -13.33 / 180 * 3.1415926;

mj_forward(m, d);

break;

case GLFW_KEY_L:

printf("Sensor 0: %0.4f\n", d->sensordata[0]);

printf("Sensor 1: %0.4f\n", d->sensordata[1]);

printf("Sensor 2: %0.4f\n", d->sensordata[2]);

printf("Sensor 3: %0.4f\n", d->sensordata[3]);

break;

case GLFW_KEY_TAB:

printf("qpos = [ %0.8f,\n", d->qpos[0]);

printf(" %0.8f,\n", d->qpos[1]);

printf(" %0.8f,\n", d->qpos[2]);

printf(" %0.8f,\n", d->qpos[3]);

printf(" %0.8f,\n", d->qpos[4]);

printf(" %0.8f,\n", d->qpos[5]);

printf(" %0.8f,\n", d->qpos[6]);

printf(" %0.8f,\n", d->qpos[7]);

printf(" %0.8f,\n", d->qpos[8]);

printf(" %0.8f,\n", d->qpos[9]);

printf(" %0.8f,\n", d->qpos[10]);

printf(" %0.8f,\n", d->qpos[11]);

printf(" %0.8f,\n", d->qpos[12]);

printf(" %0.8f,\n", d->qpos[13]);

break;

default: // toggle flag

// control keys

printf("default");

if( mods & GLFW_MOD_CONTROL )

{

if( key==GLFW_KEY_A )

autoscale(window);

else if( key==GLFW_KEY_L && lastfile[0] )

loadmodel(window, lastfile);

break;

}

// toggle visualization flag

for( int i=0; i<mjNVISFLAG; i++ )

if( key==mjVISSTRING[i][2][0] )

vopt.flags[i] = !vopt.flags[i];

// toggle rendering flag

for( int i=0; i<mjNRNDFLAG; i++ )

if( key==mjRNDSTRING[i][2][0] )

scn.flags[i] = !scn.flags[i];

// toggle geom/site group

for( int i=0; i<mjNGROUP; i++ )

if( key==i+'0')

{

if( mods & GLFW_MOD_SHIFT )

vopt.sitegroup[i] = !vopt.sitegroup[i];

else

vopt.geomgroup[i] = !vopt.geomgroup[i];

}

}

{

// past data for double-click detection

static int lastbutton = 0;

static double lastclicktm = 0;

// update button state

button_left = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT)==GLFW_PRESS);

button_middle = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_MIDDLE)==GLFW_PRESS);

button_right = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_RIGHT)==GLFW_PRESS);

// Alt: swap left and right

if( (mods & GLFW_MOD_ALT) )

{

bool tmp = button_left;

button_left = button_right;

button_right = tmp;

if( button==GLFW_MOUSE_BUTTON_LEFT )

button = GLFW_MOUSE_BUTTON_RIGHT;

else if( button==GLFW_MOUSE_BUTTON_RIGHT )

button = GLFW_MOUSE_BUTTON_LEFT;

}

// update mouse position

glfwGetCursorPos(window, &lastx, &lasty);

// require model

if( !m )

return;

// set perturbation

int newperturb = 0;

if( act==GLFW_PRESS && (mods & GLFW_MOD_CONTROL) && pert.select>0 )

{

// right: translate; left: rotate

if( button_right )

newperturb = mjPERT_TRANSLATE;

else if( button_left )

newperturb = mjPERT_ROTATE;

// perturbation onset: reset reference

if( newperturb && !pert.active )

mjv_initPerturb(m, d, &scn, &pert);

}

pert.active = newperturb;

// detect double-click (250 msec)

if( act==GLFW_PRESS && glfwGetTime()-lastclicktm<0.25 && button==lastbutton )

{

// determine selection mode

int selmode;

if( button==GLFW_MOUSE_BUTTON_LEFT )

selmode = 1;

else if( mods & GLFW_MOD_CONTROL )

selmode = 3;

else

selmode = 2;

// get current window size

int width, height;

glfwGetWindowSize(window, &width, &height);

// find geom and 3D click point, get corresponding body

mjtNum selpnt[3];

int selgeom = mjv_select(m, d, &vopt,

(mjtNum)width/(mjtNum)height,

(mjtNum)lastx/(mjtNum)width,

(mjtNum)(height-lasty)/(mjtNum)height,

&scn, selpnt);

int selbody = (selgeom>=0 ? m->geom_bodyid[selgeom] : 0);

// set lookat point, start tracking is requested

if( selmode==2 || selmode==3 )

{

// copy selpnt if geom clicked

if( selgeom>=0 )

mju_copy3(cam.lookat, selpnt);

// switch to tracking camera