Exemple #1
0
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());
}
Exemple #2
0
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();
}
Exemple #3
0
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();
}
Exemple #4
0
int main()
{
  // initialize PQP model pointers

  PQP_Model *b1 = new PQP_Model;
  PQP_Model *b2 = new PQP_Model;
  
  // Add trianges to form tori

  fprintf(stderr, "loading tris into PQP_Model objects...");  fflush(stderr);
  
  PQP_REAL a = (PQP_REAL)1.0;  // major radius of the tori
  PQP_REAL b = (PQP_REAL)0.2;  // minor radius of the tori

  int n1 = 50;     // tori will have n1*n2*2 triangles each
  int n2 = 50;

  int uc, vc;
  int count = 0;
  
  b1->BeginModel();
  b2->BeginModel();
  for(uc=0; uc<n1; uc++)
  {
    for(vc=0; vc<n2; vc++)
    {
      PQP_REAL u1 = (PQP_REAL)(2.0*PI*uc) / n1; 
      PQP_REAL u2 = (PQP_REAL)(2.0*PI*(uc+1)) / n1; 
      PQP_REAL v1 = (PQP_REAL)(2.0*PI*vc) / n2; 
      PQP_REAL v2 = (PQP_REAL)(2.0*PI*(vc+1)) / n2; 

      PQP_REAL p1[3], p2[3], p3[3], p4[3];

      p1[0] = (a - b * cos(v1)) * cos(u1);
      p2[0] = (a - b * cos(v1)) * cos(u2);
      p3[0] = (a - b * cos(v2)) * cos(u1);
      p4[0] = (a - b * cos(v2)) * cos(u2);
      p1[1] = (a - b * cos(v1)) * sin(u1);
      p2[1] = (a - b * cos(v1)) * sin(u2);
      p3[1] = (a - b * cos(v2)) * sin(u1);
      p4[1] = (a - b * cos(v2)) * sin(u2);
      p1[2] = b * sin(v1);
      p2[2] = b * sin(v1);
      p3[2] = b * sin(v2);
      p4[2] = b * sin(v2);

      b1->AddTri(p1, p2, p3, count);
      b1->AddTri(p4, p2, p3, count+1);
      b2->AddTri(p1, p2, p3, count);
      b2->AddTri(p4, p2, p3, count+1);

      count += 2;
    }
  }

  fprintf(stderr, "done\n");  fflush(stderr);
  fprintf(stderr, "Tori have %d triangles each.\n", count);
  fprintf(stderr, "building hierarchies...");  fflush(stderr);
  b1->EndModel();
  b2->EndModel();
  fprintf(stderr, "done.\n"); 
  b1->MemUsage(1);
  b2->MemUsage(1);
  fflush(stderr); 
  
  // now we are free to call the proximity routines.
  // but first, construct the transformations that define the placement
  // of our two hierarchies in world space:

  // this placement causes them to overlap a large amount.

  PQP_REAL R1[3][3], R2[3][3], T1[3], T2[3];
  
  R1[0][0] = R1[1][1] = R1[2][2] = 1.0;
  R1[0][1] = R1[1][0] = R1[2][0] = 0.0;
  R1[0][2] = R1[1][2] = R1[2][1] = 0.0;

  R2[0][0] = R2[1][1] = R2[2][2] = 1.0;
  R2[0][1] = R2[1][0] = R2[2][0] = 0.0;
  R2[0][2] = R2[1][2] = R2[2][1] = 0.0;
  
  T1[0] = 1.0;  T1[1] = 0.0; T1[2] = 0.0;
  T2[0] = 0.0;  T2[1] = 0.0; T2[2] = 0.0;

  // perform a collision query

  PQP_CollideResult cres;
  PQP_Collide(&cres, R1, T1, b1, R2, T2, b2, PQP_ALL_CONTACTS);

  // looking at the report, we can see where all the contacts were, and
  // also how many tests were necessary:

  printf("\nAll contact collision query between overlapping tori:\n");
  printf("Num BV tests: %d\n", cres.NumBVTests());
  printf("Num Tri tests: %d\n", cres.NumTriTests());
  printf("Num contact pairs: %d\n", cres.NumPairs());
#if LISTS
  int i;
  for(i=0; i<cres.NumPairs(); i++)
  {
    printf("\t contact %4d: tri %4d and tri %4d\n",
           i,
           cres.Id1(i),
           cres.Id2(i));
  }
#endif

  // Notice the PQP_ALL_CONTACTS flag we used in the call to PQP_Collide.
  // The alternative is to use the PQP_FIRST_CONTACT flag, instead.
  // The result is that the collide routine searches for any contact,
  // but not all of them.  It can take many many fewer tests to locate a single
  // contact.

  PQP_Collide(&cres, R1, T1, b1, R2, T2, b2, PQP_FIRST_CONTACT);

  printf("\nFirst contact collision query between overlapping tori:\n");
  printf("Num BV tests: %d\n", cres.NumBVTests());
  printf("Num Tri tests: %d\n", cres.NumTriTests());
  printf("Num contact pairs: %d\n", cres.NumPairs());
#if LISTS
  for(i=0; i<cres.NumPairs(); i++)
  {
    printf("\t contact %4d: tri %4d and tri %4d\n", 
           i, 
           cres.Id1(i), 
           cres.Id2(i));
  }
#endif
  
  // Perform a distance query, which should return a distance of 0.0

  PQP_DistanceResult dres;
  PQP_Distance(&dres, R1, T1, b1, R2, T2, b2, 0.0, 0.0);

  printf("\nDistance query between overlapping tori\n");
  printf("Num BV tests: %d\n", dres.NumBVTests());
  printf("Num Tri tests: %d\n", dres.NumTriTests());
  printf("Distance: %lf\n", dres.Distance());

  // by rotating one of them around the x-axis 90 degrees, they 
  // are now interlocked, but not quite touching.

  R1[0][0] = 1.0;  R1[0][1] = 0.0;  R1[0][2] = 0.0;
  R1[1][0] = 0.0;  R1[1][1] = 0.0;  R1[1][2] =-1.0;
  R1[2][0] = 0.0;  R1[2][1] = 1.0;  R1[2][2] = 0.0;
  
  PQP_Collide(&cres, R1, T1, b1, R2, T2, b2, PQP_FIRST_CONTACT);

  printf("\nCollision query between interlocked but nontouching tori:\n");
  printf("Num BV tests: %d\n", cres.NumBVTests());
  printf("Num Tri tests: %d\n", cres.NumTriTests());
  printf("Num contact pairs: %d\n", cres.NumPairs());
#if LISTS
  for(i=0; i<cres.NumPairs(); i++)
  {
    printf("\t contact %4d: tri %4d and tri %4d\n", 
           i, 
           cres.Id1(i), 
           cres.Id2(i));
  }
#endif

  // Perform a distance query - the distance found should be greater than zero

  PQP_Distance(&dres, R1, T1, b1, R2, T2, b2, 0.0, 0.0);

  printf("\nDistance query between interlocked but nontouching tori\n");
  printf("Num BV tests: %d\n", dres.NumBVTests());
  printf("Num Tri tests: %d\n", dres.NumTriTests());
  printf("Distance: %lf\n", dres.Distance());

  // Perform two tolerance queries. One tolerance setting is greater than the 
  // distance between the models, and one tolerance is less than the distance.

  PQP_ToleranceResult tres;
  PQP_REAL tolerance = (PQP_REAL).60;
  PQP_Tolerance(&tres, R1, T1, b1, R2, T2, b2, tolerance);

  printf("\nTolerance query between interlocked but nontouching tori\n"
         "with tolerance %lf\n", tolerance);
  printf("Num BV tests: %d\n", tres.NumBVTests());
  printf("Num Tri tests: %d\n", tres.NumTriTests());
  printf("Closer than tolerance? ",tolerance);
  if (tres.CloserThanTolerance()) printf("yes.\n"); else printf("no.\n");

  tolerance = (PQP_REAL).40;
  PQP_Tolerance(&tres, R1, T1, b1, R2, T2, b2, tolerance);

  printf("\nTolerance query between interlocked but nontouching tori\n"
         "with tolerance %lf\n", tolerance);
  printf("Num BV tests: %d\n", tres.NumBVTests());
  printf("Num Tri tests: %d\n", tres.NumTriTests());
  printf("Closer than tolerance? ",tolerance);
  if (tres.CloserThanTolerance()) printf("yes.\n"); else printf("no.\n");

  // by moving one of the tori closer to the other, they
  // almost touch.  This is the case that requires a lot
  // of work wiht methods which use bounding boxes of limited
  // aspect ratio.  Oriented bounding boxes are more efficient
  // at determining noncontact than spheres, octree, or axis-aligned
  // bounding boxes for scenarios like this.  In this case, the interlocked
  // tori are separated by 0.0001 at their closest point.


  T1[0] = (PQP_REAL)1.5999;
  
  PQP_Collide(&cres, R1, T1, b1, R2, T2, b2, PQP_FIRST_CONTACT);

  printf("\nCollision query on interlocked and almost touching tori:\n");
  printf("Num BV tests: %d\n", cres.NumBVTests());
  printf("Num Tri tests: %d\n", cres.NumTriTests());
  printf("Num contact pairs: %d\n", cres.NumPairs());
#if LISTS
  for(i=0; i<cres.NumPairs(); i++)
  {
    printf("\t contact %4d: tri %4d and tri %4d\n", 
           i, 
           cres.Id1(i), 
           cres.Id2(i));
  }
#endif

  PQP_Distance(&dres, R1, T1, b1, R2, T2, b2, 0.0, 0.0);

  printf("\nDistance query between interlocked and almost touching tori\n");
  printf("Num BV tests: %d\n", dres.NumBVTests());
  printf("Num Tri tests: %d\n", dres.NumTriTests());
  printf("Distance: %lf\n", dres.Distance());

  tolerance = (PQP_REAL)0.00015;
  PQP_Tolerance(&tres, R1, T1, b1, R2, T2, b2, tolerance);

  printf("\nTolerance query between interlocked and almost touching tori\n"
         "with tolerance %lf\n", tolerance);
  printf("Num BV tests: %d\n", tres.NumBVTests());
  printf("Num Tri tests: %d\n", tres.NumTriTests());
  printf("Closer than tolerance? ",tolerance);
  if (tres.CloserThanTolerance()) printf("yes.\n"); else printf("no.\n");

  tolerance = (PQP_REAL)0.00005;
  PQP_Tolerance(&tres, R1, T1, b1, R2, T2, b2, tolerance);

  printf("\nTolerance query between interlocked and almost touching tori\n"
         "with tolerance %lf\n", tolerance);
  printf("Num BV tests: %d\n", tres.NumBVTests());
  printf("Num Tri tests: %d\n", tres.NumTriTests());
  printf("Closer than tolerance? ",tolerance);
  if (tres.CloserThanTolerance()) printf("yes.\n"); else printf("no.\n");

  delete b1;
  delete b2;

  return 0;  
}