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

Go to the documentation of this file.

#ifndef OPENTISSUE_COLLISION_COLLISION_PLANE_SDF_H
#define OPENTISSUE_COLLISION_COLLISION_PLANE_SDF_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 plane_type,typename sdf_geometry_type,typename contact_point_container>
    bool plane_sdf(
      coordsys_type     const & wcsP
      , plane_type        const & plane
      , coordsys_type     const & wcsG
      , sdf_geometry_type const & geometry
      , contact_point_container & contacts
      , double envelope = 0.01
      )
    {
      typedef typename contact_point_container::value_type         contact_point_type;
      typedef typename coordsys_type::vector3_type                 vector3_type;
      typedef typename vector3_type::value_type                    real_type;
      typedef typename sdf_geometry_type::const_point_iterator     const_point_iterator;

      contacts.clear();

      vector3_type c,n,n_wcs;
      coordsys_type GtoP;
      GtoP = model_update(wcsG,wcsP);

      n = plane.n();
      n_wcs = n;
      wcsP.xform_vector(n_wcs);

      bool collision = false;

      const_point_iterator end = geometry.m_sampling.end();
      const_point_iterator p   = geometry.m_sampling.begin();

      real_type tst = boost::numeric_cast<real_type>( envelope ); 

      for (;p!=end;++p)
      {
        c = (*p);
        GtoP.xform_point(c);

        real_type dist = plane.signed_distance(c);

        if(dist < tst)
        {
          c = (*p);
          wcsG.xform_point(c);

          contact_point_type cp;
          cp.m_p = c;
          cp.m_n = n_wcs;
          cp.m_distance = dist;
          contacts.push_back(cp);

          collision = true;
        }
      }
      return collision;
    }

  } //End of namespace collision

} // namespace OpenTissue

// OPENTISSUE_COLLISION_COLLISION_PLANE_SDF_H
#endif