geom.h

mean-shift. pointer sample

  Hvec3<Real>::Hvec3(const Vec3<Real>& sv,const value_type s)
    :Vec<4,Real>(){
  
    (*this)[0] = sv.x();
    (*this)[1] = sv.y();
    (*this)[2] = sv.z();
    (*this)[3] = s;      
  }
//@}


  template<typename Real>
  const typename Hvec3<Real>::value_type&
  Hvec3<Real>::sx() const{
    return (*this)[0];
  }


  template<typename Real>
  typename Hvec3<Real>::value_type&
  Hvec3<Real>::sx(){
    return (*this)[0];
  }


  template<typename Real>
  const typename Hvec3<Real>::value_type&
  Hvec3<Real>::sy() const{
    return (*this)[1];
  }


  template<typename Real>
  typename Hvec3<Real>::value_type&
  Hvec3<Real>::sy(){
    return (*this)[1];
  }


  template<typename Real>
  const typename Hvec3<Real>::value_type&
  Hvec3<Real>::sz() const{
    return (*this)[2];
  }


  template<typename Real>
  typename Hvec3<Real>::value_type&
  Hvec3<Real>::sz(){
    return (*this)[2];
  }


  template<typename Real>
  const typename Hvec3<Real>::value_type&
  Hvec3<Real>::s() const{
    return (*this)[3];
  }


  template<typename Real>
  typename Hvec3<Real>::value_type&
  Hvec3<Real>::s(){
    return (*this)[3];
  }


  template<typename Real>
  typename Hvec3<Real>::value_type
  Hvec3<Real>::x() const{
    return ((*this)[0]/(*this)[3]);
  }


  template<typename Real>
  typename Hvec3<Real>::value_type
  Hvec3<Real>::y() const{
    return ((*this)[1]/(*this)[3]);
  }


  template<typename Real>
  typename Hvec3<Real>::value_type
  Hvec3<Real>::z() const{
    return ((*this)[2]/(*this)[3]);
  }



/*

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

*/



  template<int N_row,int N_col,typename Real>
  Matrix<N_row,N_col,Real>::Matrix(){
    for(int i=0;i<N_row;i++){
      for(int j=0;j<N_col;j++){
	component[i][j] = 0;
      }
    }
  }


  template<int N_row,int N_col,typename Real>
  Matrix<N_row,N_col,Real>::Matrix(const value_type tab[N_row][N_col]){

    for(int i=0;i<N_row;i++){
      for(int j=0;j<N_col;j++){
	component[i][j] = tab[i][j];
      }
    }
  }


  template<int N_row,int N_col,typename Real>
  Matrix<N_row,N_col,Real>::Matrix(const Matrix<N_row,N_col,Real>& m){
    for(int i=0;i<N_row;i++){
      for(int j=0;j<N_col;j++){
	component[i][j] = m.component[i][j];
      }
    }
  }


  template<int N_row,int N_col,typename Real>
  Matrix<N_col,N_row,Real>
  Matrix<N_row,N_col,Real>::
  transpose() const{
  
    Matrix<N_col,N_row,Real> res;

    for(int i = 0;i < N_row;++i){
      for(int j = 0;j < N_col;++j){
	res(j,i) = component[i][j];
      }
    }

    return res;
  }


  template<int N_row,int N_col,typename Real>
  const typename Matrix<N_row,N_col,Real>::value_type&
  Matrix<N_row,N_col,Real>::
  operator()(const int i,const int j) const{
    return component[i][j];
  }


  template<int N_row,int N_col,typename Real>
  typename Matrix<N_row,N_col,Real>::value_type&
  Matrix<N_row,N_col,Real>::
  operator()(const int i,const int j){
    return component[i][j];
  }


  template<int N_row,int N_col,typename Real>
  const typename Matrix<N_row,N_col,Real>::value_type&
  Matrix<N_row,N_col,Real>::
  operator[](const Vec2i& v) const{
    return component[v.x()][v.y()];
  }


  template<int N_row,int N_col,typename Real>
  typename Matrix<N_row,N_col,Real>::value_type&
  Matrix<N_row,N_col,Real>::
  operator[](const Vec2i& v){
    return component[v.x()][v.y()];
  }

  
  template<int N_row,int N_col,typename Real>
  Vec<N_row * N_col,Real>
  Matrix<N_row,N_col,Real>::unfold_to_vector() const{
    Vec<N_row * N_col,Real> v;

    for(int i = 0;i < N_row;++i){
      for(int j = 0;j < N_col;++j){
	v[i * N_col + j] = component[i][j];
      }
    }
 
    return v;
  }

  
  template<int N_row,int N_col,typename Real>
  Matrix<N_row,N_col,Real>&
  Matrix<N_row,N_col,Real>::fold_from_vector(const Vec<N_row * N_col,Real>& v){
    
    for(int i = 0;i < N_row;++i){
      for(int j = 0;j < N_col;++j){
	component[i][j] = v[i * N_col + j];
      }
    }
    return *this;
  }
  

  template<int N_row,int N_col,typename Real>
  Matrix<N_row,N_col,Real>&
  Matrix<N_row,N_col,Real>::
  operator=(const Matrix<N_row,N_col,Real>& m){

    if (this!=&m){
      for(int i=0;i<N_row;i++){
	for(int j=0;j<N_col;j++){
	  component[i][j] = m.component[i][j];
	}
      }
    }

    return *this;
  }


  template<int N_row,int N_col,typename Real>
  Matrix<N_row,N_col,Real>&
  Matrix<N_row,N_col,Real>::
  operator+=(const Matrix<N_row,N_col,Real>& m){

    for(int i=0;i<N_row;i++){
      for(int j=0;j<N_col;j++){
	component[i][j] += m.component[i][j];
      }
    }

    return *this;
  }


  template<int N_row,int N_col,typename Real>
  Matrix<N_row,N_col,Real>&
  Matrix<N_row,N_col,Real>::
  operator-=(const Matrix<N_row,N_col,Real>& m){

    for(int i=0;i<N_row;i++){
      for(int j=0;j<N_col;j++){
	component[i][j] -= m.component[i][j];
      }
    }

    return *this;
  }


  template<int N_row,int N_col,typename Real>
  Matrix<N_row,N_col,Real>&
  Matrix<N_row,N_col,Real>::
  operator*=(const value_type& lambda){

    for(int i=0;i<N_row;i++){
      for(int j=0;j<N_col;j++){
	component[i][j] *= lambda;
      }
    }

    return *this;
  }


