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

Go to the documentation of this file.

#ifndef OPENTISSUE_COLLISION_GJK_GJK_SIMPLEX_H
#define OPENTISSUE_COLLISION_GJK_GJK_SIMPLEX_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>

#include <stdexcept>

#include <cassert>

namespace OpenTissue
{
  namespace gjk
  {

    template<typename vector3_type>
    class Simplex
    {
    public:

      typedef typename vector3_type::value_traits  value_traits;
      typedef typename vector3_type::value_type    real_type;

    public:

      int m_bitmask;    







      vector3_type m_v[4];    
      real_type    m_w[4];    
      vector3_type m_a[4];    
      vector3_type m_b[4];    

    public:

      Simplex()
        : m_bitmask(0)
      {
        m_v[0].clear();
        m_v[1].clear();
        m_v[2].clear();
        m_v[3].clear();

        m_a[0].clear();
        m_a[1].clear();
        m_a[2].clear();
        m_a[3].clear();

        m_b[0].clear();
        m_b[1].clear();
        m_b[2].clear();
        m_b[3].clear();

        m_w[0] = value_traits::zero();
        m_w[1] = value_traits::zero();
        m_w[2] = value_traits::zero();
        m_w[3] = value_traits::zero();
      }

    };

    template<typename V>
    inline bool is_point_in_simplex( V const & p, Simplex<V> const & S)
    {
      // Note that p would be a support point of a convex set.
      //
      // We will work under the two assumptions:
      //
      // Assumption 1: Allthough a support function need not be onto we will assume
      // that if a support function will return the same support point
      // given the same search direction.
      //
      // Assumption 2:  Also we will assume that a support point is allways a corner
      // point of the convex hull of the convex set.
      //
      // Thus we know that any simplex would consist of support points that
      // are corners of the convex hull. Thus if suffice to check if p is one
      // of vertices of the simplex.
      //
      // Let us assume that p lies on some point on the boundary of the
      // simplex. Assume further that p is not the same as one of the
      // simplex vertices.
      //
      // This would imply that p is lying inside a face of the simplex (remember
      // a face of a simplex is simply the set spanned by a subset of the
      // simplex vertices). 
      //
      // Let v_i, v_j, and v_k be any three vertices of the face containing p.
      // An edge would have just two vertices and we leave this simpler case
      // to be proven by the reader. Thus continuing with our face we know by
      // the definition of a simplex that we can write
      //
      //   p = a_i v_i + a_j v_j + a_k v_k
      //
      // where 0 <= a_i, a_j, a_k <= 1 and a_i + a_j + a_k = 1
      //
      // However since v_i, v_j and v_k by definition are corners
      // of the convex hull. We know that all points of the form
      //
      //   q = b_i v_i + b_j v_j + b_k v_k
      //
      // where 0 <= b_i, b_j, b_k <= 1 and b_i + b_j + b_k = 1 lies
      // on the boundary of the convex hull. This contradicts the
      // assumption that p is a corner vertex of the convex hull.
      //
      // Thus the only way for p to be a vertex of the convex hull
      // is that if p is the same as one of the simplex vertices.
      //
      int used_bit = 1;
      for(size_t i = 0u; i < 4u; ++i)
      {
        if( (S.m_bitmask & used_bit) && (p == S.m_v[i]) )
          return true;
        used_bit <<= 1;
      }
      return false;
    }

    template<typename V>
    inline void add_point_to_simplex( V const & p, V const & p_a, V const & p_b , Simplex<V> & S)
    {
      typedef typename V::value_traits   value_traits;

      size_t free_idx  = 0u;
      int free_bit = 1;        

      // Loop until we find a free bit, that is a bit in the simplex bitmask that is set low.
      while(S.m_bitmask & free_bit)
      {
        ++free_idx;
        free_bit <<= 1;
      }

      if( free_idx >= 4u )
        throw std::logic_error("Simplex did not have any empty entries");

      S.m_a[free_idx] = p_a;
      S.m_b[free_idx] = p_b;
      S.m_v[free_idx] = p;
      S.m_w[free_idx] = value_traits::zero();

      // Update bitmask to reflect that the free bit is no longer free
      S.m_bitmask |= free_bit;       
    }

    template<typename V>
    inline size_t dimension(Simplex<V> const & S) 
    {  
      size_t size = 0u;

      if( S.m_bitmask & 1 )
        ++size;
      if( S.m_bitmask & 2 )
        ++size;
      if( S.m_bitmask & 4 )
        ++size;
      if( S.m_bitmask & 8 )
        ++size;
      return size;
    }

    template<typename V>
    inline bool is_full_simplex(Simplex<V> const & S) 
    {  
      return (S.m_bitmask == 15u);
    }

    inline void get_used_indices( int const & bitmask, size_t & idx_A, int & bit_A)
    {
     if(bitmask == 0u) 
       throw std::invalid_argument("no bits was set high in bitmask");

     size_t idx = 0u;

      bit_A = 1;
      for(; idx < 4; ++idx)
      {
        if( bitmask & bit_A )
          break;
        bit_A <<= 1;
      }
      idx_A = idx;

      assert( bit_A==1 || bit_A==2 || bit_A==4 || bit_A==8 || !"get_used_indices(): bit A was illegal");
      assert( idx_A >= 0u            || !"get_used_indices(): illegal index A value");
      assert( idx_A < 4u             || !"get_used_indices(): illegal index A value");
      assert( ((1 << idx_A) & bit_A) || !"get_used_indices(): mismatch between idx A and bit A");
    }

