2.5: Remove OOPS code from the outliner space, as discussed
[blender-staging.git] / extern / ode / dist / ode / src / scrapbook.cpp
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 }