Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/ode/ode-0.9/contrib/dCylinder/dCylinder.cpp @ 216

Last change on this file since 216 was 216, checked in by mathiask, 17 years ago

[Physik] add ode-0.9
File size: 38.4 KB
RevLine 
[216]1#include <ode/ode.h>
2#include "dCylinder.h"
3// given a pointer `p' to a dContactGeom, return the dContactGeom at
4// p + skip bytes.
5
6struct dxCylinder {     // cylinder
7  dReal radius,lz;      // radius, length along y axis //
8};
9
10int dCylinderClassUser = -1;
11
12#define NUMC_MASK (0xffff)
13
14#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip)))
15
16/////////////////////////////////////////////////////////////////////////////////////////////////
17/////////////////////////////circleIntersection//////////////////////////////////////////////////
18//this does following:
19//takes two circles as normals to planes n1,n2, center points cp1,cp2,and radiuses r1,r2
20//finds line on which circles' planes intersect
21//finds four points O1,O2 - intersection between the line and sphere with center cp1 radius r1
22//                                      O3,O4 - intersection between the line and sphere with center cp2 radius r2
23//returns false if there is no intersection
24//computes distances O1-O3, O1-O4, O2-O3, O2-O4
25//in "point" returns mean point between intersection points with smallest distance
26/////////////////////////////////////////////////////////////////////////////////////////////////
27inline bool circleIntersection(const dReal* n1,const dReal* cp1,dReal r1,const dReal* n2,const dReal* cp2,dReal r2,dVector3 point){
28dReal c1=dDOT14(cp1,n1);
29dReal c2=dDOT14(cp2,n2);
30dReal cos=dDOT44(n1,n2);
31dReal cos_2=cos*cos;
32dReal sin_2=1-cos_2;
33dReal p1=(c1-c2*cos)/sin_2;
34dReal p2=(c2-c1*cos)/sin_2;
35dVector3 lp={p1*n1[0]+p2*n2[0],p1*n1[4]+p2*n2[4],p1*n1[8]+p2*n2[8]};
36dVector3 n;
37dCROSS144(n,=,n1,n2);
38dVector3 LC1={lp[0]-cp1[0],lp[1]-cp1[1],lp[2]-cp1[2]};
39dVector3 LC2={lp[0]-cp2[0],lp[1]-cp2[1],lp[2]-cp2[2]};
40dReal A,B,C,B_A,B_A_2,D;
41dReal t1,t2,t3,t4;
42A=dDOT(n,n);
43B=dDOT(LC1,n);
44C=dDOT(LC1,LC1)-r1*r1;
45B_A=B/A;
46B_A_2=B_A*B_A;
47D=B_A_2-C;
48if(D<0.f){      //somewhat strange solution
49                        //- it is needed to set some
50                        //axis to sepparate cylinders
51                        //when their edges approach
52        t1=-B_A+sqrtf(-D);
53        t2=-B_A-sqrtf(-D);
54//      return false;
55        }
56else{
57t1=-B_A-sqrtf(D);
58t2=-B_A+sqrtf(D);
59}
60B=dDOT(LC2,n);
61C=dDOT(LC2,LC2)-r2*r2;
62B_A=B/A;
63B_A_2=B_A*B_A;
64D=B_A_2-C;
65
66if(D<0.f) {
67        t3=-B_A+sqrtf(-D);
68        t4=-B_A-sqrtf(-D);
69//      return false;
70        }
71else{
72t3=-B_A-sqrtf(D);
73t4=-B_A+sqrtf(D);
74}
75dVector3 O1={lp[0]+n[0]*t1,lp[1]+n[1]*t1,lp[2]+n[2]*t1};
76dVector3 O2={lp[0]+n[0]*t2,lp[1]+n[1]*t2,lp[2]+n[2]*t2};
77
78dVector3 O3={lp[0]+n[0]*t3,lp[1]+n[1]*t3,lp[2]+n[2]*t3};
79dVector3 O4={lp[0]+n[0]*t4,lp[1]+n[1]*t4,lp[2]+n[2]*t4};
80
81dVector3 L1_3={O3[0]-O1[0],O3[1]-O1[1],O3[2]-O1[2]};
82dVector3 L1_4={O4[0]-O1[0],O4[1]-O1[1],O4[2]-O1[2]};
83
84dVector3 L2_3={O3[0]-O2[0],O3[1]-O2[1],O3[2]-O2[2]};
85dVector3 L2_4={O4[0]-O2[0],O4[1]-O2[1],O4[2]-O2[2]};
86
87
88dReal l1_3=dDOT(L1_3,L1_3);
89dReal l1_4=dDOT(L1_4,L1_4);
90
91dReal l2_3=dDOT(L2_3,L2_3);
92dReal l2_4=dDOT(L2_4,L2_4);
93
94
95if (l1_3<l1_4)
96        if(l2_3<l2_4)
97                if(l1_3<l2_3)
98                        {
99                        //l1_3;
100                        point[0]=0.5f*(O1[0]+O3[0]);
101                        point[1]=0.5f*(O1[1]+O3[1]);
102                        point[2]=0.5f*(O1[2]+O3[2]);
103                        }
104                else{
105                        //l2_3;
106                        point[0]=0.5f*(O2[0]+O3[0]);
107                        point[1]=0.5f*(O2[1]+O3[1]);
108                        point[2]=0.5f*(O2[2]+O3[2]);
109                        }
110        else
111                if(l1_3<l2_4)
112                        {
113                        //l1_3;
114                        point[0]=0.5f*(O1[0]+O3[0]);
115                        point[1]=0.5f*(O1[1]+O3[1]);
116                        point[2]=0.5f*(O1[2]+O3[2]);
117                        }
118                else{
119                        //l2_4;
120                        point[0]=0.5f*(O2[0]+O4[0]);
121                        point[1]=0.5f*(O2[1]+O4[1]);
122                        point[2]=0.5f*(O2[2]+O4[2]);
123                        }
124
125else
126        if(l2_3<l2_4)
127                if(l1_4<l2_3)
128                        {
129                        //l1_4;
130                        point[0]=0.5f*(O1[0]+O4[0]);
131                        point[1]=0.5f*(O1[1]+O4[1]);
132                        point[2]=0.5f*(O1[2]+O4[2]);
133                        }
134                else{
135                        //l2_3;
136                        point[0]=0.5f*(O2[0]+O3[0]);
137                        point[1]=0.5f*(O2[1]+O3[1]);
138                        point[2]=0.5f*(O2[2]+O3[2]);
139                        }
140        else
141                if(l1_4<l2_4)
142                        {
143                        //l1_4;
144                        point[0]=0.5f*(O1[0]+O4[0]);
145                        point[1]=0.5f*(O1[1]+O4[1]);
146                        point[2]=0.5f*(O1[2]+O4[2]);
147                        }
148                else{
149                        //l2_4;
150                        point[0]=0.5f*(O2[0]+O4[0]);
151                        point[1]=0.5f*(O2[1]+O4[1]);
152                        point[2]=0.5f*(O2[2]+O4[2]);
153                        }
154
155return true;
156}
157
158
159
160
161void lineClosestApproach (const dVector3 pa, const dVector3 ua,
162                                 const dVector3 pb, const dVector3 ub,
163                                 dReal *alpha, dReal *beta)
164{
165  dVector3 p;
166  p[0] = pb[0] - pa[0];
167  p[1] = pb[1] - pa[1];
168  p[2] = pb[2] - pa[2];
169  dReal uaub = dDOT(ua,ub);
170  dReal q1 =  dDOT(ua,p);
171  dReal q2 = -dDOT(ub,p);
172  dReal d = 1-uaub*uaub;
173  if (d <= 0) {
174    // @@@ this needs to be made more robust
175    *alpha = 0;
176    *beta  = 0;
177  }
178  else {
179    d = dRecip(d);
180    *alpha = (q1 + uaub*q2)*d;
181    *beta  = (uaub*q1 + q2)*d;
182  }
183}
184
185
186// @@@ some stuff to optimize here, reuse code in contact point calculations.
187
188extern "C" int dCylBox (const dVector3 p1, const dMatrix3 R1,
189                        const dReal radius,const dReal lz, const dVector3 p2,
190                        const dMatrix3 R2, const dVector3 side2,
191                        dVector3 normal, dReal *depth, int *code,
192                        int maxc, dContactGeom *contact, int skip)
193{
194  dVector3 p,pp,normalC;
195  const dReal *normalR = 0;
196  dReal B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33,
197    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,sQ21,sQ22,sQ23;
198  int i,invert_normal;
199
200  // get vector from centers of box 1 to box 2, relative to box 1
201  p[0] = p2[0] - p1[0];
202  p[1] = p2[1] - p1[1];
203  p[2] = p2[2] - p1[2];
204  dMULTIPLY1_331 (pp,R1,p);             // get pp = p relative to body 1
205
206  // get side lengths / 2
207  //A1 =radius; A2 = lz*REAL(0.5); A3 = radius;
208  dReal hlz=lz/2.f;
209  B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5);
210
211  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
212  R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
213  R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
214  R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
215
216  Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
217  Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
218  Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
219
220 
221  //   * see if the axis separates the box with cylinder. if so, return 0.
222  //   * find the depth of the penetration along the separating axis (s2)
223  //   * if this is the largest depth so far, record it.
224  // the normal vector will be set to the separating axis with the smallest
225  // depth. note: normalR is set to point to a column of R1 or R2 if that is
226  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
227  // set to a vector relative to body 1. invert_normal is 1 if the sign of
228  // the normal should be flipped.
229
230#define TEST(expr1,expr2,norm,cc) \
231  s2 = dFabs(expr1) - (expr2); \
232  if (s2 > 0) return 0; \
233  if (s2 > s) { \
234    s = s2; \
235    normalR = norm; \
236    invert_normal = ((expr1) < 0); \
237    *code = (cc); \
238  }
239
240  s = -dInfinity;
241  invert_normal = 0;
242  *code = 0;
243
244  // separating axis = cylinder ax u2
245 //used when a box vertex touches a flat face of the cylinder
246  TEST (pp[1],(hlz + B1*Q21 + B2*Q22 + B3*Q23),R1+1,0);
247
248
249  // separating axis = box axis v1,v2,v3
250  //used when cylinder edge touches box face
251  //there is two ways to compute sQ: sQ21=sqrtf(1.f-Q21*Q21); or sQ21=sqrtf(Q23*Q23+Q22*Q22);
252  //if we did not need Q23 and Q22 the first way might be used to quiken the routine but then it need to
253  //check if Q21<=1.f, becouse it may slightly exeed 1.f.
254
255 
256  sQ21=sqrtf(Q23*Q23+Q22*Q22);
257  TEST (dDOT41(R2+0,p),(radius*sQ21 + hlz*Q21 + B1),R2+0,1);
258
259  sQ22=sqrtf(Q23*Q23+Q21*Q21);
260  TEST (dDOT41(R2+1,p),(radius*sQ22 + hlz*Q22 + B2),R2+1,2);
261
262  sQ23=sqrtf(Q22*Q22+Q21*Q21);
263  TEST (dDOT41(R2+2,p),(radius*sQ23 + hlz*Q23 + B3),R2+2,3);
264
265 
266#undef TEST
267#define TEST(expr1,expr2,n1,n2,n3,cc) \
268  s2 = dFabs(expr1) - (expr2); \
269  if (s2 > 0) return 0; \
270  if (s2 > s) { \
271      s = s2; \
272          normalR = 0; \
273      normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \
274      invert_normal = ((expr1) < 0); \
275      *code = (cc); \
276    }
277 
278
279
280// separating axis is a normal to the cylinder axis passing across the nearest box vertex
281//used when a box vertex touches the lateral surface of the cylinder
282
283dReal proj,boxProj,cos,sin,cos1,cos3;
284dVector3 tAx,Ax,pb;
285{
286//making Ax which is perpendicular to cyl ax to box position//
287proj=dDOT14(p2,R1+1)-dDOT14(p1,R1+1);
288
289Ax[0]=p2[0]-p1[0]-R1[1]*proj;
290Ax[1]=p2[1]-p1[1]-R1[5]*proj;
291Ax[2]=p2[2]-p1[2]-R1[9]*proj;
292dNormalize3(Ax);
293//using Ax find box vertex which is nearest to the cylinder axis
294        dReal sign;
295   
296    for (i=0; i<3; i++) pb[i] = p2[i];
297    sign = (dDOT14(Ax,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
298    for (i=0; i<3; i++) pb[i] += sign * B1 * R2[i*4];
299    sign = (dDOT14(Ax,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
300    for (i=0; i<3; i++) pb[i] += sign * B2 * R2[i*4+1];
301    sign = (dDOT14(Ax,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
302    for (i=0; i<3; i++) pb[i] += sign * B3 * R2[i*4+2];
303
304//building axis which is normal to cylinder ax to the nearest box vertex
305proj=dDOT14(pb,R1+1)-dDOT14(p1,R1+1);
306
307Ax[0]=pb[0]-p1[0]-R1[1]*proj;
308Ax[1]=pb[1]-p1[1]-R1[5]*proj;
309Ax[2]=pb[2]-p1[2]-R1[9]*proj;
310dNormalize3(Ax);
311}
312
313boxProj=dFabs(dDOT14(Ax,R2+0)*B1)+
314                dFabs(dDOT14(Ax,R2+1)*B2)+
315                dFabs(dDOT14(Ax,R2+2)*B3);
316
317TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(radius+boxProj),Ax[0],Ax[1],Ax[2],4);
318
319
320//next three test used to handle collisions between cylinder circles and box ages
321proj=dDOT14(p1,R2+0)-dDOT14(p2,R2+0);
322
323tAx[0]=-p1[0]+p2[0]+R2[0]*proj;
324tAx[1]=-p1[1]+p2[1]+R2[4]*proj;
325tAx[2]=-p1[2]+p2[2]+R2[8]*proj;
326dNormalize3(tAx);
327
328//now tAx is normal to first ax of the box to cylinder center
329//making perpendicular to tAx lying in the plane which is normal to the cylinder axis
330//it is tangent in the point where projection of tAx on cylinder's ring intersect edge circle
331
332cos=dDOT14(tAx,R1+0);
333sin=dDOT14(tAx,R1+2);
334tAx[0]=R1[2]*cos-R1[0]*sin;
335tAx[1]=R1[6]*cos-R1[4]*sin;
336tAx[2]=R1[10]*cos-R1[8]*sin;
337
338
339//use cross between tAx and first ax of the box as separating axix
340
341dCROSS114(Ax,=,tAx,R2+0);
342dNormalize3(Ax);
343
344boxProj=dFabs(dDOT14(Ax,R2+1)*B2)+
345                dFabs(dDOT14(Ax,R2+0)*B1)+
346                dFabs(dDOT14(Ax,R2+2)*B3);
347
348  cos=dFabs(dDOT14(Ax,R1+1));
349  cos1=dDOT14(Ax,R1+0);
350  cos3=dDOT14(Ax,R1+2);
351  sin=sqrtf(cos1*cos1+cos3*cos3);
352
353TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],5);
354
355
356//same thing with the second axis of the box
357proj=dDOT14(p1,R2+1)-dDOT14(p2,R2+1);
358
359tAx[0]=-p1[0]+p2[0]+R2[1]*proj;
360tAx[1]=-p1[1]+p2[1]+R2[5]*proj;
361tAx[2]=-p1[2]+p2[2]+R2[9]*proj;
362dNormalize3(tAx);
363
364
365cos=dDOT14(tAx,R1+0);
366sin=dDOT14(tAx,R1+2);
367tAx[0]=R1[2]*cos-R1[0]*sin;
368tAx[1]=R1[6]*cos-R1[4]*sin;
369tAx[2]=R1[10]*cos-R1[8]*sin;
370
371dCROSS114(Ax,=,tAx,R2+1);
372dNormalize3(Ax);
373
374boxProj=dFabs(dDOT14(Ax,R2+0)*B1)+
375                dFabs(dDOT14(Ax,R2+1)*B2)+
376                dFabs(dDOT14(Ax,R2+2)*B3);
377
378  cos=dFabs(dDOT14(Ax,R1+1));
379  cos1=dDOT14(Ax,R1+0);
380  cos3=dDOT14(Ax,R1+2);
381  sin=sqrtf(cos1*cos1+cos3*cos3);
382TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],6);
383
384//same thing with the third axis of the box
385proj=dDOT14(p1,R2+2)-dDOT14(p2,R2+2);
386
387Ax[0]=-p1[0]+p2[0]+R2[2]*proj;
388Ax[1]=-p1[1]+p2[1]+R2[6]*proj;
389Ax[2]=-p1[2]+p2[2]+R2[10]*proj;
390dNormalize3(tAx);
391
392cos=dDOT14(tAx,R1+0);
393sin=dDOT14(tAx,R1+2);
394tAx[0]=R1[2]*cos-R1[0]*sin;
395tAx[1]=R1[6]*cos-R1[4]*sin;
396tAx[2]=R1[10]*cos-R1[8]*sin;
397
398dCROSS114(Ax,=,tAx,R2+2);
399dNormalize3(Ax);
400boxProj=dFabs(dDOT14(Ax,R2+1)*B2)+
401                dFabs(dDOT14(Ax,R2+2)*B3)+
402                dFabs(dDOT14(Ax,R2+0)*B1);
403
404  cos=dFabs(dDOT14(Ax,R1+1));
405  cos1=dDOT14(Ax,R1+0);
406  cos3=dDOT14(Ax,R1+2);
407  sin=sqrtf(cos1*cos1+cos3*cos3);
408TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],7);
409
410
411#undef TEST
412
413// note: cross product axes need to be scaled when s is computed.
414// normal (n1,n2,n3) is relative to box 1.
415
416#define TEST(expr1,expr2,n1,n2,n3,cc) \
417  s2 = dFabs(expr1) - (expr2); \
418  if (s2 > 0) return 0; \
419  l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
420  if (l > 0) { \
421    s2 /= l; \
422    if (s2 > s) { \
423      s = s2; \
424      normalR = 0; \
425      normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
426      invert_normal = ((expr1) < 0); \
427      *code = (cc); \
428    } \
429  }
430
431//crosses between cylinder axis and box axes
432  // separating axis = u2 x (v1,v2,v3)
433  TEST(pp[0]*R31-pp[2]*R11,(radius+B2*Q23+B3*Q22),R31,0,-R11,8);
434  TEST(pp[0]*R32-pp[2]*R12,(radius+B1*Q23+B3*Q21),R32,0,-R12,9);
435  TEST(pp[0]*R33-pp[2]*R13,(radius+B1*Q22+B2*Q21),R33,0,-R13,10);
436
437
438#undef TEST
439
440  // if we get to this point, the boxes interpenetrate. compute the normal
441  // in global coordinates.
442  if (normalR) {
443    normal[0] = normalR[0];
444    normal[1] = normalR[4];
445    normal[2] = normalR[8];
446  }
447  else {
448          if(*code>7) dMULTIPLY0_331 (normal,R1,normalC);
449          else {normal[0] =normalC[0];normal[1] = normalC[1];normal[2] = normalC[2];}
450  }
451  if (invert_normal) {
452    normal[0] = -normal[0];
453    normal[1] = -normal[1];
454    normal[2] = -normal[2];
455  }
456  *depth = -s;
457
458  // compute contact point(s)
459
460  if (*code > 7) {
461 //find point on the cylinder pa deepest along normal
462    dVector3 pa;
463    dReal sign, cos1,cos3,factor;
464
465
466    for (i=0; i<3; i++) pa[i] = p1[i];
467
468        cos1 = dDOT14(normal,R1+0);
469        cos3 = dDOT14(normal,R1+2) ;
470        factor=sqrtf(cos1*cos1+cos3*cos3);
471
472        cos1/=factor;
473        cos3/=factor;
474       
475    for (i=0; i<3; i++) pa[i] += cos1 * radius * R1[i*4];
476
477    sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
478    for (i=0; i<3; i++) pa[i] += sign * hlz * R1[i*4+1];
479
480 
481    for (i=0; i<3; i++) pa[i] += cos3 * radius * R1[i*4+2];
482
483    // find vertex of the box  deepest along normal
484    dVector3 pb;
485    for (i=0; i<3; i++) pb[i] = p2[i];
486    sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
487    for (i=0; i<3; i++) pb[i] += sign * B1 * R2[i*4];
488    sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
489    for (i=0; i<3; i++) pb[i] += sign * B2 * R2[i*4+1];
490    sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
491    for (i=0; i<3; i++) pb[i] += sign * B3 * R2[i*4+2];
492
493
494    dReal alpha,beta;
495    dVector3 ua,ub;
496    for (i=0; i<3; i++) ua[i] = R1[1 + i*4];
497    for (i=0; i<3; i++) ub[i] = R2[*code-8 + i*4];
498
499    lineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
500    for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
501    for (i=0; i<3; i++) pb[i] += ub[i]*beta;
502
503    for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
504    contact[0].depth = *depth;
505    return 1;
506  }
507
508
509        if(*code==4){
510                for (i=0; i<3; i++) contact[0].pos[i] = pb[i];
511                contact[0].depth = *depth;
512                return 1;
513                                }
514 
515
516  dVector3 vertex;
517  if (*code == 0) {
518   
519    dReal sign;
520    for (i=0; i<3; i++) vertex[i] = p2[i];
521    sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0);
522    for (i=0; i<3; i++) vertex[i] += sign * B1 * R2[i*4];
523    sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0);
524    for (i=0; i<3; i++) vertex[i] += sign * B2 * R2[i*4+1];
525    sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0);
526    for (i=0; i<3; i++) vertex[i] += sign * B3 * R2[i*4+2];
527  }
528  else {
529   
530    dReal sign,cos1,cos3,factor;
531    for (i=0; i<3; i++) vertex[i] = p1[i];
532    cos1 = dDOT14(normal,R1+0) ;
533        cos3 = dDOT14(normal,R1+2);
534        factor=sqrtf(cos1*cos1+cos3*cos3);
535        factor= factor ? factor : 1.f;
536        cos1/=factor;
537        cos3/=factor;
538    for (i=0; i<3; i++) vertex[i] += cos1 * radius * R1[i*4];
539
540    sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
541    for (i=0; i<3; i++) vertex[i] += sign * hlz * R1[i*4+1];
542   
543    for (i=0; i<3; i++) vertex[i] += cos3 * radius * R1[i*4+2];
544  }
545  for (i=0; i<3; i++) contact[0].pos[i] = vertex[i];
546  contact[0].depth = *depth;
547  return 1;
548}
549
550//****************************************************************************
551
552extern "C" int dCylCyl (const dVector3 p1, const dMatrix3 R1,
553                        const dReal radius1,const dReal lz1, const dVector3 p2,
554                        const dMatrix3 R2, const dReal radius2,const dReal lz2,
555                        dVector3 normal, dReal *depth, int *code,
556                        int maxc, dContactGeom *contact, int skip)
557{
558  dVector3 p,pp1,pp2,normalC;
559  const dReal *normalR = 0;
560  dReal hlz1,hlz2,s,s2;
561  int i,invert_normal;
562
563  // get vector from centers of box 1 to box 2, relative to box 1
564  p[0] = p2[0] - p1[0];
565  p[1] = p2[1] - p1[1];
566  p[2] = p2[2] - p1[2];
567  dMULTIPLY1_331 (pp1,R1,p);            // get pp1 = p relative to body 1
568  dMULTIPLY1_331 (pp2,R2,p);
569  // get side lengths / 2
570  hlz1 = lz1*REAL(0.5);
571  hlz2 = lz2*REAL(0.5); 
572
573 dReal proj,cos,sin,cos1,cos3;
574
575
576
577#define TEST(expr1,expr2,norm,cc) \
578  s2 = dFabs(expr1) - (expr2); \
579  if (s2 > 0) return 0; \
580  if (s2 > s) { \
581    s = s2; \
582    normalR = norm; \
583    invert_normal = ((expr1) < 0); \
584    *code = (cc); \
585  }
586
587  s = -dInfinity;
588  invert_normal = 0;
589  *code = 0;
590
591  cos=dFabs(dDOT44(R1+1,R2+1));
592  sin=sqrtf(1.f-(cos>1.f ? 1.f : cos));
593
594  TEST (pp1[1],(hlz1 + radius2*sin + hlz2*cos ),R1+1,0);//pp
595
596  TEST (pp2[1],(radius1*sin + hlz1*cos + hlz2),R2+1,1);
597
598
599
600  // note: cross product axes need to be scaled when s is computed.
601 
602#undef TEST
603#define TEST(expr1,expr2,n1,n2,n3,cc) \
604  s2 = dFabs(expr1) - (expr2); \
605  if (s2 > 0) return 0; \
606  if (s2 > s) { \
607      s = s2; \
608          normalR = 0; \
609      normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \
610      invert_normal = ((expr1) < 0); \
611      *code = (cc); \
612    }
613 
614
615dVector3 tAx,Ax,pa,pb;
616
617//cross between cylinders' axes
618dCROSS144(Ax,=,R1+1,R2+1);
619dNormalize3(Ax);
620TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius1+radius2,Ax[0],Ax[1],Ax[2],6);
621
622
623{
624 
625    dReal sign, factor;
626
627        //making ax which is perpendicular to cyl1 ax passing across cyl2 position//
628                //(project p on cyl1 flat surface )
629    for (i=0; i<3; i++) pb[i] = p2[i];
630        //cos1 = dDOT14(p,R1+0);
631        //cos3 = dDOT14(p,R1+2) ;
632        tAx[0]=pp1[0]*R1[0]+pp1[2]*R1[2];
633        tAx[1]=pp1[0]*R1[4]+pp1[2]*R1[6];
634        tAx[2]=pp1[0]*R1[8]+pp1[2]*R1[10];
635        dNormalize3(tAx);
636
637//find deepest point pb of cyl2 on opposite direction of tAx
638        cos1 = dDOT14(tAx,R2+0);
639        cos3 = dDOT14(tAx,R2+2) ;
640        factor=sqrtf(cos1*cos1+cos3*cos3);
641        cos1/=factor;
642        cos3/=factor;
643    for (i=0; i<3; i++) pb[i] -= cos1 * radius2 * R2[i*4];
644
645    sign = (dDOT14(tAx,R2+1) > 0) ? REAL(1.0) : REAL(-1.0);
646    for (i=0; i<3; i++) pb[i] -= sign * hlz2 * R2[i*4+1];
647
648    for (i=0; i<3; i++) pb[i] -= cos3 * radius2 * R2[i*4+2];
649
650//making perpendicular to cyl1 ax passing across pb
651        proj=dDOT14(pb,R1+1)-dDOT14(p1,R1+1);
652
653        Ax[0]=pb[0]-p1[0]-R1[1]*proj;
654        Ax[1]=pb[1]-p1[1]-R1[5]*proj;
655        Ax[2]=pb[2]-p1[2]-R1[9]*proj;
656
657}
658
659dNormalize3(Ax);
660
661
662  cos=dFabs(dDOT14(Ax,R2+1));
663  cos1=dDOT14(Ax,R2+0);
664  cos3=dDOT14(Ax,R2+2);
665  sin=sqrtf(cos1*cos1+cos3*cos3);
666
667TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius1+cos*hlz2+sin*radius2,Ax[0],Ax[1],Ax[2],3);
668
669
670
671{
672   
673   dReal sign, factor;
674       
675    for (i=0; i<3; i++) pa[i] = p1[i];
676
677        //making ax which is perpendicular to cyl2 ax passing across cyl1 position//
678        //(project p on cyl2 flat surface )
679        //cos1 = dDOT14(p,R2+0);
680        //cos3 = dDOT14(p,R2+2) ;
681        tAx[0]=pp2[0]*R2[0]+pp2[2]*R2[2];
682        tAx[1]=pp2[0]*R2[4]+pp2[2]*R2[6];
683        tAx[2]=pp2[0]*R2[8]+pp2[2]*R2[10];
684        dNormalize3(tAx);
685
686        cos1 = dDOT14(tAx,R1+0);
687        cos3 = dDOT14(tAx,R1+2) ;
688        factor=sqrtf(cos1*cos1+cos3*cos3);
689        cos1/=factor;
690        cos3/=factor;
691
692//find deepest point pa of cyl2 on direction of tAx
693    for (i=0; i<3; i++) pa[i] += cos1 * radius1 * R1[i*4];
694
695    sign = (dDOT14(tAx,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
696    for (i=0; i<3; i++) pa[i] += sign * hlz1 * R1[i*4+1];
697
698 
699    for (i=0; i<3; i++) pa[i] += cos3 * radius1 * R1[i*4+2];
700
701        proj=dDOT14(pa,R2+1)-dDOT14(p2,R2+1);
702
703        Ax[0]=pa[0]-p2[0]-R2[1]*proj;
704        Ax[1]=pa[1]-p2[1]-R2[5]*proj;
705        Ax[2]=pa[2]-p2[2]-R2[9]*proj;
706
707}
708dNormalize3(Ax);
709
710
711
712  cos=dFabs(dDOT14(Ax,R1+1));
713  cos1=dDOT14(Ax,R1+0);
714  cos3=dDOT14(Ax,R1+2);
715  sin=sqrtf(cos1*cos1+cos3*cos3);
716
717TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius2+cos*hlz1+sin*radius1,Ax[0],Ax[1],Ax[2],4);
718
719
720////test circl
721
722//@ this needed to set right normal when cylinders edges intersect
723//@ the most precise axis for this test may be found as a line between nearest points of two
724//@ circles. But it needs comparatively a lot of computation.
725//@ I use a trick which lets not to solve quadric equation.
726//@ In the case when cylinder eidges touches the test below rather accurate.
727//@ I still not sure about problems with sepparation but they have not been revealed during testing.
728dVector3 point;
729{
730 dVector3 ca,cb; 
731 dReal sign;
732 for (i=0; i<3; i++) ca[i] = p1[i];
733 for (i=0; i<3; i++) cb[i] = p2[i];
734//find two nearest flat rings
735 sign = (pp1[1] > 0) ? REAL(1.0) : REAL(-1.0);
736 for (i=0; i<3; i++) ca[i] += sign * hlz1 * R1[i*4+1];
737
738 sign = (pp2[1] > 0) ? REAL(1.0) : REAL(-1.0);
739 for (i=0; i<3; i++) cb[i] -= sign * hlz2 * R2[i*4+1];
740
741 dVector3 tAx,tAx1;
742        circleIntersection(R1+1,ca,radius1,R2+1,cb,radius2,point);
743
744        Ax[0]=point[0]-ca[0];
745        Ax[1]=point[1]-ca[1];
746        Ax[2]=point[2]-ca[2];
747
748        cos1 = dDOT14(Ax,R1+0);
749        cos3 = dDOT14(Ax,R1+2) ;
750
751        tAx[0]=cos3*R1[0]-cos1*R1[2];
752        tAx[1]=cos3*R1[4]-cos1*R1[6];
753        tAx[2]=cos3*R1[8]-cos1*R1[10];
754
755        Ax[0]=point[0]-cb[0];
756        Ax[1]=point[1]-cb[1];
757        Ax[2]=point[2]-cb[2];
758
759
760        cos1 = dDOT14(Ax,R2+0);
761        cos3 = dDOT14(Ax,R2+2) ;
762
763        tAx1[0]=cos3*R2[0]-cos1*R2[2];
764        tAx1[1]=cos3*R2[4]-cos1*R2[6];
765        tAx1[2]=cos3*R2[8]-cos1*R2[10];
766        dCROSS(Ax,=,tAx,tAx1);
767       
768
769 
770
771dNormalize3(Ax);
772dReal cyl1Pr,cyl2Pr;
773
774 cos=dFabs(dDOT14(Ax,R1+1));
775 cos1=dDOT14(Ax,R1+0);
776 cos3=dDOT14(Ax,R1+2);
777 sin=sqrtf(cos1*cos1+cos3*cos3);
778 cyl1Pr=cos*hlz1+sin*radius1;
779
780 cos=dFabs(dDOT14(Ax,R2+1));
781 cos1=dDOT14(Ax,R2+0);
782 cos3=dDOT14(Ax,R2+2);
783 sin=sqrtf(cos1*cos1+cos3*cos3);
784 cyl2Pr=cos*hlz2+sin*radius2;
785TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],cyl1Pr+cyl2Pr,Ax[0],Ax[1],Ax[2],5);
786
787
788}
789
790
791#undef TEST
792
793
794
795  // if we get to this point, the cylinders interpenetrate. compute the normal
796  // in global coordinates.
797  if (normalR) {
798    normal[0] = normalR[0];
799    normal[1] = normalR[4];
800    normal[2] = normalR[8];
801  }
802  else {
803                normal[0] =normalC[0];normal[1] = normalC[1];normal[2] = normalC[2];
804                }
805  if (invert_normal) {
806    normal[0] = -normal[0];
807    normal[1] = -normal[1];
808    normal[2] = -normal[2];
809  }
810
811  *depth = -s;
812
813  // compute contact point(s)
814
815        if(*code==3){
816                for (i=0; i<3; i++) contact[0].pos[i] = pb[i];
817                contact[0].depth = *depth;
818                return 1;
819                                }
820
821        if(*code==4){
822                for (i=0; i<3; i++) contact[0].pos[i] = pa[i];
823                contact[0].depth = *depth;
824                return 1;
825                                }
826
827        if(*code==5){
828                for (i=0; i<3; i++) contact[0].pos[i] = point[i];
829                contact[0].depth = *depth;
830                return 1;
831                                }
832
833if (*code == 6) {
834            dVector3 pa;
835    dReal sign, cos1,cos3,factor;
836
837
838    for (i=0; i<3; i++) pa[i] = p1[i];
839
840        cos1 = dDOT14(normal,R1+0);
841        cos3 = dDOT14(normal,R1+2) ;
842        factor=sqrtf(cos1*cos1+cos3*cos3);
843
844        cos1/=factor;
845        cos3/=factor;
846       
847    for (i=0; i<3; i++) pa[i] += cos1 * radius1 * R1[i*4];
848
849    sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
850    for (i=0; i<3; i++) pa[i] += sign * hlz1 * R1[i*4+1];
851
852 
853    for (i=0; i<3; i++) pa[i] += cos3 * radius1 * R1[i*4+2];
854
855    // find a point pb on the intersecting edge of cylinder 2
856    dVector3 pb;
857    for (i=0; i<3; i++) pb[i] = p2[i];
858        cos1 = dDOT14(normal,R2+0);
859        cos3 = dDOT14(normal,R2+2) ;
860        factor=sqrtf(cos1*cos1+cos3*cos3);
861
862        cos1/=factor;
863        cos3/=factor;
864       
865    for (i=0; i<3; i++) pb[i] -= cos1 * radius2 * R2[i*4];
866
867    sign = (dDOT14(normal,R2+1) > 0) ? REAL(1.0) : REAL(-1.0);
868    for (i=0; i<3; i++) pb[i] -= sign * hlz2 * R2[i*4+1];
869
870 
871    for (i=0; i<3; i++) pb[i] -= cos3 * radius2 * R2[i*4+2];
872
873       
874        dReal alpha,beta;
875        dVector3 ua,ub;
876        for (i=0; i<3; i++) ua[i] = R1[1 + i*4];
877        for (i=0; i<3; i++) ub[i] = R2[1 + i*4];
878        lineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
879        for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
880        for (i=0; i<3; i++) pb[i] += ub[i]*beta;
881
882    for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
883    contact[0].depth = *depth;
884    return 1;
885  }
886
887  // okay, we have a face-something intersection (because the separating
888  // axis is perpendicular to a face).
889
890  // @@@ temporary: make deepest point on the "other" cylinder the contact point.
891  // @@@ this kind of works, but we need multiple contact points for stability,
892  // @@@ especially for face-face contact.
893
894  dVector3 vertex;
895  if (*code == 0) {
896    // flat face from cylinder 1 touches a edge/face from cylinder 2.
897    dReal sign,cos1,cos3,factor;
898    for (i=0; i<3; i++) vertex[i] = p2[i];
899    cos1 = dDOT14(normal,R2+0) ;
900        cos3 = dDOT14(normal,R2+2);
901        factor=sqrtf(cos1*cos1+cos3*cos3);
902
903        cos1/=factor;
904        cos3/=factor;
905    for (i=0; i<3; i++) vertex[i] -= cos1 * radius2 * R2[i*4];
906
907    sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
908    for (i=0; i<3; i++) vertex[i] -= sign * hlz2 * R2[i*4+1];
909   
910    for (i=0; i<3; i++) vertex[i] -= cos3 * radius2 * R2[i*4+2];
911  }
912  else {
913     // flat face from cylinder 2 touches a edge/face from cylinder 1.
914    dReal sign,cos1,cos3,factor;
915    for (i=0; i<3; i++) vertex[i] = p1[i];
916    cos1 = dDOT14(normal,R1+0) ;
917        cos3 = dDOT14(normal,R1+2);
918        factor=sqrtf(cos1*cos1+cos3*cos3);
919
920        cos1/=factor;
921        cos3/=factor;
922    for (i=0; i<3; i++) vertex[i] += cos1 * radius1 * R1[i*4];
923
924    sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0);
925    for (i=0; i<3; i++) vertex[i] += sign * hlz1 * R1[i*4+1];
926   
927    for (i=0; i<3; i++) vertex[i] += cos3 * radius1 * R1[i*4+2];
928  }
929  for (i=0; i<3; i++) contact[0].pos[i] = vertex[i];
930  contact[0].depth = *depth;
931  return 1;
932}
933
934//****************************************************************************
935
936
937int dCollideCylS (dxGeom *o1, dxGeom *o2, int flags,
938                dContactGeom *contact, int skip)
939{
940 
941
942  dIASSERT (skip >= (int)sizeof(dContactGeom));
943  dIASSERT (dGeomGetClass(o2) == dSphereClass);
944  dIASSERT (dGeomGetClass(o1) == dCylinderClassUser);
945  const dReal* p1=dGeomGetPosition(o1);
946  const dReal* p2=dGeomGetPosition(o2);
947  const dReal* R=dGeomGetRotation(o1);
948  dVector3 p,normalC,normal;
949  const dReal *normalR = 0;
950  dReal cylRadius;
951  dReal hl;
952  dGeomCylinderGetParams(o1,&cylRadius,&hl);
953  dReal sphereRadius;
954  sphereRadius=dGeomSphereGetRadius(o2);
955 
956  int i,invert_normal;
957
958  // get vector from centers of cyl to shere
959  p[0] = p2[0] - p1[0];
960  p[1] = p2[1] - p1[1];
961  p[2] = p2[2] - p1[2];
962 
963dReal s,s2;
964unsigned char code;
965#define TEST(expr1,expr2,norm,cc) \
966  s2 = dFabs(expr1) - (expr2); \
967  if (s2 > 0) return 0; \
968  if (s2 > s) { \
969    s = s2; \
970    normalR = norm; \
971    invert_normal = ((expr1) < 0); \
972    code = (cc); \
973  }
974
975  s = -dInfinity;
976  invert_normal = 0;
977  code = 0;
978
979  // separating axis cyl ax
980
981  TEST (dDOT14(p,R+1),sphereRadius+hl,R+1,2);
982  // note: cross product axes need to be scaled when s is computed.
983  // normal (n1,n2,n3) is relative to
984#undef TEST
985#define TEST(expr1,expr2,n1,n2,n3,cc) \
986  s2 = dFabs(expr1) - (expr2); \
987  if (s2 > 0) return 0; \
988  if (s2 > s) { \
989      s = s2; \
990          normalR = 0; \
991      normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \
992      invert_normal = ((expr1) < 0); \
993      code = (cc); \
994    }
995 
996//making ax which is perpendicular to cyl1 ax to sphere center//
997 
998dReal proj,cos,sin,cos1,cos3;
999dVector3 Ax;
1000        proj=dDOT14(p2,R+1)-dDOT14(p1,R+1);
1001
1002        Ax[0]=p2[0]-p1[0]-R[1]*proj;
1003        Ax[1]=p2[1]-p1[1]-R[5]*proj;
1004        Ax[2]=p2[2]-p1[2]-R[9]*proj;
1005dNormalize3(Ax);
1006TEST(dDOT(p,Ax),sphereRadius+cylRadius,Ax[0],Ax[1],Ax[2],9);
1007
1008
1009Ax[0]=p[0];
1010Ax[1]=p[1];
1011Ax[2]=p[2];
1012dNormalize3(Ax);
1013
1014        dVector3 pa;
1015    dReal sign, factor;
1016    for (i=0; i<3; i++) pa[i] = p1[i];
1017
1018        cos1 = dDOT14(Ax,R+0);
1019        cos3 = dDOT14(Ax,R+2) ;
1020        factor=sqrtf(cos1*cos1+cos3*cos3);
1021        cos1/=factor;
1022        cos3/=factor;
1023    for (i=0; i<3; i++) pa[i] += cos1 * cylRadius * R[i*4];
1024    sign = (dDOT14(normal,R+1) > 0) ? REAL(1.0) : REAL(-1.0);
1025    for (i=0; i<3; i++) pa[i] += sign * hl * R[i*4+1];
1026    for (i=0; i<3; i++) pa[i] += cos3 * cylRadius  * R[i*4+2];
1027
1028Ax[0]=p2[0]-pa[0];
1029Ax[1]=p2[1]-pa[1];
1030Ax[2]=p2[2]-pa[2];
1031dNormalize3(Ax);
1032
1033 cos=dFabs(dDOT14(Ax,R+1));
1034 cos1=dDOT14(Ax,R+0);
1035 cos3=dDOT14(Ax,R+2);
1036 sin=sqrtf(cos1*cos1+cos3*cos3);
1037TEST(dDOT(p,Ax),sphereRadius+cylRadius*sin+hl*cos,Ax[0],Ax[1],Ax[2],14);
1038
1039
1040#undef TEST
1041
1042  if (normalR) {
1043    normal[0] = normalR[0];
1044    normal[1] = normalR[4];
1045    normal[2] = normalR[8];
1046  }
1047  else {
1048
1049        normal[0] = normalC[0];
1050        normal[1] = normalC[1];
1051        normal[2] = normalC[2];
1052                }
1053  if (invert_normal) {
1054    normal[0] = -normal[0];
1055    normal[1] = -normal[1];
1056    normal[2] = -normal[2];
1057  }
1058   // compute contact point(s)
1059contact->depth=-s;
1060contact->normal[0]=-normal[0];
1061contact->normal[1]=-normal[1];
1062contact->normal[2]=-normal[2];
1063contact->g1=const_cast<dxGeom*> (o1);
1064contact->g2=const_cast<dxGeom*> (o2);
1065contact->pos[0]=p2[0]-normal[0]*sphereRadius;
1066contact->pos[1]=p2[1]-normal[1]*sphereRadius;
1067contact->pos[2]=p2[2]-normal[2]*sphereRadius;
1068return 1;
1069}
1070
1071
1072
1073int dCollideCylB (dxGeom *o1, dxGeom *o2, int flags,
1074                dContactGeom *contact, int skip)
1075{
1076  dVector3 normal;
1077  dReal depth;
1078  int code;
1079  dReal cylRadius,cylLength;
1080  dVector3 boxSides;
1081  dGeomCylinderGetParams(o1,&cylRadius,&cylLength);
1082  dGeomBoxGetLengths(o2,boxSides);
1083  int num = dCylBox(dGeomGetPosition(o1),dGeomGetRotation(o1),cylRadius,cylLength, 
1084                                        dGeomGetPosition(o2),dGeomGetRotation(o2),boxSides,
1085                                        normal,&depth,&code,flags & NUMC_MASK,contact,skip);
1086  for (int i=0; i<num; i++) {
1087    CONTACT(contact,i*skip)->normal[0] = -normal[0];
1088    CONTACT(contact,i*skip)->normal[1] = -normal[1];
1089    CONTACT(contact,i*skip)->normal[2] = -normal[2];
1090    CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
1091    CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
1092  }
1093  return num;
1094}
1095
1096int dCollideCylCyl (dxGeom *o1, dxGeom *o2, int flags,
1097                dContactGeom *contact, int skip)
1098{
1099  dVector3 normal;
1100  dReal depth;
1101  int code;
1102dReal cylRadius1,cylRadius2;
1103dReal cylLength1,cylLength2;
1104dGeomCylinderGetParams(o1,&cylRadius1,&cylLength1);
1105dGeomCylinderGetParams(o2,&cylRadius2,&cylLength2);
1106int num = dCylCyl (dGeomGetPosition(o1),dGeomGetRotation(o1),cylRadius1,cylLength1,
1107                                   dGeomGetPosition(o2),dGeomGetRotation(o2),cylRadius2,cylLength2,
1108                                     normal,&depth,&code,flags & NUMC_MASK,contact,skip);
1109
1110  for (int i=0; i<num; i++) {
1111    CONTACT(contact,i*skip)->normal[0] = -normal[0];
1112    CONTACT(contact,i*skip)->normal[1] = -normal[1];
1113    CONTACT(contact,i*skip)->normal[2] = -normal[2];
1114    CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
1115    CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
1116  }
1117  return num;
1118}
1119
1120struct dxPlane {
1121  dReal p[4];
1122};
1123
1124
1125int dCollideCylPlane 
1126        (
1127        dxGeom *o1, dxGeom *o2, int flags,
1128                          dContactGeom *contact, int skip){
1129  dIASSERT (skip >= (int)sizeof(dContactGeom));
1130  dIASSERT (dGeomGetClass(o1) == dCylinderClassUser);
1131  dIASSERT (dGeomGetClass(o2) == dPlaneClass);
1132  contact->g1 = const_cast<dxGeom*> (o1);
1133  contact->g2 = const_cast<dxGeom*> (o2);
1134 
1135 unsigned int ret = 0;
1136
1137 dReal radius;
1138 dReal hlz;
1139 dGeomCylinderGetParams(o1,&radius,&hlz);
1140 hlz /= 2;
1141 
1142 const dReal *R =       dGeomGetRotation(o1);// rotation of cylinder
1143 const dReal* p =       dGeomGetPosition(o1);
1144 dVector4 n;            // normal vector
1145 dReal pp;
1146 dGeomPlaneGetParams (o2, n);
1147 pp=n[3];
1148 dReal cos1,sin1;
1149  cos1=dFabs(dDOT14(n,R+1));
1150
1151cos1=cos1<REAL(1.) ? cos1 : REAL(1.); //cos1 may slightly exeed 1.f
1152sin1=sqrtf(REAL(1.)-cos1*cos1);
1153//////////////////////////////
1154
1155dReal sidePr=cos1*hlz+sin1*radius;
1156
1157dReal dist=-pp+dDOT(n,p);
1158dReal outDepth=sidePr-dist;
1159
1160if(outDepth<0.f) return 0;
1161
1162dVector3 pos;
1163
1164
1165/////////////////////////////////////////// from geom.cpp dCollideBP
1166  dReal Q1 = dDOT14(n,R+0);
1167  dReal Q2 = dDOT14(n,R+1);
1168  dReal Q3 = dDOT14(n,R+2);
1169  dReal factor =sqrtf(Q1*Q1+Q3*Q3);
1170  factor= factor ? factor :1.f;
1171  dReal A1 = radius *           Q1/factor;
1172  dReal A2 = hlz*Q2;
1173  dReal A3 = radius *           Q3/factor;
1174
1175  pos[0]=p[0];
1176  pos[1]=p[1];
1177  pos[2]=p[2];
1178
1179  pos[0]-= A1*R[0];
1180  pos[1]-= A1*R[4];
1181  pos[2]-= A1*R[8];
1182
1183  pos[0]-= A3*R[2];
1184  pos[1]-= A3*R[6];
1185  pos[2]-= A3*R[10];
1186
1187  pos[0]-= A2>0 ? hlz*R[1]:-hlz*R[1];
1188  pos[1]-= A2>0 ? hlz*R[5]:-hlz*R[5];
1189  pos[2]-= A2>0 ? hlz*R[9]:-hlz*R[9];
1190 
1191 
1192
1193  contact->pos[0] = pos[0];
1194  contact->pos[1] = pos[1];
1195  contact->pos[2] = pos[2];
1196   contact->depth = outDepth;
1197  ret=1;
1198 
1199if(dFabs(Q2)>M_SQRT1_2){
1200
1201  CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A1*R[0];
1202  CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A1*R[4];
1203  CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A1*R[8];
1204  CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q1*2.f*A1);
1205
1206  if(CONTACT(contact,ret*skip)->depth>0.f)
1207  ret++;
1208 
1209 
1210  CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A3*R[2];
1211  CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A3*R[6];
1212  CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A3*R[10];
1213  CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q3*2.f*A3);
1214
1215  if(CONTACT(contact,ret*skip)->depth>0.f) ret++;
1216} else {
1217
1218  CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*(A2>0 ? hlz*R[1]:-hlz*R[1]);
1219  CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*(A2>0 ? hlz*R[5]:-hlz*R[5]);
1220  CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*(A2>0 ? hlz*R[9]:-hlz*R[9]);
1221  CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q2*2.f*A2);
1222
1223  if(CONTACT(contact,ret*skip)->depth>0.f) ret++;
1224}
1225
1226
1227
1228 for (unsigned int i=0; i<ret; i++) {
1229    CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
1230    CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (o2);
1231        CONTACT(contact,i*skip)->normal[0] =n[0];
1232        CONTACT(contact,i*skip)->normal[1] =n[1];
1233        CONTACT(contact,i*skip)->normal[2] =n[2];
1234  }
1235  return ret; 
1236}
1237
1238int dCollideCylRay(dxGeom *o1, dxGeom *o2, int flags,
1239                   dContactGeom *contact, int skip) {
1240  dIASSERT (skip >= (int)sizeof(dContactGeom));
1241  dIASSERT (dGeomGetClass(o1) == dCylinderClassUser);
1242  dIASSERT (dGeomGetClass(o2) == dRayClass);
1243  contact->g1 = const_cast<dxGeom*> (o1);
1244  contact->g2 = const_cast<dxGeom*> (o2);
1245  dReal radius;
1246  dReal lz;
1247  dGeomCylinderGetParams(o1,&radius,&lz);
1248  dReal lz2=lz*REAL(0.5);
1249  const dReal *R = dGeomGetRotation(o1); // rotation of the cylinder
1250  const dReal *p = dGeomGetPosition(o1); // position of the cylinder
1251  dVector3 start,dir;
1252  dGeomRayGet(o2,start,dir); // position and orientation of the ray
1253  dReal length = dGeomRayGetLength(o2);
1254
1255  // compute some useful info
1256  dVector3 cs,q,r;
1257  dReal C,k;
1258  cs[0] = start[0] - p[0];
1259  cs[1] = start[1] - p[1];
1260  cs[2] = start[2] - p[2];
1261  k = dDOT41(R+1,cs);   // position of ray start along cyl axis (Y)
1262  q[0] = k*R[0*4+1] - cs[0];
1263  q[1] = k*R[1*4+1] - cs[1];
1264  q[2] = k*R[2*4+1] - cs[2];
1265  C = dDOT(q,q) - radius*radius;
1266  // if C < 0 then ray start position within infinite extension of cylinder
1267  // if ray start position is inside the cylinder
1268  int inside_cyl=0;
1269  if (C<0 && !(k<-lz2 || k>lz2)) inside_cyl=1;
1270  // compute ray collision with infinite cylinder, except for the case where
1271  // the ray is outside the cylinder but within the infinite cylinder
1272  // (it that case the ray can only hit endcaps)
1273  if (!inside_cyl && C < 0) {
1274    // set k to cap position to check
1275    if (k < 0) k = -lz2; else k = lz2;
1276  }
1277  else {
1278    dReal uv = dDOT41(R+1,dir);
1279    r[0] = uv*R[0*4+1] - dir[0];
1280    r[1] = uv*R[1*4+1] - dir[1];
1281    r[2] = uv*R[2*4+1] - dir[2];
1282    dReal A = dDOT(r,r);
1283    dReal B = 2*dDOT(q,r);
1284    k = B*B-4*A*C;
1285    if (k < 0) {
1286      // the ray does not intersect the infinite cylinder, but if the ray is
1287      // inside and parallel to the cylinder axis it may intersect the end
1288      // caps. set k to cap position to check.
1289      if (!inside_cyl) return 0;
1290      if (uv < 0) k = -lz2; else k = lz2;
1291    }
1292    else {
1293      k = dSqrt(k);
1294      A = dRecip (2*A);
1295      dReal alpha = (-B-k)*A;
1296      if (alpha < 0) {
1297        alpha = (-B+k)*A;
1298        if (alpha<0) return 0;
1299      }
1300      if (alpha>length) return 0;
1301      // the ray intersects the infinite cylinder. check to see if the
1302      // intersection point is between the caps
1303      contact->pos[0] = start[0] + alpha*dir[0];
1304      contact->pos[1] = start[1] + alpha*dir[1];
1305      contact->pos[2] = start[2] + alpha*dir[2];
1306      q[0] = contact->pos[0] - p[0];
1307      q[1] = contact->pos[1] - p[1];
1308      q[2] = contact->pos[2] - p[2];
1309      k = dDOT14(q,R+1);
1310      dReal nsign = inside_cyl ? -1 : 1;
1311      if (k >= -lz2 && k <= lz2) {
1312        contact->normal[0] = nsign * (contact->pos[0] -
1313                                      (p[0] + k*R[0*4+1]));
1314        contact->normal[1] = nsign * (contact->pos[1] -
1315                                      (p[1] + k*R[1*4+1]));
1316        contact->normal[2] = nsign * (contact->pos[2] -
1317                                      (p[2] + k*R[2*4+1]));
1318        dNormalize3 (contact->normal);
1319        contact->depth = alpha;
1320        return 1;
1321      }
1322      // the infinite cylinder intersection point is not between the caps.
1323      // set k to cap position to check.
1324      if (k < 0) k = -lz2; else k = lz2;
1325    }
1326  }
1327  // check for ray intersection with the caps. k must indicate the cap
1328  // position to check
1329  // perform a ray plan interesection
1330  // R+1 is the plan normal
1331  q[0] = start[0] - (p[0] + k*R[0*4+1]);
1332  q[1] = start[1] - (p[1] + k*R[1*4+1]);
1333  q[2] = start[2] - (p[2] + k*R[2*4+1]);
1334  dReal alpha = -dDOT14(q,R+1);
1335  dReal k2 = dDOT14(dir,R+1);
1336  if (k2==0) return 0; // ray parallel to the plane
1337  alpha/=k2;
1338  if (alpha<0 || alpha>length) return 0; // too short
1339  contact->pos[0]=start[0]+alpha*dir[0];
1340  contact->pos[1]=start[1]+alpha*dir[1];
1341  contact->pos[2]=start[2]+alpha*dir[2];
1342  dReal nsign = (k<0)?-1:1;
1343  contact->normal[0]=nsign*R[0*4+1];
1344  contact->normal[1]=nsign*R[1*4+1];
1345  contact->normal[2]=nsign*R[2*4+1];
1346  contact->depth=alpha;
1347  return 1;
1348}
1349
1350static  dColliderFn * dCylinderColliderFn (int num)
1351{
1352  if (num == dBoxClass) return (dColliderFn *) &dCollideCylB;
1353  else if (num == dSphereClass) return (dColliderFn *) &dCollideCylS;
1354  else if (num == dCylinderClassUser) return (dColliderFn *) &dCollideCylCyl;
1355  else if (num == dPlaneClass) return (dColliderFn *) &dCollideCylPlane;
1356  else if (num == dRayClass) return (dColliderFn *) &dCollideCylRay;
1357  return 0;
1358}
1359
1360
1361static  void dCylinderAABB (dxGeom *geom, dReal aabb[6])
1362{
1363  dReal radius,lz;
1364  dGeomCylinderGetParams(geom,&radius,&lz);
1365const dReal* R= dGeomGetRotation(geom);
1366const dReal* pos= dGeomGetPosition(geom);
1367  dReal xrange =  dFabs (R[0] *radius) +
1368    REAL(0.5) *dFabs (R[1] * lz) + dFabs (R[2] * radius);
1369
1370  dReal yrange = dFabs (R[4] *radius) +
1371    REAL(0.5) * dFabs (R[5] * lz) + dFabs (R[6] * radius);
1372
1373  dReal zrange =  dFabs (R[8] * radius) +
1374    REAL(0.5) *dFabs (R[9] * lz) + dFabs (R[10] * radius);
1375
1376  aabb[0] = pos[0] - xrange;
1377  aabb[1] = pos[0] + xrange;
1378  aabb[2] = pos[1] - yrange;
1379  aabb[3] = pos[1] + yrange;
1380  aabb[4] = pos[2] - zrange;
1381  aabb[5] = pos[2] + zrange;
1382}
1383
1384dxGeom *dCreateCylinder (dSpaceID space, dReal r, dReal lz)
1385{
1386 dAASSERT (r > 0 && lz > 0);
1387 if (dCylinderClassUser == -1)
1388  {
1389    dGeomClass c;
1390    c.bytes = sizeof (dxCylinder);
1391    c.collider = &dCylinderColliderFn;
1392    c.aabb = &dCylinderAABB;
1393    c.aabb_test = 0;
1394    c.dtor = 0;
1395    dCylinderClassUser=dCreateGeomClass (&c);
1396
1397  }
1398
1399  dGeomID g = dCreateGeom (dCylinderClassUser);
1400  if (space) dSpaceAdd (space,g);
1401  dxCylinder *c = (dxCylinder*) dGeomGetClassData(g);
1402
1403  c->radius = r;
1404  c->lz = lz;
1405  return g;
1406}
1407
1408
1409
1410void dGeomCylinderSetParams (dGeomID g, dReal radius, dReal length)
1411{
1412  dUASSERT (g && dGeomGetClass(g) == dCylinderClassUser,"argument not a cylinder");
1413  dAASSERT (radius > 0 && length > 0);
1414  dxCylinder *c = (dxCylinder*) dGeomGetClassData(g);
1415  c->radius = radius;
1416  c->lz = length;
1417}
1418
1419
1420
1421void dGeomCylinderGetParams (dGeomID g, dReal *radius, dReal *length)
1422{
1423  dUASSERT (g && dGeomGetClass(g) == dCylinderClassUser ,"argument not a cylinder");
1424  dxCylinder *c = (dxCylinder*) dGeomGetClassData(g);
1425  *radius = c->radius;
1426  *length = c->lz;
1427}
1428
1429/*
1430void dMassSetCylinder (dMass *m, dReal density,
1431                  dReal radius, dReal length)
1432{
1433  dAASSERT (m);
1434  dMassSetZero (m);
1435  dReal M = length*M_PI*radius*radius*density;
1436  m->mass = M;
1437  m->_I(0,0) = M/REAL(4.0) * (ly*ly + lz*lz);
1438  m->_I(1,1) = M/REAL(12.0) * (lx*lx + lz*lz);
1439  m->_I(2,2) = M/REAL(4.0) * (lx*lx + ly*ly);
1440
1441# ifndef dNODEBUG
1442  checkMass (m);
1443# endif
1444}
1445*/
Note: See TracBrowser for help on using the repository browser.