double PQP_Distance(double R1[3][3], double T1[3], PQP_Model *PQP_Model1, double R2[3][3], double T2[3], PQP_Model *PQP_Model2, PQP_REAL *P1, PQP_REAL *P2, int qsize) { PQP_REAL V1[3], V2[3]; PQP_DistanceResult dres; PQP_Distance(&dres, R1, T1, PQP_Model1, R2, T2, PQP_Model2, 0.0, 0.0, qsize); VcV(V1, dres.P1()); VcV(V2, dres.P2()); MxVpV(P1, R1, V1, T1); MxVpV(P2, R2, V2, T2); return (double)(dres.Distance()); }
void cb_display() { BeginDraw(); int i; PQP_CollideResult cres; PQP_DistanceResult dres; PQP_ToleranceResult tres; double oglm[16]; switch(query_type) { case 0: // draw model 1 glColor3f(1,1,1); // setup color and transform MVtoOGL(oglm,R1[step],T1[step]); glPushMatrix(); glMultMatrixd(oglm); torus1_drawn->Draw(); // do gl rendering glPopMatrix(); // restore transform // draw model 2 MVtoOGL(oglm,R2[step],T2[step]); glPushMatrix(); glMultMatrixd(oglm); torus2_drawn->Draw(); glPopMatrix(); break; case 1: // perform collision query PQP_Collide(&cres,R1[step],T1[step],torus1_tested, R2[step],T2[step],torus2_tested, PQP_ALL_CONTACTS); // draw model 1 and its overlapping tris MVtoOGL(oglm,R1[step],T1[step]); glPushMatrix(); glMultMatrixd(oglm); glColor3f(1,1,1); torus1_drawn->Draw(); glColor3f(1,0,0); for(i = 0; i < cres.NumPairs(); i++) { torus1_drawn->DrawTri(cres.Id1(i)); } glPopMatrix(); // draw model 2 and its overlapping tris MVtoOGL(oglm,R2[step],T2[step]); glPushMatrix(); glMultMatrixd(oglm); glColor3f(1,1,1); torus2_drawn->Draw(); glColor3f(1,0,0); for(i = 0; i < cres.NumPairs(); i++) { torus2_drawn->DrawTri(cres.Id2(i)); } glPopMatrix(); break; case 2: // perform distance query PQP_Distance(&dres,R1[step],T1[step],torus1_tested, R2[step],T2[step],torus2_tested, 0.0,0.0); // draw models glColor3f(1,1,1); MVtoOGL(oglm,R1[step],T1[step]); glPushMatrix(); glMultMatrixd(oglm); torus1_drawn->Draw(); glPopMatrix(); MVtoOGL(oglm,R2[step],T2[step]); glPushMatrix(); glMultMatrixd(oglm); torus2_drawn->Draw(); glPopMatrix(); // draw the closest points as small spheres glColor3f(0,1,0); PQP_REAL P1[3],P2[3],V1[3],V2[3]; VcV(P1,dres.P1()); VcV(P2,dres.P2()); // each point is in the space of its model; // transform to world space MxVpV(V1,R1[step],P1,T1[step]); glPushMatrix(); glTranslated(V1[0],V1[1],V1[2]); glutSolidSphere(.01,15,15); glPopMatrix(); MxVpV(V2,R2[step],P2,T2[step]); glPushMatrix(); glTranslated(V2[0],V2[1],V2[2]); glutSolidSphere(.01,15,15); glPopMatrix(); // draw the line between the closest points glDisable(GL_LIGHTING); glBegin(GL_LINES); glVertex3v(V1); glVertex3v(V2); glEnd(); glEnable(GL_LIGHTING); break; case 3: // perform tolerance query PQP_Tolerance(&tres,R1[step],T1[step],torus1_tested, R2[step],T2[step],torus2_tested, tolerance); if (tres.CloserThanTolerance()) glColor3f(0,0,1); else glColor3f(1,1,1); // draw models MVtoOGL(oglm,R1[step],T1[step]); glPushMatrix(); glMultMatrixd(oglm); torus1_drawn->Draw(); glPopMatrix(); MVtoOGL(oglm,R2[step],T2[step]); glPushMatrix(); glMultMatrixd(oglm); torus2_drawn->Draw(); glPopMatrix(); break; } EndDraw(); }
void DisplayCB() { BeginDraw(); // set up model transformations if (animate) { rot1 += .1; rot2 += .2; rot3 += .3; } PQP_REAL R1[3][3],R2[3][3],T1[3],T2[3]; PQP_REAL M1[3][3],M2[3][3],M3[3][3]; T1[0] = -1; T1[1] = 0.0; T1[2] = 0.0; T2[0] = 1; T2[1] = 0.0; T2[2] = 0.0; MRotX(M1,rot1); MRotY(M2,rot2); MxM(M3,M1,M2); MRotZ(M1,rot3); MxM(R1,M3,M1); MRotX(M1,rot3); MRotY(M2,rot1); MxM(M3,M1,M2); MRotZ(M1,rot2); MxM(R2,M3,M1); // perform distance query PQP_REAL rel_err = 0.0; PQP_REAL abs_err = 0.0; PQP_DistanceResult res; PQP_Distance(&res,R1,T1,&bunny,R2,T2,&torus,rel_err,abs_err); // draw the models glColor3d(0.0,0.0,1.0); double oglm[16]; MVtoOGL(oglm,R1,T1); glPushMatrix(); glMultMatrixd(oglm); bunny_to_draw->Draw(); glPopMatrix(); glColor3d(0.0,1.0,0.0); MVtoOGL(oglm,R2,T2); glPushMatrix(); glMultMatrixd(oglm); torus_to_draw->Draw(); glPopMatrix(); // draw the closest points as small spheres glColor3d(1.0,0.0,0.0); PQP_REAL P1[3],P2[3],V1[3],V2[3]; VcV(P1,res.P1()); VcV(P2,res.P2()); // each point is in the space of its model; // transform to world space MxVpV(V1,R1,P1,T1); /* glPushMatrix(); glTranslated(V1[0],V1[1],V1[2]); glutSolidSphere(.05,15,15); glPopMatrix(); */ MxVpV(V2,R2,P2,T2); /* glPushMatrix(); glTranslated(V2[0],V2[1],V2[2]); glutSolidSphere(.05,15,15); glPopMatrix(); */ // draw the line between the closest points float v1p[3], v2p[3]; for (int i = 0; i < 3; ++i) { v1p[i] = V1[i]; v2p[i] = V2[i]; } //glDisable(GL_LIGHTING); glBegin(GL_LINES); glVertex3v(v1p); glVertex3v(v2p); glEnd(); //glEnable(GL_LIGHTING); EndDraw(); }