smatrix.h

来自「A Library of Efficient Data Types and Al」· C头文件 代码 · 共 812 行 · 第 1/2 页

template <int n, typename T>
int svector<n,T>::index = 0;

//----------------------------------------------------------------------------
// vector functions
//----------------------------------------------------------------------------

template <int n, typename T>
ostream& operator<<(ostream& os, const svector<n,T>& V)
{ os << n << '\n';
  for (int i = 0; i < n; ++i) os << " " << V[i];
  os << '\n';
  return os;
}

template <int n, typename T>
svector<n,T> operator*(const T& x, const svector<n,T>& v)
{ svector<n,T> r = v;
  return r *= x;
}  

template <int n, typename T>
svector<n,T> operator*(const svector<n,T>& v, const T& x)
{ return x * v; }

template <int n, typename T>
svector<n,T> operator/(const T& x, const svector<n,T>& v)
{ svector<n,T> r = v;
  return r /= x;
}  

template <int n, typename T>
svector<n,T> operator/(const svector<n,T>& v, const T& x)
{ return x / v; }

template <int n, typename T>
inline smatrix<n,n,T> tensor_product(const svector<n,T>& v1, const svector<n,T>& v2)
{ smatrix<n,n,T> res;
  for (int i = 0; i < n; ++i)
    for (int j = 0; j < n; ++j)
       res(i,j) = v1[i] * v2[j];
  return res;  
}

template <int n, typename T>
inline svector<n,T> cross_product(const svector<n,T>& p, const svector<n,T>& q)
{ 
  if (p.dim() != 3 || q.dim() != 3) 
	  VGL_ERROR_HANDLER("cross_product(): wrong vector dimension!");
  
  svector<n,T> v;
  v[0] = p[1] * q[2] - p[2] * q[1];
  v[1] = p[2] * q[0] - p[0] * q[2];
  v[2] = p[0] * q[1] - p[1] * q[0];
  
	return v;
}                                 


template <int n, typename T>
inline T operator*(const svector<n,T>& v1, const svector<n,T>& v2) 
{ T r = 0;
  for (int i = 0; i < v1.dim(); ++i) r += v1[i] * v2[i];
  return r;
}

template <int n, typename T>
inline svector<n,T> operator+(const svector<n,T>& v1, const svector<n,T>& v2) 
{ svector<n,T> r = v1;
  return r += v2;
}

template <int n, typename T>
inline svector<n,T> operator-(const svector<n,T>& v1, const svector<n,T>& v2) 
{ svector<n,T> r = v1;
  return r -= v2;
}


template <int n, typename T>
inline T sqr_length(const svector<n,T>& v) { return v * v; }

template <int n, typename T>
inline svector<n,T> norm(const svector<n,T>& v) { 
  return v / T(vgl_detail::sqrt(sqr_length(v))); 
}

template <int n, typename T>
inline double length(const svector<n,T>& v) { 
  return vgl_detail::sqrt(sqr_length(v)); 
}

//----------------------------------------------------------------------------
// matrix functions
//----------------------------------------------------------------------------

template <int n, int m, class T>
ostream& operator<<(ostream& os, const smatrix<n,m,T>& M)
{ os << n << " " << m << '\n';
  for (int i = 0; i < n; ++i)
  { for (int j = 0; j < m; ++j) os << " " << M(i,j);
    os << endl;
  }
  return os;
}

template <int n, int m, class T>
inline smatrix<n,m,T> operator+(const smatrix<n,m,T>& M1, const smatrix<n,m,T>& M2)
{ smatrix<n,m,T> r = M1;
  return r += M2;
}

template <int n, int m, class T>
inline smatrix<n,m,T> operator-(const smatrix<n,m,T>& M1, const smatrix<n,m,T>& M2)
{ smatrix<n,m,T> r = M1;
  return r -= M2;
}

