1 /*************************************************************************\
2
3 Copyright 1995 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
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: {gottscha}@cs.unc.edu
37
38
39 \**************************************************************************/
40
41
42 #include "RAPID_version.H"
43
44 static char rapidtag_data[] = "RAPIDTAG file: " __FILE__ " date: " __DATE__ " time: " __TIME__;
45
46 // to silence the compiler's complaints about unreferenced identifiers.
47 static void r1(char *f){ r1(f); r1(rapidtag_data); r1(rapid_version);}
48
49 #include "matvec.H"
50
51 inline
52 double
53 max(double a, double b, double c)
54 {
55 double t = a;
56 if (b > t) t = b;
test(self)57 if (c > t) t = c;
58 return t;
59 }
60
61 inline
62 double
63 min(double a, double b, double c)
64 {
65 double t = a;
66 if (b < t) t = b;
67 if (c < t) t = c;
68 return t;
69 }
70
71
72 int
73 project6(double *ax,
74 double *p1, double *p2, double *p3,
75 double *q1, double *q2, double *q3)
test(self)76 {
77 double P1 = VdotV(ax, p1);
78 double P2 = VdotV(ax, p2);
79 double P3 = VdotV(ax, p3);
80 double Q1 = VdotV(ax, q1);
81 double Q2 = VdotV(ax, q2);
82 double Q3 = VdotV(ax, q3);
83
84 double mx1 = max(P1, P2, P3);
85 double mn1 = min(P1, P2, P3);
86 double mx2 = max(Q1, Q2, Q3);
87 double mn2 = min(Q1, Q2, Q3);
88
89 if (mn1 > mx2) return 0;
90 if (mn2 > mx1) return 0;
91 return 1;
92 }
93
94
95 // very robust triangle intersection test
96 // uses no divisions
97 // works on coplanar triangles
98
99 int
100 tri_contact (double *P1, double *P2, double *P3,
101 double *Q1, double *Q2, double *Q3)
102 {
103
104 /*
105 One triangle is (p1,p2,p3). Other is (q1,q2,q3).
106 Edges are (e1,e2,e3) and (f1,f2,f3).
107 Normals are n1 and m1
108 Outwards are (g1,g2,g3) and (h1,h2,h3).
109
110 We assume that the triangle vertices are in the same coordinate system.
111
112 First thing we do is establish a new c.s. so that p1 is at (0,0,0).
113
114 */
115
116 double p1[3], p2[3], p3[3];
117 double q1[3], q2[3], q3[3];
118 double e1[3], e2[3], e3[3];
119 double f1[3], f2[3], f3[3];
120 double g1[3], g2[3], g3[3];
121 double h1[3], h2[3], h3[3];
122 double n1[3], m1[3];
123 double z[3];
124
125 double ef11[3], ef12[3], ef13[3];
126 double ef21[3], ef22[3], ef23[3];
127 double ef31[3], ef32[3], ef33[3];
128
129 z[0] = 0.0; z[1] = 0.0; z[2] = 0.0;
130
131 p1[0] = P1[0] - P1[0]; p1[1] = P1[1] - P1[1]; p1[2] = P1[2] - P1[2];
132 p2[0] = P2[0] - P1[0]; p2[1] = P2[1] - P1[1]; p2[2] = P2[2] - P1[2];
133 p3[0] = P3[0] - P1[0]; p3[1] = P3[1] - P1[1]; p3[2] = P3[2] - P1[2];
134
135 q1[0] = Q1[0] - P1[0]; q1[1] = Q1[1] - P1[1]; q1[2] = Q1[2] - P1[2];
136 q2[0] = Q2[0] - P1[0]; q2[1] = Q2[1] - P1[1]; q2[2] = Q2[2] - P1[2];
137 q3[0] = Q3[0] - P1[0]; q3[1] = Q3[1] - P1[1]; q3[2] = Q3[2] - P1[2];
138
139 e1[0] = p2[0] - p1[0]; e1[1] = p2[1] - p1[1]; e1[2] = p2[2] - p1[2];
140 e2[0] = p3[0] - p2[0]; e2[1] = p3[1] - p2[1]; e2[2] = p3[2] - p2[2];
141 e3[0] = p1[0] - p3[0]; e3[1] = p1[1] - p3[1]; e3[2] = p1[2] - p3[2];
142
143 f1[0] = q2[0] - q1[0]; f1[1] = q2[1] - q1[1]; f1[2] = q2[2] - q1[2];
144 f2[0] = q3[0] - q2[0]; f2[1] = q3[1] - q2[1]; f2[2] = q3[2] - q2[2];
145 f3[0] = q1[0] - q3[0]; f3[1] = q1[1] - q3[1]; f3[2] = q1[2] - q3[2];
146
147 VcrossV(n1, e1, e2);
148 VcrossV(m1, f1, f2);
149
150 VcrossV(g1, e1, n1);
151 VcrossV(g2, e2, n1);
152 VcrossV(g3, e3, n1);
153 VcrossV(h1, f1, m1);
154 VcrossV(h2, f2, m1);
155 VcrossV(h3, f3, m1);
156
157 VcrossV(ef11, e1, f1);
158 VcrossV(ef12, e1, f2);
159 VcrossV(ef13, e1, f3);
160 VcrossV(ef21, e2, f1);
161 VcrossV(ef22, e2, f2);
162 VcrossV(ef23, e2, f3);
163 VcrossV(ef31, e3, f1);
164 VcrossV(ef32, e3, f2);
165 VcrossV(ef33, e3, f3);
166
167 // now begin the series of tests
168
169 if (!project6(n1, p1, p2, p3, q1, q2, q3)) return 0;
170 if (!project6(m1, p1, p2, p3, q1, q2, q3)) return 0;
171
172 if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return 0;
173 if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return 0;
174 if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return 0;
175 if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return 0;
176 if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return 0;
177 if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return 0;
178 if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return 0;
179 if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return 0;
180 if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return 0;
181
182 if (!project6(g1, p1, p2, p3, q1, q2, q3)) return 0;
183 if (!project6(g2, p1, p2, p3, q1, q2, q3)) return 0;
184 if (!project6(g3, p1, p2, p3, q1, q2, q3)) return 0;
185 if (!project6(h1, p1, p2, p3, q1, q2, q3)) return 0;
186 if (!project6(h2, p1, p2, p3, q1, q2, q3)) return 0;
187 if (!project6(h3, p1, p2, p3, q1, q2, q3)) return 0;
188
189 return 1;
190 }
191
192
193
194 /*
195
196 int
197 obb_disjoint(double B[3][3], double T[3], double a[3], double b[3]);
198
199 This is a test between two boxes, box A and box B. It is assumed that
200 the coordinate system is aligned and centered on box A. The 3x3
201 matrix B specifies box B's orientation with respect to box A.
202 Specifically, the columns of B are the basis vectors (axis vectors) of
203 box B. The center of box B is located at the vector T. The
204 dimensions of box B are given in the array b. The orientation and
205 placement of box A, in this coordinate system, are the identity matrix
206 and zero vector, respectively, so they need not be specified. The
207 dimensions of box A are given in array a.
208
209 This test operates in two modes, depending on how the library is
210 compiled. It indicates whether the two boxes are overlapping, by
211 returning a boolean.
212
213 The second version of the routine will return a conservative bounds on
214 the distance between the polygon sets which the boxes enclose. It is
215 used when RAPID is being used to estimate the distance between two
216 models.
217
218 */
219
220
221 int
222 obb_disjoint(double B[3][3], double T[3], double a[3], double b[3])
223 {
224 double t, s;
225 int r;
226 double Bf[3][3];
227 const double reps = 1e-6;
228
229 // Bf = fabs(B)
230 Bf[0][0] = myfabs(B[0][0]); Bf[0][0] += reps;
231 Bf[0][1] = myfabs(B[0][1]); Bf[0][1] += reps;
232 Bf[0][2] = myfabs(B[0][2]); Bf[0][2] += reps;
233 Bf[1][0] = myfabs(B[1][0]); Bf[1][0] += reps;
234 Bf[1][1] = myfabs(B[1][1]); Bf[1][1] += reps;
235 Bf[1][2] = myfabs(B[1][2]); Bf[1][2] += reps;
236 Bf[2][0] = myfabs(B[2][0]); Bf[2][0] += reps;
237 Bf[2][1] = myfabs(B[2][1]); Bf[2][1] += reps;
238 Bf[2][2] = myfabs(B[2][2]); Bf[2][2] += reps;
239
240
241 #if TRACE1
242 printf("Box test: Bf[3][3], B[3][3], T[3], a[3], b[3]\n");
243 Mprintg(Bf);
244 Mprintg(B);
245 Vprintg(T);
246 Vprintg(a);
247 Vprintg(b);
248 #endif
249
250 // if any of these tests are one-sided, then the polyhedra are disjoint
251 r = 1;
252
253 // A1 x A2 = A0
254 t = myfabs(T[0]);
255
256 r &= (t <=
257 (a[0] + b[0] * Bf[0][0] + b[1] * Bf[0][1] + b[2] * Bf[0][2]));
258 if (!r) return 1;
259
260 // B1 x B2 = B0
261 s = T[0]*B[0][0] + T[1]*B[1][0] + T[2]*B[2][0];
262 t = myfabs(s);
263
264 r &= ( t <=
265 (b[0] + a[0] * Bf[0][0] + a[1] * Bf[1][0] + a[2] * Bf[2][0]));
266 if (!r) return 2;
267
268 // A2 x A0 = A1
269 t = myfabs(T[1]);
270
271 r &= ( t <=
272 (a[1] + b[0] * Bf[1][0] + b[1] * Bf[1][1] + b[2] * Bf[1][2]));
273 if (!r) return 3;
274
275 // A0 x A1 = A2
276 t = myfabs(T[2]);
277
278 r &= ( t <=
279 (a[2] + b[0] * Bf[2][0] + b[1] * Bf[2][1] + b[2] * Bf[2][2]));
280 if (!r) return 4;
281
282 // B2 x B0 = B1
283 s = T[0]*B[0][1] + T[1]*B[1][1] + T[2]*B[2][1];
284 t = myfabs(s);
285
286 r &= ( t <=
287 (b[1] + a[0] * Bf[0][1] + a[1] * Bf[1][1] + a[2] * Bf[2][1]));
288 if (!r) return 5;
289
290 // B0 x B1 = B2
291 s = T[0]*B[0][2] + T[1]*B[1][2] + T[2]*B[2][2];
292 t = myfabs(s);
293
294 r &= ( t <=
295 (b[2] + a[0] * Bf[0][2] + a[1] * Bf[1][2] + a[2] * Bf[2][2]));
296 if (!r) return 6;
297
298 // A0 x B0
299 s = T[2] * B[1][0] - T[1] * B[2][0];
300 t = myfabs(s);
301
302 r &= ( t <=
303 (a[1] * Bf[2][0] + a[2] * Bf[1][0] +
304 b[1] * Bf[0][2] + b[2] * Bf[0][1]));
305 if (!r) return 7;
306
307 // A0 x B1
308 s = T[2] * B[1][1] - T[1] * B[2][1];
309 t = myfabs(s);
310
311 r &= ( t <=
312 (a[1] * Bf[2][1] + a[2] * Bf[1][1] +
313 b[0] * Bf[0][2] + b[2] * Bf[0][0]));
314 if (!r) return 8;
315
316 // A0 x B2
317 s = T[2] * B[1][2] - T[1] * B[2][2];
318 t = myfabs(s);
319
320 r &= ( t <=
321 (a[1] * Bf[2][2] + a[2] * Bf[1][2] +
322 b[0] * Bf[0][1] + b[1] * Bf[0][0]));
323 if (!r) return 9;
324
325 // A1 x B0
326 s = T[0] * B[2][0] - T[2] * B[0][0];
327 t = myfabs(s);
328
329 r &= ( t <=
330 (a[0] * Bf[2][0] + a[2] * Bf[0][0] +
331 b[1] * Bf[1][2] + b[2] * Bf[1][1]));
332 if (!r) return 10;
333
334 // A1 x B1
335 s = T[0] * B[2][1] - T[2] * B[0][1];
336 t = myfabs(s);
337
338 r &= ( t <=
339 (a[0] * Bf[2][1] + a[2] * Bf[0][1] +
340 b[0] * Bf[1][2] + b[2] * Bf[1][0]));
341 if (!r) return 11;
342
343 // A1 x B2
344 s = T[0] * B[2][2] - T[2] * B[0][2];
345 t = myfabs(s);
346
347 r &= (t <=
348 (a[0] * Bf[2][2] + a[2] * Bf[0][2] +
349 b[0] * Bf[1][1] + b[1] * Bf[1][0]));
350 if (!r) return 12;
351
352 // A2 x B0
353 s = T[1] * B[0][0] - T[0] * B[1][0];
354 t = myfabs(s);
355
356 r &= (t <=
357 (a[0] * Bf[1][0] + a[1] * Bf[0][0] +
358 b[1] * Bf[2][2] + b[2] * Bf[2][1]));
359 if (!r) return 13;
360
361 // A2 x B1
362 s = T[1] * B[0][1] - T[0] * B[1][1];
363 t = myfabs(s);
364
365 r &= ( t <=
366 (a[0] * Bf[1][1] + a[1] * Bf[0][1] +
367 b[0] * Bf[2][2] + b[2] * Bf[2][0]));
368 if (!r) return 14;
369
370 // A2 x B2
371 s = T[1] * B[0][2] - T[0] * B[1][2];
372 t = myfabs(s);
373
374 r &= ( t <=
375 (a[0] * Bf[1][2] + a[1] * Bf[0][2] +
376 b[0] * Bf[2][1] + b[1] * Bf[2][0]));
377 if (!r) return 15;
378
379 return 0; // should equal 0
380 }
381