experiments/include/projector.h

/********************************************************************************
 *  FARSA Experiments Library                                                   *
 *  Copyright (C) 2007-2012                                                     *
 *  Tomassino Ferrauto <tomassino.ferrauto@istc.cnr.it>                         *
 *  Stefano Nolfi <stefano.nolfi@istc.cnr.it>                                   *
 *  Onofrio Gigliotta <onofrio.gigliotta@istc.cnr.it>                           *
 *  Gianluca Massera <emmegian@yahoo.it>                                        *
 *                                                                              *
 *  This program is free software; you can redistribute it and/or modify        *
 *  it under the terms of the GNU General Public License as published by        *
 *  the Free Software Foundation; either version 2 of the License, or           *
 *  (at your option) any later version.                                         *
 *                                                                              *
 *  This program is distributed in the hope that it will be useful,             *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of              *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               *
 *  GNU General Public License for more details.                                *
 *                                                                              *
 *  You should have received a copy of the GNU General Public License           *
 *  along with this program; if not, write to the Free Software                 *
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA  *
 ********************************************************************************/

#ifndef PROJECTOR_H
#define PROJECTOR_H

#include "experimentsconfig.h"
#include "wmatrix.h"
#include "mathutils.h"

namespace farsa {

class FARSA_EXPERIMENTS_TEMPLATE ImagePoint {
public:
    ImagePoint() :
        x(0.0),
        y(0.0),
        valid(false)
    {
    }

    ImagePoint(double _x, double _y) :
        x(_x),
        y(_y),
        valid(true)
    {
    }

    bool isValid() const
    {
        return valid;
    }

    double x;

    double y;

    bool valid;
};

class FARSA_EXPERIMENTS_TEMPLATE Projector
{
public:
    Projector() :
        m_horizontalAperture(112.0),
        m_verticalAperture(94.0),
        m_halfImageWidth(0.0),
        m_halfImageHeight(0.0),
        m_inverseEyeMatrix(),
        m_imagePoint()
    {
        // Computing the width and height of the image we would obtain if the
        // focal distance was 1.0
        m_halfImageWidth = 1.0 * tan(deg2rad(m_horizontalAperture / 2.0));
        m_halfImageHeight = 1.0 * tan(deg2rad(m_verticalAperture / 2.0));
    };

    void setEyeMatrix(const wMatrix &eye)
    {
        m_inverseEyeMatrix = eye.inverse();
    }

    void set3DPointWorld(const wVector &point)
    {
        set3DPointEye(m_inverseEyeMatrix.transformVector(point));
    }

    void set3DPointEye(const wVector &point)
    {
        // Computing the position of the object on the image (remember
        // that the focal distance is 1.0). Also note that the upvector
        // for the camera is -x where x is the eye object x axis, so we
        // have to use -point.y to get the x position on the image and
        // -point.x to get the y position on the image. The formula
        // below is derived using a simple proportion

        m_imagePoint.x = -point.y / point.z;
        m_imagePoint.y = -point.x / point.z;

        if(point.z >= 0)
            m_imagePoint.valid = true;
        else
            m_imagePoint.valid = false;
    }

    const ImagePoint& getImagePoint() const
    {
        return m_imagePoint;
    }

    ImagePoint getImagePoint01() const
    {
        ImagePoint p((m_imagePoint.x + m_halfImageWidth) / (2.0 * m_halfImageWidth), (m_imagePoint.y + m_halfImageHeight) / (2.0 * m_halfImageHeight));
        return p;
    }

    bool pointInsideImage() const
    {
        return (m_imagePoint.x <= m_halfImageWidth) && (m_imagePoint.x >= -m_halfImageWidth) &&
               (m_imagePoint.y <= m_halfImageHeight) && (m_imagePoint.y >= -m_halfImageHeight)
               && m_imagePoint.valid;   //if not, works also for z < 0
    }

private:
    double m_horizontalAperture;

    double m_verticalAperture;

    double m_halfImageWidth;

    double m_halfImageHeight;

    wMatrix m_inverseEyeMatrix;

    ImagePoint m_imagePoint;
};

} //end namespace farsa

#endif