    inline void get_used_indices(
      int const & bitmask
      , size_t & idx_A
      , int & bit_A
      , size_t & idx_B
      , int & bit_B
      )
    {
     if(bitmask == 0u) 
       throw std::invalid_argument("no bits was set high in bitmask");

      size_t idx = 0u;

      bit_A = 1;
      for(; idx < 4u; ++idx)
      {
        if( bitmask & bit_A )
          break;
        bit_A <<= 1;
      }
      idx_A = idx++;

      bit_B = bit_A << 1;
      for(; idx < 4u; ++idx)
      {
        if( bitmask & bit_B )
          break;
        bit_B <<= 1;
      }
      idx_B = idx;

      if(idx_B==4u)
        throw std::logic_error("simplex only had one used entry");

      assert( bit_A==1 || bit_A==2 || bit_A==4 || bit_A==8 || !"get_used_indices(): bit A was illegal");
      assert( idx_A >= 0u            || !"get_used_indices(): illegal index A value");
      assert( idx_A < 4u             || !"get_used_indices(): illegal index A value");
      assert( ((1 << idx_A) & bit_A) || !"get_used_indices(): mismatch between idx A and bit A");

      assert( bit_B==1 || bit_B==2 || bit_B==4 || bit_B==8 || !"get_used_indices(): bit B was illegal");
      assert( idx_B >= 0u            || !"get_used_indices(): illegal index B value");
      assert( idx_B < 4u             || !"get_used_indices(): illegal index B value");
      assert( ((1 << idx_B) & bit_B) || !"get_used_indices(): mismatch between idx B and bit B");
      
      assert( ((bit_A & bit_B) == 0) || !"get_used_indices(): Bit A and Bit B was the same");
      assert(  idx_A < idx_B         || !"get_used_indices(): idx A should be smaller than idx B");
    }

    inline void get_used_indices( 
      int const & bitmask
      , size_t & idx_A
      , int & bit_A
      , size_t & idx_B
      , int & bit_B
      , size_t & idx_C
      , int & bit_C
      )
    {
     if(bitmask == 0u) 
       throw std::invalid_argument("no bits was set high in bitmask");

     size_t idx = 0u;

      bit_A = 1;
      for(; idx < 4; ++idx)
      {
        if( bitmask & bit_A )
          break;
        bit_A <<= 1;
      }
      idx_A = idx++;

      bit_B = bit_A << 1;
      for(; idx < 4; ++idx)
      {
        if( bitmask & bit_B )
          break;
        bit_B <<= 1;
      }
      idx_B = idx++;

      if(idx_B==4u)
        throw std::logic_error("simplex only had one used entry");

      bit_C = bit_B << 1;
      for(; idx < 4; ++idx)
      {
        if( bitmask & bit_C )
          break;
        bit_C <<= 1;
      }
      idx_C = idx;

      if(idx_C==4u)
        throw std::logic_error("simplex only had two used entries");


      assert( bit_A==1 || bit_A==2 || bit_A==4 || bit_A==8 || !"get_used_indices(): bit A was illegal");
      assert( idx_A >= 0u            || !"get_used_indices(): illegal index A value");
      assert( idx_A < 4u             || !"get_used_indices(): illegal index A value");
      assert( ((1 << idx_A) & bit_A) || !"get_used_indices(): mismatch between idx A and bit A");

      assert( bit_B==1 || bit_B==2 || bit_B==4 || bit_B==8 || !"get_used_indices(): bit B was illegal");
      assert( idx_B >= 0u            || !"get_used_indices(): illegal index B value");
      assert( idx_B < 4u             || !"get_used_indices(): illegal index B value");
      assert( ((1 << idx_B) & bit_B) || !"get_used_indices(): mismatch between idx B and bit B");
      
      assert( ((bit_A & bit_B) == 0) || !"get_used_indices(): Bit A and Bit B was the same");
      assert(  idx_A < idx_B         || !"get_used_indices(): idx A should be smaller than idx B");

      assert( bit_C==1 || bit_C==2 || bit_C==4 || bit_C==8 || !"get_used_indices(): bit B was illegal");
      assert( idx_C >= 0u            || !"get_used_indices(): illegal index B value");
      assert( idx_C < 4u             || !"get_used_indices(): illegal index B value");
      assert( ((1 << idx_C) & bit_C) || !"get_used_indices(): mismatch between idx B and bit B");
      
      assert( ((bit_A & bit_C) == 0) || !"get_used_indices(): Bit A and Bit C was the same");
      assert( ((bit_B & bit_C) == 0) || !"get_used_indices(): Bit B and Bit C was the same");
      assert(  idx_B < idx_C         || !"get_used_indices(): idx B should be smaller than idx C");

    }

  } // namespace gjk

} // namespace OpenTissue

// OPENTISSUE_COLLISION_GJK_GJK_SIMPLEX_H
#endif