aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp')
-rw-r--r--libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp1445
1 files changed, 0 insertions, 1445 deletions
diff --git a/libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp b/libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp
deleted file mode 100644
index 215da25..0000000
--- a/libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp
+++ /dev/null
@@ -1,1445 +0,0 @@
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*/
generated by cgit v1.1 at 2024-11-09 22:59:30 +0000