From d48ea5bb797037069d641da41da0f195f0124491 Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:48 +0000 Subject: one more for the gipper --- libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp | 1445 +++++++++++++++++++++ libraries/ode-0.9/contrib/dCylinder/dCylinder.h | 14 + libraries/ode-0.9/contrib/dCylinder/readme.txt | 62 + 3 files changed, 1521 insertions(+) create mode 100644 libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp create mode 100644 libraries/ode-0.9/contrib/dCylinder/dCylinder.h create mode 100644 libraries/ode-0.9/contrib/dCylinder/readme.txt (limited to 'libraries/ode-0.9/contrib/dCylinder') diff --git a/libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp b/libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp new file mode 100644 index 0000000..215da25 --- /dev/null +++ b/libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp @@ -0,0 +1,1445 @@ +#include +#include "dCylinder.h" +// given a pointer `p' to a dContactGeom, return the dContactGeom at +// p + skip bytes. + +struct dxCylinder { // cylinder + dReal radius,lz; // radius, length along y axis // +}; + +int dCylinderClassUser = -1; + +#define NUMC_MASK (0xffff) + +#define CONTACT(p,skip) ((dContactGeom*) (((char*)p) + (skip))) + +///////////////////////////////////////////////////////////////////////////////////////////////// +/////////////////////////////circleIntersection////////////////////////////////////////////////// +//this does following: +//takes two circles as normals to planes n1,n2, center points cp1,cp2,and radiuses r1,r2 +//finds line on which circles' planes intersect +//finds four points O1,O2 - intersection between the line and sphere with center cp1 radius r1 +// O3,O4 - intersection between the line and sphere with center cp2 radius r2 +//returns false if there is no intersection +//computes distances O1-O3, O1-O4, O2-O3, O2-O4 +//in "point" returns mean point between intersection points with smallest distance +///////////////////////////////////////////////////////////////////////////////////////////////// +inline bool circleIntersection(const dReal* n1,const dReal* cp1,dReal r1,const dReal* n2,const dReal* cp2,dReal r2,dVector3 point){ +dReal c1=dDOT14(cp1,n1); +dReal c2=dDOT14(cp2,n2); +dReal cos=dDOT44(n1,n2); +dReal cos_2=cos*cos; +dReal sin_2=1-cos_2; +dReal p1=(c1-c2*cos)/sin_2; +dReal p2=(c2-c1*cos)/sin_2; +dVector3 lp={p1*n1[0]+p2*n2[0],p1*n1[4]+p2*n2[4],p1*n1[8]+p2*n2[8]}; +dVector3 n; +dCROSS144(n,=,n1,n2); +dVector3 LC1={lp[0]-cp1[0],lp[1]-cp1[1],lp[2]-cp1[2]}; +dVector3 LC2={lp[0]-cp2[0],lp[1]-cp2[1],lp[2]-cp2[2]}; +dReal A,B,C,B_A,B_A_2,D; +dReal t1,t2,t3,t4; +A=dDOT(n,n); +B=dDOT(LC1,n); +C=dDOT(LC1,LC1)-r1*r1; +B_A=B/A; +B_A_2=B_A*B_A; +D=B_A_2-C; +if(D<0.f){ //somewhat strange solution + //- it is needed to set some + //axis to sepparate cylinders + //when their edges approach + t1=-B_A+sqrtf(-D); + t2=-B_A-sqrtf(-D); +// return false; + } +else{ +t1=-B_A-sqrtf(D); +t2=-B_A+sqrtf(D); +} +B=dDOT(LC2,n); +C=dDOT(LC2,LC2)-r2*r2; +B_A=B/A; +B_A_2=B_A*B_A; +D=B_A_2-C; + +if(D<0.f) { + t3=-B_A+sqrtf(-D); + t4=-B_A-sqrtf(-D); +// return false; + } +else{ +t3=-B_A-sqrtf(D); +t4=-B_A+sqrtf(D); +} +dVector3 O1={lp[0]+n[0]*t1,lp[1]+n[1]*t1,lp[2]+n[2]*t1}; +dVector3 O2={lp[0]+n[0]*t2,lp[1]+n[1]*t2,lp[2]+n[2]*t2}; + +dVector3 O3={lp[0]+n[0]*t3,lp[1]+n[1]*t3,lp[2]+n[2]*t3}; +dVector3 O4={lp[0]+n[0]*t4,lp[1]+n[1]*t4,lp[2]+n[2]*t4}; + +dVector3 L1_3={O3[0]-O1[0],O3[1]-O1[1],O3[2]-O1[2]}; +dVector3 L1_4={O4[0]-O1[0],O4[1]-O1[1],O4[2]-O1[2]}; + +dVector3 L2_3={O3[0]-O2[0],O3[1]-O2[1],O3[2]-O2[2]}; +dVector3 L2_4={O4[0]-O2[0],O4[1]-O2[1],O4[2]-O2[2]}; + + +dReal l1_3=dDOT(L1_3,L1_3); +dReal l1_4=dDOT(L1_4,L1_4); + +dReal l2_3=dDOT(L2_3,L2_3); +dReal l2_4=dDOT(L2_4,L2_4); + + +if (l1_3 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1) < 0); \ + *code = (cc); \ + } + + s = -dInfinity; + invert_normal = 0; + *code = 0; + + // separating axis = cylinder ax u2 + //used when a box vertex touches a flat face of the cylinder + TEST (pp[1],(hlz + B1*Q21 + B2*Q22 + B3*Q23),R1+1,0); + + + // separating axis = box axis v1,v2,v3 + //used when cylinder edge touches box face + //there is two ways to compute sQ: sQ21=sqrtf(1.f-Q21*Q21); or sQ21=sqrtf(Q23*Q23+Q22*Q22); + //if we did not need Q23 and Q22 the first way might be used to quiken the routine but then it need to + //check if Q21<=1.f, becouse it may slightly exeed 1.f. + + + sQ21=sqrtf(Q23*Q23+Q22*Q22); + TEST (dDOT41(R2+0,p),(radius*sQ21 + hlz*Q21 + B1),R2+0,1); + + sQ22=sqrtf(Q23*Q23+Q21*Q21); + TEST (dDOT41(R2+1,p),(radius*sQ22 + hlz*Q22 + B2),R2+1,2); + + sQ23=sqrtf(Q22*Q22+Q21*Q21); + TEST (dDOT41(R2+2,p),(radius*sQ23 + hlz*Q23 + B3),R2+2,3); + + +#undef TEST +#define TEST(expr1,expr2,n1,n2,n3,cc) \ + s2 = dFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \ + invert_normal = ((expr1) < 0); \ + *code = (cc); \ + } + + + +// separating axis is a normal to the cylinder axis passing across the nearest box vertex +//used when a box vertex touches the lateral surface of the cylinder + +dReal proj,boxProj,cos,sin,cos1,cos3; +dVector3 tAx,Ax,pb; +{ +//making Ax which is perpendicular to cyl ax to box position// +proj=dDOT14(p2,R1+1)-dDOT14(p1,R1+1); + +Ax[0]=p2[0]-p1[0]-R1[1]*proj; +Ax[1]=p2[1]-p1[1]-R1[5]*proj; +Ax[2]=p2[2]-p1[2]-R1[9]*proj; +dNormalize3(Ax); +//using Ax find box vertex which is nearest to the cylinder axis + dReal sign; + + for (i=0; i<3; i++) pb[i] = p2[i]; + sign = (dDOT14(Ax,R2+0) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B1 * R2[i*4]; + sign = (dDOT14(Ax,R2+1) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B2 * R2[i*4+1]; + sign = (dDOT14(Ax,R2+2) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B3 * R2[i*4+2]; + +//building axis which is normal to cylinder ax to the nearest box vertex +proj=dDOT14(pb,R1+1)-dDOT14(p1,R1+1); + +Ax[0]=pb[0]-p1[0]-R1[1]*proj; +Ax[1]=pb[1]-p1[1]-R1[5]*proj; +Ax[2]=pb[2]-p1[2]-R1[9]*proj; +dNormalize3(Ax); +} + +boxProj=dFabs(dDOT14(Ax,R2+0)*B1)+ + dFabs(dDOT14(Ax,R2+1)*B2)+ + dFabs(dDOT14(Ax,R2+2)*B3); + +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(radius+boxProj),Ax[0],Ax[1],Ax[2],4); + + +//next three test used to handle collisions between cylinder circles and box ages +proj=dDOT14(p1,R2+0)-dDOT14(p2,R2+0); + +tAx[0]=-p1[0]+p2[0]+R2[0]*proj; +tAx[1]=-p1[1]+p2[1]+R2[4]*proj; +tAx[2]=-p1[2]+p2[2]+R2[8]*proj; +dNormalize3(tAx); + +//now tAx is normal to first ax of the box to cylinder center +//making perpendicular to tAx lying in the plane which is normal to the cylinder axis +//it is tangent in the point where projection of tAx on cylinder's ring intersect edge circle + +cos=dDOT14(tAx,R1+0); +sin=dDOT14(tAx,R1+2); +tAx[0]=R1[2]*cos-R1[0]*sin; +tAx[1]=R1[6]*cos-R1[4]*sin; +tAx[2]=R1[10]*cos-R1[8]*sin; + + +//use cross between tAx and first ax of the box as separating axix + +dCROSS114(Ax,=,tAx,R2+0); +dNormalize3(Ax); + +boxProj=dFabs(dDOT14(Ax,R2+1)*B2)+ + dFabs(dDOT14(Ax,R2+0)*B1)+ + dFabs(dDOT14(Ax,R2+2)*B3); + + cos=dFabs(dDOT14(Ax,R1+1)); + cos1=dDOT14(Ax,R1+0); + cos3=dDOT14(Ax,R1+2); + sin=sqrtf(cos1*cos1+cos3*cos3); + +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],5); + + +//same thing with the second axis of the box +proj=dDOT14(p1,R2+1)-dDOT14(p2,R2+1); + +tAx[0]=-p1[0]+p2[0]+R2[1]*proj; +tAx[1]=-p1[1]+p2[1]+R2[5]*proj; +tAx[2]=-p1[2]+p2[2]+R2[9]*proj; +dNormalize3(tAx); + + +cos=dDOT14(tAx,R1+0); +sin=dDOT14(tAx,R1+2); +tAx[0]=R1[2]*cos-R1[0]*sin; +tAx[1]=R1[6]*cos-R1[4]*sin; +tAx[2]=R1[10]*cos-R1[8]*sin; + +dCROSS114(Ax,=,tAx,R2+1); +dNormalize3(Ax); + +boxProj=dFabs(dDOT14(Ax,R2+0)*B1)+ + dFabs(dDOT14(Ax,R2+1)*B2)+ + dFabs(dDOT14(Ax,R2+2)*B3); + + cos=dFabs(dDOT14(Ax,R1+1)); + cos1=dDOT14(Ax,R1+0); + cos3=dDOT14(Ax,R1+2); + sin=sqrtf(cos1*cos1+cos3*cos3); +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],6); + +//same thing with the third axis of the box +proj=dDOT14(p1,R2+2)-dDOT14(p2,R2+2); + +Ax[0]=-p1[0]+p2[0]+R2[2]*proj; +Ax[1]=-p1[1]+p2[1]+R2[6]*proj; +Ax[2]=-p1[2]+p2[2]+R2[10]*proj; +dNormalize3(tAx); + +cos=dDOT14(tAx,R1+0); +sin=dDOT14(tAx,R1+2); +tAx[0]=R1[2]*cos-R1[0]*sin; +tAx[1]=R1[6]*cos-R1[4]*sin; +tAx[2]=R1[10]*cos-R1[8]*sin; + +dCROSS114(Ax,=,tAx,R2+2); +dNormalize3(Ax); +boxProj=dFabs(dDOT14(Ax,R2+1)*B2)+ + dFabs(dDOT14(Ax,R2+2)*B3)+ + dFabs(dDOT14(Ax,R2+0)*B1); + + cos=dFabs(dDOT14(Ax,R1+1)); + cos1=dDOT14(Ax,R1+0); + cos3=dDOT14(Ax,R1+2); + sin=sqrtf(cos1*cos1+cos3*cos3); +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],(sin*radius+cos*hlz+boxProj),Ax[0],Ax[1],Ax[2],7); + + +#undef TEST + +// note: cross product axes need to be scaled when s is computed. +// normal (n1,n2,n3) is relative to box 1. + +#define TEST(expr1,expr2,n1,n2,n3,cc) \ + s2 = dFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \ + if (l > 0) { \ + s2 /= l; \ + if (s2 > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \ + invert_normal = ((expr1) < 0); \ + *code = (cc); \ + } \ + } + +//crosses between cylinder axis and box axes + // separating axis = u2 x (v1,v2,v3) + TEST(pp[0]*R31-pp[2]*R11,(radius+B2*Q23+B3*Q22),R31,0,-R11,8); + TEST(pp[0]*R32-pp[2]*R12,(radius+B1*Q23+B3*Q21),R32,0,-R12,9); + TEST(pp[0]*R33-pp[2]*R13,(radius+B1*Q22+B2*Q21),R33,0,-R13,10); + + +#undef TEST + + // if we get to this point, the boxes interpenetrate. compute the normal + // in global coordinates. + if (normalR) { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { + if(*code>7) dMULTIPLY0_331 (normal,R1,normalC); + else {normal[0] =normalC[0];normal[1] = normalC[1];normal[2] = normalC[2];} + } + if (invert_normal) { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + *depth = -s; + + // compute contact point(s) + + if (*code > 7) { + //find point on the cylinder pa deepest along normal + dVector3 pa; + dReal sign, cos1,cos3,factor; + + + for (i=0; i<3; i++) pa[i] = p1[i]; + + cos1 = dDOT14(normal,R1+0); + cos3 = dDOT14(normal,R1+2) ; + factor=sqrtf(cos1*cos1+cos3*cos3); + + cos1/=factor; + cos3/=factor; + + for (i=0; i<3; i++) pa[i] += cos1 * radius * R1[i*4]; + + sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pa[i] += sign * hlz * R1[i*4+1]; + + + for (i=0; i<3; i++) pa[i] += cos3 * radius * R1[i*4+2]; + + // find vertex of the box deepest along normal + dVector3 pb; + for (i=0; i<3; i++) pb[i] = p2[i]; + sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B1 * R2[i*4]; + sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B2 * R2[i*4+1]; + sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) pb[i] += sign * B3 * R2[i*4+2]; + + + dReal alpha,beta; + dVector3 ua,ub; + for (i=0; i<3; i++) ua[i] = R1[1 + i*4]; + for (i=0; i<3; i++) ub[i] = R2[*code-8 + i*4]; + + lineClosestApproach (pa,ua,pb,ub,&alpha,&beta); + for (i=0; i<3; i++) pa[i] += ua[i]*alpha; + for (i=0; i<3; i++) pb[i] += ub[i]*beta; + + for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]); + contact[0].depth = *depth; + return 1; + } + + + if(*code==4){ + for (i=0; i<3; i++) contact[0].pos[i] = pb[i]; + contact[0].depth = *depth; + return 1; + } + + + dVector3 vertex; + if (*code == 0) { + + dReal sign; + for (i=0; i<3; i++) vertex[i] = p2[i]; + sign = (dDOT14(normal,R2+0) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) vertex[i] += sign * B1 * R2[i*4]; + sign = (dDOT14(normal,R2+1) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) vertex[i] += sign * B2 * R2[i*4+1]; + sign = (dDOT14(normal,R2+2) > 0) ? REAL(-1.0) : REAL(1.0); + for (i=0; i<3; i++) vertex[i] += sign * B3 * R2[i*4+2]; + } + else { + + dReal sign,cos1,cos3,factor; + for (i=0; i<3; i++) vertex[i] = p1[i]; + cos1 = dDOT14(normal,R1+0) ; + cos3 = dDOT14(normal,R1+2); + factor=sqrtf(cos1*cos1+cos3*cos3); + factor= factor ? factor : 1.f; + cos1/=factor; + cos3/=factor; + for (i=0; i<3; i++) vertex[i] += cos1 * radius * R1[i*4]; + + sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) vertex[i] += sign * hlz * R1[i*4+1]; + + for (i=0; i<3; i++) vertex[i] += cos3 * radius * R1[i*4+2]; + } + for (i=0; i<3; i++) contact[0].pos[i] = vertex[i]; + contact[0].depth = *depth; + return 1; +} + +//**************************************************************************** + +extern "C" int dCylCyl (const dVector3 p1, const dMatrix3 R1, + const dReal radius1,const dReal lz1, const dVector3 p2, + const dMatrix3 R2, const dReal radius2,const dReal lz2, + dVector3 normal, dReal *depth, int *code, + int maxc, dContactGeom *contact, int skip) +{ + dVector3 p,pp1,pp2,normalC; + const dReal *normalR = 0; + dReal hlz1,hlz2,s,s2; + int i,invert_normal; + + // get vector from centers of box 1 to box 2, relative to box 1 + p[0] = p2[0] - p1[0]; + p[1] = p2[1] - p1[1]; + p[2] = p2[2] - p1[2]; + dMULTIPLY1_331 (pp1,R1,p); // get pp1 = p relative to body 1 + dMULTIPLY1_331 (pp2,R2,p); + // get side lengths / 2 + hlz1 = lz1*REAL(0.5); + hlz2 = lz2*REAL(0.5); + + dReal proj,cos,sin,cos1,cos3; + + + +#define TEST(expr1,expr2,norm,cc) \ + s2 = dFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1) < 0); \ + *code = (cc); \ + } + + s = -dInfinity; + invert_normal = 0; + *code = 0; + + cos=dFabs(dDOT44(R1+1,R2+1)); + sin=sqrtf(1.f-(cos>1.f ? 1.f : cos)); + + TEST (pp1[1],(hlz1 + radius2*sin + hlz2*cos ),R1+1,0);//pp + + TEST (pp2[1],(radius1*sin + hlz1*cos + hlz2),R2+1,1); + + + + // note: cross product axes need to be scaled when s is computed. + +#undef TEST +#define TEST(expr1,expr2,n1,n2,n3,cc) \ + s2 = dFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \ + invert_normal = ((expr1) < 0); \ + *code = (cc); \ + } + + +dVector3 tAx,Ax,pa,pb; + +//cross between cylinders' axes +dCROSS144(Ax,=,R1+1,R2+1); +dNormalize3(Ax); +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius1+radius2,Ax[0],Ax[1],Ax[2],6); + + +{ + + dReal sign, factor; + + //making ax which is perpendicular to cyl1 ax passing across cyl2 position// + //(project p on cyl1 flat surface ) + for (i=0; i<3; i++) pb[i] = p2[i]; + //cos1 = dDOT14(p,R1+0); + //cos3 = dDOT14(p,R1+2) ; + tAx[0]=pp1[0]*R1[0]+pp1[2]*R1[2]; + tAx[1]=pp1[0]*R1[4]+pp1[2]*R1[6]; + tAx[2]=pp1[0]*R1[8]+pp1[2]*R1[10]; + dNormalize3(tAx); + +//find deepest point pb of cyl2 on opposite direction of tAx + cos1 = dDOT14(tAx,R2+0); + cos3 = dDOT14(tAx,R2+2) ; + factor=sqrtf(cos1*cos1+cos3*cos3); + cos1/=factor; + cos3/=factor; + for (i=0; i<3; i++) pb[i] -= cos1 * radius2 * R2[i*4]; + + sign = (dDOT14(tAx,R2+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pb[i] -= sign * hlz2 * R2[i*4+1]; + + for (i=0; i<3; i++) pb[i] -= cos3 * radius2 * R2[i*4+2]; + +//making perpendicular to cyl1 ax passing across pb + proj=dDOT14(pb,R1+1)-dDOT14(p1,R1+1); + + Ax[0]=pb[0]-p1[0]-R1[1]*proj; + Ax[1]=pb[1]-p1[1]-R1[5]*proj; + Ax[2]=pb[2]-p1[2]-R1[9]*proj; + +} + +dNormalize3(Ax); + + + cos=dFabs(dDOT14(Ax,R2+1)); + cos1=dDOT14(Ax,R2+0); + cos3=dDOT14(Ax,R2+2); + sin=sqrtf(cos1*cos1+cos3*cos3); + +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius1+cos*hlz2+sin*radius2,Ax[0],Ax[1],Ax[2],3); + + + +{ + + dReal sign, factor; + + for (i=0; i<3; i++) pa[i] = p1[i]; + + //making ax which is perpendicular to cyl2 ax passing across cyl1 position// + //(project p on cyl2 flat surface ) + //cos1 = dDOT14(p,R2+0); + //cos3 = dDOT14(p,R2+2) ; + tAx[0]=pp2[0]*R2[0]+pp2[2]*R2[2]; + tAx[1]=pp2[0]*R2[4]+pp2[2]*R2[6]; + tAx[2]=pp2[0]*R2[8]+pp2[2]*R2[10]; + dNormalize3(tAx); + + cos1 = dDOT14(tAx,R1+0); + cos3 = dDOT14(tAx,R1+2) ; + factor=sqrtf(cos1*cos1+cos3*cos3); + cos1/=factor; + cos3/=factor; + +//find deepest point pa of cyl2 on direction of tAx + for (i=0; i<3; i++) pa[i] += cos1 * radius1 * R1[i*4]; + + sign = (dDOT14(tAx,R1+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pa[i] += sign * hlz1 * R1[i*4+1]; + + + for (i=0; i<3; i++) pa[i] += cos3 * radius1 * R1[i*4+2]; + + proj=dDOT14(pa,R2+1)-dDOT14(p2,R2+1); + + Ax[0]=pa[0]-p2[0]-R2[1]*proj; + Ax[1]=pa[1]-p2[1]-R2[5]*proj; + Ax[2]=pa[2]-p2[2]-R2[9]*proj; + +} +dNormalize3(Ax); + + + + cos=dFabs(dDOT14(Ax,R1+1)); + cos1=dDOT14(Ax,R1+0); + cos3=dDOT14(Ax,R1+2); + sin=sqrtf(cos1*cos1+cos3*cos3); + +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],radius2+cos*hlz1+sin*radius1,Ax[0],Ax[1],Ax[2],4); + + +////test circl + +//@ this needed to set right normal when cylinders edges intersect +//@ the most precise axis for this test may be found as a line between nearest points of two +//@ circles. But it needs comparatively a lot of computation. +//@ I use a trick which lets not to solve quadric equation. +//@ In the case when cylinder eidges touches the test below rather accurate. +//@ I still not sure about problems with sepparation but they have not been revealed during testing. +dVector3 point; +{ + dVector3 ca,cb; + dReal sign; + for (i=0; i<3; i++) ca[i] = p1[i]; + for (i=0; i<3; i++) cb[i] = p2[i]; +//find two nearest flat rings + sign = (pp1[1] > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) ca[i] += sign * hlz1 * R1[i*4+1]; + + sign = (pp2[1] > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) cb[i] -= sign * hlz2 * R2[i*4+1]; + + dVector3 tAx,tAx1; + circleIntersection(R1+1,ca,radius1,R2+1,cb,radius2,point); + + Ax[0]=point[0]-ca[0]; + Ax[1]=point[1]-ca[1]; + Ax[2]=point[2]-ca[2]; + + cos1 = dDOT14(Ax,R1+0); + cos3 = dDOT14(Ax,R1+2) ; + + tAx[0]=cos3*R1[0]-cos1*R1[2]; + tAx[1]=cos3*R1[4]-cos1*R1[6]; + tAx[2]=cos3*R1[8]-cos1*R1[10]; + + Ax[0]=point[0]-cb[0]; + Ax[1]=point[1]-cb[1]; + Ax[2]=point[2]-cb[2]; + + + cos1 = dDOT14(Ax,R2+0); + cos3 = dDOT14(Ax,R2+2) ; + + tAx1[0]=cos3*R2[0]-cos1*R2[2]; + tAx1[1]=cos3*R2[4]-cos1*R2[6]; + tAx1[2]=cos3*R2[8]-cos1*R2[10]; + dCROSS(Ax,=,tAx,tAx1); + + + + +dNormalize3(Ax); +dReal cyl1Pr,cyl2Pr; + + cos=dFabs(dDOT14(Ax,R1+1)); + cos1=dDOT14(Ax,R1+0); + cos3=dDOT14(Ax,R1+2); + sin=sqrtf(cos1*cos1+cos3*cos3); + cyl1Pr=cos*hlz1+sin*radius1; + + cos=dFabs(dDOT14(Ax,R2+1)); + cos1=dDOT14(Ax,R2+0); + cos3=dDOT14(Ax,R2+2); + sin=sqrtf(cos1*cos1+cos3*cos3); + cyl2Pr=cos*hlz2+sin*radius2; +TEST(p[0]*Ax[0]+p[1]*Ax[1]+p[2]*Ax[2],cyl1Pr+cyl2Pr,Ax[0],Ax[1],Ax[2],5); + + +} + + +#undef TEST + + + + // if we get to this point, the cylinders interpenetrate. compute the normal + // in global coordinates. + if (normalR) { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { + normal[0] =normalC[0];normal[1] = normalC[1];normal[2] = normalC[2]; + } + if (invert_normal) { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + + *depth = -s; + + // compute contact point(s) + + if(*code==3){ + for (i=0; i<3; i++) contact[0].pos[i] = pb[i]; + contact[0].depth = *depth; + return 1; + } + + if(*code==4){ + for (i=0; i<3; i++) contact[0].pos[i] = pa[i]; + contact[0].depth = *depth; + return 1; + } + + if(*code==5){ + for (i=0; i<3; i++) contact[0].pos[i] = point[i]; + contact[0].depth = *depth; + return 1; + } + +if (*code == 6) { + dVector3 pa; + dReal sign, cos1,cos3,factor; + + + for (i=0; i<3; i++) pa[i] = p1[i]; + + cos1 = dDOT14(normal,R1+0); + cos3 = dDOT14(normal,R1+2) ; + factor=sqrtf(cos1*cos1+cos3*cos3); + + cos1/=factor; + cos3/=factor; + + for (i=0; i<3; i++) pa[i] += cos1 * radius1 * R1[i*4]; + + sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pa[i] += sign * hlz1 * R1[i*4+1]; + + + for (i=0; i<3; i++) pa[i] += cos3 * radius1 * R1[i*4+2]; + + // find a point pb on the intersecting edge of cylinder 2 + dVector3 pb; + for (i=0; i<3; i++) pb[i] = p2[i]; + cos1 = dDOT14(normal,R2+0); + cos3 = dDOT14(normal,R2+2) ; + factor=sqrtf(cos1*cos1+cos3*cos3); + + cos1/=factor; + cos3/=factor; + + for (i=0; i<3; i++) pb[i] -= cos1 * radius2 * R2[i*4]; + + sign = (dDOT14(normal,R2+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pb[i] -= sign * hlz2 * R2[i*4+1]; + + + for (i=0; i<3; i++) pb[i] -= cos3 * radius2 * R2[i*4+2]; + + + dReal alpha,beta; + dVector3 ua,ub; + for (i=0; i<3; i++) ua[i] = R1[1 + i*4]; + for (i=0; i<3; i++) ub[i] = R2[1 + i*4]; + lineClosestApproach (pa,ua,pb,ub,&alpha,&beta); + for (i=0; i<3; i++) pa[i] += ua[i]*alpha; + for (i=0; i<3; i++) pb[i] += ub[i]*beta; + + for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]); + contact[0].depth = *depth; + return 1; + } + + // okay, we have a face-something intersection (because the separating + // axis is perpendicular to a face). + + // @@@ temporary: make deepest point on the "other" cylinder the contact point. + // @@@ this kind of works, but we need multiple contact points for stability, + // @@@ especially for face-face contact. + + dVector3 vertex; + if (*code == 0) { + // flat face from cylinder 1 touches a edge/face from cylinder 2. + dReal sign,cos1,cos3,factor; + for (i=0; i<3; i++) vertex[i] = p2[i]; + cos1 = dDOT14(normal,R2+0) ; + cos3 = dDOT14(normal,R2+2); + factor=sqrtf(cos1*cos1+cos3*cos3); + + cos1/=factor; + cos3/=factor; + for (i=0; i<3; i++) vertex[i] -= cos1 * radius2 * R2[i*4]; + + sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) vertex[i] -= sign * hlz2 * R2[i*4+1]; + + for (i=0; i<3; i++) vertex[i] -= cos3 * radius2 * R2[i*4+2]; + } + else { + // flat face from cylinder 2 touches a edge/face from cylinder 1. + dReal sign,cos1,cos3,factor; + for (i=0; i<3; i++) vertex[i] = p1[i]; + cos1 = dDOT14(normal,R1+0) ; + cos3 = dDOT14(normal,R1+2); + factor=sqrtf(cos1*cos1+cos3*cos3); + + cos1/=factor; + cos3/=factor; + for (i=0; i<3; i++) vertex[i] += cos1 * radius1 * R1[i*4]; + + sign = (dDOT14(normal,R1+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) vertex[i] += sign * hlz1 * R1[i*4+1]; + + for (i=0; i<3; i++) vertex[i] += cos3 * radius1 * R1[i*4+2]; + } + for (i=0; i<3; i++) contact[0].pos[i] = vertex[i]; + contact[0].depth = *depth; + return 1; +} + +//**************************************************************************** + + +int dCollideCylS (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + + + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (dGeomGetClass(o2) == dSphereClass); + dIASSERT (dGeomGetClass(o1) == dCylinderClassUser); + const dReal* p1=dGeomGetPosition(o1); + const dReal* p2=dGeomGetPosition(o2); + const dReal* R=dGeomGetRotation(o1); + dVector3 p,normalC,normal; + const dReal *normalR = 0; + dReal cylRadius; + dReal hl; + dGeomCylinderGetParams(o1,&cylRadius,&hl); + dReal sphereRadius; + sphereRadius=dGeomSphereGetRadius(o2); + + int i,invert_normal; + + // get vector from centers of cyl to shere + p[0] = p2[0] - p1[0]; + p[1] = p2[1] - p1[1]; + p[2] = p2[2] - p1[2]; + +dReal s,s2; +unsigned char code; +#define TEST(expr1,expr2,norm,cc) \ + s2 = dFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = norm; \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } + + s = -dInfinity; + invert_normal = 0; + code = 0; + + // separating axis cyl ax + + TEST (dDOT14(p,R+1),sphereRadius+hl,R+1,2); + // note: cross product axes need to be scaled when s is computed. + // normal (n1,n2,n3) is relative to +#undef TEST +#define TEST(expr1,expr2,n1,n2,n3,cc) \ + s2 = dFabs(expr1) - (expr2); \ + if (s2 > 0) return 0; \ + if (s2 > s) { \ + s = s2; \ + normalR = 0; \ + normalC[0] = (n1); normalC[1] = (n2); normalC[2] = (n3); \ + invert_normal = ((expr1) < 0); \ + code = (cc); \ + } + +//making ax which is perpendicular to cyl1 ax to sphere center// + +dReal proj,cos,sin,cos1,cos3; +dVector3 Ax; + proj=dDOT14(p2,R+1)-dDOT14(p1,R+1); + + Ax[0]=p2[0]-p1[0]-R[1]*proj; + Ax[1]=p2[1]-p1[1]-R[5]*proj; + Ax[2]=p2[2]-p1[2]-R[9]*proj; +dNormalize3(Ax); +TEST(dDOT(p,Ax),sphereRadius+cylRadius,Ax[0],Ax[1],Ax[2],9); + + +Ax[0]=p[0]; +Ax[1]=p[1]; +Ax[2]=p[2]; +dNormalize3(Ax); + + dVector3 pa; + dReal sign, factor; + for (i=0; i<3; i++) pa[i] = p1[i]; + + cos1 = dDOT14(Ax,R+0); + cos3 = dDOT14(Ax,R+2) ; + factor=sqrtf(cos1*cos1+cos3*cos3); + cos1/=factor; + cos3/=factor; + for (i=0; i<3; i++) pa[i] += cos1 * cylRadius * R[i*4]; + sign = (dDOT14(normal,R+1) > 0) ? REAL(1.0) : REAL(-1.0); + for (i=0; i<3; i++) pa[i] += sign * hl * R[i*4+1]; + for (i=0; i<3; i++) pa[i] += cos3 * cylRadius * R[i*4+2]; + +Ax[0]=p2[0]-pa[0]; +Ax[1]=p2[1]-pa[1]; +Ax[2]=p2[2]-pa[2]; +dNormalize3(Ax); + + cos=dFabs(dDOT14(Ax,R+1)); + cos1=dDOT14(Ax,R+0); + cos3=dDOT14(Ax,R+2); + sin=sqrtf(cos1*cos1+cos3*cos3); +TEST(dDOT(p,Ax),sphereRadius+cylRadius*sin+hl*cos,Ax[0],Ax[1],Ax[2],14); + + +#undef TEST + + if (normalR) { + normal[0] = normalR[0]; + normal[1] = normalR[4]; + normal[2] = normalR[8]; + } + else { + + normal[0] = normalC[0]; + normal[1] = normalC[1]; + normal[2] = normalC[2]; + } + if (invert_normal) { + normal[0] = -normal[0]; + normal[1] = -normal[1]; + normal[2] = -normal[2]; + } + // compute contact point(s) +contact->depth=-s; +contact->normal[0]=-normal[0]; +contact->normal[1]=-normal[1]; +contact->normal[2]=-normal[2]; +contact->g1=const_cast (o1); +contact->g2=const_cast (o2); +contact->pos[0]=p2[0]-normal[0]*sphereRadius; +contact->pos[1]=p2[1]-normal[1]*sphereRadius; +contact->pos[2]=p2[2]-normal[2]*sphereRadius; +return 1; +} + + + +int dCollideCylB (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dVector3 normal; + dReal depth; + int code; + dReal cylRadius,cylLength; + dVector3 boxSides; + dGeomCylinderGetParams(o1,&cylRadius,&cylLength); + dGeomBoxGetLengths(o2,boxSides); + int num = dCylBox(dGeomGetPosition(o1),dGeomGetRotation(o1),cylRadius,cylLength, + dGeomGetPosition(o2),dGeomGetRotation(o2),boxSides, + normal,&depth,&code,flags & NUMC_MASK,contact,skip); + for (int i=0; inormal[0] = -normal[0]; + CONTACT(contact,i*skip)->normal[1] = -normal[1]; + CONTACT(contact,i*skip)->normal[2] = -normal[2]; + CONTACT(contact,i*skip)->g1 = const_cast (o1); + CONTACT(contact,i*skip)->g2 = const_cast (o2); + } + return num; +} + +int dCollideCylCyl (dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) +{ + dVector3 normal; + dReal depth; + int code; +dReal cylRadius1,cylRadius2; +dReal cylLength1,cylLength2; +dGeomCylinderGetParams(o1,&cylRadius1,&cylLength1); +dGeomCylinderGetParams(o2,&cylRadius2,&cylLength2); +int num = dCylCyl (dGeomGetPosition(o1),dGeomGetRotation(o1),cylRadius1,cylLength1, + dGeomGetPosition(o2),dGeomGetRotation(o2),cylRadius2,cylLength2, + normal,&depth,&code,flags & NUMC_MASK,contact,skip); + + for (int i=0; inormal[0] = -normal[0]; + CONTACT(contact,i*skip)->normal[1] = -normal[1]; + CONTACT(contact,i*skip)->normal[2] = -normal[2]; + CONTACT(contact,i*skip)->g1 = const_cast (o1); + CONTACT(contact,i*skip)->g2 = const_cast (o2); + } + return num; +} + +struct dxPlane { + dReal p[4]; +}; + + +int dCollideCylPlane + ( + dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip){ + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (dGeomGetClass(o1) == dCylinderClassUser); + dIASSERT (dGeomGetClass(o2) == dPlaneClass); + contact->g1 = const_cast (o1); + contact->g2 = const_cast (o2); + + unsigned int ret = 0; + + dReal radius; + dReal hlz; + dGeomCylinderGetParams(o1,&radius,&hlz); + hlz /= 2; + + const dReal *R = dGeomGetRotation(o1);// rotation of cylinder + const dReal* p = dGeomGetPosition(o1); + dVector4 n; // normal vector + dReal pp; + dGeomPlaneGetParams (o2, n); + pp=n[3]; + dReal cos1,sin1; + cos1=dFabs(dDOT14(n,R+1)); + +cos1=cos10 ? hlz*R[1]:-hlz*R[1]; + pos[1]-= A2>0 ? hlz*R[5]:-hlz*R[5]; + pos[2]-= A2>0 ? hlz*R[9]:-hlz*R[9]; + + + + contact->pos[0] = pos[0]; + contact->pos[1] = pos[1]; + contact->pos[2] = pos[2]; + contact->depth = outDepth; + ret=1; + +if(dFabs(Q2)>M_SQRT1_2){ + + CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A1*R[0]; + CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A1*R[4]; + CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A1*R[8]; + CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q1*2.f*A1); + + if(CONTACT(contact,ret*skip)->depth>0.f) + ret++; + + + CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*A3*R[2]; + CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*A3*R[6]; + CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*A3*R[10]; + CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q3*2.f*A3); + + if(CONTACT(contact,ret*skip)->depth>0.f) ret++; +} else { + + CONTACT(contact,ret*skip)->pos[0]=pos[0]+2.f*(A2>0 ? hlz*R[1]:-hlz*R[1]); + CONTACT(contact,ret*skip)->pos[1]=pos[1]+2.f*(A2>0 ? hlz*R[5]:-hlz*R[5]); + CONTACT(contact,ret*skip)->pos[2]=pos[2]+2.f*(A2>0 ? hlz*R[9]:-hlz*R[9]); + CONTACT(contact,ret*skip)->depth=outDepth-dFabs(Q2*2.f*A2); + + if(CONTACT(contact,ret*skip)->depth>0.f) ret++; +} + + + + for (unsigned int i=0; ig1 = const_cast (o1); + CONTACT(contact,i*skip)->g2 = const_cast (o2); + CONTACT(contact,i*skip)->normal[0] =n[0]; + CONTACT(contact,i*skip)->normal[1] =n[1]; + CONTACT(contact,i*skip)->normal[2] =n[2]; + } + return ret; +} + +int dCollideCylRay(dxGeom *o1, dxGeom *o2, int flags, + dContactGeom *contact, int skip) { + dIASSERT (skip >= (int)sizeof(dContactGeom)); + dIASSERT (dGeomGetClass(o1) == dCylinderClassUser); + dIASSERT (dGeomGetClass(o2) == dRayClass); + contact->g1 = const_cast (o1); + contact->g2 = const_cast (o2); + dReal radius; + dReal lz; + dGeomCylinderGetParams(o1,&radius,&lz); + dReal lz2=lz*REAL(0.5); + const dReal *R = dGeomGetRotation(o1); // rotation of the cylinder + const dReal *p = dGeomGetPosition(o1); // position of the cylinder + dVector3 start,dir; + dGeomRayGet(o2,start,dir); // position and orientation of the ray + dReal length = dGeomRayGetLength(o2); + + // compute some useful info + dVector3 cs,q,r; + dReal C,k; + cs[0] = start[0] - p[0]; + cs[1] = start[1] - p[1]; + cs[2] = start[2] - p[2]; + k = dDOT41(R+1,cs); // position of ray start along cyl axis (Y) + q[0] = k*R[0*4+1] - cs[0]; + q[1] = k*R[1*4+1] - cs[1]; + q[2] = k*R[2*4+1] - cs[2]; + C = dDOT(q,q) - radius*radius; + // if C < 0 then ray start position within infinite extension of cylinder + // if ray start position is inside the cylinder + int inside_cyl=0; + if (C<0 && !(k<-lz2 || k>lz2)) inside_cyl=1; + // compute ray collision with infinite cylinder, except for the case where + // the ray is outside the cylinder but within the infinite cylinder + // (it that case the ray can only hit endcaps) + if (!inside_cyl && C < 0) { + // set k to cap position to check + if (k < 0) k = -lz2; else k = lz2; + } + else { + dReal uv = dDOT41(R+1,dir); + r[0] = uv*R[0*4+1] - dir[0]; + r[1] = uv*R[1*4+1] - dir[1]; + r[2] = uv*R[2*4+1] - dir[2]; + dReal A = dDOT(r,r); + dReal B = 2*dDOT(q,r); + k = B*B-4*A*C; + if (k < 0) { + // the ray does not intersect the infinite cylinder, but if the ray is + // inside and parallel to the cylinder axis it may intersect the end + // caps. set k to cap position to check. + if (!inside_cyl) return 0; + if (uv < 0) k = -lz2; else k = lz2; + } + else { + k = dSqrt(k); + A = dRecip (2*A); + dReal alpha = (-B-k)*A; + if (alpha < 0) { + alpha = (-B+k)*A; + if (alpha<0) return 0; + } + if (alpha>length) return 0; + // the ray intersects the infinite cylinder. check to see if the + // intersection point is between the caps + contact->pos[0] = start[0] + alpha*dir[0]; + contact->pos[1] = start[1] + alpha*dir[1]; + contact->pos[2] = start[2] + alpha*dir[2]; + q[0] = contact->pos[0] - p[0]; + q[1] = contact->pos[1] - p[1]; + q[2] = contact->pos[2] - p[2]; + k = dDOT14(q,R+1); + dReal nsign = inside_cyl ? -1 : 1; + if (k >= -lz2 && k <= lz2) { + contact->normal[0] = nsign * (contact->pos[0] - + (p[0] + k*R[0*4+1])); + contact->normal[1] = nsign * (contact->pos[1] - + (p[1] + k*R[1*4+1])); + contact->normal[2] = nsign * (contact->pos[2] - + (p[2] + k*R[2*4+1])); + dNormalize3 (contact->normal); + contact->depth = alpha; + return 1; + } + // the infinite cylinder intersection point is not between the caps. + // set k to cap position to check. + if (k < 0) k = -lz2; else k = lz2; + } + } + // check for ray intersection with the caps. k must indicate the cap + // position to check + // perform a ray plan interesection + // R+1 is the plan normal + q[0] = start[0] - (p[0] + k*R[0*4+1]); + q[1] = start[1] - (p[1] + k*R[1*4+1]); + q[2] = start[2] - (p[2] + k*R[2*4+1]); + dReal alpha = -dDOT14(q,R+1); + dReal k2 = dDOT14(dir,R+1); + if (k2==0) return 0; // ray parallel to the plane + alpha/=k2; + if (alpha<0 || alpha>length) return 0; // too short + contact->pos[0]=start[0]+alpha*dir[0]; + contact->pos[1]=start[1]+alpha*dir[1]; + contact->pos[2]=start[2]+alpha*dir[2]; + dReal nsign = (k<0)?-1:1; + contact->normal[0]=nsign*R[0*4+1]; + contact->normal[1]=nsign*R[1*4+1]; + contact->normal[2]=nsign*R[2*4+1]; + contact->depth=alpha; + return 1; +} + +static dColliderFn * dCylinderColliderFn (int num) +{ + if (num == dBoxClass) return (dColliderFn *) &dCollideCylB; + else if (num == dSphereClass) return (dColliderFn *) &dCollideCylS; + else if (num == dCylinderClassUser) return (dColliderFn *) &dCollideCylCyl; + else if (num == dPlaneClass) return (dColliderFn *) &dCollideCylPlane; + else if (num == dRayClass) return (dColliderFn *) &dCollideCylRay; + return 0; +} + + +static void dCylinderAABB (dxGeom *geom, dReal aabb[6]) +{ + dReal radius,lz; + dGeomCylinderGetParams(geom,&radius,&lz); +const dReal* R= dGeomGetRotation(geom); +const dReal* pos= dGeomGetPosition(geom); + dReal xrange = dFabs (R[0] *radius) + + REAL(0.5) *dFabs (R[1] * lz) + dFabs (R[2] * radius); + + dReal yrange = dFabs (R[4] *radius) + + REAL(0.5) * dFabs (R[5] * lz) + dFabs (R[6] * radius); + + dReal zrange = dFabs (R[8] * radius) + + REAL(0.5) *dFabs (R[9] * lz) + dFabs (R[10] * radius); + + aabb[0] = pos[0] - xrange; + aabb[1] = pos[0] + xrange; + aabb[2] = pos[1] - yrange; + aabb[3] = pos[1] + yrange; + aabb[4] = pos[2] - zrange; + aabb[5] = pos[2] + zrange; +} + +dxGeom *dCreateCylinder (dSpaceID space, dReal r, dReal lz) +{ + dAASSERT (r > 0 && lz > 0); + if (dCylinderClassUser == -1) + { + dGeomClass c; + c.bytes = sizeof (dxCylinder); + c.collider = &dCylinderColliderFn; + c.aabb = &dCylinderAABB; + c.aabb_test = 0; + c.dtor = 0; + dCylinderClassUser=dCreateGeomClass (&c); + + } + + dGeomID g = dCreateGeom (dCylinderClassUser); + if (space) dSpaceAdd (space,g); + dxCylinder *c = (dxCylinder*) dGeomGetClassData(g); + + c->radius = r; + c->lz = lz; + return g; +} + + + +void dGeomCylinderSetParams (dGeomID g, dReal radius, dReal length) +{ + dUASSERT (g && dGeomGetClass(g) == dCylinderClassUser,"argument not a cylinder"); + dAASSERT (radius > 0 && length > 0); + dxCylinder *c = (dxCylinder*) dGeomGetClassData(g); + c->radius = radius; + c->lz = length; +} + + + +void dGeomCylinderGetParams (dGeomID g, dReal *radius, dReal *length) +{ + dUASSERT (g && dGeomGetClass(g) == dCylinderClassUser ,"argument not a cylinder"); + dxCylinder *c = (dxCylinder*) dGeomGetClassData(g); + *radius = c->radius; + *length = c->lz; +} + +/* +void dMassSetCylinder (dMass *m, dReal density, + dReal radius, dReal length) +{ + dAASSERT (m); + dMassSetZero (m); + dReal M = length*M_PI*radius*radius*density; + m->mass = M; + m->_I(0,0) = M/REAL(4.0) * (ly*ly + lz*lz); + m->_I(1,1) = M/REAL(12.0) * (lx*lx + lz*lz); + m->_I(2,2) = M/REAL(4.0) * (lx*lx + ly*ly); + +# ifndef dNODEBUG + checkMass (m); +# endif +} +*/ diff --git a/libraries/ode-0.9/contrib/dCylinder/dCylinder.h b/libraries/ode-0.9/contrib/dCylinder/dCylinder.h new file mode 100644 index 0000000..06e3a0b --- /dev/null +++ b/libraries/ode-0.9/contrib/dCylinder/dCylinder.h @@ -0,0 +1,14 @@ + +#ifndef dCylinder_h +#define dCylinder_h + +struct dxCylinder; +extern int dCylinderClassUser; + + +dxGeom *dCreateCylinder (dSpaceID space, dReal r, dReal lz); +void dGeomCylinderSetParams (dGeomID g, dReal radius, dReal length); + +void dGeomCylinderGetParams (dGeomID g, dReal *radius, dReal *length); +#endif //dCylinder_h + diff --git a/libraries/ode-0.9/contrib/dCylinder/readme.txt b/libraries/ode-0.9/contrib/dCylinder/readme.txt new file mode 100644 index 0000000..facd13e --- /dev/null +++ b/libraries/ode-0.9/contrib/dCylinder/readme.txt @@ -0,0 +1,62 @@ +readme.txt + +WARNING: THIS IS NOT VERY RELIABLE CODE. IT HAS BUGS. YOUR + SUCCESS MAY VARY. CONTRIBUTIONS OF FIXES/REWRITES ARE + WELCOME. + +/////////////////////////////////////////////////////////////////////// + +Cylinder geometry class. + +New in this version: + +Cylinder class implemented as User Geometry Class so it now can be +used with old and new ODE collision detection. + +Cylinder - Ray has been contributed by Olivier Michel. + +THE IDENTIFIER dCylinderClass HAS BEEN REPLACED BY dCylinderClassUser + +to avoid conflict with dCylinderClass in the enum definite in collision.h + +/////////////////////////////////////////////////////////////////////// +The dCylinder class includes the following collisions: + +Cylinder - Box +Cylinder - Cylinder +Cylinder - Sphere +Cylinder - Plane +Cylinder - Ray (contributed by Olivier Michel) + +Cylinder aligned along axis - Y when created. (Not like Capped +Cylinder which aligned along axis - Z). + +Interface is just the same as Capped Cylinder has. + +Use functions which have one "C" instead of double "C". + +to create: +dGeomID dCreateCylinder (dSpaceID space, dReal radius, dReal length); + +to set params: +void dGeomCylinderSetParams (dGeomID cylinder, + dReal radius, dReal length); + + +to get params: +void dGeomCylinderGetParams (dGeomID cylinder, + dReal *radius, dReal *length); + +Return in radius and length the parameters of the given cylinder. + +Identification number of the class: + dCylinderClassUser + + I do not include a function that sets inertia tensor for cylinder. + One may use existing ODE functions dMassSetCappedCylinder or dMassSetBox. + To set exact tensor for cylinder use dMassSetParameters. + Remember cylinder aligned along axis - Y. + + /////////////////////////////////////////////////////////////////////////// + Konstantin Slipchenko + February 5, 2002 -- cgit v1.1