File content as of revision 47:199042980678:

#include "LaserRenderer.h"
using namespace std;

LaserRenderer lsr; // THIS WILL BE A GLOBAL OBJECT (declared extern in the header file).

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

LaserRenderer::LaserRenderer()
{
    init();
}

LaserRenderer::~LaserRenderer()
{
    // myScene.clear(); // this should be done explicitly, because objects are stored as an array of pointers... for all the other iVars, this is not necessary.
    // Deactivate the display ISR routine?
    close();
}

void LaserRenderer::init()
{
    // Initialization of others things that are not set in the constructor:
    setIdentityPose();
    EXTRINSICS=RT; // set the Extrinsics too as identity
    setIdentityProjection();

    setColor(0x04); // RGB ON

    // In particular, this method is called when the scene is built - or re-built - and we are ready to go.
    // Ativate the laser sensing display ISR? NO, this is done in WRAPPER FUNCTIONS?
    // lsd.setSceneToDisplay(&myScene);
    // ATTACH THE ISR: unfortunately, I cannot do it here!
    // lsd.startFullDisplay();
}


void LaserRenderer::close()
{
    // this should deactivate the laser sensing display ISR, as well as free all memory.
    // (1) deactivate laser display:
    // ...
    // (2) clear the whole scene (free memory), if we want (this is done in the destructor anyway...)
    // myScene.clear();
}



// Load the MODELVIEW matrix from data sent by the computer:
void LaserRenderer::loadPoseMatrix(float m[12])   // note: m is in "row/column" format (first row, then columns)
{
    RT.set(m);
}

// Load the EXTRINSICS matrix from data sent by the computer:
void LaserRenderer::loadExtrinsicsMatrix(float m[12])
{
    EXTRINSICS.set(m);
}

void LaserRenderer::setIdentityPose(void)
{
    RT.setIdentity();
}

void LaserRenderer::setExtrinsicsPose(void) // this is faster than first loading the identity, then multiplying by the extrinsics...
{
    RT=EXTRINSICS; //note: I am not using here a copy CONSTRUCTOR, but a copy method of my own. RT must be allocated first.
}

void LaserRenderer::setIdentityProjection(void)
{
    K.setIdentity();
    scaleFactorProjector=1.0;
}

void LaserRenderer::setOrthoProjection()
{
    K.setIdentity();
    K.at[2][2]=0;
    scaleFactorProjector=1.0;
}

// Load the projection matrix from data sent by the computer opr from a file (note: last row is always {0 0 1} and is not loaded)
void LaserRenderer::loadProjMatrix(float m[6], float _scaleFactorProjector=1.0)   // note: m is in "row/column" format (first row, then columns)
{
    K.set(m);
    // Also, specify the scale factor in case we computed the intrinsics by laser scanning with different resolution:
    scaleFactorProjector=_scaleFactorProjector;
}

void LaserRenderer::translate(float x, float y, float z)
{
    float trans[] = {1, 0, 0, x,
                     0, 1, 0, y,
                     0, 0, 1, z
                    };

    multPoseMatrix(trans); // result in RT
}

void LaserRenderer::rotateX(float thetadeg) // in degrees
{
    float thetarad=PI/180.0*thetadeg;
    float rotX[] = {1,         0,              0,        0,
                    0,    cos(thetarad), -sin(thetarad), 0,
                    0,    sin(thetarad),  cos(thetarad), 0
                   };

    multPoseMatrix(rotX); // result in RT
}

void LaserRenderer::rotateY(float thetadeg)
{
    float thetarad=PI/180.0*thetadeg;
    float rotY[] = {cos(thetarad),  0, -sin(thetarad), 0,
                    0,              1,              0, 0,
                    sin(thetarad),  0,  cos(thetarad), 0
                   };

    multPoseMatrix(rotY);// result in RT
}

void LaserRenderer::rotateZ(float thetadeg)
{
    float thetarad=PI/180.0*thetadeg;
    float rotZ[] = {cos(thetarad), -sin(thetarad), 0, 0,
                    sin(thetarad),  cos(thetarad), 0, 0,
                    0,                          0, 1, 0
                   };

    multPoseMatrix(rotZ);// result in RT
}