template <int n, int m, int m1, class T>
inline smatrix<n,m1,T> operator*(const smatrix<n,m,T>& M1, const smatrix<m,m1,T>& M2)
{ smatrix<n,m1,T> res;
  for (int i = 0; i < n; ++i)
    for (int j = 0; j < m1; ++j)
      for (int k = 0; k < m; ++k)   
        res(i,j) += M1(i,k) * M2(k,j);
    return res;       
}
  
template <int n, int m, class T>
inline smatrix<n,m,T> operator*(const T& v, const smatrix<n,m,T>& M)
{ smatrix<n,m,T> res;
  for (int i = 0; i < n; ++i)
    for (int j = 0; j < m; ++j)
        res(i,j) = M(i,j) * v;
  return res;       
}

template <int n, int m, class T>
inline smatrix<n,m,T> operator*(const smatrix<n,m,T>& M, const T& v)
{ return v * M; }

template <int n, int m, class T>
inline svector<n,T> operator*(const smatrix<n,m,T>& M, const svector<m,T>& V) 
{ svector<n,T> res;
  for (int i = 0; i < n; ++i)
    for (int j = 0; j < m; ++j)
       res[i] += M(i,j) * V[j];
  return res;
}  

template <int n, int m, class T>   
inline smatrix<m,n,T> trans(const smatrix<n,m,T>& M) 
{ smatrix<m,n,T> res;    
  for (int i = 0; i < n; ++i)
    for (int j = 0; j < m; ++j)
      res(j,i) = M(i,j);
  return res;
}

template <class matrix>
inline void identity(matrix& M)
{ typedef typename matrix::value_type T; 
  int n = M.dim1();
  for (int i = 0; i < n; ++i)
    for (int j = 0; j < n; ++j)
       M(i,j) = (i == j) ? T(1) : T(0);
}

template <class matrix1, class matrix2>
inline void sub_matrix(const matrix1& M, matrix2& S, int i, int j)
{ int n1 = S.dim1(), m1 = S.dim2();	
  for (int k = 0; k < n1; ++i, ++k)
    for (int l = 0, p = j; l < m1; ++p, ++l) S(k,l) = M(i,p);
}

template <class matrix, class vector>
inline int lu_decomposition(const matrix& A, matrix& L, matrix& U, vector& p)
{
  bool B = false;

  int i, j, k, n = A.dim1(), c = 0;  
  for (i = 0; i < n; ++i)
  { p[i] = i;
    for (j = 0; j < n; ++j)
    { if (i == j && A(i,i) == 0) 
      { B = false; 
        i = n; 
        break; 
      }
    }
  }
  
  typedef typename matrix::value_type T;

  identity(L);
  U = A;    

  if (B)
  { for (k = 0; k < n; ++k)
    { 
      T max = T(0);
      int m = k;
      
      for (i = k; i < n; ++i)
      { T sum = T(0);  
        for (j = k; j < n; ++j) sum += vgl_detail::abs(U(i,j));      
      
       sum = vgl_detail::abs(U(i,k)) / sum;
        if (sum > max) { max = sum; m = i; }      
      }
      
      if (m != k) 
      { c++;
        std::swap(p[k],p[m]);
      }
      
      for (i = k + 1;  i < n; ++i)
      {  T z = U(p[i],k) / U(p[k],k);
         L(p[i],k) = z;
    
		     for (j = k + 1; j < n; ++j)
				   U(p[i],j) = U(p[i],j) - z * U(p[k],j);
      }
		}
		
    return c;
  }

  for (k = 0; k < n; ++k)
  {
    for (i = k + 1; i < n; ++i)
    { L(i,k) = U(i,k) / U(k,k);
    
      for (j = k + 1; j < n; ++j)
        U(i,j) = U(i,j) - L(i,k) * U(k,j);    
        
      U(i,k) = 0;
    }
  } 
	
	return 0;
}

