aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/scrapbook.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/src/scrapbook.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/scrapbook.cpp485
1 files changed, 485 insertions, 0 deletions
diff --git a/libraries/ode-0.9/ode/src/scrapbook.cpp b/libraries/ode-0.9/ode/src/scrapbook.cpp
new file mode 100644
index 0000000..2621814
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/scrapbook.cpp
@@ -0,0 +1,485 @@
1
2/*
3
4this is code that was once useful but has now been obseleted.
5
6this file should not be compiled as part of ODE!
7
8*/
9
10//***************************************************************************
11// intersect a line segment with a plane
12
13extern "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
82int 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
281static 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
304int 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}