PlanePrimitiveShape.cpp

Go to the documentation of this file.

#include "PlanePrimitiveShape.h"
 
#include <fstream>
#include <iomanip>
#include <iostream>
#include <limits>
#include <sstream>
 
#include "PrimitiveShapeVisitor.h"
#include "ScoreComputer.h"
#include <GfxTL/NullClass.h>
#include <MiscLib/Performance.h>
extern MiscLib::performance_t totalTime_planeConnected;
 
PlanePrimitiveShape::PlanePrimitiveShape(const Vec3f& a, const Vec3f& b, const Vec3f& c) :
    m_plane(a, b, c)
{
    m_hcs.FromNormal(m_plane.getNormal());
}
 
PlanePrimitiveShape::PlanePrimitiveShape(const Plane& plane) : m_plane(plane)
{
    m_hcs.FromNormal(m_plane.getNormal());
}
 
size_t
PlanePrimitiveShape::Identifier() const
{
    return 0;
}
 
PrimitiveShape*
PlanePrimitiveShape::Clone() const
{
    return new PlanePrimitiveShape(*this);
}
 
float
PlanePrimitiveShape::Distance(const Vec3f& p) const
{
    return m_plane.getDistance(p);
}
 
float
PlanePrimitiveShape::SignedDistance(const Vec3f& p) const
{
    return m_plane.SignedDistance(p);
}
 
float
PlanePrimitiveShape::NormalDeviation(const Vec3f&, const Vec3f& n) const
{
    return m_plane.getNormal().dot(n);
}
 
void
PlanePrimitiveShape::DistanceAndNormalDeviation(const Vec3f& p,
                                                const Vec3f& n,
                                                std::pair<float, float>* dn) const
{
    dn->first = m_plane.getDistance(p);
    dn->second = m_plane.getNormal().dot(n);
}
 
void
PlanePrimitiveShape::Project(const Vec3f& p, Vec3f* pp) const
{
    *pp = p - ((m_plane.getNormal().dot(p - m_plane.getPosition())) * m_plane.getNormal());
}
 
void
PlanePrimitiveShape::Normal(const Vec3f& p, Vec3f* n) const
{
    *n = m_plane.getNormal();
}
 
unsigned int
PlanePrimitiveShape::ConfidenceTests(unsigned int numTests,
                                     float epsilon,
                                     float normalThresh,
                                     float rms,
                                     const PointCloud& pc,
                                     const MiscLib::Vector<size_t>& indices) const
{
    return BasePrimitiveShape::ConfidenceTests<Plane>(
        numTests, epsilon, normalThresh, rms, pc, indices);
}
 
void
PlanePrimitiveShape::Description(std::string* s) const
{
    *s = "Plane";
}
 
bool
PlanePrimitiveShape::Fit(const PointCloud& pc,
                         float epsilon,
                         float normalThresh,
                         MiscLib::Vector<size_t>::const_iterator begin,
                         MiscLib::Vector<size_t>::const_iterator end)
{
    Plane fit = m_plane;
    //if(fit.LeastSquaresFit(pc, begin, end))
    if (fit.LeastSquaresFit(pc, begin, end))
    {
        m_plane = fit;
        m_hcs.FromNormal(m_plane.getNormal());
        return true;
    }
    return false;
}
 
PrimitiveShape*
PlanePrimitiveShape::LSFit(const PointCloud& pc,
                           float epsilon,
                           float normalThresh,
                           MiscLib::Vector<size_t>::const_iterator begin,
                           MiscLib::Vector<size_t>::const_iterator end,
                           std::pair<size_t, float>* score) const
{
    Plane fit = m_plane;
    if (fit.LeastSquaresFit(pc, begin, end))
    {
        score->first = -1;
        return new PlanePrimitiveShape(fit);
    }
    score->first = 0;
    return NULL;
}
 
void
PlanePrimitiveShape::Serialize(std::ostream* o, bool binary) const
{
    if (binary)
    {
        const char id = 0;
        (*o) << id;
    }
    else
    {
        (*o) << "0"
             << " ";
    }
    m_plane.Serialize(binary, o);
    if (!binary)
    {
        *o << std::endl;
    }
}
 
size_t
PlanePrimitiveShape::SerializedSize() const
{
    return m_plane.SerializedSize() + 1;
}
 
void
PlanePrimitiveShape::Transform(float scale, const Vec3f& translate)
{
    m_plane.Transform(scale, translate);
}
 