template <class MatrixType, class VectorType>
inline int lu_decomposition(MatrixType& A, VectorType& p)
{
  bool B = false;

  int i, j, k, n = A.dim1(), c = 0;  
  for (i = 0; i < n; ++i)
  { p[i] = i;
    for (j = 0; j < n; ++j)
    { if (i == j && A(i,i) == 0) 
      { B = false; 
        i = n; 
        break; 
      }
    }
  }
  
  typedef typename MatrixType::value_type T;
  
  if (B)
  { 
    for (k = 0; k < n; ++k)
    { 
      T max = T(0);
      int m = k;
      
      for (i = k; i < n; ++i)
      { T sum = T(0);  
        for (j = k; j < n; ++j) sum += vgl_detail::abs(A(i,j));      
      
        sum = vgl_detail::abs(A(i,k)) / sum;
        if (sum > max) { max = sum; m = i; }      
      }
      
      if (m != k) 
      { c++;
	    
		T swv = p[k];
		p[k] = p[m];
		p[m] = swv;
        //std::swap(p[k],p[m]);
      }
      
      for (i = k + 1;  i < n; ++i)
      {  T z = A(p[i],k) / A(p[k],k);
         A(p[i],k) = z;
    
		     for (j = k + 1; j < n; ++j)
				   A(p[i],j) = A(p[i],j) - z * A(p[k],j);
      }
		}
		
    return c;
  }

  for (k = 0; k < n; ++k)
  { for (i = k + 1; i < n; ++i)
    { A(i,k) = A(i,k) / A(k,k);    
      for (j = k + 1; j < n; ++j)
        A(i,j) = A(i,j) - A(i,k) * A(k,j);    
    }
  } 
      
  return 0;
}


template <class MatrixType>
inline typename MatrixType::value_type det(const MatrixType& M) 
{ 
  MatrixType A = M;

	int n = A.dim1(), c = 0;
	int*p = new int[n];	

  try {
    c = lu_decomposition(A,p);
  }
  catch (...) { 
    VGL_ERROR_HANDLER("det(): matrix is singular!");  
  }
		
	typedef typename MatrixType::value_type T;

	T val = c % 2 > 0 ? T(-1) : T(1);
  
  int i, j;  
  for (i = 0; i < n; ++i)
    for (j = 0; j < n; ++j)
      if (i == j) val *= A(i,i); 
  
	delete [] p;	
  return val;
}


inline smatrix<2,2,float> inv(const smatrix<2,2,float>& M)
{ 
  float D = det(M);
  if (D == 0) 
	  VGL_ERROR_HANDLER("inv(): matrix isn't invertible!");

  smatrix<2,2,float> r;
  r =  M(1,1), -M(0,1),
      -M(1,0),  M(0,0);
  
  return r * (1/D);
}

inline smatrix<2,2,double> inv(const smatrix<2,2,double>& M)
{ 
  double D = det(M);
  if (D == 0) 
	  VGL_ERROR_HANDLER("inv(): matrix isn't invertible!");

  smatrix<2,2,double> res;
  res =  M(1,1), -M(0,1),
        -M(1,0),  M(0,0);
  
  return res * (1/D);
}
 
template <int n, class T>
inline smatrix<n,n,T> inv(const smatrix<n,n,T>& M)
{ VGL_ERROR_HANDLER("inv(): inv() not implemented!");
  return smatrix<n,n,T>();
}


//----------------------------------------------------------------------------
// typedefs
//----------------------------------------------------------------------------

typedef svector<2>         vector2d;
typedef svector<2,float>   vector2f;
typedef svector<2,int>     vector2i;

typedef svector<3>         vector3d;
typedef svector<3,float>   vector3f;
typedef svector<3,int>     vector3i;

typedef svector<4>         vector4d;
typedef svector<4,float>   vector4f;
typedef svector<4,int>     vector4i;


typedef smatrix<2,2>       matrix22d;
typedef smatrix<2,2,float> matrix22f;
typedef smatrix<2,2,int>   matrix22i;

typedef smatrix<3,3>       matrix33d;
typedef smatrix<3,3,float> matrix33f;
typedef smatrix<3,3,int>   matrix33i;

typedef smatrix<4,4>       matrix44d;
typedef smatrix<4,4,float> matrix44f;
typedef smatrix<4,4,int>   matrix44i;

VGL_END_NAMESPACE

#endif