cd_pqp.cpp
Go to the documentation of this file.
1 
10 #if _HAVE_PQP
11 
12 #include "cd_pqp.h"
13 
14 #include <PQP/PQP.h>
15 
16 extern "C" {
17  #include "error.h"
18 }
19 
20 Tpqp *DefinePQPModel(double **v,unsigned int nf,unsigned int *nvf,unsigned int **fv)
21 {
22  PQP_Model *m;
23  unsigned int i,j,k;
24 
25  m=new(PQP_Model);
26 
27  m->BeginModel();
28 
29  k=0;
30  for(i=0;i<nf;i++)
31  {
32  /* If the face has more then 3 vertexes, we trivially triangulate it.
33  In this triangulation we assume the face to be convex. */
34  if (nvf[i]<3)
35  Error("Detected a face with less than 3 vertexes in DefinePQPModel");
36 
37  for(j=2;j<nvf[i];j++)
38  {
39  m->AddTri(v[fv[i][0]],v[fv[i][j-1]],v[fv[i][j]],k);
40  k++;
41  }
42  }
43 
44  m->EndModel();
45 
46  return((Tpqp *)m);
47 }
48 
49 
50 boolean PQPTest(THTransform *t1,Tpqp *m1,THTransform *t2,Tpqp *m2)
51 {
52  unsigned int i,j;
53  PQP_Model *m1o,*m2o;
54  PQP_CollideResult report;
55  double R1[3][3]={{1,0,0},{0,1,0},{0,0,1}},T1[3]={0,0,0};
56  double R2[3][3]={{1,0,0},{0,1,0},{0,0,1}},T2[3]={0,0,0};
57 
58  m1o=(PQP_Model *)m1;
59  m2o=(PQP_Model *)m2;
60 
61  if (t1!=NULL)
62  {
63  for(i=0;i<3;i++)
64  {
65  T1[i]=HTransformGetElement(i,AXIS_H,t1);
66  for(j=0;j<3;j++)
67  R1[i][j]=HTransformGetElement(i,j,t1);
68  }
69  }
70  if (t2!=NULL)
71  {
72  for(i=0;i<3;i++)
73  {
74  T2[i]=HTransformGetElement(i,AXIS_H,t2);
75  for(j=0;j<3;j++)
76  R2[i][j]=HTransformGetElement(i,j,t2);
77  }
78  }
79 
80  PQP_Collide(&report,R1,T1,m1o,R2,T2,m2o,PQP_FIRST_CONTACT);
81 
82  if (report.NumPairs()>0)
83  return(TRUE);
84  else
85  return(FALSE);
86 }
87 
89 {
90  PQP_Model *mo;
91 
92  mo=(PQP_Model *)m;
93 
94  delete mo;
95 }
96 
97 
98 #endif
#define FALSE
FALSE.
Definition: boolean.h:30
A homgeneous transform in R^3.
Headers of the C interface for the pqp collision detection engine.
#define TRUE
TRUE.
Definition: boolean.h:21
void Error(const char *s)
General error function.
Definition: error.c:80
#define AXIS_H
The homogeneous dimension in R^3.
Definition: htransform.h:108
Error and warning functions.
double HTransformGetElement(unsigned int i, unsigned int j, THTransform *t)
Gets an element in a homogeneous transform.
Definition: htransform.c:323
Tpqp * DefinePQPModel(double **v, unsigned int nf, unsigned int *nvf, unsigned int **fv)
Adds a polyhedron to the pqp object.
Definition: cd_pqp.cpp:20
boolean PQPTest(THTransform *t1, Tpqp *m1, THTransform *t2, Tpqp *m2)
Test for collision.
Definition: cd_pqp.cpp:50
void Tpqp
The PQP object.
Definition: cd_pqp.h:33
void DeletePQPModel(Tpqp *m)
Deletes a given PQP object.
Definition: cd_pqp.cpp:88