void LaserRenderer::flipX()
{
    // float flipX[] = { -1,    0,    0,   0,
    //                   0,    1,    0,   0,
    //                   0,    0,    1,   0
    //                 };
    // multPoseMatrix(flipX);// result in RT
    // Easier way to do this (but less code-clear):
    RT.at[0][0]*=-1;
}
void LaserRenderer::flipY()
{
    RT.at[1][1]*=-1;
}
void LaserRenderer::flipZ()
{
    RT.at[2][2]*=-1;
}

void LaserRenderer::resize(float rx, float ry, float rz)   // multiplies the current RT matrix by a diagonal resizing matrix
{
    float ResizeMat[] = {rx,  0,  0, 0,
                         0,  ry,  0, 0,
                         0,   0, rz, 0
                        };
    multPoseMatrix(ResizeMat);// result in RT
}


void LaserRenderer::multPoseMatrix(const Mat44 M) // this does: RT=RTxM
{
    RT*=M;
}
void LaserRenderer::multPoseMatrix(const float m[12]) // this does: RT=RTxm, assuming m[12] in column/row format, and adding {0,0,0,1} as last row
{
    RT*=m;
}

void LaserRenderer::pushPoseMatrix(void)
{
    RT_Stack.push_back(RT); // this is ok, because RT is a class or struct, not an array
}
void LaserRenderer::popPoseMatrix(void)
{
    // First, load RT with the "back" element in the list:
    RT = RT_Stack.back(); // This uses the copy method for matrices

    // Finally, delete the object in the vector array:
    RT_Stack.pop_back();
}

void LaserRenderer::pushProjMatrix(void)
{
    K_Stack.push_back(K);
    scaleFactor_Stack.push_back(scaleFactorProjector);
}

void LaserRenderer::popProjMatrix(void)
{
    K=K_Stack.back();
    K_Stack.pop_back();
    scaleFactorProjector=scaleFactor_Stack.back();
    scaleFactor_Stack.pop_back();
}

void LaserRenderer::pushColor(void)
{
    color_Stack.push_back(color);
}
void LaserRenderer::popColor(void)
{
    color=color_Stack.back();
    color_Stack.pop_back();
}

// other rendering attributes (we could have a render attributes struct, and do push and pop too):
// ... TO DO

// ======================================================================================================================================
// QUESTION: we may have render methods that belongs to the LaserRenderer class, or use instead Scene/BaseObject methods.
// Both methodologies can be argued for (is rendering a method of a BaseObject, taking parameters from the global state machine lsr, or
// is rendering a method of the lsr applied to an object?). Therefore I will make them both available.
// NOTE: it is important to realize that these methods (belonging to lsr or the Scene/BaseObject class) do NOT properly disable the lsd display engine: this is done in WRAPPER FUNCTIONS.

