From fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:07 +0000 Subject: dont ask --- libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp | 1445 --------------------- 1 file changed, 1445 deletions(-) delete mode 100644 libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp (limited to 'libraries/ode-0.9/contrib/dCylinder/dCylinder.cpp') 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 @@ -#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 -} -*/ -- cgit v1.1