/*!
  No check for division by 0.
*/
  template<int N_row,int N_col,typename Real>
  Matrix<N_row,N_col,Real>&
  Matrix<N_row,N_col,Real>::
  operator/=(const value_type& lambda){
  
    for(int i=0;i<N_row;i++){
      for(int j=0;j<N_col;j++){
	component[i][j] /= lambda;
      }
    }

    return *this;
  }

  
  template<int N_row,int N_col,typename Real>
  Matrix<N_row,N_col,Real>
  Matrix<N_row,N_col,Real>::
  operator-() const{
    Matrix<N_row,N_col,Real> res;
    
    for(int i=0;i<N_row;i++){
      for(int j=0;j<N_col;j++){
	res.component[i][j] = -component[i][j];
      }
    }

    return res;
  }


  template<int N_row,int N_col,typename Real>
  void
  Matrix<N_row,N_col,Real>::
  swap_rows(const int row1,const int row2){
    
    for(int j=0;j<N_col;j++){
      std::swap(component[row1][j],component[row2][j]);
    }    
  }

  template<int N_row,int N_col,typename Real>
  void
  Matrix<N_row,N_col,Real>::
  multiply_row(const int row,const value_type lambda){
    
    for(int j=0;j<N_col;j++){
      component[row][j] *= lambda;
    }    
  }
  
  template<int N_row,int N_col,typename Real>
  Vec<N_col,Real>
  Matrix<N_row,N_col,Real>::
  get_vector_from_row(const int row) const{

    Vec<N_col,Real> res;
    
    for(int j=0;j<N_col;j++){
      res[j] = component[row][j];
    }

    return res;
  }

  
  template<int N_row,int N_col,typename Real>
  void
  Matrix<N_row,N_col,Real>::
  add_vector_to_row(const int row,const Vec<N_col,Real>& vec){
    for(int j=0;j<N_col;j++){
      component[row][j] += vec[j];
    }
  }



/*

  #######################
  # class Square_matrix #
  #######################

*/



  template<int N,typename Real>
  Square_matrix<N,Real>::Square_matrix()
    :Matrix<N,N,Real>(){}


  template<int N,typename Real>
  Square_matrix<N,Real>::Square_matrix(const value_type tab[N][N])
    :Matrix<N,N,Real>(tab){}

  
  template<int N,typename Real>
  Square_matrix<N,Real>::Square_matrix(const Matrix<N,N,Real>& m)
    :Matrix<N,N,Real>(m){}


  template<int N,typename Real>
  typename Square_matrix<N,Real>::value_type
  Square_matrix<N,Real>::trace() const{

    value_type res = 0;

    for(int i=0;i<N;i++) res += (*this)(i,i);

    return res;
  }
  

  template<int N,typename Real>
  Square_matrix<N,Real>
  Square_matrix<N,Real>::
  identity(){
  
    Square_matrix<N,Real> res;

    for(int i=0;i<N;i++){
//       res.component[i][i] = 1;
      res(i,i) = 1;
    }

    return res;
  }




