File content as of revision 47:199042980678:

#ifndef MATRIX_CLASS_H
#define MATRIX_CLASS_H

#include "myVectorClass.h"

// Note: the renderer needs very basic matrix operations (4x4 and 3x3), and I am not going to include any matrix library (we just need a product of matrices).
// So, I will define here the types used (as struct or class, so as to take advantage of the default copy constructor that is NOT shallow for the float arrays, and then can be
// stored in vector structures). Indeed, the thing stored in a Standard Library container must be assignable and copyable - arrays are neither. A solution is to wrap the array in a struct:
// MORE: The point to note is that the array members are not shallow copied, compiler automatically performs Deep Copy  for array members!! (this is interesting, because
// it may seem that defining a member "float a[100]" is in fact defining a pointer, but this is not so! and array is NOT a pointer (float *a).

// More handy typedefs:
typedef vector2D<unsigned short> V2; // this is an IMAGE POINT (in projector image plane - i.e., the mirror azimuth and elevation)
// NOTE: unsigned short is two bytes, 0 to 65535 (16 bits, at least in the mbed). This is also uint16_t, perhaps better used for clarity. 
typedef vector3D<float> V3;// this is a 3D point

// ------------  Pose matrices. For simplifiying calculations, I will add a [0 0 0 1] row to the 3x4 matrix of "extrinsics" [R | t] ----------------------------
class Mat44
{
public:
    Mat44(); // (remember: if I declare a constructor, the compiler no longer provides an implicit default constructor)
    Mat44(const float m[12]);
   // ~Mat44(); use default

    //copy contructor: do I need to specify it? no! because I am not using pointers to store data (beware: arrays are NOT pointers!), so copy is not shallow...
    //Mat44 (const Mat44& M);

    // Methods:
    //copy:
    void operator=( const Mat44& M );
    
    // set: 
    void set(const float m[12]);
    void setIdentity();

    // matrix products:
    Mat44 operator*( const Mat44& ) const;
    Mat44 operator*(const float m[12]) const;
    Mat44& operator*=( const Mat44& M);
    Mat44& operator*=( const float m[12] );

    // matrix vector operations:
    V3 operator*(const V3&) const; //note: although this is not needed for the purposes of computing the final image projection, I will normalize coordinates by dividing by the w coordinate.

    // data (private? not for the time being)
    float at[4][4];
};


// ------------- Projection matrices - this is the "Intrinsics" matrix (or K, a 3x3 matrix) ---------------------------------------------------------------------
class Mat33
{
public:
    Mat33();
    Mat33(const float m[6]); // note: last row is always {0,0,1}, and is not loaded
    // ~Mat33(); // use default
    
    //copy:
    void operator=( const Mat33& M );
    
    // set:
    void set(const float m[6]);
    void setIdentity();
     
    // matrix vector operation (att: this may seem weird: a 3d vector multiplied by a 3x3 matrix gives a 2d vector? In fact I am converting the 2d homogeneous coordinates to real image coord)
    V2 operator*(const V3& _v3) const;

    float at[3][3];
};

// =============================================================================================================================================================================

// NOTE: I have a lot of Flash memory on the Cortex M3 (512kb), but not much speed.. so, let's inline these operations to avoid functions calls on the following
// methods that are heavily called.  SO, IN THAT CASE, the body of all inline function needs to be in the header so that the compiler can actually substitute it wherever required!!
// (this means, I won't be using a matrixClass.cpp file to contain the definitions. This is NOT going to be a problem for the compiler, as long as all the methods are INLINE (otherwise
// it will give "multiple definition" errors). 

// =============================== Mat44 Class definitions =====================================

// Constructors:
inline Mat44::Mat44()
{
    setIdentity();
}
inline Mat44::Mat44(const float m[12])
{
    set(m);
}

// Destructor: use default
//Mat44::~Mat44() {}

inline void Mat44::operator=( const Mat44& M )
{
// do a deep copy of the data:
    memcpy(&at[0][0], &(M.at[0][0]), sizeof(at)); // this is possible because array data is contiguously allocated
}

inline void Mat44::set(const float m[12]) // the matrix is loaded in ROW-COLUMN order (meaning, we fill rows by rows, or "row first")  
{
    for (int i=0; i<3; i++)
        for (int j=0; j<4; j++) at[i][j]=m[4*i+j];
    // Also, the last row is set to {0,0,0,1}:
    at[3][0]=0; at[3][1]=0; at[3][2]=0; at[3][3]=1;
}
inline void Mat44::setIdentity()
{
    static const float ID[12] = { 1, 0, 0, 0,
                                  0, 1, 0, 0,
                                  0, 0, 1, 0 };
    set(ID); // we assume last row is {0,0,0,1}
}

inline Mat44 Mat44::operator*( const Mat44& A) const
{
    Mat44 Res;
    for (int i=0; i<4; i++) {
        for (int j=0; j<4; j++) {
            float sum=0;
            for (int k=0; k<4; k++) sum+=at[i][k]*A.at[k][j];
            Res.at[i][j]=sum;
        }
    }
    return Res;
}

// use reduced matrix as float (sent by computer or defined by the user):
inline Mat44 Mat44::operator*(const float m[12]) const
{
    Mat44 Res;
    for (int i=0; i<4; i++) {
        for (int j=0; j<4; j++) {
            float sum=0;
            for (int k=0; k<3; k++) sum+=at[i][k]*m[4*k+j]; // we assume the last raw of matrix "augmented" matrix m is {0, 0, 0, 1}
            Res.at[i][j]=sum;
        }
        // we assume the last raw of matrix "augmented" matrix m is {0, 0, 0, 1}, so we need to add:
        Res.at[i][3]+=at[i][3];
    }
    return Res;
}

