OrientedPoint.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX 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, see <http://www.gnu.org/licenses/>.
*
* @author     Martin Miller (martin dot miller at student dot kit dot edu)
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "OrientedPoint.h"
 
using namespace armarx;
using namespace Eigen;
 
OrientedPoint::OrientedPoint()
{
    px = 0;
    py = 0;
    pz = 0;
 
    nx = 0;
    ny = 0;
    nz = 0;
}
 
OrientedPoint::OrientedPoint(const Vector3f& position, const Vector3f& normal)
{
    px = position(0);
    py = position(1);
    pz = position(2);
 
    nx = normal(0);
    ny = normal(1);
    nz = normal(2);
}
 
OrientedPoint::OrientedPoint(::Ice::Float px,
                             ::Ice::Float py,
                             ::Ice::Float pz,
                             ::Ice::Float nx,
                             ::Ice::Float ny,
                             ::Ice::Float nz)
{
    this->px = px;
    this->py = py;
    this->pz = pz;
 
    this->nx = nx;
    this->ny = ny;
    this->nz = nz;
}
 
Vector3f
OrientedPoint::positionToEigen() const
{
    Vector3f p;
    p << this->px, this->py, this->pz;
    return p;
}
 
Vector3f
OrientedPoint::normalToEigen() const
{
    Vector3f n;
    n << this->nx, this->ny, this->nz;
    return n;
}
 
std::string
OrientedPoint::output(const Ice::Current& c) const
{
    Eigen::IOFormat vecfmt(Eigen::FullPrecision, 0, "", ", ", "", "", "(", ")");
    std::stringstream s;
    s << positionToEigen().format(vecfmt) << ", " << normalToEigen().format(vecfmt);
    return s.str();
}
 
void
OrientedPoint::serialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current&) const
{
    AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
    obj->setFloat("px", px);
    obj->setFloat("py", py);
    obj->setFloat("pz", pz);
 
    obj->setFloat("nx", nx);
    obj->setFloat("ny", ny);
    obj->setFloat("nz", nz);
}
 
void
OrientedPoint::deserialize(const ObjectSerializerBasePtr& serializer, const ::Ice::Current& c)
{
    AbstractObjectSerializerPtr obj = AbstractObjectSerializerPtr::dynamicCast(serializer);
 
    px = obj->getFloat("px");
    py = obj->getFloat("py");
    pz = obj->getFloat("pz");
 
    nx = obj->getFloat("nx");
    ny = obj->getFloat("ny");
    nz = obj->getFloat("nz");
}