void LaserRenderer::renderObject(BaseObject* ptObject)
{
// IMPORTANT: when rendering the object, I first CLEAR the projected points and add them again. This is ok, because things work differentl from the current
// OpenGL implementation: rendering is done ONLY once (after we create an object or modify the modelview and re-render). Perhaps a slightly more optimized
// implementation is to check if the number of points in the lsdTrajectory is differerent from the current number of 3d points (in vertexArray). This will be very
// useful when transforming an object already created:
    if (ptObject->vertexArray.size()!=ptObject->displaySensingBuffer.lsdTrajectory.size()) {
        // Then we clear the display sensing buffer, and ADD 2d points:
        ptObject->displaySensingBuffer.lsdTrajectory.clear();
        if (K.at[2][2]!=0) // this means normal PROJECTION
// Note: perhaps slightly more efficient using object iterator (actually, a pointer, using pointer aritmetics to increment...)
// Also, I am going to use the renderPointProj/Raw methods which are inline (but... can I be sure the compiler is gonna expand it?)
            for (unsigned short i=0; i<ptObject->vertexArray.size(); i++) {
                LaserPoint newLp=renderPointProj(ptObject->vertexArray[i]);
                // Set per-vertex color too? No, for the time being one color per object (and this is checked in the laserSensingDisplay display engine)
                // newLp.myColor=color;
                ptObject->displaySensingBuffer.lsdTrajectory.push_back(newLp);
            }
        else // this means ORTHOGRAPHIC PROJECTION
            for (unsigned short i=0; i<ptObject->vertexArray.size(); i++) {
                LaserPoint newLp=renderPointOrth(ptObject->vertexArray[i]);
                // Set per-vertex color too? No, for the time being one color per object...
                // newLp.myColor=color;
                ptObject->displaySensingBuffer.lsdTrajectory.push_back(newLp);
            }
    } else { // This means that the number of vertices in the object, and the projected 2d points in its lsdTrajectory are the same: we don't need to clear anything nor add,
        // just transform (but attention: do not modify the sensed data, so we cannot use the copy constructor for vectors V2, but a special setter setCoord()):
        if (K.at[2][2]!=0) // this means normal PROJECTION
            for ( unsigned short i=0; i<ptObject->vertexArray.size(); i++)
                ptObject->displaySensingBuffer.lsdTrajectory[i].setCoord(renderPointProj(ptObject->vertexArray[i]));
        else // this means ORTHOGRAPHIC PROJECTION
            for (unsigned short i=0; i<ptObject->vertexArray.size(); i++)
                ptObject->displaySensingBuffer.lsdTrajectory[i].setCoord(renderPointOrth(ptObject->vertexArray[i]));
    }
}

// Render, but adding a supplementary transformation (this will NOT modify the current stored 3d points in the object):
// NOTE: I could have made an optional argument to this method: (..., Mat44& moreRT=ID44), but this means that I have to declare ID44, and also that when not using the transformation, there
// will be a supplementary call to the matrix product method for each rendered point... conclusion: I prefer to overload the function.
void LaserRenderer::renderObject(BaseObject* ptObject, Mat44& moreRT)
{
    if (ptObject->vertexArray.size()!=ptObject->displaySensingBuffer.lsdTrajectory.size()) {
        ptObject->displaySensingBuffer.lsdTrajectory.clear();
        if (K.at[2][2]!=0) // this means normal PROJECTION
            for (unsigned short i=0; i<ptObject->vertexArray.size(); i++) {
                LaserPoint newLp=renderPointProj(moreRT*ptObject->vertexArray[i]);
                ptObject->displaySensingBuffer.lsdTrajectory.push_back(newLp);
            }
        else // this means ORTHOGRAPHIC PROJECTION
            for (unsigned short i=0; i<ptObject->vertexArray.size(); i++) {
                LaserPoint newLp=renderPointOrth(moreRT*ptObject->vertexArray[i]);
                ptObject->displaySensingBuffer.lsdTrajectory.push_back(newLp);
            }
    } else { // this means that the number of points in the object did not change. We don't have to create a new laser point, otherwise
    // we will be affecting the previous sensed trajectory... only modify the point coordinates! 
        if (K.at[2][2]!=0) // this means normal PROJECTION
            for (unsigned short i=0; i<ptObject->vertexArray.size(); i++)
                ptObject->displaySensingBuffer.lsdTrajectory[i].setCoord(renderPointProj(moreRT*ptObject->vertexArray[i]));
        else // this means ORTHOGRAPHIC PROJECTION
            for (unsigned short i=0; i<ptObject->vertexArray.size(); i++)
                ptObject->displaySensingBuffer.lsdTrajectory[i].setCoord(renderPointOrth(moreRT*ptObject->vertexArray[i]));
    }
}

void LaserRenderer::renderScene(Scene* ptr_scene)
{
    for (int i=0; i<ptr_scene->totalObjects(); i++) renderObject(ptr_scene->objectArray[i]);
    //Another way: Scene.objectArray[i]->render();
}


void LaserRenderer::renderScene(Scene* ptr_scene, Mat44& moreRT)
{
    for (int i=0; i<ptr_scene->totalObjects(); i++) renderObject(ptr_scene->objectArray[i], moreRT);
    //Another way: Scene.objectArray[i]->render();
}