matrixCTH.hpp

//
// Copyright (c) Singular Inversions Inc. 2000
//
// Authors:     Andrew Beatty
// Created:     30/06/00
//
// Square matrices for representing transforms of projective space using
// so-called homogeneous coordinates in which RP^N is represented by the
// projection of an R^(N+1) vector onto the hyper-plane x_(N+1) = 1.
//
// Vectors in this representation are of length N with the N+1'th component
// implied and equal to one.
//
// The matrix representation is (N+1) by (N+1) and is only unique to within
// a scalar multiple.
//
// Multiplication of an RP^N matrix by an RP^N vector is performed by adding
// the N+1'th component (1) to the vector then multiplying the (N+1)x(N+1)
// matrix by that vector and re-projecting the vector.
//
// Matrix-matrix multiplication is not re-projected.
//
 
#ifndef FUT_MATRIXCTH_HPP
#define FUT_MATRIXCTH_HPP
 
#include "FgMatrixC.hpp"
#include "matrixCT.hpp"
#include "vect3.hpp"
 
namespace Fg {
 
template <class T,const int size>
struct  FutMatrixCTHC
{
        FR2MatrixCTC<T,size,size>       m_mat;
 
        FutMatrixCTHC() {};
        FutMatrixCTHC(const FutMatrixCTHC& m) : m_mat(m.m_mat) {};
 
    FutMatrixCTHC(const Mat<T,size,size> & m)
    {
        for (uint ii=0; ii<size; ++ii)
            for (uint jj=0; jj<size; ++jj)
                m_mat.data[ii][jj] = m.rc(ii,jj);
    }
 
        FutMatrixCTHC(const FR2MatrixCTC<T,size-1,size-1>&,     // Construct from
                                  const FR2MatrixCTC<T,size-1,1>&);             // linear plus trans.
 
    inline T*       operator[](size_t i) { return m_mat[i]; }
    inline T const* operator[](size_t i) const { return m_mat[i]; }
        inline void             setZero() {m_mat.setZero();}
        inline void             setIdentity() {m_mat.setIdentity();}
        inline FutMatrixCTHC transpose() const;
 
        FutMatrixCTHC   operator*(const FutMatrixCTHC&) const;
        FR2MatrixCTC<T,size-1,1> operator*(const FR2MatrixCTC<T,size-1,1>&) const;
        bool                    operator==(const FutMatrixCTHC& v) const 
                                        {return (m_mat == v.m_mat);}
};
 
// ***********************************************************************
// ***********************************************************************
 
typedef FutMatrixCTHC<float,3>          FutMatrix2FHC;
typedef FutMatrixCTHC<double,3>         FutMatrix2DHC;
typedef FutMatrixCTHC<float,4>          FutMatrix3FHC;
typedef FutMatrixCTHC<double,4>         FutMatrix3DHC;
 
// ***********************************************************************
// ***********************************************************************
 
template<class T,const int size>
FutMatrixCTHC<T,size>::FutMatrixCTHC(
        const FR2MatrixCTC<T,size-1,size-1>&    lin,    // Linear component
        const FR2MatrixCTC<T,size-1,1>&                 tran)   // Translational component
{
        int             row,col;
        m_mat.setZero();
        for (row=0; row<size-1; row++) {
                for (col=0; col<size-1; col++) {
                        m_mat[row][col] = lin[row][col];                                // Copy linear
                }
                m_mat[row][size-1] = tran[row][0];                                      // Copy translational
        }
        m_mat[size-1][size-1] = T(1);
}
 
template <class T,const int size>
inline FutMatrixCTHC<T,size> FutMatrixCTHC<T,size>::operator*(
        const FutMatrixCTHC&    m) const
{
        FutMatrixCTHC<T,size>   retval;
        retval.m_mat = m_mat * m.m_mat;
        return  retval;
}
 
template <class T,const int size>
FR2MatrixCTC<T,size-1,1>
FutMatrixCTHC<T,size>::operator*(const FR2MatrixCTC<T,size-1,1> & vec) const
{
        FR2MatrixCTC<T,size-1,1>        retval;
        int                                                     row,col;
        T                                                       acc;
        for (row=0; row<size-1; row++) {
                acc = T(0);
                for (col=0; col<size-1; col++) {
                        acc += m_mat[row][col] * vec[col][0];
                }
                acc += m_mat[row][size-1];
                retval[row][0] = acc;
    }
        acc = T(0);
        for (col=0; col<size-1; col++)
                acc += m_mat[size-1][col] * vec[col][0];
        acc += m_mat[size-1][size-1];
        if (acc != T(0)) {   // Used to be an ASSERT but this was occasionally killing photofit:
            for (col=0; col<size-1; col++)
                    retval[col][0] /= acc;
    }
        return retval;
}
 
// The operator* for the old FR2Vect3FC classes must be global since
// they are defined only for specific instantiations of FutMatrixCTHC.
// The example for FR2Vect3FC below can be used to create similar ones
// as needed.
template <class T>
inline FR2Vect3TC<T>    operator*(
        const FutMatrixCTHC<T,4>&       mat,
        const FR2Vect3TC<T>&            vec)
{
        FR2Vect3TC<T>                           retval;
        FR2MatrixCTC<T,3,1>                     in,             // Homogeneous vector representation
                                                                out;
        in[0][0] = vec.x1;
        in[1][0] = vec.x2;
        in[2][0] = vec.x3;
        out = mat * in;
        retval.x1 = out[0][0];
        retval.x2 = out[1][0];
        retval.x3 = out[2][0];
        return retval;
}
 
template <class T,const int size>
std::ostream& operator<<(std::ostream& s,FutMatrixCTHC<T,size> m)
{
    for (int i=0; i<size; i++) {
        for (int j=0; j<size; j++) {
            s << m[i][j] << "   "; }
        s << "\n"; }
    return s;
}
 
}
 
#endif