void
PlanePrimitiveShape::Transform(const GfxTL::MatrixXX<3, 3, float>& rot,
                               const GfxTL::Vector3Df& trans)
{
    GfxTL::Vector3Df transNormal = rot * GfxTL::Vector3Df(m_plane.getNormal());
    GfxTL::Vector3Df transOrigin = rot * GfxTL::Vector3Df(m_plane.getPosition()) + trans;
    m_plane = Plane(Vec3f(transOrigin.Data()), Vec3f(transNormal.Data()));
    m_hcs[0] = rot * m_hcs[0];
    m_hcs[1] = rot * m_hcs[1];
}
 
void
PlanePrimitiveShape::Visit(PrimitiveShapeVisitor* visitor) const
{
    visitor->Visit(*this);
}
 
bool
PlanePrimitiveShape::Similar(float, const PlanePrimitiveShape&) const
{
    return true; // planes are always similar!
}
 
LevMarFunc<float>*
PlanePrimitiveShape::SignedDistanceFunc() const
{
    return new PlaneLevMarFunc(m_plane);
}
 
void
PlanePrimitiveShape::Parameters(const Vec3f& p, std::pair<float, float>* param) const
{
    Vec3f pp = p - m_plane.getPosition();
    param->first = pp.dot(m_hcs[0].Data());
    param->second = pp.dot(m_hcs[1].Data());
}
 
void
PlanePrimitiveShape::Parameters(
    GfxTL::IndexedIterator<MiscLib::Vector<size_t>::iterator, PointCloud::const_iterator> begin,
    GfxTL::IndexedIterator<MiscLib::Vector<size_t>::iterator, PointCloud::const_iterator> end,
    MiscLib::Vector<std::pair<float, float>>* bmpParams) const
{
    ParametersImpl(begin, end, bmpParams);
}
 
void
PlanePrimitiveShape::Parameters(
    GfxTL::IndexedIterator<IndexIterator, PointCloud::const_iterator> begin,
    GfxTL::IndexedIterator<IndexIterator, PointCloud::const_iterator> end,
    MiscLib::Vector<std::pair<float, float>>* bmpParams) const
{
    ParametersImpl(begin, end, bmpParams);
}
 
void
PlanePrimitiveShape::BitmapExtent(float epsilon,
                                  GfxTL::AABox<GfxTL::Vector2Df>* bbox,
                                  MiscLib::Vector<std::pair<float, float>>* params,
                                  size_t* uextent,
                                  size_t* vextent)
{
    *uextent = size_t(std::ceil((bbox->Max()[0] - bbox->Min()[0]) / epsilon)) + 1;
    *vextent = size_t(std::ceil((bbox->Max()[1] - bbox->Min()[1]) / epsilon)) + 1;
}
 
void
PlanePrimitiveShape::InBitmap(const std::pair<float, float>& param,
                              float epsilon,
                              const GfxTL::AABox<GfxTL::Vector2Df>& bbox,
                              size_t,
                              size_t,
                              std::pair<int, int>* inBmp) const
{
    inBmp->first = std::floor((param.first - bbox.Min()[0]) / epsilon);
    inBmp->second = std::floor((param.second - bbox.Min()[1]) / epsilon);
}
 
void
PlanePrimitiveShape::WrapBitmap(const GfxTL::AABox<GfxTL::Vector2Df>& bbox,
                                float epsilon,
                                bool* uwrap,
                                bool* vwrap) const
{
    *uwrap = false;
    *vwrap = false;
}
 
void
PlanePrimitiveShape::SetExtent(const GfxTL::AABox<GfxTL::Vector2Df>& bbox,
                               const MiscLib::Vector<int>&,
                               size_t,
                               size_t,
                               float,
                               int)
{
}
 
bool
PlanePrimitiveShape::InSpace(float u, float v, Vec3f* p, Vec3f* n) const
{
    *p = m_plane.getPosition() + Vec3f((u * m_hcs[0] + v * m_hcs[1]).Data());
    *n = m_plane.getNormal();
    return true;
}
 
bool
PlanePrimitiveShape::InSpace(size_t u,
                             size_t v,
                             float epsilon,
                             const GfxTL::AABox<GfxTL::Vector2Df>& bbox,
                             size_t uextent,
                             size_t vextent,
                             Vec3f* p,
                             Vec3f* n) const
{
    *p = Vec3f(((bbox.Min()[0] + epsilon * (float(u) + .5f)) * m_hcs[0] +
                (bbox.Min()[1] + epsilon * (float(v) + .5f)) * m_hcs[1])
                   .Data()) +
         m_plane.getPosition();
    *n = m_plane.getNormal();
    return true;
}