/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/OpenTissue/OpenTissue/collision/collision_box_sphere.h

Go to the documentation of this file.

#ifndef OPENTISSUE_COLLISION_COLLISION_BOX_SPHERE_H
#define OPENTISSUE_COLLISION_COLLISION_BOX_SPHERE_H
//
// OpenTissue Template Library
// - A generic toolbox for physics-based modeling and simulation.
// Copyright (C) 2008 Department of Computer Science, University of Copenhagen.
//
// OTTL is licensed under zlib: http://opensource.org/licenses/zlib-license.php
//
#include <OpenTissue/configuration.h>

namespace OpenTissue
{
  namespace collision
  {

    template<typename coordsys_type,typename box_type,typename sphere_type,typename real_type,typename vector3_type>
    bool box_sphere(
      coordsys_type const & BtoWCS
      , coordsys_type const & StoWCS
      , box_type const & box
      , sphere_type const & sphere
      , real_type const & envelope
      , vector3_type & p
      , vector3_type & n
      , real_type & distance
      )
    {
      using std::fabs;
      using std::sqrt;

      //--- Transform sphere center into model frame of box
      coordsys_type StoB = model_update(StoWCS,BtoWCS);
      vector3_type c = sphere.center();
      StoB.xform_point(c);
      //--- get sphere radius (r) and half side lengths (called
      //--- extensions) of the box (a)
      real_type r = sphere.radius();
      vector3_type a = box.ext();
      //--- Cut line from center of box to center of sphere by the box
      //--- faces. This is done in the model frame of the box.
      bool inside = true;
      p = c;
      if(c(0)>a(0))
      {
        p(0) = a(0);
        n(0) = 1;
        inside = false;
      }
      if(c(0) < -a(0))
      {
        p(0) = -a(0);
        n(0) = -1;
        inside = false;
      }
      if(c(1)>a(1))
      {
        p(1) = a(1);
        n(1) = 1;
        inside = false;
      }
      if(c(1) < -a(1))
      {
        p(1) = -a(1);
        n(1) = -1;
        inside = false;
      }
      if(c(2) > a(2))
      {
        p(2) = a(2);
        n(2) = 1;
        inside = false;
      }
      if(c(2) < -a(2))
      {
        p(2) = -a(2);
        n(2) = -1;
        inside = false;
      }
      //--- This is the center-line did intersect the box faces or
      //--- if the sphere center lies inside the box.
      if(inside)
      {
        //--- We search for the face of the box closest to the center of the sphere.
        //---
        //--- We then set the contact point equal to the deepst point of the sphere (in
        //--- direction of the normal), and the contact normal equal to the normal of
        //--- the closest box face.
        //---
        //--- Penetration depth is simply the distance between the sphere and the
        //--- closest face plus the radius of the sphere.
        int cnt_closest_faces = 0;

        p.clear();
        vector3_type f = a - fabs( c );
        //f(0) = a(0) - fabs( c(0) );
        //f(1) = a(1) - fabs( c(1) );
        //f(2) = a(2) - fabs( c(2) );
        if(f(0) <= f(1) && f(0) <= f(2))
        {
          distance = -f(0) -r;
          n(0) = (c(0) > 0)?1:-1;
          ++cnt_closest_faces;
          p(0) = (c(0) > 0)?a(0):-a(0);
        }
        if(f(1) <= f(0) && f(1) <= f(2))
        {
          distance = -f(1) -r;
          n(1) = (c(1) > 0)?1:-1;
          ++cnt_closest_faces;
          p(1) = (c(1) > 0)?a(1):-a(1);
        }
        if(f(2) <= f(0) && f(2) <= f(1))
        {
          distance = -f(2) -r;
          n(2) = (c(2) >0)?1:-1;
          ++cnt_closest_faces;
          p(2) = (c(2) > 0)?a(2):-a(2);
        }
        if(cnt_closest_faces>1)//--- more than one closest face?
        {
          n = unit(n);
          f = p-c; //--- reuse f
          distance = - sqrt(f*f) - r;
        }
        //p = c - n*r; //--- point on sphere
        p = c - n*(distance+r); //--- point on box
      }
      else
      {
        //--- Sphere center was outside box, contact is generated by using the
        //--- intersection point at as contact point, and contact normal is set
        //--- equal to the normal of the box at the intersection point.
        //--- Separation (or penetration) distance is equal to the distance
        //--- between the intersection point and the sphere center (projected
        //--- onto the contact normal) minus the sphere radius.
        real_type tmp = sqrt(n*n);
        n /= tmp;
        vector3_type diff = c - p;
        //--- Bug-reported by Stefan Glimberg!!!
        //distance = diff*n - r;  
        distance = sqrt(diff*diff) - r;
      }
      if(distance>envelope)
        return false;
      BtoWCS.xform_vector(n);
      BtoWCS.xform_point(p);
      return true;
    }

  } //End of namespace collision

} //End of namespace OpenTissue

// OPENTISSUE_COLLISION_COLLISION_BOX_SPHERE_H
#endif