/************************************************************************* * * * 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 "ode_collision.hpp" #include "ode_rotation.hpp" #include "ode_objects.hpp" #include "matrix.h" #include "odemath.h" #include "collision_kernel.h" // for dxGeom #include "collision_util.h" int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (Cylinder->type == dCylinderClass); dIASSERT (Plane->type == dPlaneClass); dIASSERT ((flags & NUMC_MASK) >= 1); int GeomCount = 0; // count of used contactgeoms #ifdef dSINGLE const dReal toleranz = REAL(0.0001); #endif #ifdef dDOUBLE const dReal toleranz = REAL(0.0000001); #endif // 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 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]}; 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 = length * REAL(0.5); G1Pos2[0] = vDir1[0] * s + cylpos[0]; G1Pos2[1] = vDir1[1] * s + cylpos[1]; G1Pos2[2] = vDir1[2] * s + cylpos[2]; G1Pos1[0] = vDir1[0] * -s + cylpos[0]; G1Pos1[1] = vDir1[1] * -s + cylpos[1]; G1Pos1[2] = vDir1[2] * -s + cylpos[2]; dVector3 C; // parallel-check s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + 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] - dVector3Dot(planevec, G1Pos1); dReal t; t = planevec[3] - dVector3Dot(planevec, G1Pos2); if(s >= t) // s == t does never happen, { if(s >= 0) { // 1. Disc dVector3Copy(G1Pos1, P); } else return GeomCount; // no contacts } else { if(t >= 0) { // 2. Disc dVector3Copy(G1Pos2, P); } 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 dVector3Cross(V1, vDir1, V2); // make unit V2 t = dVector3Length(V2); t = radius / t; dVector3Scale(V2, t); // cross again dVector3Cross(V2, vDir1, V1); // |V2| is 'radius' and vDir1 unit, so |V1| is 'radius' // V1 = first axis // V2 = second axis // 3. generate contactpoints // Potential contact 1 dVector3Add(P, V1, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; contact->side1 = -1; contact->side2 = -1; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // Potential contact 2 dVector3Subtract(P, V1, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; contact->side1 = -1; contact->side2 = -1; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // Potential contact 3 dVector3Add(P, V2, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; contact->side1 = -1; contact->side2 = -1; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // Potential contact 4 dVector3Subtract(P, V2, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; contact->side1 = -1; contact->side2 = -1; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } } else { dReal t = dVector3Dot(PlaneNormal, vDir1); C[0] = vDir1[0] * t - PlaneNormal[0]; C[1] = vDir1[1] * t - PlaneNormal[1]; C[2] = vDir1[2] * t - PlaneNormal[2]; s = dVector3Length(C); // move C onto the circle s = radius / s; dVector3Scale(C, s); // deepest point of disc 1 dVector3Add(C, G1Pos1, contact->pos); // depth of the deepest point contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth >= 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; contact->side1 = -1; contact->side2 = -1; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // C is still computed // deepest point of disc 2 dVector3Add(C, G1Pos2, contact->pos); // depth of the deepest point contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2]; if(contact->depth >= 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; contact->side1 = -1; contact->side2 = -1; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } } return GeomCount; }