1
2 /*************************************************************************\
3
4 Copyright 1995 The University of North Carolina at Chapel Hill.
5 All Rights Reserved.
6
7 Permission to use, copy, modify and distribute this software and its
8 documentation for educational, research and non-profit purposes, without
9 fee, and without a written agreement is hereby granted, provided that the
10 above copyright notice and the following three paragraphs appear in all
11 copies.
12
13 IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
14 LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
15 CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
16 USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
17 OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
18 DAMAGES.
19
20 THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
21 WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
22 MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE
23 PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
24 NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
25 UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
26
27 The authors may be contacted via:
28
29 US Mail: S. Gottschalk
30 Department of Computer Science
31 Sitterson Hall, CB #3175
32 University of N. Carolina
33 Chapel Hill, NC 27599-3175
34
35 Phone: (919)962-1749
36
37 EMail: {gottscha}@cs.unc.edu
38
39
40 \**************************************************************************/
41
42
43 #include "RAPID_version.H"
44
45 static char rapidtag_data[] = "RAPIDTAG file: " __FILE__ " date: " __DATE__ " time: " __TIME__;
46
47 // to silence the compiler's complaints about unreferenced identifiers.
r1(char * f)48 static void r1(char *f){ r1(f); r1(rapidtag_data); r1(rapid_version);}
49
50 extern int RAPID_initialized;
51 void RAPID_initialize();
52
53 #include "RAPID.H"
54 #include "matvec.H"
55 #include "overlap.H"
56 #include "obb.H"
57
58 double RAPID_mR[3][3];
59 double RAPID_mT[3];
60 double RAPID_ms;
61
62
63 int RAPID_first_contact;
64
65 int RAPID_num_box_tests;
66 int RAPID_num_tri_tests;
67 int RAPID_num_contacts;
68
69 int RAPID_num_cols_alloced = 0;
70 collision_pair *RAPID_contact = 0;
71
72 int add_collision(int id1, int id2);
73
74 int
tri_contact(box * b1,box * b2)75 tri_contact(box *b1, box *b2)
76 {
77 // assume just one triangle in each box.
78
79 // the vertices of the tri in b2 is in model1 C.S. The vertices of
80 // the other triangle is in model2 CS. Use RAPID_mR, RAPID_mT, and
81 // RAPID_ms to transform into model2 CS.
82
83 double i1[3];
84 double i2[3];
85 double i3[3];
86 int rc; // return code
87
88 sMxVpV(i1, RAPID_ms, RAPID_mR, b1->trp->p1, RAPID_mT);
89 sMxVpV(i2, RAPID_ms, RAPID_mR, b1->trp->p2, RAPID_mT);
90 sMxVpV(i3, RAPID_ms, RAPID_mR, b1->trp->p3, RAPID_mT);
91
92 RAPID_num_tri_tests++;
93
94 int f = tri_contact(i1, i2, i3, b2->trp->p1,b2->trp->p2, b2->trp->p3);
95
96 if (f)
97 {
98 // add_collision may be unable to allocate enough memory,
99 // so be prepared to pass along an OUT_OF_MEMORY return code.
100 if ((rc = add_collision(b1->trp->id, b2->trp->id)) != RAPID_OK)
101 return rc;
102 }
103
104 return RAPID_OK;
105 }
106
107
108
109 int
collide_recursive(box * b1,box * b2,double R[3][3],double T[3],double s)110 collide_recursive(box *b1, box *b2, double R[3][3], double T[3], double s)
111 {
112 double d[3]; // temp storage for scaled dimensions of box b2.
113 int rc; // return codes
114
115 if (1)
116 {
117 #if TRACE1
118 printf("Next collision: b1, b2, R, T, s\n");
119 printf("b1=%x, b2=%x\n", b1, b2);
120 Mprint(R);
121 Vprint(T);
122 printf("%lf\n", s);
123 #endif
124
125 if (RAPID_first_contact && (RAPID_num_contacts > 0)) return RAPID_OK;
126
127 // test top level
128
129 RAPID_num_box_tests++;
130
131 int f1;
132
133 d[0] = s * b2->d[0];
134 d[1] = s * b2->d[1];
135 d[2] = s * b2->d[2];
136 f1 = obb_disjoint(R, T, b1->d, d);
137
138 #if TRACE1
139 if (f1 != 0)
140 {
141 printf("BOX TEST %d DISJOINT! (code %d)\n", RAPID_num_box_tests, f1);
142 }
143 else
144 {
145 printf("BOX TEST %d OVERLAP! (code %d)\n", RAPID_num_box_tests, f1);
146 }
147
148 #endif
149
150 if (f1 != 0)
151 {
152 return RAPID_OK; // stop processing this test, go to top of loop
153 }
154
155 // contact between boxes
156 if (b1->leaf() && b2->leaf())
157 {
158 // it is a leaf pair - compare the polygons therein
159 // tri_contact uses the model-to-model transforms stored in
160 // RAPID_mR, RAPID_mT, RAPID_ms.
161
162 // this will pass along any OUT_OF_MEMORY return codes which
163 // may be generated.
164 return tri_contact(b1, b2);
165 }
166
167 double U[3];
168
169 double cR[3][3], cT[3], cs;
170
171 // Currently, the transform from model 2 to model 1 space is
172 // given by [B T s], where y = [B T s].x = s.B.x + T.
173
174 if (b2->leaf() || (!b1->leaf() && (b1->size() > b2->size())))
175 {
176 // here we descend to children of b1. The transform from
177 // a child of b1 to b1 is stored in [b1->N->pR,b1->N->pT],
178 // but we will denote it [B1 T1 1] for short. Notice that
179 // children boxes always have same scaling as parent, so s=1
180 // for such nested transforms.
181
182 // Here, we compute [B1 T1 1]'[B T s] = [B1'B B1'(T-T1) s]
183 // for each child, and store the transform into the collision
184 // test queue.
185
186 MTxM(cR, b1->N->pR, R);
187 VmV(U, T, b1->N->pT); MTxV(cT, b1->N->pR, U);
188 cs = s;
189
190 if ((rc = collide_recursive(b1->N, b2, cR, cT, cs)) != RAPID_OK)
191 return rc;
192
193 MTxM(cR, b1->P->pR, R);
194 VmV(U, T, b1->P->pT); MTxV(cT, b1->P->pR, U);
195 cs = s;
196
197 if ((rc = collide_recursive(b1->P, b2, cR, cT, cs)) != RAPID_OK)
198 return rc;
199
200 return RAPID_OK;
201 }
202 else
203 {
204 // here we descend to the children of b2. See comments for
205 // other 'if' clause for explanation.
206
207 MxM(cR, R, b2->N->pR); sMxVpV(cT, s, R, b2->N->pT, T);
208 cs = s;
209
210 if ((rc = collide_recursive(b1, b2->N, cR, cT, cs)) != RAPID_OK)
211 return rc;
212
213 MxM(cR, R, b2->P->pR); sMxVpV(cT, s, R, b2->P->pT, T);
214 cs = s;
215
216 if ((rc = collide_recursive(b1, b2->P, cR, cT, cs)) != RAPID_OK)
217 return rc;
218
219 return RAPID_OK;
220 }
221
222 }
223
224 return RAPID_OK;
225 }
226
227 int
RAPID_Collide(double R1[3][3],double T1[3],RAPID_model * RAPID_model1,double R2[3][3],double T2[3],RAPID_model * RAPID_model2,int flag)228 RAPID_Collide(double R1[3][3], double T1[3], RAPID_model *RAPID_model1,
229 double R2[3][3], double T2[3], RAPID_model *RAPID_model2,
230 int flag)
231 {
232 return RAPID_Collide(R1, T1, 1.0, RAPID_model1, R2, T2, 1.0, RAPID_model2, flag);
233 }
234
235
236 int
RAPID_Collide(double R1[3][3],double T1[3],double s1,RAPID_model * RAPID_model1,double R2[3][3],double T2[3],double s2,RAPID_model * RAPID_model2,int flag)237 RAPID_Collide(double R1[3][3], double T1[3], double s1, RAPID_model *RAPID_model1,
238 double R2[3][3], double T2[3], double s2, RAPID_model *RAPID_model2,
239 int flag)
240 {
241 if (!RAPID_initialized) RAPID_initialize();
242
243 if (RAPID_model1->build_state != RAPID_BUILD_STATE_PROCESSED)
244 return RAPID_ERR_UNPROCESSED_MODEL;
245
246 if (RAPID_model2->build_state != RAPID_BUILD_STATE_PROCESSED)
247 return RAPID_ERR_UNPROCESSED_MODEL;
248
249 box *b1 = RAPID_model1->b;
250 box *b2 = RAPID_model2->b;
251
252 RAPID_first_contact = 0;
253 if (flag == RAPID_FIRST_CONTACT) RAPID_first_contact = 1;
254
255 double tR1[3][3], tR2[3][3], R[3][3];
256 double tT1[3], tT2[3], T[3], U[3];
257 double s;
258
259 // [R1,T1,s1] and [R2,T2,s2] are how the two triangle sets
260 // (i.e. models) are positioned in world space. [tR1,tT1,s1] and
261 // [tR2,tT2,s2] are how the top level boxes are positioned in world
262 // space
263
264 MxM(tR1, R1, b1->pR); // tR1 = R1 * b1->pR;
265 sMxVpV(tT1, s1, R1, b1->pT, T1); // tT1 = s1 * R1 * b1->pT + T1;
266 MxM(tR2, R2, b2->pR); // tR2 = R2 * b2->pR;
267 sMxVpV(tT2, s2, R2, b2->pT, T2); // tT2 = s2 * R2 * b2->pT + T2;
268
269 // (R,T,s) is the placement of b2's top level box within
270 // the coordinate system of b1's top level box.
271
272 MTxM(R, tR1, tR2); // R = tR1.T()*tR2;
273 VmV(U, tT2, tT1); sMTxV(T, 1.0/s1, tR1, U); // T = tR1.T()*(tT2-tT1)/s1;
274
275 s = s2/s1;
276
277 // To transform tri's from model1's CS to model2's CS use this:
278 // x2 = ms . mR . x1 + mT
279
280 {
281 MTxM(RAPID_mR, R2, R1);
282 VmV(U, T1, T2); sMTxV(RAPID_mT, 1.0/s2, R2, U);
283 RAPID_ms = s1/s2;
284 }
285
286
287 // reset the report fields
288 RAPID_num_box_tests = 0;
289 RAPID_num_tri_tests = 0;
290 RAPID_num_contacts = 0;
291
292 // make the call
293 return collide_recursive(b1, b2, R, T, s);
294 }
295
296 int
add_collision(int id1,int id2)297 add_collision(int id1, int id2)
298 {
299 if (!RAPID_contact)
300 {
301 RAPID_contact = new collision_pair[10];
302 if (!RAPID_contact)
303 {
304 return RAPID_ERR_COLLIDE_OUT_OF_MEMORY;
305 }
306 RAPID_num_cols_alloced = 10;
307 RAPID_num_contacts = 0;
308 }
309
310 if (RAPID_num_contacts == RAPID_num_cols_alloced)
311 {
312 collision_pair *t = new collision_pair[RAPID_num_cols_alloced*2];
313 if (!t)
314 {
315 return RAPID_ERR_COLLIDE_OUT_OF_MEMORY;
316 }
317 RAPID_num_cols_alloced *= 2;
318
319 for(int i=0; i<RAPID_num_contacts; i++) t[i] = RAPID_contact[i];
320 delete [] RAPID_contact;
321 RAPID_contact = t;
322 }
323
324 RAPID_contact[RAPID_num_contacts].id1 = id1;
325 RAPID_contact[RAPID_num_contacts].id2 = id2;
326 RAPID_num_contacts++;
327
328 return RAPID_OK;
329 }
330