opende-svn Mailing List for Open Dynamics Engine

SourceForge Headquarters 225 Broadway Suite 1600 San Diego, CA 92101 +1 (858) 454-5900

Revision: 1929
          http://opende.svn.sourceforge.net/opende/?rev=1929&view=rev
Author:   bram
Date:     2013-03-06 05:09:43 +0000 (Wed, 06 Mar 2013)
Log Message:
-----------
Applied JC's patch for bug #87 cyl-ray collisions missing.

Modified Paths:
--------------
    trunk/ode/demo/demo_collision.cpp
    trunk/ode/src/ray.cpp

Modified: trunk/ode/demo/demo_collision.cpp
===================================================================

--- trunk/ode/demo/demo_collision.cpp	2013-03-03 17:08:44 UTC (rev 1928)
+++ trunk/ode/demo/demo_collision.cpp	2013-03-06 05:09:43 UTC (rev 1929)
@@ -44,6 +44,7 @@
 #define dsDrawBox dsDrawBoxD
 #define dsDrawLine dsDrawLineD
 #define dsDrawCapsule dsDrawCapsuleD
+#define dsDrawCylinder dsDrawCylinderD
 #endif
 
 //****************************************************************************
@@ -190,6 +191,13 @@
       dsDrawCapsule (pos,dGeomGetRotation(g),length,radius);
       break;
     }
+    case dCylinderClass: {
+      dsSetColorAlpha (0,1,0,0.8);
+      dReal radius,length;
+      dGeomCylinderGetParams (g,&radius,&length);
+      dsDrawCylinder (pos,dGeomGetRotation(g),length,radius);
+      break;
+    }
 
     case dPlaneClass: {
       dVector4 n;
@@ -846,7 +854,69 @@
   PASSED();
 }
 
+/*
+  Test rays within the cylinder
+  -completely inside
+  -exiting through side
+  -exiting through cap
+  -exiting through corner
+  Test rays outside the cylinder
+*/
+int test_ray_and_cylinder()
+{
+  int j;
+  dContactGeom contact;
+  dVector3 p,a,b,n;
+  dMatrix3 R;
+  dReal r,l,k,x,y;
+  const int positions = 5;
+  const int slices = 13;
+  const int pitch = 11;
 
+  dSimpleSpace space(0);
+  dGeomID ray = dCreateRay(space,4);
+  dGeomID cyl = dCreateCylinder(space,0.5,1);
+
+  // The first thing that happens is the ray is
+  // rotated into cylinder coordinates.  We'll trust that's
+  // done right.  The major axis is in the z-dir.
+
+
+  // Random tests
+  /*b[0]=4*dRandReal()-2;
+  b[1]=4*dRandReal()-2;
+  b[2]=4*dRandReal()-2;
+  a[0]=2*dRandReal()-1;
+  a[1]=2*dRandReal()-1;
+  a[2]=2*dRandReal()-1;*/
+  
+  // Inside out
+  b[0]=dRandReal()-0.5;
+  b[1]=dRandReal()-0.5;
+  b[2]=dRandReal()-0.5;
+  a[0]=2*dRandReal()-1;
+  a[1]=2*dRandReal()-1;
+  a[2]=2*dRandReal()-1;
+
+  // Outside in
+  /*b[0]=4*dRandReal()-2;
+  b[1]=4*dRandReal()-2;
+  b[2]=4*dRandReal()-2;
+  a[0]=-b[0];
+  a[1]=-b[1];
+  a[2]=-b[2];*/
+
+  
+  dGeomRaySet (ray,b[0],b[1],b[2],a[0],a[1],a[2]);
+  // This is just for visual inspection right now.
+  //if (dCollide (ray,cyl,1,&contact,sizeof(dContactGeom)) != 1) FAILED();
+
+  draw_all_objects (space);
+
+  PASSED();
+}
+
+
 int test_ray_and_plane()
 {
   int j;
@@ -1364,6 +1434,7 @@
   MAKE_TEST(11,test_ray_and_box);
   MAKE_TEST(12,test_ray_and_ccylinder);
   MAKE_TEST(13,test_ray_and_plane);
+  MAKE_TEST(14,test_ray_and_cylinder);
 
   MAKE_TEST(100,test_dBoxTouchesBox);
   MAKE_TEST(101,test_dBoxBox);

Modified: trunk/ode/src/ray.cpp
===================================================================
--- trunk/ode/src/ray.cpp	2013-03-03 17:08:44 UTC (rev 1928)
+++ trunk/ode/src/ray.cpp	2013-03-06 05:09:43 UTC (rev 1929)
@@ -496,7 +496,7 @@
     return 1;
 }
 
