1 | |
---|
2 | /* |
---|
3 | |
---|
4 | this is code that was once useful but has now been obseleted. |
---|
5 | |
---|
6 | this file should not be compiled as part of ODE! |
---|
7 | |
---|
8 | */ |
---|
9 | |
---|
10 | //*************************************************************************** |
---|
11 | // intersect a line segment with a plane |
---|
12 | |
---|
13 | extern "C" int dClipLineToBox (const dVector3 p1, const dVector3 p2, |
---|
14 | const dVector3 p, const dMatrix3 R, |
---|
15 | const dVector3 side) |
---|
16 | { |
---|
17 | // compute the start and end of the line (p1 and p2) relative to the box. |
---|
18 | // we will do all subsequent computations in this box-relative coordinate |
---|
19 | // system. we have to do a translation and rotation for each point. |
---|
20 | dVector3 tmp,s,e; |
---|
21 | tmp[0] = p1[0] - p[0]; |
---|
22 | tmp[1] = p1[1] - p[1]; |
---|
23 | tmp[2] = p1[2] - p[2]; |
---|
24 | dMULTIPLY1_331 (s,R,tmp); |
---|
25 | tmp[0] = p2[0] - p[0]; |
---|
26 | tmp[1] = p2[1] - p[1]; |
---|
27 | tmp[2] = p2[2] - p[2]; |
---|
28 | dMULTIPLY1_331 (e,R,tmp); |
---|
29 | |
---|
30 | // compute the vector 'v' from the start point to the end point |
---|
31 | dVector3 v; |
---|
32 | v[0] = e[0] - s[0]; |
---|
33 | v[1] = e[1] - s[1]; |
---|
34 | v[2] = e[2] - s[2]; |
---|
35 | |
---|
36 | // a point on the line is defined by the parameter 't'. t=0 corresponds |
---|
37 | // to the start of the line, t=1 corresponds to the end of the line. |
---|
38 | // we will clip the line to the box by finding the range of t where a |
---|
39 | // point on the line is inside the box. the currently known bounds for |
---|
40 | // t and tlo..thi. |
---|
41 | dReal tlo=0,thi=1; |
---|
42 | |
---|
43 | // clip in the X/Y/Z direction |
---|
44 | for (int i=0; i<3; i++) { |
---|
45 | // first adjust s,e for the current t range. this is redundant for the |
---|
46 | // first iteration, but never mind. |
---|
47 | e[i] = s[i] + thi*v[i]; |
---|
48 | s[i] = s[i] + tlo*v[i]; |
---|
49 | // compute where t intersects the positive and negative sides. |
---|
50 | dReal tp = ( side[i] - s[i])/v[i]; // @@@ handle case where denom=0 |
---|
51 | dReal tm = (-side[i] - s[i])/v[i]; |
---|
52 | // handle 9 intersection cases |
---|
53 | if (s[i] <= -side[i]) { |
---|
54 | tlo = tm; |
---|
55 | if (e[i] <= -side[i]) return 0; |
---|
56 | else if (e[i] >= side[i]) thi = tp; |
---|
57 | } |
---|
58 | else if (s[i] <= side[i]) { |
---|
59 | if (e[i] <= -side[i]) thi = tm; |
---|
60 | else if (e[i] >= side[i]) thi = tp; |
---|
61 | } |
---|
62 | else { |
---|
63 | tlo = tp; |
---|
64 | if (e[i] <= -side[i]) thi = tm; |
---|
65 | else if (e[i] >= side[i]) return 0; |
---|
66 | } |
---|
67 | } |
---|
68 | |
---|
69 | //... @@@ AT HERE @@@ |
---|
70 | |
---|
71 | return 1; |
---|
72 | } |
---|
73 | |
---|
74 | |
---|
75 | //*************************************************************************** |
---|
76 | // a nice try at C-B collision. unfortunately it doesn't work. the logic |
---|
77 | // for testing for line-box intersection is correct, but unfortunately the |
---|
78 | // closest-point distance estimates are often too large. as a result contact |
---|
79 | // points are placed incorrectly. |
---|
80 | |
---|
81 | |
---|
82 | int dCollideCB (const dxGeom *o1, const dxGeom *o2, int flags, |
---|
83 | dContactGeom *contact, int skip) |
---|
84 | { |
---|
85 | int i; |
---|
86 | |
---|
87 | dIASSERT (skip >= (int)sizeof(dContactGeom)); |
---|
88 | dIASSERT (o1->_class->num == dCCylinderClass); |
---|
89 | dIASSERT (o2->_class->num == dBoxClass); |
---|
90 | contact->g1 = const_cast<dxGeom*> (o1); |
---|
91 | contact->g2 = const_cast<dxGeom*> (o2); |
---|
92 | dxCCylinder *cyl = (dxCCylinder*) CLASSDATA(o1); |
---|
93 | dxBox *box = (dxBox*) CLASSDATA(o2); |
---|
94 | |
---|
95 | // get p1,p2 = cylinder axis endpoints, get radius |
---|
96 | dVector3 p1,p2; |
---|
97 | dReal clen = cyl->lz * REAL(0.5); |
---|
98 | p1[0] = o1->pos[0] + clen * o1->R[2]; |
---|
99 | p1[1] = o1->pos[1] + clen * o1->R[6]; |
---|
100 | p1[2] = o1->pos[2] + clen * o1->R[10]; |
---|
101 | p2[0] = o1->pos[0] - clen * o1->R[2]; |
---|
102 | p2[1] = o1->pos[1] - clen * o1->R[6]; |
---|
103 | p2[2] = o1->pos[2] - clen * o1->R[10]; |
---|
104 | dReal radius = cyl->radius; |
---|
105 | |
---|
106 | // copy out box center, rotation matrix, and side array |
---|
107 | dReal *c = o2->pos; |
---|
108 | dReal *R = o2->R; |
---|
109 | dReal *side = box->side; |
---|
110 | |
---|
111 | // compute the start and end of the line (p1 and p2) relative to the box. |
---|
112 | // we will do all subsequent computations in this box-relative coordinate |
---|
113 | // system. we have to do a translation and rotation for each point. |
---|
114 | dVector3 tmp3,s,e; |
---|
115 | tmp3[0] = p1[0] - c[0]; |
---|
116 | tmp3[1] = p1[1] - c[1]; |
---|
117 | tmp3[2] = p1[2] - c[2]; |
---|
118 | dMULTIPLY1_331 (s,R,tmp3); |
---|
119 | tmp3[0] = p2[0] - c[0]; |
---|
120 | tmp3[1] = p2[1] - c[1]; |
---|
121 | tmp3[2] = p2[2] - c[2]; |
---|
122 | dMULTIPLY1_331 (e,R,tmp3); |
---|
123 | |
---|
124 | // compute the vector 'v' from the start point to the end point |
---|
125 | dVector3 v; |
---|
126 | v[0] = e[0] - s[0]; |
---|
127 | v[1] = e[1] - s[1]; |
---|
128 | v[2] = e[2] - s[2]; |
---|
129 | |
---|
130 | // compute the half-sides of the box |
---|
131 | dReal S0 = side[0] * REAL(0.5); |
---|
132 | dReal S1 = side[1] * REAL(0.5); |
---|
133 | dReal S2 = side[2] * REAL(0.5); |
---|
134 | |
---|
135 | // compute the size of the bounding box around the line segment |
---|
136 | dReal B0 = dFabs (v[0]); |
---|
137 | dReal B1 = dFabs (v[1]); |
---|
138 | dReal B2 = dFabs (v[2]); |
---|
139 | |
---|
140 | // for all 6 separation axes, measure the penetration depth. if any depth is |
---|
141 | // less than 0 then the objects don't penetrate at all so we can just |
---|
142 | // return 0. find the axis with the smallest depth, and record its normal. |
---|
143 | |
---|
144 | // note: normalR is set to point to a column of R if that is the smallest |
---|
145 | // depth normal so far. otherwise normalR is 0 and normalC is set to a |
---|
146 | // vector relative to the box. invert_normal is 1 if the sign of the normal |
---|
147 | // should be flipped. |
---|
148 | |
---|
149 | dReal depth,trial_depth,tmp,length; |
---|
150 | const dReal *normalR=0; |
---|
151 | dVector3 normalC; |
---|
152 | int invert_normal = 0; |
---|
153 | int code = 0; // 0=no contact, 1-3=face contact, 4-6=edge contact |
---|
154 | |
---|
155 | depth = dInfinity; |
---|
156 | |
---|
157 | // look at face-normal axes |
---|
158 | |
---|
159 | #undef TEST |
---|
160 | #define TEST(center,depth_expr,norm,contact_code) \ |
---|
161 | tmp = (center); \ |
---|
162 | trial_depth = radius + REAL(0.5) * ((depth_expr) - dFabs(tmp)); \ |
---|
163 | if (trial_depth < 0) return 0; \ |
---|
164 | if (trial_depth < depth) { \ |
---|
165 | depth = trial_depth; \ |
---|
166 | normalR = (norm); \ |
---|
167 | invert_normal = (tmp < 0); \ |
---|
168 | code = contact_code; \ |
---|
169 | } |
---|
170 | |
---|
171 | TEST (s[0]+e[0], side[0] + B0, R+0, 1); |
---|
172 | TEST (s[1]+e[1], side[1] + B1, R+1, 2); |
---|
173 | TEST (s[2]+e[2], side[2] + B2, R+2, 3); |
---|
174 | |
---|
175 | // look at v x box-edge axes |
---|
176 | |
---|
177 | #undef TEST |
---|
178 | #define TEST(box_radius,line_offset,nx,ny,nz,contact_code) \ |
---|
179 | tmp = (line_offset); \ |
---|
180 | trial_depth = (box_radius) - dFabs(tmp); \ |
---|
181 | length = dSqrt ((nx)*(nx) + (ny)*(ny) + (nz)*(nz)); \ |
---|
182 | if (length > 0) { \ |
---|
183 | length = dRecip(length); \ |
---|
184 | trial_depth = trial_depth * length + radius; \ |
---|
185 | if (trial_depth < 0) return 0; \ |
---|
186 | if (trial_depth < depth) { \ |
---|
187 | depth = trial_depth; \ |
---|
188 | normalR = 0; \ |
---|
189 | normalC[0] = (nx)*length; \ |
---|
190 | normalC[1] = (ny)*length; \ |
---|
191 | normalC[2] = (nz)*length; \ |
---|
192 | invert_normal = (tmp < 0); \ |
---|
193 | code = contact_code; \ |
---|
194 | } \ |
---|
195 | } |
---|
196 | |
---|
197 | TEST (B2*S1+B1*S2,v[1]*s[2]-v[2]*s[1], 0,-v[2],v[1], 4); |
---|
198 | TEST (B2*S0+B0*S2,v[2]*s[0]-v[0]*s[2], v[2],0,-v[0], 5); |
---|
199 | TEST (B1*S0+B0*S1,v[0]*s[1]-v[1]*s[0], -v[1],v[0],0, 6); |
---|
200 | |
---|
201 | #undef TEST |
---|
202 | |
---|
203 | // if we get to this point, the box and ccylinder interpenetrate. |
---|
204 | // compute the normal in global coordinates. |
---|
205 | dReal *normal = contact[0].normal; |
---|
206 | if (normalR) { |
---|
207 | normal[0] = normalR[0]; |
---|
208 | normal[1] = normalR[4]; |
---|
209 | normal[2] = normalR[8]; |
---|
210 | } |
---|
211 | else { |
---|
212 | dMULTIPLY0_331 (normal,R,normalC); |
---|
213 | } |
---|
214 | if (invert_normal) { |
---|
215 | normal[0] = -normal[0]; |
---|
216 | normal[1] = -normal[1]; |
---|
217 | normal[2] = -normal[2]; |
---|
218 | } |
---|
219 | |
---|
220 | // set the depth |
---|
221 | contact[0].depth = depth; |
---|
222 | |
---|
223 | if (code == 0) { |
---|
224 | return 0; // should never get here |
---|
225 | } |
---|
226 | else if (code >= 4) { |
---|
227 | // handle edge contacts |
---|
228 | // find an endpoint q1 on the intersecting edge of the box |
---|
229 | dVector3 q1; |
---|
230 | dReal sign[3]; |
---|
231 | for (i=0; i<3; i++) q1[i] = c[i]; |
---|
232 | sign[0] = (dDOT14(normal,R+0) > 0) ? REAL(1.0) : REAL(-1.0); |
---|
233 | for (i=0; i<3; i++) q1[i] += sign[0] * S0 * R[i*4]; |
---|
234 | sign[1] = (dDOT14(normal,R+1) > 0) ? REAL(1.0) : REAL(-1.0); |
---|
235 | for (i=0; i<3; i++) q1[i] += sign[1] * S1 * R[i*4+1]; |
---|
236 | sign[2] = (dDOT14(normal,R+2) > 0) ? REAL(1.0) : REAL(-1.0); |
---|
237 | for (i=0; i<3; i++) q1[i] += sign[2] * S2 * R[i*4+2]; |
---|
238 | |
---|
239 | // find the other endpoint q2 of the intersecting edge |
---|
240 | dVector3 q2; |
---|
241 | for (i=0; i<3; i++) |
---|
242 | q2[i] = q1[i] - R[code-4 + i*4] * (sign[code-4] * side[code-4]); |
---|
243 | |
---|
244 | // determine the closest point between the box edge and the line segment |
---|
245 | dVector3 cp1,cp2; |
---|
246 | dClosestLineSegmentPoints (q1,q2, p1,p2, cp1,cp2); |
---|
247 | for (i=0; i<3; i++) contact[0].pos[i] = cp1[i] - REAL(0.5)*normal[i]*depth; |
---|
248 | return 1; |
---|
249 | } |
---|
250 | else { |
---|
251 | // handle face contacts. |
---|
252 | // @@@ temporary: make deepest vertex on the line the contact point. |
---|
253 | // @@@ this kind of works, but we sometimes need two contact points for |
---|
254 | // @@@ stability. |
---|
255 | |
---|
256 | // compute 'v' in global coordinates |
---|
257 | dVector3 gv; |
---|
258 | for (i=0; i<3; i++) gv[i] = p2[i] - p1[i]; |
---|
259 | |
---|
260 | if (dDOT (normal,gv) > 0) { |
---|
261 | for (i=0; i<3; i++) |
---|
262 | contact[0].pos[i] = p1[i] + (depth*REAL(0.5)-radius)*normal[i]; |
---|
263 | } |
---|
264 | else { |
---|
265 | for (i=0; i<3; i++) |
---|
266 | contact[0].pos[i] = p2[i] + (depth*REAL(0.5)-radius)*normal[i]; |
---|
267 | } |
---|
268 | return 1; |
---|
269 | } |
---|
270 | } |
---|
271 | |
---|
272 | //*************************************************************************** |
---|
273 | // this function works, it's just not being used for anything at the moment: |
---|
274 | |
---|
275 | // given a box (R,side), `R' is the rotation matrix for the box, and `side' |
---|
276 | // is a vector of x/y/z side lengths, return the size of the interval of the |
---|
277 | // box projected along the given axis. if the axis has unit length then the |
---|
278 | // return value will be the actual diameter, otherwise the result will be |
---|
279 | // scaled by the axis length. |
---|
280 | |
---|
281 | static inline dReal boxDiameter (const dMatrix3 R, const dVector3 side, |
---|
282 | const dVector3 axis) |
---|
283 | { |
---|
284 | dVector3 q; |
---|
285 | dMULTIPLY1_331 (q,R,axis); // transform axis to body-relative |
---|
286 | return dFabs(q[0])*side[0] + dFabs(q[1])*side[1] + dFabs(q[2])*side[2]; |
---|
287 | } |
---|
288 | |
---|
289 | //*************************************************************************** |
---|
290 | // the old capped cylinder to capped cylinder collision code. this fails to |
---|
291 | // detect cap-to-cap contact points when the cylinder axis are aligned, but |
---|
292 | // other that that it is pretty robust. |
---|
293 | |
---|
294 | // this returns at most one contact point when the two cylinder's axes are not |
---|
295 | // aligned, and at most two (for stability) when they are aligned. |
---|
296 | // the algorithm minimizes the distance between two "sample spheres" that are |
---|
297 | // positioned along the cylinder axes according to: |
---|
298 | // sphere1 = pos1 + alpha1 * axis1 |
---|
299 | // sphere2 = pos2 + alpha2 * axis2 |
---|
300 | // alpha1 and alpha2 are limited to +/- half the length of the cylinders. |
---|
301 | // the algorithm works by finding a solution that has both alphas free, or |
---|
302 | // a solution that has one or both alphas fixed to the ends of the cylinder. |
---|
303 | |
---|
304 | int dCollideCCylinderCCylinder (dxGeom *o1, dxGeom *o2, |
---|
305 | int flags, dContactGeom *contact, int skip) |
---|
306 | { |
---|
307 | int i; |
---|
308 | const dReal tolerance = REAL(1e-5); |
---|
309 | |
---|
310 | dIASSERT (skip >= (int)sizeof(dContactGeom)); |
---|
311 | dIASSERT (o1->type == dCCylinderClass); |
---|
312 | dIASSERT (o2->type == dCCylinderClass); |
---|
313 | dxCCylinder *cyl1 = (dxCCylinder*) o1; |
---|
314 | dxCCylinder *cyl2 = (dxCCylinder*) o2; |
---|
315 | |
---|
316 | contact->g1 = o1; |
---|
317 | contact->g2 = o2; |
---|
318 | |
---|
319 | // copy out some variables, for convenience |
---|
320 | dReal lz1 = cyl1->lz * REAL(0.5); |
---|
321 | dReal lz2 = cyl2->lz * REAL(0.5); |
---|
322 | dReal *pos1 = o1->pos; |
---|
323 | dReal *pos2 = o2->pos; |
---|
324 | dReal axis1[3],axis2[3]; |
---|
325 | axis1[0] = o1->R[2]; |
---|
326 | axis1[1] = o1->R[6]; |
---|
327 | axis1[2] = o1->R[10]; |
---|
328 | axis2[0] = o2->R[2]; |
---|
329 | axis2[1] = o2->R[6]; |
---|
330 | axis2[2] = o2->R[10]; |
---|
331 | |
---|
332 | dReal alpha1,alpha2,sphere1[3],sphere2[3]; |
---|
333 | int fix1 = 0; // 0 if alpha1 is free, +/-1 to fix at +/- lz1 |
---|
334 | int fix2 = 0; // 0 if alpha2 is free, +/-1 to fix at +/- lz2 |
---|
335 | |
---|
336 | for (int count=0; count<9; count++) { |
---|
337 | // find a trial solution by fixing or not fixing the alphas |
---|
338 | if (fix1) { |
---|
339 | if (fix2) { |
---|
340 | // alpha1 and alpha2 are fixed, so the solution is easy |
---|
341 | if (fix1 > 0) alpha1 = lz1; else alpha1 = -lz1; |
---|
342 | if (fix2 > 0) alpha2 = lz2; else alpha2 = -lz2; |
---|
343 | for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; |
---|
344 | for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; |
---|
345 | } |
---|
346 | else { |
---|
347 | // fix alpha1 but let alpha2 be free |
---|
348 | if (fix1 > 0) alpha1 = lz1; else alpha1 = -lz1; |
---|
349 | for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; |
---|
350 | alpha2 = (axis2[0]*(sphere1[0]-pos2[0]) + |
---|
351 | axis2[1]*(sphere1[1]-pos2[1]) + |
---|
352 | axis2[2]*(sphere1[2]-pos2[2])); |
---|
353 | for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; |
---|
354 | } |
---|
355 | } |
---|
356 | else { |
---|
357 | if (fix2) { |
---|
358 | // fix alpha2 but let alpha1 be free |
---|
359 | if (fix2 > 0) alpha2 = lz2; else alpha2 = -lz2; |
---|
360 | for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; |
---|
361 | alpha1 = (axis1[0]*(sphere2[0]-pos1[0]) + |
---|
362 | axis1[1]*(sphere2[1]-pos1[1]) + |
---|
363 | axis1[2]*(sphere2[2]-pos1[2])); |
---|
364 | for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; |
---|
365 | } |
---|
366 | else { |
---|
367 | // let alpha1 and alpha2 be free |
---|
368 | // compute determinant of d(d^2)\d(alpha) jacobian |
---|
369 | dReal a1a2 = dDOT (axis1,axis2); |
---|
370 | dReal det = REAL(1.0)-a1a2*a1a2; |
---|
371 | if (det < tolerance) { |
---|
372 | // the cylinder axes (almost) parallel, so we will generate up to two |
---|
373 | // contacts. the solution matrix is rank deficient so alpha1 and |
---|
374 | // alpha2 are related by: |
---|
375 | // alpha2 = alpha1 + (pos1-pos2)'*axis1 (if axis1==axis2) |
---|
376 | // or alpha2 = -(alpha1 + (pos1-pos2)'*axis1) (if axis1==-axis2) |
---|
377 | // first compute where the two cylinders overlap in alpha1 space: |
---|
378 | if (a1a2 < 0) { |
---|
379 | axis2[0] = -axis2[0]; |
---|
380 | axis2[1] = -axis2[1]; |
---|
381 | axis2[2] = -axis2[2]; |
---|
382 | } |
---|
383 | dReal q[3]; |
---|
384 | for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i]; |
---|
385 | dReal k = dDOT (axis1,q); |
---|
386 | dReal a1lo = -lz1; |
---|
387 | dReal a1hi = lz1; |
---|
388 | dReal a2lo = -lz2 - k; |
---|
389 | dReal a2hi = lz2 - k; |
---|
390 | dReal lo = (a1lo > a2lo) ? a1lo : a2lo; |
---|
391 | dReal hi = (a1hi < a2hi) ? a1hi : a2hi; |
---|
392 | if (lo <= hi) { |
---|
393 | int num_contacts = flags & NUMC_MASK; |
---|
394 | if (num_contacts >= 2 && lo < hi) { |
---|
395 | // generate up to two contacts. if one of those contacts is |
---|
396 | // not made, fall back on the one-contact strategy. |
---|
397 | for (i=0; i<3; i++) sphere1[i] = pos1[i] + lo*axis1[i]; |
---|
398 | for (i=0; i<3; i++) sphere2[i] = pos2[i] + (lo+k)*axis2[i]; |
---|
399 | int n1 = dCollideSpheres (sphere1,cyl1->radius, |
---|
400 | sphere2,cyl2->radius,contact); |
---|
401 | if (n1) { |
---|
402 | for (i=0; i<3; i++) sphere1[i] = pos1[i] + hi*axis1[i]; |
---|
403 | for (i=0; i<3; i++) sphere2[i] = pos2[i] + (hi+k)*axis2[i]; |
---|
404 | dContactGeom *c2 = CONTACT(contact,skip); |
---|
405 | int n2 = dCollideSpheres (sphere1,cyl1->radius, |
---|
406 | sphere2,cyl2->radius, c2); |
---|
407 | if (n2) { |
---|
408 | c2->g1 = o1; |
---|
409 | c2->g2 = o2; |
---|
410 | return 2; |
---|
411 | } |
---|
412 | } |
---|
413 | } |
---|
414 | |
---|
415 | // just one contact to generate, so put it in the middle of |
---|
416 | // the range |
---|
417 | alpha1 = (lo + hi) * REAL(0.5); |
---|
418 | alpha2 = alpha1 + k; |
---|
419 | for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; |
---|
420 | for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; |
---|
421 | return dCollideSpheres (sphere1,cyl1->radius, |
---|
422 | sphere2,cyl2->radius,contact); |
---|
423 | } |
---|
424 | else return 0; |
---|
425 | } |
---|
426 | det = REAL(1.0)/det; |
---|
427 | dReal delta[3]; |
---|
428 | for (i=0; i<3; i++) delta[i] = pos1[i] - pos2[i]; |
---|
429 | dReal q1 = dDOT (delta,axis1); |
---|
430 | dReal q2 = dDOT (delta,axis2); |
---|
431 | alpha1 = det*(a1a2*q2-q1); |
---|
432 | alpha2 = det*(q2-a1a2*q1); |
---|
433 | for (i=0; i<3; i++) sphere1[i] = pos1[i] + alpha1*axis1[i]; |
---|
434 | for (i=0; i<3; i++) sphere2[i] = pos2[i] + alpha2*axis2[i]; |
---|
435 | } |
---|
436 | } |
---|
437 | |
---|
438 | // if the alphas are outside their allowed ranges then fix them and |
---|
439 | // try again |
---|
440 | if (fix1==0) { |
---|
441 | if (alpha1 < -lz1) { |
---|
442 | fix1 = -1; |
---|
443 | continue; |
---|
444 | } |
---|
445 | if (alpha1 > lz1) { |
---|
446 | fix1 = 1; |
---|
447 | continue; |
---|
448 | } |
---|
449 | } |
---|
450 | if (fix2==0) { |
---|
451 | if (alpha2 < -lz2) { |
---|
452 | fix2 = -1; |
---|
453 | continue; |
---|
454 | } |
---|
455 | if (alpha2 > lz2) { |
---|
456 | fix2 = 1; |
---|
457 | continue; |
---|
458 | } |
---|
459 | } |
---|
460 | |
---|
461 | // unfix the alpha variables if the local distance gradient indicates |
---|
462 | // that we are not yet at the minimum |
---|
463 | dReal tmp[3]; |
---|
464 | for (i=0; i<3; i++) tmp[i] = sphere1[i] - sphere2[i]; |
---|
465 | if (fix1) { |
---|
466 | dReal gradient = dDOT (tmp,axis1); |
---|
467 | if ((fix1 > 0 && gradient > 0) || (fix1 < 0 && gradient < 0)) { |
---|
468 | fix1 = 0; |
---|
469 | continue; |
---|
470 | } |
---|
471 | } |
---|
472 | if (fix2) { |
---|
473 | dReal gradient = -dDOT (tmp,axis2); |
---|
474 | if ((fix2 > 0 && gradient > 0) || (fix2 < 0 && gradient < 0)) { |
---|
475 | fix2 = 0; |
---|
476 | continue; |
---|
477 | } |
---|
478 | } |
---|
479 | return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact); |
---|
480 | } |
---|
481 | // if we go through the loop too much, then give up. we should NEVER get to |
---|
482 | // this point (i hope). |
---|
483 | dMessage (0,"dCollideCC(): too many iterations"); |
---|
484 | return 0; |
---|
485 | } |
---|