one more for the gipper

1 files changed, 1445 insertions, 0 deletions

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 <ode/ode.h>
+#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<l1_4)
+        if(l2_3<l2_4)
+                if(l1_3<l2_3)
+                        {
+                        //l1_3;
+                        point[0]=0.5f*(O1[0]+O3[0]);
+                        point[1]=0.5f*(O1[1]+O3[1]);
+                        point[2]=0.5f*(O1[2]+O3[2]);
+                        }
+                else{
+                        //l2_3;
+                        point[0]=0.5f*(O2[0]+O3[0]);
+                        point[1]=0.5f*(O2[1]+O3[1]);
+                        point[2]=0.5f*(O2[2]+O3[2]);
+                        }
+        else
+                if(l1_3<l2_4)
+                        {
+                        //l1_3;
+                        point[0]=0.5f*(O1[0]+O3[0]);
+                        point[1]=0.5f*(O1[1]+O3[1]);
+                        point[2]=0.5f*(O1[2]+O3[2]);
+                        }
+                else{
+                        //l2_4;
+                        point[0]=0.5f*(O2[0]+O4[0]);
+                        point[1]=0.5f*(O2[1]+O4[1]);
+                        point[2]=0.5f*(O2[2]+O4[2]);
+                        }
+
+else
+        if(l2_3<l2_4)
+                if(l1_4<l2_3)
+                        {
+                        //l1_4;
+                        point[0]=0.5f*(O1[0]+O4[0]);
+                        point[1]=0.5f*(O1[1]+O4[1]);
+                        point[2]=0.5f*(O1[2]+O4[2]);
+                        }
+                else{
+                        //l2_3;
+                        point[0]=0.5f*(O2[0]+O3[0]);
+                        point[1]=0.5f*(O2[1]+O3[1]);
+                        point[2]=0.5f*(O2[2]+O3[2]);
+                        }
+        else
+                if(l1_4<l2_4)
+                        {
+                        //l1_4;
+                        point[0]=0.5f*(O1[0]+O4[0]);
+                        point[1]=0.5f*(O1[1]+O4[1]);
+                        point[2]=0.5f*(O1[2]+O4[2]);
+                        }
+                else{
+                        //l2_4;
+                        point[0]=0.5f*(O2[0]+O4[0]);
+                        point[1]=0.5f*(O2[1]+O4[1]);
+                        point[2]=0.5f*(O2[2]+O4[2]);
+                        }
+
+return true;
+}
+
+
+
+
+void lineClosestApproach (const dVector3 pa, const dVector3 ua,
+                                 const dVector3 pb, const dVector3 ub,
+                                 dReal *alpha, dReal *beta)
+{
+  dVector3 p;
+  p[0] = pb[0] - pa[0];
+  p[1] = pb[1] - pa[1];
+  p[2] = pb[2] - pa[2];
+  dReal uaub = dDOT(ua,ub);
+  dReal q1 =  dDOT(ua,p);
+  dReal q2 = -dDOT(ub,p);
+  dReal d = 1-uaub*uaub;
+  if (d <= 0) {
+    // @@@ this needs to be made more robust
+    *alpha = 0;
+    *beta  = 0;
+  }
+  else {
+    d = dRecip(d);
+    *alpha = (q1 + uaub*q2)*d;
+    *beta  = (uaub*q1 + q2)*d;
+  }
+}
+
+
+// @@@ some stuff to optimize here, reuse code in contact point calculations.
+
+extern "C" int dCylBox (const dVector3 p1, const dMatrix3 R1,
+                        const dReal radius,const dReal lz, const dVector3 p2,
+                        const dMatrix3 R2, const dVector3 side2,
+                        dVector3 normal, dReal *depth, int *code,
+                        int maxc, dContactGeom *contact, int skip)
+{
+  dVector3 p,pp,normalC;
+  const dReal *normalR = 0;
+  dReal B1,B2,B3,R11,R12,R13,R21,R22,R23,R31,R32,R33,
+    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,sQ21,sQ22,sQ23;
+  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 (pp,R1,p);             // get pp = p relative to body 1
+
+  // get side lengths / 2
+  //A1 =radius; A2 = lz*REAL(0.5); A3 = radius;
+  dReal hlz=lz/2.f;
+  B1 = side2[0]*REAL(0.5); B2 = side2[1]*REAL(0.5); B3 = side2[2]*REAL(0.5);
+
+  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
+  R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
+  R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
+  R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);
+
+  Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
+  Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
+  Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);
+
+  
+  //   * see if the axis separates the box with cylinder. if so, return 0.
+  //   * find the depth of the penetration along the separating axis (s2)
+  //   * if this is the largest depth so far, record it.
+  // the normal vector will be set to the separating axis with the smallest
+  // depth. note: normalR is set to point to a column of R1 or R2 if that is
+  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
+  // set to a vector relative to body 1. invert_normal is 1 if the sign of
+  // the normal should be flipped.
+
+#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 = 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<dxGeom*> (o1);
+contact->g2=const_cast<dxGeom*> (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; i<num; i++) {
+    CONTACT(contact,i*skip)->normal[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<dxGeom*> (o1);
+    CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (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; i<num; i++) {
+    CONTACT(contact,i*skip)->normal[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<dxGeom*> (o1);
+    CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (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<dxGeom*> (o1);
+  contact->g2 = const_cast<dxGeom*> (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=cos1<REAL(1.) ? cos1 : REAL(1.); //cos1 may slightly exeed 1.f
+sin1=sqrtf(REAL(1.)-cos1*cos1);
+//////////////////////////////
+
+dReal sidePr=cos1*hlz+sin1*radius;
+
+dReal dist=-pp+dDOT(n,p);
+dReal outDepth=sidePr-dist;
+
+if(outDepth<0.f) return 0;
+
+dVector3 pos;
+
+
+/////////////////////////////////////////// from geom.cpp dCollideBP
+  dReal Q1 = dDOT14(n,R+0);
+  dReal Q2 = dDOT14(n,R+1);
+  dReal Q3 = dDOT14(n,R+2);
+  dReal factor =sqrtf(Q1*Q1+Q3*Q3);
+  factor= factor ? factor :1.f;
+  dReal A1 = radius *           Q1/factor;
+  dReal A2 = hlz*Q2;
+  dReal A3 = radius *           Q3/factor;
+
+  pos[0]=p[0];
+  pos[1]=p[1];
+  pos[2]=p[2];
+
+  pos[0]-= A1*R[0];
+  pos[1]-= A1*R[4];
+  pos[2]-= A1*R[8];
+
+  pos[0]-= A3*R[2];
+  pos[1]-= A3*R[6];
+  pos[2]-= A3*R[10];
+
+  pos[0]-= A2>0 ? 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; i<ret; i++) {
+    CONTACT(contact,i*skip)->g1 = const_cast<dxGeom*> (o1);
+    CONTACT(contact,i*skip)->g2 = const_cast<dxGeom*> (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<dxGeom*> (o1);
+  contact->g2 = const_cast<dxGeom*> (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
+}
+*/