-// Ray - Cylinder collider by David Walters (June 2006)
+// Ray-Cylinder collider by Joseph Cooper (2011)
 int dCollideRayCylinder( dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip )
 {
     dIASSERT( skip >= (int)sizeof( dContactGeom ) );
@@ -515,184 +515,182 @@
 
     const dReal half_length = cyl->lz * REAL( 0.5 );
 
-    //
-    // Compute some useful info
-    //
 
-    dVector3 q, r;
-    dReal d, C, k;
-
-    // Vector 'r', line segment from C to R (ray start) ( r = R - C )
-    r[ 0 ] = ray->final_posr->pos[0] - cyl->final_posr->pos[0];
-    r[ 1 ] = ray->final_posr->pos[1] - cyl->final_posr->pos[1];
-    r[ 2 ] = ray->final_posr->pos[2] - cyl->final_posr->pos[2];
-
-    // Distance that ray start is along cyl axis ( Z-axis direction )
-    d = dCalcVectorDot3_41( cyl->final_posr->R + 2, r );
-
+    // Possible collision cases:
+    //  Ray origin between/outside caps
+    //  Ray origin within/outside radius
+    //  Ray direction left/right/perpendicular
+    //  Ray direction parallel/perpendicular/other
+    // 
+    //  Ray origin cases (ignoring origin on surface)
     //
-    // Compute vector 'q' representing the shortest line from R to the cylinder z-axis (Cz).
+    //  A          B
+    //     /-\-----------\
+    //  C (   )    D      )
+    //     \_/___________/
     //
-    // Point on axis ( in world space ):	cp = ( d * Cz ) + C
-    //
-    // Line 'q' from R to cp:				q = cp - R
-    //										q = ( d * Cz ) + C - R
-    //										q = ( d * Cz ) - ( R - C )
+    //  Cases A and D can collide with caps or cylinder
+    //  Case C can only collide with the caps
+    //  Case B can only collide with the cylinder
+    //  Case D will produce inverted normals
+    //  If the ray is perpendicular, only check the cylinder
+    //  If the ray is parallel to cylinder axis,
+    //  we can only check caps
+    //  If the ray points right,
+    //    Case A,C Check left cap
+    //    Case  D  Check right cap
+    //  If the ray points left
+    //    Case A,C Check right cap
+    //    Case  D  Check left cap
+    //  Case B, check only first possible cylinder collision
+    //  Case D, check only second possible cylinder collision
 
-    q[ 0 ] = ( d * cyl->final_posr->R[0*4+2] ) - r[ 0 ];
-    q[ 1 ] = ( d * cyl->final_posr->R[1*4+2] ) - r[ 1 ];
-    q[ 2 ] = ( d * cyl->final_posr->R[2*4+2] ) - r[ 2 ];
+    // Find the ray in the cylinder coordinate frame:
+    dVector3 tmp;
+    dVector3 pos;  // Ray origin in cylinder frame
+    dVector3 dir;  // Ray direction in cylinder frame
+    // Translate ray start by inverse cyl
+    dSubtractVectors3(tmp,ray->final_posr->pos,cyl->final_posr->pos);
+    // Rotate ray start by inverse cyl
+    dMultiply1_331(pos,cyl->final_posr->R,tmp);
 
+    // Get the ray's direction
+    tmp[0] = ray->final_posr->R[2];
+    tmp[1] = ray->final_posr->R[6];
+    tmp[2] = ray->final_posr->R[10];
+    // Rotate the ray direction by inverse cyl
+    dMultiply1_331(dir,cyl->final_posr->R,tmp); 
 
-    // Compute square length of 'q'. Subtract from radius squared to
-    // get square distance 'C' between the line q and the radius.
+    // Is the ray origin inside of the (extended) cylinder?
+    dReal r2 = cyl->radius*cyl->radius;
+    dReal C = pos[0]*pos[0] + pos[1]*pos[1] - r2;
 
-    // if C < 0 then ray start position is within infinite extension of cylinder
+    // Find the different cases
+    // Is ray parallel to the cylinder length?
+    int parallel = (dir[0]==0 && dir[1]==0);
+    // Is ray perpendicular to the cylinder length?
+    int perpendicular = (dir[2]==0);
+    // Is ray origin within the radius of the caps?
+    int inRadius = (C<=0);
+    // Is ray origin between the top and bottom caps?
+    int inCaps   = (dFabs(pos[2])<=half_length);
 
-    C = dCalcVectorDot3( q, q ) - ( cyl->radius * cyl->radius );
+    int checkCaps = (!perpendicular && (!inCaps || inRadius));
+    int checkCyl  = (!parallel && (!inRadius || inCaps));
+    int flipNormals = (inCaps&&inRadius);
 
-    // Compute the projection of ray direction normal onto cylinder direction normal.
-    dReal uv = dCalcVectorDot3_44( cyl->final_posr->R+2, ray->final_posr->R+2 );
+    dReal tt=-dInfinity; // Depth to intersection
+    dVector3 tmpNorm;
 
-
-
-    //
-    // Find ray collision with infinite cylinder
-    //
-
-    // Compute vector from end of ray direction normal to projection on cylinder direction normal.
-    r[ 0 ] = ( uv * cyl->final_posr->R[0*4+2] ) - ray->final_posr->R[0*4+2];
-    r[ 1 ] = ( uv * cyl->final_posr->R[1*4+2] ) - ray->final_posr->R[1*4+2];
-    r[ 2 ] = ( uv * cyl->final_posr->R[2*4+2] ) - ray->final_posr->R[2*4+2];
-
-
-    // Quadratic Formula Magic
-    // Compute discriminant 'k':
-
-    // k < 0 : No intersection
-    // k = 0 : Tangent
-    // k > 0 : Intersection
-
-    dReal A = dCalcVectorDot3( r, r );
-    dReal B = 2 * dCalcVectorDot3( q, r );
-
-    k = B*B - 4*A*C;
-
-
-
-
-    //
-    // Collision with Flat Caps ?
-    //
-
-    // No collision with cylinder edge. ( Use epsilon here or we miss some obvious cases )
-    if ( k < dEpsilon && C <= 0 )
-    {
-        // The ray does not intersect the edge of the infinite cylinder,
-        // but the ray start is inside and so must run parallel to the axis.
-        // It may yet intersect an end cap. The following cases are valid:
-
-        //        -ve-cap , -half              centre               +half , +ve-cap
-        //  <<================|-------------------|------------->>>---|================>>
-        //                    |                                       |
-        //                    |                              d------------------->    1.
-        //   2.    d------------------>                               |
-        //   3.    <------------------d                               |
-        //                    |                              <-------------------d    4.
-        //                    |                                       |
-        //  <<================|-------------------|------------->>>---|===============>>
-
-        // Negative if the ray and cylinder axes point in opposite directions.
-        const dReal uvsign = ( uv < 0 ) ? REAL( -1.0 ) : REAL( 1.0 );
-
-        // Negative if the ray start is inside the cylinder
-        const dReal internal = ( d >= -half_length && d <= +half_length ) ? REAL( -1.0 ) : REAL( 1.0 );
-
-        // Ray and Cylinder axes run in the same direction ( cases 1, 2 )
-        // Ray and Cylinder axes run in opposite directions ( cases 3, 4 )
-        if ( ( ( uv > 0 ) && ( d + ( uvsign * ray->length ) < half_length * internal ) ) ||
-            ( ( uv < 0 ) && ( d + ( uvsign * ray->length ) > half_length * internal ) ) )
-        {
-            return 0; // No intersection with caps or curved surface.
+    if (checkCaps) {
+        // Make it so we only need to check one cap
+        int flipDir = 0;
+        // Wish c had logical xor...
+        if ((dir[2]<0 && flipNormals) || (dir[2]>0 && !flipNormals)) {
+            flipDir = 1;
+            dir[2]=-dir[2];
+            pos[2]=-pos[2];
         }
-
-        // Compute depth (distance from ray to cylinder)
-        contact->depth = ( ( -uvsign * d ) - ( internal * half_length ) );
-
-        // Compute contact point.
-        contact->pos[0] = ray->final_posr->pos[0] + ( contact->depth * ray->final_posr->R[0*4+2] );
-        contact->pos[1] = ray->final_posr->pos[1] + ( contact->depth * ray->final_posr->R[1*4+2] );
-        contact->pos[2] = ray->final_posr->pos[2] + ( contact->depth * ray->final_posr->R[2*4+2] );
-
-        // Compute reflected contact normal.
-        contact->normal[0] = uvsign * ( cyl->final_posr->R[0*4+2] );
-        contact->normal[1] = uvsign * ( cyl->final_posr->R[1*4+2] );
-        contact->normal[2] = uvsign * ( cyl->final_posr->R[2*4+2] );
-
-        // Contact!
-        return 1;
+        // The cap is half the cylinder's length
+        // from the cylinder's origin
+        // We only checkCaps if dir[2]!=0
+        tt = (half_length-pos[2])/dir[2];
+        if (tt>=0 && tt<=ray->length) {
+            tmp[0] = pos[0] + tt*dir[0];
+            tmp[1] = pos[1] + tt*dir[1];
+            // Ensure collision point is within cap circle
+            if (tmp[0]*tmp[0] + tmp[1]*tmp[1] <= r2) {
+                // Successful collision
+                tmp[2] = (flipDir)?-half_length:half_length;
+                tmpNorm[0]=0;
+                tmpNorm[1]=0;
+                tmpNorm[2]=(flipDir!=flipNormals)?-1:1;
+                checkCyl = 0;  // Short circuit cylinder check
+            } else {
+                // Ray hits cap plane outside of cap circle
+                tt=-dInfinity; // No collision yet
+            }
+        } else {
+            // The cap plane is beyond (or behind) the ray length
+            tt=-dInfinity; // No collision yet
+        }
+        if (flipDir) {
+            // Flip back
+            dir[2]=-dir[2];
+            pos[2]=-pos[2];
+        }
     }
+    if (checkCyl) {
+        // Compute quadratic formula for parametric ray equation
+        dReal A =    dir[0]*dir[0] + dir[1]*dir[1];
+        dReal B = 2*(pos[0]*dir[0] + pos[1]*dir[1]);
+        // Already computed C
 
-
-
-    //
-    // Collision with Curved Edge ?
-    //
-
-    if ( k > 0 )
-    {
-        // Finish off quadratic formula to get intersection co-efficient
-        k = dSqrt( k );
-        A = dRecip( 2 * A );
-
-        // Compute distance along line to contact point.
-        dReal alpha = ( -B - k ) * A;
-        if ( alpha < 0 )
-        {
-            // Flip in the other direction.
-            alpha = ( -B + k ) * A;
-        }
-
-        // Intersection point is within ray length?
-        if ( alpha >= 0 && alpha <= ray->length )
-        {
-            // The ray intersects the infinite cylinder!
-
-            // Compute contact point.
-            contact->pos[0] = ray->final_posr->pos[0] + ( alpha * ray->final_posr->R[0*4+2] );
-            contact->pos[1] = ray->final_posr->pos[1] + ( alpha * ray->final_posr->R[1*4+2] );
-            contact->pos[2] = ray->final_posr->pos[2] + ( alpha * ray->final_posr->R[2*4+2] );
-
-            // q is the vector from the cylinder centre to the contact point.
-            q[0] = contact->pos[0] - cyl->final_posr->pos[0];
-            q[1] = contact->pos[1] - cyl->final_posr->pos[1];
-            q[2] = contact->pos[2] - cyl->final_posr->pos[2];
-
-            // Compute the distance along the cylinder axis of this contact point.
-            d = dCalcVectorDot3_14( q, cyl->final_posr->R+2 );
-
-            // Check to see if the intersection point is between the flat end caps
-            if ( d >= -half_length && d <= +half_length )
-            {
-                // Flip the normal if the start point is inside the cylinder.
-                const dReal nsign = ( C < 0 ) ? REAL( -1.0 ) : REAL( 1.0 );
-
-                // Compute contact normal.
-                contact->normal[0] = nsign * (contact->pos[0] - (cyl->final_posr->pos[0] + d*cyl->final_posr->R[0*4+2]));
-                contact->normal[1] = nsign * (contact->pos[1] - (cyl->final_posr->pos[1] + d*cyl->final_posr->R[1*4+2]));
-                contact->normal[2] = nsign * (contact->pos[2] - (cyl->final_posr->pos[2] + d*cyl->final_posr->R[2*4+2]));
-                dNormalize3( contact->normal );
-
-                // Store depth.
-                contact->depth = alpha;
-
-                // Contact!
-                return 1;
+        dReal k = B*B - 4*A*C;
+        // Check collision with infinite cylinder
+        // k<0 means the ray passes outside the cylinder
+        // k==0 means ray is tangent to cylinder (or parallel)
+        //
+        //  Our quadratic formula: tt = (-B +- sqrt(k))/(2*A)   
+        // 
+        // A must be positive (otherwise we wouldn't be checking
+        // cylinder because ray is parallel)
+        //    if (k<0) ray doesn't collide with sphere
+        //    if (B > sqrt(k)) then both times are negative
+        //         -- don't calculate
+        //    if (B<-sqrt(k)) then both times are positive (Case A or B)
+        //         -- only calculate first, if first isn't valid
+        //         -- second can't be without first going through a cap
+        //    otherwise (fabs(B)<=sqrt(k)) then C<=0 (ray-origin inside/on cylinder)
+        //         -- only calculate second collision
+        if (k>=0 && (B<0 || B*B<=k)) {
+            k = dSqrt(k); 
+            A = dRecip(2*A);
+            if (dFabs(B)<=k) {
+                tt = (-B + k)*A; // Second solution
+                // If ray origin is on surface and pointed out, we
+                // can get a tt=0 solution...
+            } else {
+                tt = (-B - k)*A; // First solution
             }
+            if (tt<=ray->length) {
+                tmp[2] = pos[2] + tt*dir[2];
+                if (dFabs(tmp[2])<=half_length) {
+                    // Valid solution
+                    tmp[0] = pos[0] + tt*dir[0];
+                    tmp[1] = pos[1] + tt*dir[1];
+                    tmpNorm[0] = tmp[0]/cyl->radius;
+                    tmpNorm[1] = tmp[1]/cyl->radius;
+                    tmpNorm[2] = 0;
+                    if (flipNormals) {
+                        // Ray origin was inside cylinder
+                        tmpNorm[0] = -tmpNorm[0];
+                        tmpNorm[1] = -tmpNorm[1];
+                    }
+                } else {
+                    // Ray hits cylinder outside of caps
+                    tt=-dInfinity;
+                }
+            } else {
+                // Ray doesn't reach the cylinder
+                tt=-dInfinity;
+            }
         }
     }
 
+    if (tt>0) {
+        contact->depth = tt;
+        // Transform the point back to world coordinates
+        tmpNorm[3]=0;
+        tmp[3] = 0;
+        dMultiply0_331(contact->normal,cyl->final_posr->R,tmpNorm);
+        dMultiply0_331(contact->pos,cyl->final_posr->R,tmp);
+        contact->pos[0]+=cyl->final_posr->pos[0];
+        contact->pos[1]+=cyl->final_posr->pos[1];
+        contact->pos[2]+=cyl->final_posr->pos[2];
+
+        return 1;
+    }
     // No contact with anything.
     return 0;
 }
-

This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.





2006	Jan	Feb	Mar (18)	Apr (29)	May (32)	Jun (17)	Jul (34)	Aug (12)	Sep (39)	Oct (9)	Nov (20)	Dec (1)
2007	Jan (13)	Feb (9)	Mar (1)	Apr (17)	May (5)	Jun (1)	Jul	Aug (9)	Sep (65)	Oct (22)	Nov (23)	Dec (28)
2008	Jan (18)	Feb (4)	Mar (69)	Apr (33)	May (11)	Jun (39)	Jul (22)	Aug (13)	Sep (1)	Oct (43)	Nov (5)	Dec (13)
2009	Jan (31)	Feb (12)	Mar (6)	Apr (6)	May (9)	Jun (9)	Jul (1)	Aug (5)	Sep (5)	Oct (11)	Nov (4)	Dec (6)
2010	Jan (8)	Feb (3)	Mar (1)	Apr	May (2)	Jun (1)	Jul (5)	Aug (17)	Sep (6)	Oct (5)	Nov (6)	Dec (5)
2011	Jan (13)	Feb (2)	Mar (4)	Apr (12)	May (16)	Jun (4)	Jul (3)	Aug	Sep (4)	Oct (8)	Nov (5)	Dec (12)
2012	Jan (2)	Feb (8)	Mar (13)	Apr (6)	May (4)	Jun (3)	Jul (9)	Aug (1)	Sep	Oct	Nov (3)	Dec (16)
2013	Jan (2)	Feb (2)	Mar (2)	Apr	May	Jun	Jul	Aug	Sep	Oct	Nov	Dec

opende-svn Mailing List for Open Dynamics Engine

opende-svn — Open Dynamics Engine Subversion commit notifications