qttf.cpp

Go to the documentation of this file.

#include "qttf.hpp"

#include <iostream>

namespace lvr2 {

namespace qttf {

Transformd getTransformation(
    QTreeWidgetItem* from,
    QTreeWidgetItem* to
)
{
    // default identity
    Transformd ret = Transformd::Identity();

    QTreeWidgetItem* it = from;

    while(it != to || it != NULL)
    {
        auto transform_obj = dynamic_cast< Transformable* >(it);
        if(transform_obj)
        {
            ret = ret * transform_obj->getTransform();
        }
        it = it->parent();
    }

    return ret;
}

PointBufferPtr transform(
    PointBufferPtr pc_in,
    const Transformd& T
)
{
    // generate copy
    PointBufferPtr buffer_ptr = std::make_shared<PointBuffer>(pc_in->clone());
    
    Transformd T_trans = T;
    T_trans.transpose();

    // 1) transform points
    const size_t num_points = buffer_ptr->numPoints();
    BaseVector<float>* points_raw = reinterpret_cast<BaseVector<float>* >(
        buffer_ptr->getPointArray().get()
    );

    for(size_t i=0; i<num_points; i++)
    {
        points_raw[i] = T_trans * points_raw[i];
    }

    // 2) transform normals
    if(buffer_ptr->hasNormals())
    {
        Normal<float>* normals_raw = reinterpret_cast<Normal<float>* >(
            buffer_ptr->getNormalArray().get()
        );
        for(size_t i=0; i<num_points; i++)
        {
            normals_raw[i] = T_trans * normals_raw[i];
        }
    }


    return buffer_ptr;
}

PointBufferPtr transform(
    PointBufferPtr pc_in,
    QTreeWidgetItem* from,
    QTreeWidgetItem* to
)
{
    return transform(pc_in, getTransformation(from, to)); 
}

} // namespace qttf

} // namespace lvr2