PQP_Internal.h
Go to the documentation of this file.
1 /*************************************************************************\
2 
3  Copyright 1999 The University of North Carolina at Chapel Hill.
4  All Rights Reserved.
5 
6  Permission to use, copy, modify and distribute this software and its
7  documentation for educational, research and non-profit purposes, without
8  fee, and without a written agreement is hereby granted, provided that the
9  above copyright notice and the following three paragraphs appear in all
10  copies.
11 
12  IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
13  LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
14  CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
15  USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
16  OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
17  DAMAGES.
18 
19  THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
20  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE
22  PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
23  NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
24  UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
25 
26  The authors may be contacted via:
27 
28  US Mail: S. Gottschalk, E. Larsen
29  Department of Computer Science
30  Sitterson Hall, CB #3175
31  University of N. Carolina
32  Chapel Hill, NC 27599-3175
33 
34  Phone: (919)962-1749
35 
36  EMail: geom@cs.unc.edu
37 
38 
39 \**************************************************************************/
40 
41 #include "Tri.h"
42 #include "BV.h"
43 
44 class PQP_Model
45 {
46 
47 public:
48 
50 
51  Tri *tris;
52  int num_tris;
54 
55  BV *b;
56  int num_bvs;
58 
59  Tri *last_tri; // closest tri on this model in last distance test
60 
61  BV *child(int n) { return &b[n]; }
62 
63  PQP_Model();
64  ~PQP_Model();
65 
66  int BeginModel(int num_tris = 8); // preallocate for num_tris triangles;
67  // the parameter is optional, since
68  // arrays are reallocated as needed
69  int AddTri(const PQP_REAL *p1, const PQP_REAL *p2, const PQP_REAL *p3,
70 // added by yamane ->
71  const PQP_REAL *n1, const PQP_REAL *n2, const PQP_REAL *n3,
72  int _vertex_id[3], int _neighbor_id[3],
73 // <-
74  int id);
75  int EndModel();
76  int MemUsage(int msg); // returns model mem usage.
77  // prints message to stderr if msg == TRUE
78 };
79 
81 {
82  int id1;
83  int id2;
84  PQP_REAL depth;
85  PQP_REAL position1[3];
87  PQP_REAL position2[3];
89  PQP_REAL normal[3];
90 };
91 
93 {
94  // stats
95 
99 
100  // xform from model 1 to model 2
101 
102  PQP_REAL RR[3][3];
103  PQP_REAL T[3];
104 
108 
109  void SizeTo(int n);
110  void Add(int i1, int i2, PQP_REAL _depth, PQP_REAL position1[3], int _vertex_id1, PQP_REAL position2[3], int _vertex_id2, PQP_REAL normal[3]);
111  void Remove(int id);
112 
113  int have_vertex1(int vertex_id) {
114  if(vertex_id < 0) return -1;
115  for(int i=0; i<num_pairs; i++)
116  {
117  if(pairs[i].vertex_id1 == vertex_id) return i;
118  }
119  return -1;
120  }
121  int have_vertex2(int vertex_id) {
122  if(vertex_id < 0) return -1;
123  for(int i=0; i<num_pairs; i++)
124  {
125  if(pairs[i].vertex_id2 == vertex_id) return i;
126  }
127  return -1;
128  }
129 
130 
133 
134  // statistics
135 
136  int NumBVTests() { return num_bv_tests; }
137  int NumTriTests() { return num_tri_tests; }
138  double QueryTimeSecs() { return query_time_secs; }
139 
140  // free the list of contact pairs; ordinarily this list is reused
141  // for each query, and only deleted in the destructor.
142 
143  void FreePairsList();
144 
145  // query results
146 
147  int Colliding() { return (num_pairs > 0); }
148  int NumPairs() { return num_pairs; }
149  int Id1(int k) { return pairs[k].id1; }
150  int Id2(int k) { return pairs[k].id2; }
151  PQP_REAL Depth(int k) {
152  return pairs[k].depth;
153  }
154  void Position1(int k, PQP_REAL _pos[3]) {
155  _pos[0] = pairs[k].position1[0];
156  _pos[1] = pairs[k].position1[1];
157  _pos[2] = pairs[k].position1[2];
158  }
159  void Position2(int k, PQP_REAL _pos[3]) {
160  _pos[0] = pairs[k].position2[0];
161  _pos[1] = pairs[k].position2[1];
162  _pos[2] = pairs[k].position2[2];
163  }
164  void Normal(int k, PQP_REAL _norm[3]) {
165  _norm[0] = pairs[k].normal[0];
166  _norm[1] = pairs[k].normal[1];
167  _norm[2] = pairs[k].normal[2];
168  }
169 
170 };
171 
172 #if PQP_BV_TYPE & RSS_TYPE // distance/tolerance are only available with RSS
173 
174 struct PQP_DistanceResult
175 {
176  // stats
177 
178  int num_bv_tests;
179  int num_tri_tests;
180  double query_time_secs;
181 
182  // xform from model 1 to model 2
183 
184  PQP_REAL RR[3][3];
185  PQP_REAL T[3];
186 
187  PQP_REAL rel_err;
188  PQP_REAL abs_err;
189 
190  PQP_REAL distance;
191  PQP_REAL p1[3];
192  PQP_REAL p2[3];
193  int qsize;
194  int id1;
195  int id2;
196 
197  // statistics
198 
199  int NumBVTests() { return num_bv_tests; }
200  int NumTriTests() { return num_tri_tests; }
201  double QueryTimeSecs() { return query_time_secs; }
202 
203  // The following distance and points established the minimum distance
204  // for the models, within the relative and absolute error bounds
205  // specified.
206  // Points are defined: PQP_REAL p1[3], p2[3];
207 
208  PQP_REAL Distance() { return distance; }
209  const PQP_REAL *P1() { return p1; }
210  const PQP_REAL *P2() { return p2; }
211  int ID1() { return id1; }
212  int ID2() { return id2; }
213 
214 };
215 
216 struct PQP_ToleranceResult
217 {
218  // stats
219 
220  int num_bv_tests;
221  int num_tri_tests;
222  double query_time_secs;
223 
224  // xform from model 1 to model 2
225 
226  PQP_REAL RR[3][3];
227  PQP_REAL T[3];
228 
229  int closer_than_tolerance;
230  PQP_REAL tolerance;
231 
232  PQP_REAL distance;
233  PQP_REAL p1[3];
234  PQP_REAL p2[3];
235  int qsize;
236 
237  // statistics
238 
239  int NumBVTests() { return num_bv_tests; }
240  int NumTriTests() { return num_tri_tests; }
241  double QueryTimeSecs() { return query_time_secs; }
242 
243  // If the models are closer than ( <= ) tolerance, these points
244  // and distance were what established this. Otherwise,
245  // distance and point values are not meaningful.
246 
247  PQP_REAL Distance() { return distance; }
248  const PQP_REAL *P1() { return p1; }
249  const PQP_REAL *P2() { return p2; }
250 
251  // boolean says whether models are closer than tolerance distance
252 
253  int CloserThanTolerance() { return closer_than_tolerance; }
254 };
255 
256 #endif
int num_bvs_alloced
Definition: PQP_Internal.h:57
int EndModel()
void Position1(int k, PQP_REAL _pos[3])
Definition: PQP_Internal.h:154
int num_tris_alloced
Definition: PQP_Internal.h:53
void Position2(int k, PQP_REAL _pos[3])
Definition: PQP_Internal.h:159
int BeginModel(int num_tris=8)
int AddTri(const PQP_REAL *p1, const PQP_REAL *p2, const PQP_REAL *p3, const PQP_REAL *n1, const PQP_REAL *n2, const PQP_REAL *n3, int _vertex_id[3], int _neighbor_id[3], int id)
int have_vertex2(int vertex_id)
Definition: PQP_Internal.h:121
double QueryTimeSecs()
Definition: PQP_Internal.h:138
int build_state
Definition: PQP_Internal.h:49
png_uint_32 i
Definition: png.h:2735
PQP_REAL normal[3]
Definition: PQP_Internal.h:89
PQP_REAL Depth(int k)
Definition: PQP_Internal.h:151
PQP_REAL position1[3]
Definition: PQP_Internal.h:85
Tri * tris
Definition: PQP_Internal.h:51
CollisionPair * pairs
Definition: PQP_Internal.h:107
Tri * last_tri
Definition: PQP_Internal.h:59
int have_vertex1(int vertex_id)
Definition: PQP_Internal.h:113
void Normal(int k, PQP_REAL _norm[3])
Definition: PQP_Internal.h:164
double query_time_secs
Definition: PQP_Internal.h:98
BV * child(int n)
Definition: PQP_Internal.h:61
PQP_REAL position2[3]
Definition: PQP_Internal.h:87
int num_tris
Definition: PQP_Internal.h:52
int MemUsage(int msg)
PQP_REAL depth
Definition: PQP_Internal.h:84


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:25