diff -r 000000000000 -r 2f259fa3e83a ode/src/collision_cylinder_plane.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ode/src/collision_cylinder_plane.cpp Tue Feb 02 01:00:49 2010 +0200 @@ -0,0 +1,286 @@ +/************************************************************************* +* * +* Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * +* All rights reserved. Email: russ@q12.org Web: www.q12.org * +* * +* This library is free software; you can redistribute it and/or * +* modify it under the terms of EITHER: * +* (1) The GNU Lesser General Public License as published by the Free * +* Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. The text of the GNU Lesser * +* General Public License is included with this library in the * +* file LICENSE.TXT. * +* (2) The BSD-style license that is included with this library in * +* the file LICENSE-BSD.TXT. * +* * +* This library is distributed in the hope that it will be useful, * +* but WITHOUT ANY WARRANTY; without even the implied warranty of * +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * +* LICENSE.TXT and LICENSE-BSD.TXT for more details. * +* * +*************************************************************************/ + + +/* + * Cylinder-Plane collider by Christoph Beyer ( boernerb@web.de ) + * + * This testing basically comes down to testing the intersection + * of the cylinder caps (discs) with the plane. + * + */ + +#include +#include +#include +#include +#include + +#include "collision_kernel.h" // for dxGeom + +int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip) +{ + + unsigned char* pContactData = (unsigned char*)contact; + int GeomCount = 0; // count of used contactgeoms + + const dReal toleranz = REAL(0.0001f); + + + // Get the properties of the cylinder (length+radius) + dReal radius, length; + dGeomCylinderGetParams(Cylinder, &radius, &length); + dVector3 &cylpos = Cylinder->final_posr->pos; + // and the plane + dVector4 planevec; + dGeomPlaneGetParams(Plane, planevec); + dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]}; + + dVector3 G1Pos1, G1Pos2, vDir1; + vDir1[0] = Cylinder->final_posr->R[2]; + vDir1[1] = Cylinder->final_posr->R[6]; + vDir1[2] = Cylinder->final_posr->R[10]; + + dReal s; + s = dMUL(length,REAL(0.5)); + G1Pos2[0] = dMUL(vDir1[0],s) + cylpos[0]; + G1Pos2[1] = dMUL(vDir1[1],s) + cylpos[1]; + G1Pos2[2] = dMUL(vDir1[2],s) + cylpos[2]; + + G1Pos1[0] = dMUL(vDir1[0],-s) + cylpos[0]; + G1Pos1[1] = dMUL(vDir1[1],-s) + cylpos[1]; + G1Pos1[2] = dMUL(vDir1[2],-s) + cylpos[2]; + + dVector3 C; + + // parallel-check + s = dMUL(vDir1[0],PlaneNormal[0]) + dMUL(vDir1[1],PlaneNormal[1]) + dMUL(vDir1[2],PlaneNormal[2]); + if(s < 0) + s += REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel + else + s -= REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel + if(s < toleranz && s > (-toleranz)) + { + // discs are parallel to the plane + + // 1.compute if, and where contacts are + dVector3 P; + s = planevec[3] - dMUL(planevec[0],G1Pos1[0]) - dMUL(planevec[1],G1Pos1[1]) - dMUL(planevec[2],G1Pos1[2]); + dReal t; + t = planevec[3] - dMUL(planevec[0],G1Pos2[0]) - dMUL(planevec[1],G1Pos2[1]) - dMUL(planevec[2],G1Pos2[2]); + if(s >= t) // s == t does never happen, + { + if(s >= 0) + { + // 1. Disc + P[0] = G1Pos1[0]; + P[1] = G1Pos1[1]; + P[2] = G1Pos1[2]; + } + else + return GeomCount; // no contacts + } + else + { + if(t >= 0) + { + // 2. Disc + P[0] = G1Pos2[0]; + P[1] = G1Pos2[1]; + P[2] = G1Pos2[2]; + } + else + return GeomCount; // no contacts + } + + // 2. generate a coordinate-system on the disc + dVector3 V1, V2; + if(vDir1[0] < toleranz && vDir1[0] > (-toleranz)) + { + // not x-axis + V1[0] = vDir1[0] + REAL(1.0); // random value + V1[1] = vDir1[1]; + V1[2] = vDir1[2]; + } + else + { + // maybe x-axis + V1[0] = vDir1[0]; + V1[1] = vDir1[1] + REAL(1.0); // random value + V1[2] = vDir1[2]; + } + // V1 is now another direction than vDir1 + // Cross-product + V2[0] = dMUL(V1[1],vDir1[2]) - dMUL(V1[2],vDir1[1]); + V2[1] = dMUL(V1[2],vDir1[0]) - dMUL(V1[0],vDir1[2]); + V2[2] = dMUL(V1[0],vDir1[1]) - dMUL(V1[1],vDir1[0]); + // make unit V2 + t = dSqrt(dMUL(V2[0],V2[0]) + dMUL(V2[1],V2[1]) + dMUL(V2[2],V2[2])); + t = dDIV(radius,t); + V2[0] = dMUL(V2[0],t); + V2[1] = dMUL(V2[1],t); + V2[2] = dMUL(V2[2],t); + // cross again + V1[0] = dMUL(V2[1],vDir1[2]) - dMUL(V2[2],vDir1[1]); + V1[1] = dMUL(V2[2],vDir1[0]) - dMUL(V2[0],vDir1[2]); + V1[2] = dMUL(V2[0],vDir1[1]) - dMUL(V2[1],vDir1[0]); + // |V2| is 'radius' and vDir1 unit, so |V1| is 'radius' + // V1 = first axis + // V2 = second axis + + // 3. generate contactpoints + + // Potential contact 1 + contact->pos[0] = P[0] + V1[0]; + contact->pos[1] = P[1] + V1[1]; + contact->pos[2] = P[2] + V1[2]; + contact->depth = planevec[3] - dMUL(planevec[0],contact->pos[0]) - dMUL(planevec[1],contact->pos[1]) - dMUL(planevec[2],contact->pos[2]); + if(contact->depth > 0) + { + contact->normal[0] = PlaneNormal[0]; + contact->normal[1] = PlaneNormal[1]; + contact->normal[2] = PlaneNormal[2]; + contact->g1 = Cylinder; + contact->g2 = Plane; + GeomCount++; + if( GeomCount >= (flags & 0x0ffff)) + return GeomCount; // enough contactgeoms + pContactData += skip; + contact = (dContactGeom*)pContactData; + } + + // Potential contact 2 + contact->pos[0] = P[0] - V1[0]; + contact->pos[1] = P[1] - V1[1]; + contact->pos[2] = P[2] - V1[2]; + contact->depth = planevec[3] - dMUL(planevec[0],contact->pos[0]) - dMUL(planevec[1],contact->pos[1]) - dMUL(planevec[2],contact->pos[2]); + if(contact->depth > 0) + { + contact->normal[0] = PlaneNormal[0]; + contact->normal[1] = PlaneNormal[1]; + contact->normal[2] = PlaneNormal[2]; + contact->g1 = Cylinder; + contact->g2 = Plane; + GeomCount++; + if( GeomCount >= (flags & 0x0ffff)) + return GeomCount; // enough contactgeoms + pContactData += skip; + contact = (dContactGeom*)pContactData; + } + + // Potential contact 3 + contact->pos[0] = P[0] + V2[0]; + contact->pos[1] = P[1] + V2[1]; + contact->pos[2] = P[2] + V2[2]; + contact->depth = planevec[3] - dMUL(planevec[0],contact->pos[0]) - dMUL(planevec[1],contact->pos[1]) - dMUL(planevec[2],contact->pos[2]); + if(contact->depth > 0) + { + contact->normal[0] = PlaneNormal[0]; + contact->normal[1] = PlaneNormal[1]; + contact->normal[2] = PlaneNormal[2]; + contact->g1 = Cylinder; + contact->g2 = Plane; + GeomCount++; + if( GeomCount >= (flags & 0x0ffff)) + return GeomCount; // enough contactgeoms + pContactData += skip; + contact = (dContactGeom*)pContactData; + } + + // Potential contact 4 + contact->pos[0] = P[0] - V2[0]; + contact->pos[1] = P[1] - V2[1]; + contact->pos[2] = P[2] - V2[2]; + contact->depth = planevec[3] - dMUL(planevec[0],contact->pos[0]) - dMUL(planevec[1],contact->pos[1]) - dMUL(planevec[2],contact->pos[2]); + if(contact->depth > 0) + { + contact->normal[0] = PlaneNormal[0]; + contact->normal[1] = PlaneNormal[1]; + contact->normal[2] = PlaneNormal[2]; + contact->g1 = Cylinder; + contact->g2 = Plane; + GeomCount++; + if( GeomCount >= (flags & 0x0ffff)) + return GeomCount; // enough contactgeoms + pContactData += skip; + contact = (dContactGeom*)pContactData; + } + } + else + { + dReal t = -(dMUL((-PlaneNormal[0]),vDir1[0]) + dMUL((-PlaneNormal[1]),vDir1[1]) + dMUL((-PlaneNormal[2]),vDir1[2])); + C[0] = dMUL(vDir1[0],t) - PlaneNormal[0]; + C[1] = dMUL(vDir1[1],t) - PlaneNormal[1]; + C[2] = dMUL(vDir1[2],t) - PlaneNormal[2]; + s = dSqrt(dMUL(C[0],C[0]) + dMUL(C[1],C[1]) + dMUL(C[2],C[2])); + // move C onto the circle + s = dDIV(radius,s); + C[0] = dMUL(C[0],s); + C[1] = dMUL(C[1],s); + C[2] = dMUL(C[2],s); + + // deepest point of disc 1 + contact->pos[0] = C[0] + G1Pos1[0]; + contact->pos[1] = C[1] + G1Pos1[1]; + contact->pos[2] = C[2] + G1Pos1[2]; + + // depth of the deepest point + contact->depth = planevec[3] - dMUL(planevec[0],contact->pos[0]) - dMUL(planevec[1],contact->pos[1]) - dMUL(planevec[2],contact->pos[2]); + if(contact->depth >= 0) + { + contact->normal[0] = PlaneNormal[0]; + contact->normal[1] = PlaneNormal[1]; + contact->normal[2] = PlaneNormal[2]; + contact->g1 = Cylinder; + contact->g2 = Plane; + GeomCount++; + if( GeomCount >= (flags & 0x0ffff)) + return GeomCount; // enough contactgeoms + pContactData += skip; + contact = (dContactGeom*)pContactData; + } + + // C is still computed + + // deepest point of disc 2 + contact->pos[0] = C[0] + G1Pos2[0]; + contact->pos[1] = C[1] + G1Pos2[1]; + contact->pos[2] = C[2] + G1Pos2[2]; + + // depth of the deepest point + contact->depth = planevec[3] - dMUL(planevec[0],contact->pos[0]) - dMUL(planevec[1],contact->pos[1]) - dMUL(planevec[2],contact->pos[2]); + if(contact->depth >= 0) + { + contact->normal[0] = PlaneNormal[0]; + contact->normal[1] = PlaneNormal[1]; + contact->normal[2] = PlaneNormal[2]; + contact->g1 = Cylinder; + contact->g2 = Plane; + GeomCount++; + if( GeomCount >= (flags & 0x0ffff)) + return GeomCount; // enough contactgeoms + pContactData += skip; + contact = (dContactGeom*)pContactData; + } + } + return GeomCount; +}