geom.h

mean-shift. pointer sample

/*! \file

\verbatim

Copyright (c) 2004, Sylvain Paris and Francois Sillion
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

    * Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.
    
    * Redistributions in binary form must reproduce the above
    copyright notice, this list of conditions and the following
    disclaimer in the documentation and/or other materials provided
    with the distribution.

    * Neither the name of ARTIS, GRAVIR-IMAG nor the names of its
    contributors may be used to endorse or promote products derived
    from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

\endverbatim

 *  This file contains code made by Sylvain Paris under supervision of
 * Fran鏾is Sillion for his PhD work with <a
 * href="http://www-artis.imag.fr">ARTIS project</a>. ARTIS is a
 * research project in the GRAVIR/IMAG laboratory, a joint unit of
 * CNRS, INPG, INRIA and UJF.
 *
 *  Use <a href="http://www.stack.nl/~dimitri/doxygen/">Doxygen</a>
 * with DISTRIBUTE_GROUP_DOC option to produce an nice html
 * documentation.
 *
 *  Defines matrices and vectors with basic operators: * / + -...
 *
 *  Dimensions and scalar type are templated. Usual types are
 * predefined: Vec3f, Vec2D, Matrix44f, etc.
 *
 *  Visual studio may cause trouble, depending on its version.
 */


#ifndef __GEOM__
#define __GEOM__

#include <cmath>

#include <vector>
#include <iostream>

namespace Geometry {

/*

  ################
  # typedefs ... #
  ################

*/


  template<int N,typename Real> class Vec;
  template<typename Real>       class Vec2;
  template<typename Real>       class Vec3;
  template<typename Real>       class Hvec2;
  template<typename Real>       class Hvec3;

  template<int N_row,int N_col,typename Real> class Matrix;
  template<int N,typename Real>               class Square_matrix;

  typedef Vec2<int>     Vec2i;
  typedef Vec2<float>   Vec2f;
  typedef Vec2<double>  Vec2d;

  typedef Vec3<int>     Vec3i;
  typedef Vec3<float>   Vec3f;
  typedef Vec3<double>  Vec3d;

  typedef Hvec2<int>    Hvec2i;
  typedef Hvec2<float>  Hvec2f;
  typedef Hvec2<double> Hvec2d;

  typedef Hvec3<int>    Hvec3i;
  typedef Hvec3<float>  Hvec3f;
  typedef Hvec3<double> Hvec3d;

  typedef Square_matrix<2,int>    Matrix22i;
  typedef Square_matrix<2,float>  Matrix22f;
  typedef Square_matrix<2,double> Matrix22d;

  typedef Square_matrix<3,int>    Matrix33i;
  typedef Square_matrix<3,float>  Matrix33f;
  typedef Square_matrix<3,double> Matrix33d;

  typedef Square_matrix<4,int>    Matrix44i;
  typedef Square_matrix<4,float>  Matrix44f;
  typedef Square_matrix<4,double> Matrix44d;


  
  
/*

  #############
  # class Vec #
  #############

*/

  //@{
  //! Pre-declaration needed for friend declarations.
  
  template<int N,typename Real>
  inline typename Vec<N,Real>::value_type operator*(const Vec<N,Real>& v1,
						    const Vec<N,Real>& v2);

  
  template<int N,typename Real>
  std::ostream& operator<<(std::ostream& s,
			   const Vec<N,Real>& v);
  //@}


  //! Represents a vector with fixed dimension \e N with coordinates
  //! of type \e Real.
  template<int N,typename Real>
  class Vec{

  public:
    typedef Real value_type;
    static const unsigned int dimension = N;
    
    //@{
    //! Classical constructor.

    inline Vec();
    explicit inline Vec(const value_type tab[N]);
    explicit inline Vec(const std::vector<value_type>& tab);
    inline Vec(const Vec<N,Real>& v);
    //@}

    //@{
    //! Classical access operator.
  
    inline const value_type& operator[](const int i) const;
    inline value_type& operator[](const int i);
    //@}


    //@{
    //! Classical value.
  
    inline value_type norm() const;
    inline value_type square_norm() const;
    //@}

    //! Normalizes the vector in place.
    inline Vec<N,Real>& normalize();

