From c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 Mon Sep 17 00:00:00 2001 From: sanine Date: Sat, 1 Oct 2022 20:59:36 -0500 Subject: add ode --- .../ode/src/collision_cylinder_plane.cpp | 266 +++++++++++++++++++++ 1 file changed, 266 insertions(+) create mode 100644 libs/ode-0.16.1/ode/src/collision_cylinder_plane.cpp (limited to 'libs/ode-0.16.1/ode/src/collision_cylinder_plane.cpp') diff --git a/libs/ode-0.16.1/ode/src/collision_cylinder_plane.cpp b/libs/ode-0.16.1/ode/src/collision_cylinder_plane.cpp new file mode 100644 index 0000000..67424ad --- /dev/null +++ b/libs/ode-0.16.1/ode/src/collision_cylinder_plane.cpp @@ -0,0 +1,266 @@ +/************************************************************************* + * * + * 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 "config.h" +#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; +} -- cgit v1.2.1