inline Mat44& Mat44::operator*=(const Mat44& M )
{
    float row[4];
    for (int i=0; i<4; i++) {
        for (int j=0; j<4; j++) {
            float sum=0;
            for (int k=0; k<4; k++) sum+=at[i][k]*M.at[k][j];
            row[j]=sum;
        }
        // we don't use any more the row at[i][..], so we can update the result:
        for (int k=0; k<4; k++)  at[i][k]=row[k];
    }
    // return this object, so we can chain:
    return *this;
}

// use reduced matrix as float (sent by computer or defined by the user):
inline Mat44& Mat44::operator*=( const float m[12] )
{
    float row[4];
    for (int i=0; i<4; i++) {
        for (int j=0; j<4; j++) {
            float sum=0;
            for (int k=0; k<3; k++) sum+=at[i][k]*m[4*k+j]; // we assume the last raw of matrix "augmented" matrix m is {0, 0, 0, 1}
            row[j]=sum;
        }
        // we don't use any more the row at[i][..], so we can update the result:
        for (int k=0; k<3; k++)  at[i][k]=row[k];
        at[i][3]+=row[3];
    }
    // return this object, so we can chain:
    return *this;
}

// Matrix vector operations:
// Note: this may seems weird: a 3d vector multiplied by a 4x4 matrix? in fact, I am assuming the 3d vector is not at infinity, so W=1, and then the result is normalized (not homogeneous)
inline V3 Mat44::operator*(const V3& _v3) const // note: second const means that this method does not changes any member variable
{
    V3 Res3d; // note: this Res3d object is a local variable, but if we return it as an OBJECT (not pointer), then it will be first copied...
    float w;

    //note: although this is not needed for the purposes of computing the final image projection, I will normalize coordinates by dividing by the w coordinate:
    w = at[3][0]*_v3.x + at[3][1]*_v3.y + at[3][2]*_v3.z+at[3][3]*1.0;     // we assume 3d points are not at infinity with W=1 (when added using vertex() method)
    // NOTE: it is NOT generally true that at[3][0], at[3][1] and at[3][2] are 0, and at[3][3]=1 after all the transformations (another way to say it, 
    // if we associate the product of matrices from the right: the homogeneous coordinates of the transformed point has W!=1)
    Res3d.x = (at[0][0]*_v3.x + at[0][1]*_v3.y + at[0][2]*_v3.z+at[0][3]*1.0)/w;
    Res3d.y = (at[1][0]*_v3.x + at[1][1]*_v3.y + at[1][2]*_v3.z+at[1][3]*1.0)/w;
    Res3d.z = (at[2][0]*_v3.x + at[2][1]*_v3.y + at[2][2]*_v3.z+at[2][3]*1.0)/w;

    return Res3d; // return a 3d vector (att: NOT homogeneous coordinates)
}

// =============================== Mat33 Class definitions =====================================
// Constructors:
inline Mat33::Mat33()
{
    setIdentity();
}
inline Mat33::Mat33(const float m[6])
{
    set(m);
}

inline void Mat33::operator=( const Mat33& M )
{
// do a deep copy of the data:
    memcpy(&at[0][0], &M.at[0][0], sizeof(at)); // this is possible because array data is contiguously allocated in the "at" array (containing 9 floats)
}

inline void Mat33::set(const float m[6])
{
    for (int i=0; i<2; i++)
        for (int j=0; j<3; j++) at[i][j]=m[3*i+j]; // m is in "column-row" order
    // Also, the last row is set to {0,0,1} by default (is this a good idea? the matrix may be set for orthographic projection...)
    at[2][0]=0; at[2][1]=0; at[2][2]=1;
}

inline void Mat33::setIdentity()
{
    static const float ID[6] = { 1, 0, 0,
                                 0, 1, 0 };
    set(ID); // last row set to {0,0,1}
}

// Matrix vector operations (note: the matrix is 3x3, the vector is 3x1, but the output is 2x1 because it's NOT in homogeneous coordinates. 
inline V2 Mat33::operator*(const V3& _v3) const
{
    V2 Res2d;
    float w;

   // w =( at[2][0]*_v3.x + at[2][1]*_v3.y + at[2][2]*_v3.z); // this is in fact unnecessary, since at[2][0] and at[2][1] are 0,
   // Note that at[2][2] is usually 1 (when real perspective projection), but can be 0 in case of orthographic projection.
   // HOWEVER, in case of orthographic projection, we will NOT call this function - otherwise we need a check to see if w==0...
    w = _v3.z;
    Res2d.x = (at[0][0]*_v3.x + at[0][1]*_v3.y + at[0][2]*_v3.z)/w;
    Res2d.y = (at[1][0]*_v3.x + at[1][1]*_v3.y + at[1][2]*_v3.z)/w;

    return Res2d; // return a 2d vector (att: NOT homogeneous coordinates)
                  // ALSO: do not forget to apply the rescale factor (this is necessary when we use a different projector resolution while "scanning" with the laser, say, we 
                  // have a laser scanner capable of 4095x4095 "pixels", but we form an image which is 600x600. If we then run a camera-projector calibration routine, the 
                  // final intrinsics matrix won't be right: the "pixel size" is wrong, by a factor 4096/600 - remember that the camera intrinsics contain the focal length 
                  // and the origin IN PIXELS units). This is the role of "scaleFactorProjector". Hopefully, it will be alwyas set to 1 (because we either correct the intrinsics
                  // before loading, or because we calibrated the projector USING A METHOD THAT DOES NOT IMPLIES SUB-RESOLUTION "SCANNING")
}

#endif