    //! Return the corresponding unit vector.
    inline Vec<N,Real> unit() const;

    //! Return the corresponding column matrix.
    inline Matrix<N,1,Real> column_matrix() const;

    //@{
    //! Classical operator.

    inline Vec<N,Real>& operator=(const Vec<N,Real>& v);
    inline bool operator==(const Vec<N,Real>& v) const;
    inline bool operator!=(const Vec<N,Real>& v) const;
    inline Vec<N,Real>& operator+=(const Vec<N,Real>& v);
    inline Vec<N,Real>& operator-=(const Vec<N,Real>& v);
    inline Vec<N,Real>& operator*=(const value_type r);
    inline Vec<N,Real>& operator/=(const value_type r);
    inline Vec<N,Real> operator-() const;
    //@}
    
    friend value_type operator*<N,Real>(const Vec<N,Real>& v1,
					const Vec<N,Real>& v2);

    template <int Row,int Col,typename Real_t>
    friend inline Vec<Row,Real_t>
    operator*(const Matrix<Row,Col,Real_t>& m,
	      const Vec<Col,Real_t>& v);

  
    friend std::ostream& operator<<<N,Real>(std::ostream& s,
					    const Vec<N,Real>& v);


  private:
    value_type coordinate[N];
  };


  

/*

  ########################
  # Comparison predicate #
  ########################

*/


  //! < on the Nth coordinate.
  template<typename Vector,int N> class Compare_coordinate {
  public:
    inline Compare_coordinate(){}
    
    inline bool operator()(const Vector& v1,const Vector v2) const{
      return (v1[N]<v2[N]);
    }
  };



  template<typename Vector> class Lexicographical_order {
  public:
    inline Lexicographical_order(){}
    
    inline bool operator()(const Vector& v1,const Vector v2) const{

      for(unsigned int n = 0;n < Vector::dimension;n++){
	
	const typename Vector::value_type v1_n = v1[n];
	const typename Vector::value_type v2_n = v2[n];
	  
	if (v1_n < v2_n) return true;
	if (v1_n > v2_n) return false;
      }
      return false;
    }
  };

  

/*

  ##############
  # class Vec2 #
  ##############

*/


  //! Represents a vector of dimension 2 with \e Real coordinates.
  template<typename Real>
  class Vec2:public Vec<2,Real>{
  
  public:

    //! Used type.
    typedef typename Vec<2,Real>::value_type value_type;

    //@{
    //! Classical constructor.

    inline Vec2();
    explicit inline Vec2(const value_type tab[2]);
    explicit inline Vec2(const std::vector<value_type>& tab);
    inline Vec2(const Vec<2,Real>& v);
    //@}
    
    //! 2D constructor.
    inline Vec2(const value_type x,const value_type y);

    //@{
    //! 2D access.
    
    inline const value_type& x() const;
    inline value_type& x();

    inline const value_type& y() const;
    inline value_type& y();
    //@}
  };



/*

  ##############
  # class Vec3 #
  ##############

*/



//! Represents a vector of dimension 3 with \e Real coordinates.
  template<typename Real>
  class Vec3:public Vec<3,Real>{
  
  public:
    
    //! Used type.
    typedef typename Vec<3,Real>::value_type value_type;

    //@{
    //! Classical constructor.

    inline Vec3();
    explicit inline Vec3(const value_type tab[3]);
    explicit inline Vec3(const std::vector<value_type>& tab);
    inline Vec3(const Vec<3,Real>& v);
    //@}

    //@{
    //! 3D constructor.
  
    inline Vec3(const value_type x,const value_type y,const value_type z=0);
    inline Vec3(const Vec2<Real>& v,const value_type z=0);
    inline Vec3(const Hvec3<Real>& v);
    //@}

    //@{
    //! 3D access.
    
    inline const value_type& x() const;
    inline value_type& x();

    inline const value_type& y() const;
    inline value_type& y();

    inline const value_type& z() const;
    inline value_type& z();
    //@}

