Permalink
Browse files

Fix ray vs convex collisions

Fix issues in ray vs convex collider and port fix for collider not considering
the position of the convex geometry from upstream.
See https://bitbucket.org/odedevs/ode/commits/abe81189b2038427b116458c961b491ecc836e2f?at=default
  • Loading branch information...
Piotr Piastucki
Piotr Piastucki committed Nov 6, 2018
1 parent 7bf670d commit 0f8d40477eda9908089a35bdaed62889fbe0a903
@@ -708,34 +708,30 @@ public static void dCROSSMAT(DMatrix3 A, DVector3C a, int skip, int plus, int mi
* special case matrix multipication, with operator selection
*/
//#define dMULTIPLYOP0_331(A,op,B,C) \
//do { \
// (A)[0] op dDOT((B),(C)); \
// (A)[1] op dDOT((B+4),(C)); \
// (A)[2] op dDOT((B+8),(C)); \
//} while(0)
//#define dMULTIPLYOP1_331(A,op,B,C) \
//do { \
// (A)[0] op dDOT41((B),(C)); \
// (A)[1] op dDOT41((B+1),(C)); \
// (A)[2] op dDOT41((B+2),(C)); \
//} while(0)
//#define dMULTIPLYOP0_133(A,op,B,C) \
//do { \
// (A)[0] op dDOT14((B),(C)); \
// (A)[1] op dDOT14((B),(C+1)); \
// (A)[2] op dDOT14((B),(C+2)); \
//} while(0)
private static void dMULTIPLYOP0_331(DVector3 A, DMatrix3C B, DVector3C C) {
//DMatrix3 B = (DMatrix3) B2;
// A.set0( dDOT(B.v, 0, C) );
// A.set1( dDOT(B.v, 4, C) );
// A.set2( dDOT(B.v, 8, C) );
A.set0( B.dotRow(0, C) );
A.set1( B.dotRow(1, C) );
A.set2( B.dotRow(2, C) );
}
//TZ
private static void dMultiplyHelper0_331(DVector3 res, DMatrix3C a, DVector3C b) {
double res_0 = a.dotRow(0, b);
double res_1 = a.dotRow(1, b);
double res_2 = a.dotRow(2, b);
/* Only assign after all the calculations are over to avoid incurring memory aliasing*/
res.set(res_0, res_1, res_2);
}
private static void dMultiplyHelper1_331(DVector3 res, DMatrix3C a, DVector3C b) {
double res_0 = a.dotCol(0, b);
double res_1 = a.dotCol(1, b);
double res_2 = a.dotCol(2, b);
/* Only assign after all the calculations are over to avoid incurring memory aliasing*/
res.set(res_0, res_1, res_2);
}
private static void dMultiplyHelper0_133(DVector3 res, DVector3C a, DMatrix3C b) {
double res_0 = a.dotCol(b, 0);
double res_1 = a.dotCol(b, 1);
double res_2 = a.dotCol(b, 2);
/* Only assign after all the calculations are over to avoid incurring memory aliasing*/
res.set(res_0, res_1, res_2);
}
private static void dMULTIPLYOP0_331(DMatrix3 A, DMatrix3C B, DVector3C C) {
//DMatrix3 B = (DMatrix3) B2;
A.set00( B.dotRow(0, C) );
@@ -768,23 +764,6 @@ private static void dMULTIPLYOP0_331(double[] A, int a, double[] B, int b,
A[1+a] = dCalcVectorDot3(B, 4+b, C);
A[2+a] = dCalcVectorDot3(B, 8+b, C);
}
private static void dMULTIPLYOP1_331(DVector3 A, DMatrix3C B, DVector3C C) {
// A.set0( dDOT41(B.v, 0, C) );
// A.set1( dDOT41(B.v, 1, C) );
// A.set2( dDOT41(B.v, 2, C) );
A.set0( B.dotCol(0, C) );
A.set1( B.dotCol(1, C) );
A.set2( B.dotCol(2, C) );
}
private static void dMULTIPLYOP0_133(DVector3 A, DVector3C B, DMatrix3C C) {
// A.set0( dDOT14(B, C.v,0) );
// A.set1( dDOT14(B, C.v,1) );
// A.set2( dDOT14(B, C.v,2) );
A.set0( B.dotCol(C, 0) );
A.set1( B.dotCol(C, 1) );
A.set2( B.dotCol(C, 2) );
}
private static void dMULTIPLYOP0_333(double[] A, int a, DMatrix3C B, DMatrix3C C) {
A[0+a] = B.dotRowCol(0, C, 0);//dDOT14(B.v,0,C.v,0);
@@ -882,8 +861,8 @@ private static void dMULTIPLYOP2_333(DMatrix3 A, DMatrix3C B, DMatrix3C C) {
//#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C)
//#define dMULTIPLY1_333(A,B,C) dMULTIPLYOP1_333(A,=,B,C)
//#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C)
public static void dMultiply0_331(DVector3 A, DMatrix3C B, DVector3C C) {
dMULTIPLYOP0_331(A,B,C); }
public static void dMultiply0_331(DVector3 res, DMatrix3C a, DVector3C b) {
dMultiplyHelper0_331(res, a, b); }
public static void dMultiply0_331(DMatrix3 A, DMatrix3C B, DVector3C C) {
dMULTIPLYOP0_331(A,B,C); }
// public static void dMULTIPLY0_331(DVector3 A, DMatrix3 B, DVector4 C) {
@@ -913,10 +892,10 @@ public static void dMULTIPLY0_331(DVector3 A, DMatrix3C B, double[] C, int c) {
dMultiply0_331(A, B, C, c);
}
public static void dMultiply1_331(DVector3 A, DMatrix3C B, DVector3C C) {
dMULTIPLYOP1_331(A,B,C); }
public static void dMultiply0_133(DVector3 A, DVector3C B, DMatrix3C C) {
dMULTIPLYOP0_133(A,B,C); }
public static void dMultiply1_331(DVector3 res, DMatrix3C a, DVector3C b) {
dMultiplyHelper1_331(res, a, b); }
public static void dMultiply0_133(DVector3 res, DVector3C a, DMatrix3C b) {
dMultiplyHelper0_133(res, a, b); }
public static void dMultiply0_133(double[] A, int a, double[] B, int b,
double[] C, int c) {
A[0+a] = dCalcVectorDot3_14(B, b, C, 0 + c);
@@ -1949,23 +1949,25 @@ int dCollideRayConvex( DxRay ray, DxConvex convex,
contact.side2 = -1; // TODO: set plane index?
double alpha, beta, nsign;
boolean flag;
//
// Compute some useful info
//
flag = false; // Assume start point is behind all planes.
DVector3 ray_pos = new DVector3();
DVector3 ray_dir = new DVector3();
dMultiply1_331(ray_pos, convex.final_posr().R(), new DVector3(ray.final_posr().pos()).sub(convex.final_posr().pos()));
dMultiply1_331(ray_dir, convex.final_posr().R(), ray.final_posr().R().columnAsNewVector(2));
boolean flag = false;
for ( int i = 0; i < convex.planecount; ++i )
{
// Alias this plane.
//double* plane = convex.planes + ( i * 4 );
int planePos = i;//*4;
// If alpha >= 0 then start point is outside of plane.
//alpha = dDOT( convex.planes, planePos, ray._final_posr.pos.v, 0 ) - convex.planes[planePos+3];//] - plane[3];
alpha = dCalcVectorDot3( convex.planesV[planePos], ray.final_posr().pos() ) - convex.planesD[planePos];//] - plane[3];
alpha = dCalcVectorDot3( convex.planesV[planePos], ray_pos ) - convex.planesD[planePos];//] - plane[3];
// If any alpha is positive, then
// the ray start is _outside_ of the hull
@@ -1975,7 +1977,6 @@ int dCollideRayConvex( DxRay ray, DxConvex convex,
break;
}
}
// If the ray starts inside the convex hull, then everything is flipped.
nsign = ( flag ) ? ( 1.0 ) : ( -1.0 );
@@ -1994,14 +1995,10 @@ int dCollideRayConvex( DxRay ray, DxConvex convex,
int planePos = i;//*4;
// If alpha >= 0 then point is outside of plane.
//alpha = nsign * ( dDOT( plane, ray.final_posr.pos ) - plane[3] );
alpha = nsign * ( dCalcVectorDot3( convex.planesV[planePos], ray.final_posr().pos() ) - convex.planesD[planePos] );
alpha = nsign * ( dCalcVectorDot3( convex.planesV[planePos], ray_pos ) - convex.planesD[planePos] );
// Compute [ plane-normal DOT ray-normal ], (/flip)
//beta = dDOT13( convex.planes, planePos, ray._final_posr.R.v,2 ) * nsign;
//beta = dDOT13( convex.planesV[planePos], ray._final_posr.R.viewCol(2) ) * nsign;
beta = convex.planesV[planePos].dot( ray.final_posr().R().viewCol(2) ) * nsign;
beta = convex.planesV[planePos].dot( ray_dir ) * nsign;
// Ray is pointing at the plane? ( beta < 0 )
// Ray start to plane is within maximum ray length?
// Ray start to plane is closer than the current best distance?
@@ -2010,10 +2007,7 @@ int dCollideRayConvex( DxRay ray, DxConvex convex,
alpha < contact.depth )
{
// Compute contact point on convex hull surface.
// 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];
contact.pos.eqSum(ray.final_posr().pos(), 0, ray.final_posr().R().columnAsNewVector(2), alpha);
contact.pos.eqSum(ray_pos, 1, ray_dir, alpha);
flag = false;
@@ -2031,7 +2025,7 @@ int dCollideRayConvex( DxRay ray, DxConvex convex,
//beta = dDOT( planej, contact.pos ) - plane[3];
//TODO use planePos+3 or planePosJ+3 ???
//beta = dDOT( convex.planesV[planePosJ], contact.pos) - convex.planesD[planePosJ];
beta = dCalcVectorDot3( convex.planesV[planePosJ], contact.pos) - convex.planesD[planePos];
beta = dCalcVectorDot3( convex.planesV[planePosJ], contact.pos) - convex.planesD[planePosJ];
// If any beta is positive, then the contact point
// is not on the surface of the convex hull - it's just
@@ -2047,9 +2041,6 @@ int dCollideRayConvex( DxRay ray, DxConvex convex,
if ( flag == false )
{
// Store the contact normal, possibly flipped.
// contact.normal[0] = nsign * plane[0];
// contact.normal[1] = nsign * plane[1];
// contact.normal[2] = nsign * plane[2];
contact.normal.set(convex.planesV[planePos]).scale(nsign);
// Store depth
@@ -2064,7 +2055,14 @@ int dCollideRayConvex( DxRay ray, DxConvex convex,
}
}
// Contact?
return ( contact.depth <= ray.getLength() ? 1 : 0 );
if (contact.depth <= ray.getLength()) {
// Adjust contact position and normal back to global space
dMultiply0_331(contact.pos, convex.final_posr().R(), contact.pos);
dMultiply0_331(contact.normal, convex.final_posr().R(), contact.normal);
contact.pos.add(convex.final_posr().pos());
return 1;
}
return 0;
}
@Override
@@ -0,0 +1,94 @@
package org.ode4j.tests;
import static org.junit.Assert.assertEquals;
import org.junit.Test;
import org.ode4j.math.DMatrix3;
import org.ode4j.ode.DContactGeomBuffer;
import org.ode4j.ode.DGeom;
import org.ode4j.ode.OdeHelper;
import org.ode4j.ode.internal.DxRay;
public class ConvexRayCollisionTest {
private static final double EPSILON = Double.MIN_VALUE;
private final int prism_pointcount = 8;
private final int prism_planecount = 6;
private final double prism_points[] = { 10.0, 1.0, -1.0, 10.0, -1.0, -1.0, -10.0, -1.0, -1.0, -10.0, 1.0, -1.0,
10.0, 1.0, 1.0, 10.0, -1.0, 1.0, -10.0, -1.0, 1.0, -10.0, 1.0, 1.0 };
private final int prism_polygons[] = { 4, 0, 1, 2, 3, 4, 4, 7, 6, 5, 4, 0, 4, 5, 1, 4, 1, 5, 6, 2, 4, 2, 6, 7, 3, 4,
4, 0, 3, 7, };
private final double prism_planes[] = { 0.0, 0.0, -1.0, 1.0, 0.0, 0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 10.0, 0.0, -1.0,
0.0, 1.0, -1.0, 0.0, -0.0, 10.0, 0.0, 1.0, 0.0, 1.0, };
@Test
public void testConvexAndRayColliderConsidersPositions() {
OdeHelper.initODE2(0);
// Create convex
DGeom convex = OdeHelper.createConvex(prism_planes, prism_planecount, prism_points, prism_pointcount,
prism_polygons);
convex.setPosition(0, 0, 0);
// Create ray
DxRay ray = (DxRay) OdeHelper.createRay(20);
ray.set(0, -10, 0, 0, 1, 0);
DContactGeomBuffer contact = new DContactGeomBuffer(1);
int count = OdeHelper.collide(ray, convex, 1, contact);
assertEquals(1, count);
assertEquals(0.0, contact.get().pos.get0(), EPSILON);
assertEquals(-1.0, contact.get().pos.get1(), EPSILON);
assertEquals(0.0, contact.get().pos.get2(), EPSILON);
assertEquals(0.0, contact.get().normal.get0(), EPSILON);
assertEquals(-1.0, contact.get().normal.get1(), EPSILON);
assertEquals(0.0, contact.get().normal.get2(), EPSILON);
assertEquals(9.0, contact.get().depth, EPSILON);
// Move Ray
ray.set(5, -10, 0, 0, 1, 0);
count = OdeHelper.collide(ray, convex, 1, contact);
assertEquals(1, count);
assertEquals(5.0, contact.get().pos.get0(), EPSILON);
assertEquals(-1.0, contact.get().pos.get1(), EPSILON);
assertEquals(0.0, contact.get().pos.get2(), EPSILON);
assertEquals(0.0, contact.get().normal.get0(), EPSILON);
assertEquals(-1.0, contact.get().normal.get1(), EPSILON);
assertEquals(0.0, contact.get().normal.get2(), EPSILON);
assertEquals(9.0, contact.get().depth, EPSILON);
// Rotate Convex
DMatrix3 rotate90z = new DMatrix3(0, -1, 0, 1, 0, 0, 0, 0, 1);
convex.setRotation(rotate90z);
count = OdeHelper.collide(ray, convex, 1, contact);
assertEquals(0, count);
// Move Ray
ray.set(10, 0, 0, -1, 0, 0);
count = OdeHelper.collide(ray, convex, 1, contact);
assertEquals(1, count);
assertEquals(1.0, contact.get().pos.get0(), EPSILON);
assertEquals(0.0, contact.get().pos.get1(), EPSILON);
assertEquals(0.0, contact.get().pos.get2(), EPSILON);
assertEquals(1.0, contact.get().normal.get0(), EPSILON);
assertEquals(0.0, contact.get().normal.get1(), EPSILON);
assertEquals(0.0, contact.get().normal.get2(), EPSILON);
assertEquals(9.0, contact.get().depth, EPSILON);
// Move Ray
ray.set(10, 1000, 1000, -1, 0, 0);
// Move Geom
convex.setPosition(0, 1000, 1000);
count = OdeHelper.collide(ray, convex, 1, contact);
assertEquals(1, count);
assertEquals(1.0, contact.get().pos.get0(), EPSILON);
assertEquals(1000.0, contact.get().pos.get1(), EPSILON);
assertEquals(1000.0, contact.get().pos.get2(), EPSILON);
assertEquals(1.0, contact.get().normal.get0(), EPSILON);
assertEquals(0.0, contact.get().normal.get1(), EPSILON);
assertEquals(0.0, contact.get().normal.get2(), EPSILON);
assertEquals(9.0, contact.get().depth, EPSILON);
}
}

0 comments on commit 0f8d404

Please sign in to comment.