/home/hauberg/Dokumenter/Capture/humim-tracker-0.1/src/OpenTissue/OpenTissue/core/geometry/geometry_quadric.h

Go to the documentation of this file.

#ifndef OPENTISSUE_CORE_GEOMETRY_GEOMETRY_QUADRIC_H
#define OPENTISSUE_CORE_GEOMETRY_GEOMETRY_QUADRIC_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 <OpenTissue/core/math/math_vector3.h>
#include <OpenTissue/core/math/math_matrix3x3.h>
#include <cmath>

namespace OpenTissue
{
  namespace geometry
  {

    template<typename real_type_>
    class Quadric
    {
    public:

      typedef real_type_                             real_type;
      typedef OpenTissue::math::Vector3<real_type>   vector3_type;
      typedef OpenTissue::math::Matrix3x3<real_type> matrix3x3_type;
      typedef Quadric<real_type>                     quadric_type;

    public:

      matrix3x3_type  m_A;
      vector3_type    m_B;
      real_type       m_C;

    public:

      Quadric()
        : m_A(1,0,0,0,1,0,0,0,1)
        , m_B(0,0,0)
        , m_C(0)
      {}

      Quadric(matrix3x3_type const & A,vector3_type const & B,real_type const & C)
        : m_A(A)
        , m_B(B)
        , m_C(C)
      {}

    public:

      real_type operator()(vector3_type const & x){ return x*(m_A*x) + 2.0*m_B*x + m_C; }

      quadric_type & operator=(quadric_type const & q)
      {
        m_A = q.m_A;
        m_B = q.m_B;
        m_C = q.m_C;
        return (*this);
      }

      quadric_type & operator*(real_type const & s)
      {
        m_A *= s;
        m_B *= s;
        m_C *= s;
        return (*this);
      }

      quadric_type operator+(quadric_type const & q){ return quadric_type( m_A + q.m_A, m_B + q.m_B, m_C + q.m_C ); }

    public:

      void set_sphere(real_type const & radius, vector3_type const & center)
      {
        m_A = matrix3x3_type( 1,0,0, 0,1,0, 0,0,1);
        m_B = - center;
        m_C = center*center - radius*radius;
      }

      void set_plane_product(vector3_type const & p,vector3_type const & np,vector3_type const & q,vector3_type const & nq)
      {
        //--- A   =  -  np nq^T
        //--- 2 B =  np (nq^T q)  + nq (np^T p)
        //--- C   =  - p^T np * q^T nq
        m_A   = - outer_prod(np,nq);
        real_type nqq = nq*q;
        real_type npp = np*p;
        m_B   = (np*nqq + nq*npp)*.5;
        m_C   = - npp*nqq;

        //--- Unfortunately we might be in a mess if A is
        //--- not symmetric, however computing A = (A+A^T)/2 solves
        //--- the problem, the quadric is still the same quadric
        m_A(0,1) += m_A(1,0);
        m_A(0,1) *= .5;
        m_A(0,2) += m_A(2,0);
        m_A(0,2) *= .5;
        m_A(1,2) += m_A(2,1);
        m_A(1,2) *= .5;
        m_A(1,0) = m_A(0,1);
        m_A(2,0) = m_A(0,2);
        m_A(2,1) = m_A(1,2);
      }

      void set_cylinder(
        vector3_type const & center
        , vector3_type const & axis0, real_type const & radius0
        , vector3_type const & axis1, real_type const & radius1
        )
      {
        matrix3x3_type A0  = outer_prod(axis0,axis0);
        matrix3x3_type A1  = outer_prod(axis1,axis1);
        real_type r0r0 = radius0*radius0;
        real_type r1r1 = radius1*radius1;
        m_A = r1r1*A0 + r0r0*A1;
        real_type ca0 = center*axis0;
        real_type ca1 = center*axis1;
        m_B = - (r1r1*ca0)*axis0 - (r0r0*ca1)*axis1;
        m_C = r1r1*ca0*ca0 + r0r0*ca1*ca1 - r0r0*r1r1;
      }

    };


    template<typename real_type,typename vector3_type>
    Quadric<real_type> make_sphere_quadric(real_type const & radius, vector3_type const & center)
    {
      Quadric<real_type> Q;
      Q.set_sphere(radius,center);
      return Q;
    }

    template<typename real_type,typename vector3_type>
    Quadric<real_type> make_cylinder_quadric(
      vector3_type const & center
      , vector3_type const & axis0
      , real_type    const & radius0
      , vector3_type const & axis1
      , real_type    const & radius1
      )
    {
      Quadric<real_type> Q;
      Q.set_cylinder(center,axis0,radius0,axis1,radius1);
      return Q;
    }

    template<typename vector3_type>
    Quadric<typename vector3_type::value_type> make_plane_product_quadric(vector3_type const & p,vector3_type const & np,vector3_type const & q,vector3_type const & nq)
    {
      Quadric<typename vector3_type::value_type> Q;
      Q.set_plane_product(p,np,q,nq);
      return Q;
    }

    template<typename quadric_type>
    bool is_valid_ellipsoid(quadric_type const & Q)
    {
      typename quadric_type::matrix3x3_type V;
      typename quadric_type::vector3_type   d;
      math::eigen(Q.m_A,V,d);

      if(d(0)<0)
        return false;
      if(d(1)<0)
        return false;
      if(d(2)<0)
        return false;
      return true;
    }

  } // namespace geometry
} // namespace OpenTissue

//OPENTISSUE_CORE_GEOMETRY_GEOMETRY_QUADRIC_H
#endif