    //! Produit vectoriel.
    template<typename Real_t>
    friend Vec3<Real_t> operator^(const Vec3<Real_t>& v1,
				  const Vec3<Real_t>& v2);

  };


  
/*

  ###############
  # class Hvec2 #
  ###############

*/

  
  //! Represents a vector of dimension 2 with \e Real homogeneous coordinates.
/*!
  The vector has 3 coordinates: \e (sx,sy,s).
*/
  template<typename Real>
  class Hvec2:public Vec<3,Real>{
  
  public:

    //! Used type.
    typedef typename Vec<3,Real>::value_type value_type;

    //@{
    //! Classical constructor.

    inline Hvec2();
    explicit inline Hvec2(const value_type tab[3]);
    explicit inline Hvec2(const std::vector<value_type>& tab);
    inline Hvec2(const Vec<3,Real>& v);
    //@}

    //@{
    //! 2D homogeneous constructor.
  
    inline Hvec2(const value_type sx,
		 const value_type sy,
		 const value_type s=1);
    
    inline Hvec2(const Vec2<Real>& sv,
		 const value_type s=1);
    
    //@}

    //@{
    //! 2D homogeneous access.
    
    inline const value_type& sx() const;
    inline value_type& sx();

    inline const value_type& sy() const;
    inline value_type& sy();

    inline const value_type& s() const;
    inline value_type& s();
    //@}

    //@{
    //! Access as a 3D entity.

    inline value_type x() const;
    inline value_type y() const;
    inline value_type z() const;
    //@}
  };



/*

  ###############
  # class Hvec3 #
  ###############

*/

//! Represents a vector of dimension 3 with \e Real homogeneous coordinates.
/*!
  The vector has 3 coordinates: \e (sx,sy,sz,s).
*/
  template<typename Real>
  class Hvec3:public Vec<4,Real>{
  
  public:

    //! Used type.
    typedef typename Vec<4,Real>::value_type value_type;

    //@{
    //! Classical constructor.

    inline Hvec3();
    explicit inline Hvec3(const value_type tab[4]);
    explicit inline Hvec3(const std::vector<value_type>& tab);
    inline Hvec3(const Vec<4,Real>& v);
    //@}

    //@{
    //! 3D homogeneous constructor.
  
    inline Hvec3(const value_type sx,
		 const value_type sy,
		 const value_type sz=0,
		 const value_type s=1);
    
    inline Hvec3(const Vec2<Real>& sv,
		 const value_type sz=0,
		 const value_type s=1);
    
    inline Hvec3(const Vec3<Real>& sv,
		 const value_type s=1);  
    //@}

    //@{
    //! 3D homogeneous access.
    
    inline const value_type& sx() const;
    inline value_type& sx();

    inline const value_type& sy() const;
    inline value_type& sy();

    inline const value_type& sz() const;
    inline value_type& sz();

    inline const value_type& s() const;
    inline value_type& s();
    //@}

    //@{
    //! Access as a non-homogenous 3D entity.

    inline value_type x() const;
    inline value_type y() const;
    inline value_type z() const;
    //@}
  };



/*

  ################
  # class Matrix #
  ################

*/

  //@{
  //! Pre-declaration needed for friend declarations.
  
  template<int N_row,int N_col,typename Real>
  inline std::ostream& operator<<(std::ostream& s,
				  const Matrix<N_row,N_col,Real>& m);


  template<int N_row,int N_col,typename Real>
  inline Matrix<N_row,N_col,Real>
  operator+(const Matrix<N_row,N_col,Real>& m1,
	    const Matrix<N_row,N_col,Real>& m2);


  template<int N_row,int N_col,typename Real>
  inline Matrix<N_row,N_col,Real>
  operator-(const Matrix<N_row,N_col,Real>& m1,
	    const Matrix<N_row,N_col,Real>& m2);


  template<int N_row,int N_col,typename Real>
  inline Matrix<N_row,N_col,Real>
  operator*(const Matrix<N_row,N_col,Real>& m,
	    const typename Matrix<N_row,N_col,Real>::value_type lambda);


  template<int N_row,int N_col,typename Real>
  inline Matrix<N_row,N_col,Real>
  operator*(const typename Matrix<N_row,N_col,Real>::value_type lambda,
	    const Matrix<N_row,N_col,Real>& m);

  
  template<int N,int P,int Q,typename Real_t>
  inline Matrix<N,Q,Real_t>
  operator*(const Matrix<N,P,Real_t>& m1,
	      const Matrix<P,Q,Real_t>& m2);


