Position3D.cpp

Go to the documentation of this file.

//
// Position3D.cpp
//
// 3D-Punkt mit Orientierung.
//
// SICK AG, 11.11.2011, willhvo
//

#include "Position3D.hpp"
#include "Point2D.hpp"
#include "Point3D.hpp"
#include <cmath>        // for sin, cos, ...
#include <cassert>
#include <iostream>
#include <sstream>


namespace datatypes
{
        
Position3D::Position3D()
        : m_yawAngle(0)
        , m_pitchAngle(0)
        , m_rollAngle(0)
        , m_point(0.0, 0.0, 0.0)
{
}

Position3D::Position3D(value_type yaw, value_type pitch, value_type roll,
                                                                   value_type x, value_type y, value_type z)
        : m_yawAngle(yaw)
        , m_pitchAngle(pitch)
        , m_rollAngle(roll)
        , m_point(x, y, z)
{
}

Position3D::Position3D(value_type yaw, value_type pitch, value_type roll,
                                                                   const Point3D& pt)
        : m_yawAngle(yaw)
        , m_pitchAngle(pitch)
        , m_rollAngle(roll)
        , m_point(pt)
{
}

//
// **********************************************
//

Vector::Vector(UINT16 numOfElements)
{
        if (numOfElements > 0)
        {
                m_elements = new double[numOfElements];
        }
        else
        {
                m_elements = NULL;
        }
        m_numOfElements = numOfElements;
}

Vector::~Vector()
{
        if (m_elements != NULL)
        {
                delete m_elements;
                m_elements = NULL;
        }
}

// --------------------------------------------------

Matrix::Matrix(UINT16 numOfRows, UINT16 numOfColumns)
{
        if ((numOfRows > 0) && (numOfColumns > 0))
        {
                m_elements = new double*[numOfRows];
                for (UINT16 r=0; r < numOfRows; r++)
                {
                        m_elements[r] = new double[numOfColumns];
                }
                m_numOfRows = numOfRows;
                m_numOfColumns = numOfColumns;
        }
        else
        {
                m_elements = NULL;
                m_numOfRows = 0;
                m_numOfColumns = 0;
        }
}

Matrix::~Matrix()
{
        if (m_elements != NULL)
        {
                for (UINT16 r=0; r < m_numOfRows; r++)
                {
                        delete m_elements[r];
                }
                
                delete m_elements;
                m_elements = NULL;
        }
}

//
// ***********************************************
//

bool Position3D::transformToVehicle(Point3D* pt)        // double *dXPos, double *dYPos, double *dZPos, const CRect& r)
{
        //Punkt in Laserscannkoordinaten
        Vector P_LS(4);

        //Scanpunkt in Scannerkoordinaten
        P_LS[0] = pt->getX();   // *dXPos;
        P_LS[1] = pt->getY();   // *dYPos;
        P_LS[2] = pt->getZ();   // *dZPos;
        P_LS[3] = 1.0;

        // Sensorposition in Carcoordinates
        double dXOffset_LS, dYOffset_LS, dZOffset_LS;

        // Nick-, Gier- und Wankwinkel des Laserscanners
        double dPitch_LS, dYaw_LS, dRoll_LS;

        //Laserscanner to Vehicle transformation Matrix
        Matrix H_LS_Car(4,4);

        dRoll_LS                = m_rollAngle;
        dYaw_LS                 = m_yawAngle;
        dPitch_LS               = m_pitchAngle;
        
        dXOffset_LS             = m_point.getX();
        dYOffset_LS             = m_point.getY();
        dZOffset_LS             = m_point.getZ();
                
        //sin und cos der Nick-, Gier- und Wankwinkel des Laserscanners
        double dSPitch_LS       = sin(dPitch_LS);
        double dCPitch_LS       = cos(dPitch_LS);
        double dSYaw_LS         = sin(dYaw_LS);
        double dCYaw_LS         = cos(dYaw_LS);
        double dSRoll_LS        = sin(dRoll_LS);
        double dCRoll_LS        = cos(dRoll_LS);

        //Trafo-Matrix von Laserscannkoordinaten in Fahrzeugkoordinaten
        H_LS_Car(0,0) = dCYaw_LS*dCPitch_LS - dSYaw_LS*dSPitch_LS*dSRoll_LS;
        H_LS_Car(0,1) = -dSYaw_LS*dCRoll_LS;
        H_LS_Car(0,2) = dCYaw_LS*dSPitch_LS + dSYaw_LS*dSRoll_LS*dCPitch_LS;
        H_LS_Car(0,3) = dXOffset_LS;
        
        H_LS_Car(1,0) = dSYaw_LS*dCPitch_LS + dCYaw_LS*dSRoll_LS*dSPitch_LS;
        H_LS_Car(1,1) = dCYaw_LS*dCRoll_LS;
        H_LS_Car(1,2) = dSYaw_LS*dSPitch_LS - dCYaw_LS*dSRoll_LS*dCPitch_LS;
        H_LS_Car(1,3) = dYOffset_LS;
        
        H_LS_Car(2,0) = -dCRoll_LS*dSPitch_LS;
        H_LS_Car(2,1) = dSRoll_LS;
        H_LS_Car(2,2) = dCRoll_LS*dCPitch_LS;
        H_LS_Car(2,3) = dZOffset_LS;
        
        H_LS_Car(3,0) = 0;
        H_LS_Car(3,1) = 0;
        H_LS_Car(3,2) = 0;
        H_LS_Car(3,3) = 1;


        Vector pt_vehicle(4);
        pt_vehicle = H_LS_Car * P_LS;
        pt->setXYZ(pt_vehicle[0], pt_vehicle[1], pt_vehicle[2]);

        return true;
}



bool Position3D::operator==(const Position3D& other) const
{
        return
                m_yawAngle == other.m_yawAngle
                && m_pitchAngle == other.m_pitchAngle
                && m_rollAngle == other.m_rollAngle
                && m_point == other.m_point
                ;
}

Position3D& Position3D::set(value_type yaw, value_type pitch, value_type roll,
                                                                                value_type x, value_type y, value_type z)
{
        m_yawAngle = yaw;
        m_pitchAngle = pitch;
        m_rollAngle = roll;
        m_point.setXYZ(x, y, z);

        return *this;
}


Point3D Position3D::toPoint3D() const
{
        return m_point;
}

Point2D Position3D::toPoint2D() const
{
        Point2D result(m_point.getX(), m_point.getY());
        return result;
}


std::string Position3D::toString() const
{
        std::ostringstream ostr;
        ostr << "(x " << getX()
                 << ", y " << getY()
                 << ", z " << getZ() << "m"
                 << "; yaw " << getYawAngle() * rad2deg
                 << ", pitch " << getPitchAngle() * rad2deg
                 << ", roll " << getRollAngle() * rad2deg
                 << "deg)";
        return ostr.str();
}


void Position3D::normalizeAngles()
{
        m_yawAngle = normalizeRadians(m_yawAngle);
        m_pitchAngle = normalizeRadians(m_pitchAngle);
        m_rollAngle = normalizeRadians(m_rollAngle);
}

}       // namespace datatypes