/*

  #########################
  # Fonctions hors classe #
  #########################

*/


  template<int N,typename Real>
  inline typename Vec<N,Real>::value_type operator*(const Vec<N,Real>& v1,
						    const Vec<N,Real>& v2){

    typename Vec<N,Real>::value_type sum = 0;
  
    for(int i=0;i<N;i++){
      sum += v1.coordinate[i] * v2.coordinate[i];
    }

    return sum;
  }


  template<int N,typename Real>
  inline Vec<N,Real> operator+(const Vec<N,Real>& v1,
			       const Vec<N,Real>& v2){

    Vec<N,Real> res(v1);

    res += v2;

    return res;
  }


  template<int N,typename Real>
  inline Vec<N,Real> operator-(const Vec<N,Real>& v1,
			       const Vec<N,Real>& v2){
  
    Vec<N,Real> res(v1);
  
    res -= v2;
  
    return res;
  }


  template<int N,typename Real>
  inline Vec<N,Real> operator*(const Vec<N,Real>& v,
			       const typename Vec<N,Real>::value_type r){

    Vec<N,Real> res(v);
  
    res *= r;
  
    return res;
  }


  template<int N,typename Real>
  inline Vec<N,Real> operator*(const typename Vec<N,Real>::value_type r,
			       const Vec<N,Real>& v){

    Vec<N,Real> res(v);
  
    res *= r;
  
    return res;
  }


  template<int N,typename Real>
  inline Vec<N,Real> operator/(const Vec<N,Real>& v,
			       const typename Vec<N,Real>::value_type r){

    Vec<N,Real> res(v);
  
    res /= r;
  
    return res;
  }


  template<typename Real>
  inline Vec3<Real> operator^(const Vec3<Real>& v1,
			      const Vec3<Real>& v2){

    return Vec3<Real>(v1.y()*v2.z() - v1.z()*v2.y(),
		      v1.z()*v2.x() - v1.x()*v2.z(),
		      v1.x()*v2.y() - v1.y()*v2.x());
  }


  template<int N,typename Real>
  inline std::ostream& operator<<(std::ostream& s,
				  const Vec<N,Real>& v){

    for(int i=0;i<N;i++){
      s<<v.coordinate[i]<<"\t";
    }

    return s;
  }


  template<int N_row,int N_col,typename Real>
  inline std::ostream& operator<<(std::ostream& s,
				  const Matrix<N_row,N_col,Real>& m){
    
    for(int i=0;i<N_row;i++){
      for(int j=0;j<N_col;j++){
	s<<m.component[i][j]<<"\t";
      }
      s<<"\n";
    }

    return s;
  }


  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){
  
    Matrix<N_row,N_col,Real> res = m1;

    res += m2;

    return res;
  }


  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){
  
    Matrix<N_row,N_col,Real> res = m1;

    res -= m2;

    return res;
  }


  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){

    Matrix<N_row,N_col,Real> res = m;

    res *= lambda;

    return res;
  }


  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){

    Matrix<N_row,N_col,Real> res = m;

    res *= lambda;

    return res;
  }


  template<int N,int P,int Q,typename Real>
  inline Matrix<N,Q,Real>
  operator*(const Matrix<N,P,Real>& m1,
	    const Matrix<P,Q,Real>& m2){
    int i,j,k;

    Matrix<N,Q,Real> res;

    for(i=0;i<N;i++){
      for(j=0;j<Q;j++){
	res.component[i][j] = 0;
      }
    }

    typename  Matrix<N,P,Real>::value_type scale;
  
    for(j=0;j<Q;j++){
      for(k=0;k<P;k++){
      
	scale = m2.component[k][j];

	for(i=0;i<N;i++){
	  res.component[i][j] += m1.component[i][k] * scale;
	}
      }
    }

    return res;
  }


  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){

    Matrix<N_row,N_col,Real> res = m;

    res /= lambda;

    return res;
  }


  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) {
  
    Vec<Row,Real_t> res;

    typename Matrix<Row,Col,Real_t>::value_type scale;
  
    for(int j=0;j<Col;j++){

      scale = v.coordinate[j];

      for(int i=0;i<Row;i++){
	res.coordinate[i] += m.component[i][j] * scale;
      }
    }

    return res;
  }


} // END OF namespace Geometry.

#endif