PlanePrimitiveShape.cpp

Go to the documentation of this file.

#include "PlanePrimitiveShape.h"
#include "ScoreComputer.h"
#include "PrimitiveShapeVisitor.h"
#include <GfxTL/NullClass.h>
#include <limits>
#include <iostream>
#include <fstream>
#include <sstream>
#include <iomanip>
#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;
}