server
UtDynamicsSimulator
sDIMS
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
49
int
build_state
;
50
51
Tri
*
tris
;
52
int
num_tris
;
53
int
num_tris_alloced
;
54
55
BV *
b
;
56
int
num_bvs
;
57
int
num_bvs_alloced
;
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
80
struct
CollisionPair
81
{
82
int
id1
;
83
int
id2
;
84
PQP_REAL
depth
;
85
PQP_REAL
position1
[3];
86
int
vertex_id1
;
87
PQP_REAL
position2
[3];
88
int
vertex_id2
;
89
PQP_REAL
normal
[3];
90
};
91
92
struct
PQP_CollideResult
93
{
94
// stats
95
96
int
num_bv_tests
;
97
int
num_tri_tests
;
98
double
query_time_secs
;
99
100
// xform from model 1 to model 2
101
102
PQP_REAL
RR
[3][3];
103
PQP_REAL
T
[3];
104
105
int
num_pairs_alloced
;
106
int
num_pairs
;
107
CollisionPair
*
pairs
;
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
131
PQP_CollideResult
();
132
~PQP_CollideResult
();
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
PQP_CollideResult
Definition:
PQP_Internal.h:92
PQP_CollideResult::NumTriTests
int NumTriTests()
Definition:
PQP_Internal.h:137
PQP_CollideResult::query_time_secs
double query_time_secs
Definition:
PQP_Internal.h:98
i
png_uint_32 i
Definition:
png.h:2732
CollisionPair::position1
PQP_REAL position1[3]
Definition:
PQP_Internal.h:85
PQP_Model::tris
Tri * tris
Definition:
PQP_Internal.h:51
PQP_CollideResult::num_tri_tests
int num_tri_tests
Definition:
PQP_Internal.h:97
PQP_CollideResult::have_vertex2
int have_vertex2(int vertex_id)
Definition:
PQP_Internal.h:121
PQP_CollideResult::PQP_CollideResult
PQP_CollideResult()
PQP_Model::last_tri
Tri * last_tri
Definition:
PQP_Internal.h:59
CollisionPair::id1
int id1
Definition:
PQP_Internal.h:82
PQP_CollideResult::Normal
void Normal(int k, PQP_REAL _norm[3])
Definition:
PQP_Internal.h:164
PQP_Model::num_tris
int num_tris
Definition:
PQP_Internal.h:52
PQP_Model::child
BV * child(int n)
Definition:
PQP_Internal.h:61
PQP_CollideResult::Position1
void Position1(int k, PQP_REAL _pos[3])
Definition:
PQP_Internal.h:154
autoplay.n
n
Definition:
autoplay.py:12
CollisionPair
Definition:
PQP_Internal.h:80
PQP_Model::PQP_Model
PQP_Model()
CollisionPair::vertex_id1
int vertex_id1
Definition:
PQP_Internal.h:86
PQP_CollideResult::NumBVTests
int NumBVTests()
Definition:
PQP_Internal.h:136
PQP_CollideResult::~PQP_CollideResult
~PQP_CollideResult()
CollisionPair::depth
PQP_REAL depth
Definition:
PQP_Internal.h:84
CollisionPair::normal
PQP_REAL normal[3]
Definition:
PQP_Internal.h:89
PQP_Model::num_bvs_alloced
int num_bvs_alloced
Definition:
PQP_Internal.h:57
PQP_Model
Definition:
PQP_Internal.h:44
PQP_CollideResult::T
PQP_REAL T[3]
Definition:
PQP_Internal.h:103
CollisionPair::vertex_id2
int vertex_id2
Definition:
PQP_Internal.h:88
PQP_CollideResult::num_pairs
int num_pairs
Definition:
PQP_Internal.h:106
PQP_Model::MemUsage
int MemUsage(int msg)
CollisionPair::id2
int id2
Definition:
PQP_Internal.h:83
PQP_CollideResult::pairs
CollisionPair * pairs
Definition:
PQP_Internal.h:107
PQP_Model::AddTri
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)
PQP_CollideResult::num_bv_tests
int num_bv_tests
Definition:
PQP_Internal.h:96
PQP_CollideResult::NumPairs
int NumPairs()
Definition:
PQP_Internal.h:148
Tri
PQP_CollideResult::have_vertex1
int have_vertex1(int vertex_id)
Definition:
PQP_Internal.h:113
PQP_Model::~PQP_Model
~PQP_Model()
PQP_Model::b
BV * b
Definition:
PQP_Internal.h:55
PQP_Model::EndModel
int EndModel()
PQP_CollideResult::Remove
void Remove(int id)
PQP_CollideResult::Id2
int Id2(int k)
Definition:
PQP_Internal.h:150
PQP_CollideResult::Position2
void Position2(int k, PQP_REAL _pos[3])
Definition:
PQP_Internal.h:159
PQP_CollideResult::QueryTimeSecs
double QueryTimeSecs()
Definition:
PQP_Internal.h:138
PQP_Model::num_bvs
int num_bvs
Definition:
PQP_Internal.h:56
PQP_CollideResult::FreePairsList
void FreePairsList()
PQP_CollideResult::SizeTo
void SizeTo(int n)
PQP_CollideResult::Id1
int Id1(int k)
Definition:
PQP_Internal.h:149
PQP_CollideResult::Depth
PQP_REAL Depth(int k)
Definition:
PQP_Internal.h:151
PQP_Model::num_tris_alloced
int num_tris_alloced
Definition:
PQP_Internal.h:53
PQP_Model::BeginModel
int BeginModel(int num_tris=8)
PQP_CollideResult::RR
PQP_REAL RR[3][3]
Definition:
PQP_Internal.h:102
CollisionPair::position2
PQP_REAL position2[3]
Definition:
PQP_Internal.h:87
PQP_Model::build_state
int build_state
Definition:
PQP_Internal.h:49
PQP_CollideResult::num_pairs_alloced
int num_pairs_alloced
Definition:
PQP_Internal.h:105
PQP_CollideResult::Add
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])
PQP_CollideResult::Colliding
int Colliding()
Definition:
PQP_Internal.h:147
openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:04