ode/src/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 standard ODE geometry primitives: public API and pairwise collision functions.
  26
  27 the rule is that only the low level primitive collision functions should set
  28 dContactGeom::g1 and dContactGeom::g2.
  29
  30 */
  31
  32 #include <ode/common.h>
  33 #include <ode/collision.h>
  34 #include <ode/matrix.h>
  35 #include <ode/rotation.h>
  36 #include <ode/odemath.h>
  37 #include "collision_kernel.h"
  38 #include "collision_std.h"
  39 #include "collision_util.h"
  40
  41 #ifdef _MSC_VER
  42 #pragma warning(disable:4291)  // for VC++, no complaints about "no matching operator delete found"
  43 #endif
  44
  45
  46 //****************************************************************************
  47 // sphere public API
  48
  49 dxSphere::dxSphere (dSpaceID space, dReal _radius) : dxGeom (space,1)
  50 {
  51   dAASSERT (_radius > 0);
  52   type = dSphereClass;
  53   radius = _radius;
  54 }
  55
  56
  57 void dxSphere::computeAABB()
  58 {
  59   aabb[0] = final_posr->pos[0] - radius;
  60   aabb[1] = final_posr->pos[0] + radius;
  61   aabb[2] = final_posr->pos[1] - radius;
  62   aabb[3] = final_posr->pos[1] + radius;
  63   aabb[4] = final_posr->pos[2] - radius;
  64   aabb[5] = final_posr->pos[2] + radius;
  65 }
  66
  67
  68 dGeomID dCreateSphere (dSpaceID space, dReal radius)
  69 {
  70   return new dxSphere (space,radius);
  71 }
  72
  73
  74 void dGeomSphereSetRadius (dGeomID g, dReal radius)
  75 {
  76   dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
  77   dAASSERT (radius > 0);
  78   dxSphere *s = (dxSphere*) g;
  79   s->radius = radius;
  80   dGeomMoved (g);
  81 }
  82
  83
  84 dReal dGeomSphereGetRadius (dGeomID g)
  85 {
  86   dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
  87   dxSphere *s = (dxSphere*) g;
  88   return s->radius;
  89 }
  90
  91
  92 dReal dGeomSpherePointDepth (dGeomID g, dReal x, dReal y, dReal z)
  93 {
  94   dUASSERT (g && g->type == dSphereClass,"argument not a sphere");
  95   g->recomputePosr();
  96
  97   dxSphere *s = (dxSphere*) g;
  98   dReal * pos = s->final_posr->pos;
  99   return s->radius - dSqrt ((x-pos[0])*(x-pos[0]) +
 100                             (y-pos[1])*(y-pos[1]) +
 101                             (z-pos[2])*(z-pos[2]));
 102 }
 103
 104 //****************************************************************************
 105 // pairwise collision functions for standard geom types
 106
 107 int dCollideSphereSphere (dxGeom *o1, dxGeom *o2, int flags,
 108                           dContactGeom *contact, int skip)
 109 {
 110   dIASSERT (skip >= (int)sizeof(dContactGeom));
 111   dIASSERT (o1->type == dSphereClass);
 112   dIASSERT (o2->type == dSphereClass);
 113   dIASSERT ((flags & NUMC_MASK) >= 1);
 114
 115   dxSphere *sphere1 = (dxSphere*) o1;
 116   dxSphere *sphere2 = (dxSphere*) o2;
 117
 118   contact->g1 = o1;
 119   contact->g2 = o2;
 120
 121   return dCollideSpheres (o1->final_posr->pos,sphere1->radius,
 122                           o2->final_posr->pos,sphere2->radius,contact);
 123 }
 124
 125
 126 int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags,
 127                        dContactGeom *contact, int skip)
 128 {
 129   dIASSERT (skip >= (int)sizeof(dContactGeom));
 130   dIASSERT (o1->type == dSphereClass);
 131   dIASSERT (o2->type == dBoxClass);
 132   dIASSERT ((flags & NUMC_MASK) >= 1);
 133
 134   // this is easy. get the sphere center `p' relative to the box, and then clip
 135   // that to the boundary of the box (call that point `q'). if q is on the
 136   // boundary of the box and |p-q| is <= sphere radius, they touch.
 137   // if q is inside the box, the sphere is inside the box, so set a contact
 138   // normal to push the sphere to the closest box face.
 139
 140   dVector3 l,t,p,q,r;
 141   dReal depth;
 142   int onborder = 0;
 143
 144   dxSphere *sphere = (dxSphere*) o1;
 145   dxBox *box = (dxBox*) o2;
 146
 147   contact->g1 = o1;
 148   contact->g2 = o2;
 149
 150   p[0] = o1->final_posr->pos[0] - o2->final_posr->pos[0];
 151   p[1] = o1->final_posr->pos[1] - o2->final_posr->pos[1];
 152   p[2] = o1->final_posr->pos[2] - o2->final_posr->pos[2];
 153
 154   l[0] = box->side[0]*REAL(0.5);
 155   t[0] = dDOT14(p,o2->final_posr->R);
 156   if (t[0] < -l[0]) { t[0] = -l[0]; onborder = 1; }
 157   if (t[0] >  l[0]) { t[0] =  l[0]; onborder = 1; }
 158
 159   l[1] = box->side[1]*REAL(0.5);
 160   t[1] = dDOT14(p,o2->final_posr->R+1);
 161   if (t[1] < -l[1]) { t[1] = -l[1]; onborder = 1; }
 162   if (t[1] >  l[1]) { t[1] =  l[1]; onborder = 1; }
 163
 164   t[2] = dDOT14(p,o2->final_posr->R+2);
 165   l[2] = box->side[2]*REAL(0.5);
 166   if (t[2] < -l[2]) { t[2] = -l[2]; onborder = 1; }
 167   if (t[2] >  l[2]) { t[2] =  l[2]; onborder = 1; }
 168
 169   if (!onborder) {
 170     // sphere center inside box. find closest face to `t'
 171     dReal min_distance = l[0] - dFabs(t[0]);
 172     int mini = 0;
 173     for (int i=1; i<3; i++) {
 174       dReal face_distance = l[i] - dFabs(t[i]);
 175       if (face_distance < min_distance) {
 176         min_distance = face_distance;
 177         mini = i;
 178       }
 179     }
 180     // contact position = sphere center
 181     contact->pos[0] = o1->final_posr->pos[0];
 182     contact->pos[1] = o1->final_posr->pos[1];
 183     contact->pos[2] = o1->final_posr->pos[2];
 184     // contact normal points to closest face
 185     dVector3 tmp;
 186     tmp[0] = 0;
 187     tmp[1] = 0;
 188     tmp[2] = 0;
 189     tmp[mini] = (t[mini] > 0) ? REAL(1.0) : REAL(-1.0);
 190     dMULTIPLY0_331 (contact->normal,o2->final_posr->R,tmp);
 191     // contact depth = distance to wall along normal plus radius
 192     contact->depth = min_distance + sphere->radius;
 193     return 1;
 194   }
 195
 196   t[3] = 0;                     //@@@ hmmm
 197   dMULTIPLY0_331 (q,o2->final_posr->R,t);
 198   r[0] = p[0] - q[0];
 199   r[1] = p[1] - q[1];
 200   r[2] = p[2] - q[2];
 201   depth = sphere->radius - dSqrt(dDOT(r,r));
 202   if (depth < 0) return 0;
 203   contact->pos[0] = q[0] + o2->final_posr->pos[0];
 204   contact->pos[1] = q[1] + o2->final_posr->pos[1];
 205   contact->pos[2] = q[2] + o2->final_posr->pos[2];
 206   contact->normal[0] = r[0];
 207   contact->normal[1] = r[1];
 208   contact->normal[2] = r[2];
 209   dNormalize3 (contact->normal);
 210   contact->depth = depth;
 211   return 1;
 212 }
 213
 214
 215 int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags,
 216                          dContactGeom *contact, int skip)
 217 {
 218   dIASSERT (skip >= (int)sizeof(dContactGeom));
 219   dIASSERT (o1->type == dSphereClass);
 220   dIASSERT (o2->type == dPlaneClass);
 221   dIASSERT ((flags & NUMC_MASK) >= 1);
 222
 223   dxSphere *sphere = (dxSphere*) o1;
 224   dxPlane *plane = (dxPlane*) o2;
 225
 226   contact->g1 = o1;
 227   contact->g2 = o2;
 228   dReal k = dDOT (o1->final_posr->pos,plane->p);
 229   dReal depth = plane->p[3] - k + sphere->radius;
 230   if (depth >= 0) {
 231     contact->normal[0] = plane->p[0];
 232     contact->normal[1] = plane->p[1];
 233     contact->normal[2] = plane->p[2];
 234     contact->pos[0] = o1->final_posr->pos[0] - plane->p[0] * sphere->radius;
 235     contact->pos[1] = o1->final_posr->pos[1] - plane->p[1] * sphere->radius;
 236     contact->pos[2] = o1->final_posr->pos[2] - plane->p[2] * sphere->radius;
 237     contact->depth = depth;
 238     return 1;
 239   }
 240   else return 0;
 241 }