Menu
▾
▴
[Opende-svn] SF.net SVN: opende:[1929] trunk/ode
|
From: <br...@us...> - 2013-03-06 05:09:51
|
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.
|