ode/src/collision_cylinder_sphere.cpp

   1 /*************************************************************************
   2 *                                                                       *
   3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
   4 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
   5 *                                                                       *
   6 * This library is free software; you can redistribute it and/or         *
   7 * modify it under the terms of EITHER:                                  *
   8 *   (1) The GNU Lesser General Public License as published by the Free  *
   9 *       Software Foundation; either version 2.1 of the License, or (at  *
  10 *       your option) any later version. The text of the GNU Lesser      *
  11 *       General Public License is included with this library in the     *
  12 *       file LICENSE.TXT.                                               *
  13 *   (2) The BSD-style license that is included with this library in     *
  14 *       the file LICENSE-BSD.TXT.                                       *
  15 *                                                                       *
  16 * This library is distributed in the hope that it will be useful,       *
  17 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
  19 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
  20 *                                                                       *
  21 *************************************************************************/
  22
  23
  24 /*******************************************************************
  25  *                                                                 *
  26  * cylinder-sphere collider by Christoph Beyer (boernerb@web.de)   *
  27  *                                                                 *
  28  * In Cylinder/Sphere-collisions, there are three possibilies:     *
  29  * 1. collision with the cylinder's nappe                          *
  30  * 2. collision with one of the cylinder's disc                    *
  31  * 3. collision with one of the disc's border                      *
  32  *                                                                 *
  33  * This collider computes two distances (s, t) and based on them,  *
  34  * it decides, which collision we have.                            *
  35  * This collider always generates 1 (or 0, if we have no collison) *
  36  * contacts.                                                       *
  37  * It is able to "separate" cylinder and sphere in all             *
  38  * configurations, but it never pays attention to velocity.        *
  39  * So, in extrem situations, "tunneling-effect" is possible.       *
  40  *                                                                 *
  41  *******************************************************************/
  42
  43 #include <ode/collision.h>
  44 #include <ode/matrix.h>
  45 #include <ode/rotation.h>
  46 #include <ode/odemath.h>
  47 #include <ode/objects.h>
  48 #include "collision_kernel.h"   // for dxGeom
  49 #include "collision_util.h"
  50
  51 int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere,
  52                            int flags, dContactGeom *contact, int skip)
  53 {
  54         dIASSERT (skip >= (int)sizeof(dContactGeom));
  55         dIASSERT (Cylinder->type == dCylinderClass);
  56         dIASSERT (Sphere->type == dSphereClass);
  57         dIASSERT ((flags & NUMC_MASK) >= 1);
  58
  59         //unsigned char* pContactData = (unsigned char*)contact;
  60         int GeomCount = 0; // count of used contacts
  61
  62 #ifdef dSINGLE
  63         const dReal toleranz = REAL(0.0001);
  64 #endif
  65 #ifdef dDOUBLE
  66         const dReal toleranz = REAL(0.0000001);
  67 #endif
  68
  69         // get the data from the geoms
  70         dReal radius, length;
  71         dGeomCylinderGetParams(Cylinder, &radius, &length);
  72     dVector3 &cylpos = Cylinder->final_posr->pos;
  73         //const dReal* pfRot1 = dGeomGetRotation(Cylinder);
  74
  75         dReal radius2;
  76         radius2 = dGeomSphereGetRadius(Sphere);
  77         const dReal* SpherePos = dGeomGetPosition(Sphere);
  78
  79         // G1Pos1 is the middle of the first disc
  80         // G1Pos2 is the middle of the second disc
  81         // vDir1 is the unit direction of the cylinderaxis
  82         dVector3 G1Pos1, G1Pos2, vDir1;
  83         vDir1[0] = Cylinder->final_posr->R[2];
  84         vDir1[1] = Cylinder->final_posr->R[6];
  85         vDir1[2] = Cylinder->final_posr->R[10];
  86
  87         dReal s;
  88         s = length * REAL(0.5); // just a precomputed factor
  89         G1Pos2[0] = vDir1[0] * s + cylpos[0];
  90         G1Pos2[1] = vDir1[1] * s + cylpos[1];
  91         G1Pos2[2] = vDir1[2] * s + cylpos[2];
  92
  93         G1Pos1[0] = vDir1[0] * -s + cylpos[0];
  94         G1Pos1[1] = vDir1[1] * -s + cylpos[1];
  95         G1Pos1[2] = vDir1[2] * -s + cylpos[2];
  96
  97         dVector3 C;
  98         dReal t;
  99         // Step 1: compute the two distances 's' and 't'
 100         // 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle
 101         s = (SpherePos[0] - G1Pos1[0]) * vDir1[0] - (G1Pos1[1] - SpherePos[1]) * vDir1[1] - (G1Pos1[2] - SpherePos[2]) * vDir1[2];
 102         if(s < (-radius2) || s > (length + radius2) )
 103         {
 104                 // Sphere is too far away from the discs
 105                 // no collision
 106                 return 0;
 107         }
 108
 109         // C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis
 110         C[0] = s * vDir1[0] + G1Pos1[0] - SpherePos[0];
 111         C[1] = s * vDir1[1] + G1Pos1[1] - SpherePos[1];
 112         C[2] = s * vDir1[2] + G1Pos1[2] - SpherePos[2];
 113         // t is the distance from the Sphere-middle to the cylinder-axis!
 114         t = dVector3Length(C);
 115         if(t > (radius + radius2) )
 116         {
 117                 // Sphere is too far away from the cylinder axis!
 118                 // no collision
 119                 return 0;
 120         }
 121
 122         // decide which kind of collision we have:
 123         if(t > radius && (s < 0 || s > length) )
 124         {
 125                 // 3. collision
 126                 if(s <= 0)
 127                 {
 128                         contact->depth = radius2 - dSqrt( (s) * (s) + (t - radius) * (t - radius) );
 129                         if(contact->depth < 0)
 130                         {
 131                                 // no collision!
 132                                 return 0;
 133                         }
 134                         contact->pos[0] = C[0] / t * -radius + G1Pos1[0];
 135                         contact->pos[1] = C[1] / t * -radius + G1Pos1[1];
 136                         contact->pos[2] = C[2] / t * -radius + G1Pos1[2];
 137                         contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
 138                         contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
 139                         contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
 140                         contact->g1 = Cylinder;
 141                         contact->g2 = Sphere;
 142                         GeomCount++;
 143                         return GeomCount;
 144                 }
 145                 else
 146                 {
 147                         // now s is bigger than length here!
 148                         contact->depth = radius2 - dSqrt( (s - length) * (s - length) + (t - radius) * (t - radius) );
 149                         if(contact->depth < 0)
 150                         {
 151                                 // no collision!
 152                                 return 0;
 153                         }
 154                         contact->pos[0] = C[0] / t * -radius + G1Pos2[0];
 155                         contact->pos[1] = C[1] / t * -radius + G1Pos2[1];
 156                         contact->pos[2] = C[2] / t * -radius + G1Pos2[2];
 157                         contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
 158                         contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
 159                         contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
 160                         contact->g1 = Cylinder;
 161                         contact->g2 = Sphere;
 162                         GeomCount++;
 163                         return GeomCount;
 164                 }
 165         }
 166         else if( (radius - t) <= s && (radius - t) <= (length - s) )
 167         {
 168                 // 1. collsision
 169                 if(t > (radius2 + toleranz))
 170                 {
 171                         // cylinder-axis is outside the sphere
 172                         contact->depth = (radius2 + radius) - t;
 173                         if(contact->depth < 0)
 174                         {
 175                                 // should never happen, but just for safeness
 176                                 return 0;
 177                         }
 178                         else
 179                         {
 180                                 C[0] /= t;
 181                                 C[1] /= t;
 182                                 C[2] /= t;
 183                                 contact->pos[0] = C[0] * radius2 + SpherePos[0];
 184                                 contact->pos[1] = C[1] * radius2 + SpherePos[1];
 185                                 contact->pos[2] = C[2] * radius2 + SpherePos[2];
 186                                 contact->normal[0] = C[0];
 187                                 contact->normal[1] = C[1];
 188                                 contact->normal[2] = C[2];
 189                                 contact->g1 = Cylinder;
 190                                 contact->g2 = Sphere;
 191                                 GeomCount++;
 192                                 return GeomCount;
 193                         }
 194                 }
 195                 else
 196                 {
 197                         // cylinder-axis is outside of the sphere
 198                         contact->depth = (radius2 + radius) - t;
 199                         if(contact->depth < 0)
 200                         {
 201                                 // should never happen, but just for safeness
 202                                 return 0;
 203                         }
 204                         else
 205                         {
 206                                 contact->pos[0] = C[0] + SpherePos[0];
 207                                 contact->pos[1] = C[1] + SpherePos[1];
 208                                 contact->pos[2] = C[2] + SpherePos[2];
 209                                 contact->normal[0] = C[0] / t;
 210                                 contact->normal[1] = C[1] / t;
 211                                 contact->normal[2] = C[2] / t;
 212                                 contact->g1 = Cylinder;
 213                                 contact->g2 = Sphere;
 214                                 GeomCount++;
 215                                 return GeomCount;
 216                         }
 217                 }
 218         }
 219         else
 220         {
 221                 // 2. collision
 222                 if(s <= (length * REAL(0.5)) )
 223                 {
 224                         // collsision with the first disc
 225                         contact->depth = s + radius2;
 226                         if(contact->depth < 0)
 227                         {
 228                                 // should never happen, but just for safeness
 229                                 return 0;
 230                         }
 231                         contact->pos[0] = radius2 * vDir1[0] + SpherePos[0];
 232                         contact->pos[1] = radius2 * vDir1[1] + SpherePos[1];
 233                         contact->pos[2] = radius2 * vDir1[2] + SpherePos[2];
 234                         contact->normal[0] = vDir1[0];
 235                         contact->normal[1] = vDir1[1];
 236                         contact->normal[2] = vDir1[2];
 237                         contact->g1 = Cylinder;
 238                         contact->g2 = Sphere;
 239                         GeomCount++;
 240                         return GeomCount;
 241                 }
 242                 else
 243                 {
 244                         // collsision with the second disc
 245                         contact->depth = (radius2 + length - s);
 246                         if(contact->depth < 0)
 247                         {
 248                                 // should never happen, but just for safeness
 249                                 return 0;
 250                         }
 251                         contact->pos[0] = radius2 * -vDir1[0] + SpherePos[0];
 252                         contact->pos[1] = radius2 * -vDir1[1] + SpherePos[1];
 253                         contact->pos[2] = radius2 * -vDir1[2] + SpherePos[2];
 254                         contact->normal[0] = -vDir1[0];
 255                         contact->normal[1] = -vDir1[1];
 256                         contact->normal[2] = -vDir1[2];
 257                         contact->g1 = Cylinder;
 258                         contact->g2 = Sphere;
 259                         GeomCount++;
 260                         return GeomCount;
 261                 }
 262         }
 263         return GeomCount;
 264 }