  template<int N_row,int N_col,typename Real>
  inline Matrix<N_row,N_col,Real>
  operator/(const Matrix<N_row,N_col,Real>& m,
	    const typename Matrix<N_row,N_col,Real>::value_type lambda);
  
  
  template <int Row,int Col,typename Real_t>
  inline Vec<Row,Real_t>
  operator*(const Matrix<Row,Col,Real_t>& m,
	    const Vec<Col,Real_t>& v); 
  //@}

  
  
  template<int N_row,int N_col,typename Real>
  class Matrix{

  public:

    //! Used type.
    typedef Real value_type;
    
    //@{
    //! Classical constructor.
  
    inline Matrix();
    explicit inline Matrix(const value_type tab[N_row][N_col]);
    inline Matrix(const Matrix<N_row,N_col,Real>& m);
    //@}

    inline Matrix<N_col,N_row,Real> transpose() const;

    //@{
    //! Classical operator.
  
    inline Matrix<N_row,N_col,Real>& operator=(const Matrix<N_row,N_col,Real>& m);
    inline Matrix<N_row,N_col,Real>& operator+=(const Matrix<N_row,N_col,Real>& m);
    inline Matrix<N_row,N_col,Real>& operator-=(const Matrix<N_row,N_col,Real>& m);
    inline Matrix<N_row,N_col,Real>& operator*=(const value_type& lambda);
    inline Matrix<N_row,N_col,Real>& operator/=(const value_type& lambda);
    inline Matrix<N_row,N_col,Real> operator-() const;
    //@}

    //@{
    //! Classical access operator.

    inline const value_type& operator()(const int i,const int j) const;
    inline value_type& operator()(const int i,const int j);

    inline const value_type& operator[](const Vec2i& v) const;
    inline value_type& operator[](const Vec2i& v);
    //@}

    //! Creates a single-column vector with all the elements.
    inline Vec<N_row * N_col,Real> unfold_to_vector() const;
    
    //! Fills the matrix from a single-column vector.
    inline Matrix<N_row,N_col,Real>&
    fold_from_vector(const Vec<N_row * N_col,Real>& v);
  
    //@{
    //! Classical operator.

    friend Matrix<N_row,N_col,Real>
    operator+<N_row,N_col,Real>(const Matrix<N_row,N_col,Real>& m1,
				const Matrix<N_row,N_col,Real>& m2);

//     friend Matrix<N_row,N_col,Real>
//     operator-<N_row,N_col,Real>(const Matrix<N_row,N_col,Real>& m1,
// 				const Matrix<N_row,N_col,Real>& m2);
    
    friend Matrix<N_row,N_col,Real>
    operator*<N_row,N_col,Real>(const Matrix<N_row,N_col,Real>& m,
				const value_type lambda);

    friend Matrix<N_row,N_col,Real>
    operator*<N_row,N_col,Real>(const value_type lambda,
				const Matrix<N_row,N_col,Real>& m);

    template<int N,int P,int Q,typename Real_t>
    friend Matrix<N,Q,Real_t>
    operator*(const Matrix<N,P,Real_t>& m1,
	      const Matrix<P,Q,Real_t>& m2);

    friend Matrix<N_row,N_col,Real>
    operator/<N_row,N_col,Real>(const Matrix<N_row,N_col,Real>& m,
				const value_type lambda);
 
    friend Vec<N_row,Real>
    operator*<N_row,N_col,Real>(const Matrix<N_row,N_col,Real>& m,
				const Vec<N_col,Real>& v);  


    friend std::ostream& operator<<<N_row,N_col,Real>(std::ostream& s,
						      const Matrix<N_row,N_col,Real>& m);

    //@}


    //@{
    //! Row and column operators

    inline void swap_rows(const int row1,const int row2);
    inline void multiply_row(const int row,const value_type lambda);
    inline Vec<N_col,Real> get_vector_from_row(const int row) const;
    inline void add_vector_to_row(const int row,const Vec<N_col,Real>& vec);

    //@}
    
  private: