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/rotation.h>
  45 #include <ode/objects.h>
  46 #include "config.h"
  47 #include "matrix.h"
  48 #include "odemath.h"
  49 #include "collision_kernel.h"   // for dxGeom
  50 #include "collision_util.h"
  51
  52 int dCollideCylinderSphere(dxGeom* Cylinder, dxGeom* Sphere,
  53                            int flags, dContactGeom *contact, int skip)
  54 {
  55     dIASSERT (skip >= (int)sizeof(dContactGeom));
  56     dIASSERT (Cylinder->type == dCylinderClass);
  57     dIASSERT (Sphere->type == dSphereClass);
  58     dIASSERT ((flags & NUMC_MASK) >= 1);
  59
  60     //unsigned char* pContactData = (unsigned char*)contact;
  61     int GeomCount = 0; // count of used contacts
  62
  63 #ifdef dSINGLE
  64     const dReal toleranz = REAL(0.0001);
  65 #endif
  66 #ifdef dDOUBLE
  67     const dReal toleranz = REAL(0.0000001);
  68 #endif
  69
  70     // get the data from the geoms
  71     dReal radius, length;
  72     dGeomCylinderGetParams(Cylinder, &radius, &length);
  73     dVector3 &cylpos = Cylinder->final_posr->pos;
  74     //const dReal* pfRot1 = dGeomGetRotation(Cylinder);
  75
  76     dReal radius2;
  77     radius2 = dGeomSphereGetRadius(Sphere);
  78     const dReal* SpherePos = dGeomGetPosition(Sphere);
  79
  80     // G1Pos1 is the middle of the first disc
  81     // G1Pos2 is the middle of the second disc
  82     // vDir1 is the unit direction of the cylinderaxis
  83     dVector3 G1Pos1, G1Pos2, vDir1;
  84     vDir1[0] = Cylinder->final_posr->R[2];
  85     vDir1[1] = Cylinder->final_posr->R[6];
  86     vDir1[2] = Cylinder->final_posr->R[10];
  87
  88     dReal s;
  89     s = length * REAL(0.5); // just a precomputed factor
  90     G1Pos2[0] = vDir1[0] * s + cylpos[0];
  91     G1Pos2[1] = vDir1[1] * s + cylpos[1];
  92     G1Pos2[2] = vDir1[2] * s + cylpos[2];
  93
  94     G1Pos1[0] = vDir1[0] * -s + cylpos[0];
  95     G1Pos1[1] = vDir1[1] * -s + cylpos[1];
  96     G1Pos1[2] = vDir1[2] * -s + cylpos[2];
  97
  98     dVector3 C;
  99     dReal t;
 100     // Step 1: compute the two distances 's' and 't'
 101     // 's' is the distance from the first disc (in vDir1-/Zylinderaxis-direction), the disc with G1Pos1 in the middle
 102     s = (SpherePos[0] - G1Pos1[0]) * vDir1[0] - (G1Pos1[1] - SpherePos[1]) * vDir1[1] - (G1Pos1[2] - SpherePos[2]) * vDir1[2];
 103     if(s < (-radius2) || s > (length + radius2) )
 104     {
 105         // Sphere is too far away from the discs
 106         // no collision
 107         return 0;
 108     }
 109
 110     // C is the direction from Sphere-middle to the cylinder-axis (vDir1); C is orthogonal to the cylinder-axis
 111     C[0] = s * vDir1[0] + G1Pos1[0] - SpherePos[0];
 112     C[1] = s * vDir1[1] + G1Pos1[1] - SpherePos[1];
 113     C[2] = s * vDir1[2] + G1Pos1[2] - SpherePos[2];
 114     // t is the distance from the Sphere-middle to the cylinder-axis!
 115     t = dVector3Length(C);
 116     if(t > (radius + radius2) )
 117     {
 118         // Sphere is too far away from the cylinder axis!
 119         // no collision
 120         return 0;
 121     }
 122
 123     // decide which kind of collision we have:
 124     if(t > radius && (s < 0 || s > length) )
 125     {
 126         // 3. collision
 127         if(s <= 0)
 128         {
 129             contact->depth = radius2 - dSqrt( (s) * (s) + (t - radius) * (t - radius) );
 130             if(contact->depth < 0)
 131             {
 132                 // no collision!
 133                 return 0;
 134             }
 135             contact->pos[0] = C[0] / t * -radius + G1Pos1[0];
 136             contact->pos[1] = C[1] / t * -radius + G1Pos1[1];
 137             contact->pos[2] = C[2] / t * -radius + G1Pos1[2];
 138             contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
 139             contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
 140             contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
 141             contact->g1 = Cylinder;
 142             contact->g2 = Sphere;
 143             contact->side1 = -1;
 144             contact->side2 = -1;
 145             GeomCount++;
 146             return GeomCount;
 147         }
 148         else
 149         {
 150             // now s is bigger than length here!
 151             contact->depth = radius2 - dSqrt( (s - length) * (s - length) + (t - radius) * (t - radius) );
 152             if(contact->depth < 0)
 153             {
 154                 // no collision!
 155                 return 0;
 156             }
 157             contact->pos[0] = C[0] / t * -radius + G1Pos2[0];
 158             contact->pos[1] = C[1] / t * -radius + G1Pos2[1];
 159             contact->pos[2] = C[2] / t * -radius + G1Pos2[2];
 160             contact->normal[0] = (contact->pos[0] - SpherePos[0]) / (radius2 - contact->depth);
 161             contact->normal[1] = (contact->pos[1] - SpherePos[1]) / (radius2 - contact->depth);
 162             contact->normal[2] = (contact->pos[2] - SpherePos[2]) / (radius2 - contact->depth);
 163             contact->g1 = Cylinder;
 164             contact->g2 = Sphere;
 165             contact->side1 = -1;
 166             contact->side2 = -1;
 167             GeomCount++;
 168             return GeomCount;
 169         }
 170     }
 171     else if( (radius - t) <= s && (radius - t) <= (length - s) )
 172     {
 173         // 1. collision
 174         if(t > (radius2 + toleranz))
 175         {
 176             // cylinder-axis is outside the sphere
 177             contact->depth = (radius2 + radius) - t;
 178             if(contact->depth < 0)
 179             {
 180                 // should never happen, but just for safeness
 181                 return 0;
 182             }
 183             else
 184             {
 185                 C[0] /= t;
 186                 C[1] /= t;
 187                 C[2] /= t;
 188                 contact->pos[0] = C[0] * radius2 + SpherePos[0];
 189                 contact->pos[1] = C[1] * radius2 + SpherePos[1];
 190                 contact->pos[2] = C[2] * radius2 + SpherePos[2];
 191                 contact->normal[0] = C[0];
 192                 contact->normal[1] = C[1];
 193                 contact->normal[2] = C[2];
 194                 contact->g1 = Cylinder;
 195                 contact->g2 = Sphere;
 196                 contact->side1 = -1;
 197                 contact->side2 = -1;
 198                 GeomCount++;
 199                 return GeomCount;
 200             }
 201         }
 202         else
 203         {
 204             // cylinder-axis is outside of the sphere
 205             contact->depth = (radius2 + radius) - t;
 206             if(contact->depth < 0)
 207             {
 208                 // should never happen, but just for safeness
 209                 return 0;
 210             }
 211             else
 212             {
 213                 contact->pos[0] = C[0] + SpherePos[0];
 214                 contact->pos[1] = C[1] + SpherePos[1];
 215                 contact->pos[2] = C[2] + SpherePos[2];
 216                 contact->normal[0] = C[0] / t;
 217                 contact->normal[1] = C[1] / t;
 218                 contact->normal[2] = C[2] / t;
 219                 contact->g1 = Cylinder;
 220                 contact->g2 = Sphere;
 221                 contact->side1 = -1;
 222                 contact->side2 = -1;
 223                 GeomCount++;
 224                 return GeomCount;
 225             }
 226         }
 227     }
 228     else
 229     {
 230         // 2. collision
 231         if(s <= (length * REAL(0.5)) )
 232         {
 233             // collision with the first disc
 234             contact->depth = s + radius2;
 235             if(contact->depth < 0)
 236             {
 237                 // should never happen, but just for safeness
 238                 return 0;
 239             }
 240             contact->pos[0] = radius2 * vDir1[0] + SpherePos[0];
 241             contact->pos[1] = radius2 * vDir1[1] + SpherePos[1];
 242             contact->pos[2] = radius2 * vDir1[2] + SpherePos[2];
 243             contact->normal[0] = vDir1[0];
 244             contact->normal[1] = vDir1[1];
 245             contact->normal[2] = vDir1[2];
 246             contact->g1 = Cylinder;
 247             contact->g2 = Sphere;
 248             contact->side1 = -1;
 249             contact->side2 = -1;
 250             GeomCount++;
 251             return GeomCount;
 252         }
 253         else
 254         {
 255             // collision with the second disc
 256             contact->depth = (radius2 + length - s);
 257             if(contact->depth < 0)
 258             {
 259                 // should never happen, but just for safeness
 260                 return 0;
 261             }
 262             contact->pos[0] = radius2 * -vDir1[0] + SpherePos[0];
 263             contact->pos[1] = radius2 * -vDir1[1] + SpherePos[1];
 264             contact->pos[2] = radius2 * -vDir1[2] + SpherePos[2];
 265             contact->normal[0] = -vDir1[0];
 266             contact->normal[1] = -vDir1[1];
 267             contact->normal[2] = -vDir1[2];
 268             contact->g1 = Cylinder;
 269             contact->g2 = Sphere;
 270             contact->side1 = -1;
 271             contact->side2 = -1;
 272             GeomCount++;
 273             return GeomCount;
 274         }
 275     }
 276     return GeomCount;
 277 }