Geometric.hpp

//
// Geometric Tool for SAL3D.
// Copyright 2010 AQSENSE, S.L.
//
#if !defined (AQSENSE_SAL3DPP_GEOMETRIC_HPP)
#define AQSENSE_SAL3DPP_GEOMETRIC_HPP

#if defined (_MSC_VER) && (_MSC_VER >= 1020)
#pragma once
#endif

#include <cassert>
#include <cmath>
#include <stdexcept>
#include <vector>

#include <sal3d/cop.h>
#include <sal3d/geometric.h>
#include <sal3dpp/Common.hpp>
#include <sal3dpp/COP.hpp>

namespace sal3d
{
    namespace Geometric
    {
        class InsufficientPoints: public std::invalid_argument
        {
            public:
                InsufficientPoints ():
                    invalid_argument ("insufficient points to find the result")
                {
                }
        };

        class Plane
        {
            public:
                Plane () :
                    _plane ()
                {}

                explicit Plane (const sal3d_geometric_plane &plane) :
                    _plane (plane)
                {}

                Plane (const Plane &plane) :
                    _plane (plane._plane)
                {}

                Plane (float A, float B, float C, float D)
                {
                    _plane.p[0] = A;
                    _plane.p[1] = B;
                    _plane.p[2] = C;
                    _plane.p[3] = D;
                }

                Plane (const Point3D& p1, const Point3D& p2, const Point3D& p3)
                {
                    sal3d_geometric_plane_from_three_points (p1.c_point_3d (),
                            p2.c_point_3d (), p3.c_point_3d (), &_plane);
                }

                const sal3d_geometric_plane &
                c_plane () const
                {
                    return _plane;
                }

                const float &
                a() const
                {
                    return _plane.p[0];
                }

                float &
                a()
                {
                    return _plane.p[0];
                }

                const float &
                b() const
                {
                    return _plane.p[1];
                }

                float &
                b()
                {
                    return _plane.p[1];
                }

                const float &
                c() const
                {
                    return _plane.p[2];
                }

                float &
                c()
                {
                    return _plane.p[2];
                }

                const float &
                d() const {
                    return _plane.p[3];
                }

                float &
                d() {
                    return _plane.p[3];
                }

                Movement3D
                alignToZ (const Point3D& point) const
                {
                    sal3d_movement_3d m;

                    sal3d_geometric_align_plane_to_z (point.c_point_3d (),
                            _plane, &m);

                    return Movement3D (m);
                }

                Plane &
                operator= (const Plane& rhs)
                {
                    for (int i=0; i < 4; ++i)
                        (*this)[i] = rhs[i];

                    return *this;
                }

                const float &
                operator[] (int idx) const
                {
                    assert (idx >= 0 && idx < 4 && "invalid index");
                    return _plane.p[idx];
                }

                float &
                operator[] (int idx)
                {
                    return const_cast<float &> (
                            static_cast<const Plane &>(*this)[idx]);
                }

            private:
                sal3d_geometric_plane _plane;
        };

        class Sphere
        {
            public:
                Sphere () :
                    _sphere ()
                {}

                explicit Sphere (const sal3d_geometric_sphere &sphere) :
                    _sphere (sphere)
                {}
               
                Sphere (const Sphere &sphere) :
                    _sphere (sphere._sphere)
                {}

                Sphere (const Point3D &centre, float r) 
                {
                    _sphere.centre.p[0] = centre.x();
                    _sphere.centre.p[1] = centre.y();
                    _sphere.centre.p[2] = centre.z();
                    _sphere.radius = r;
                }

                const sal3d_geometric_sphere &
                c_sphere () const
                {
                    return _sphere;
                }

                Point3D
                centre () const
                {
                    return Point3D (_sphere.centre.p[0],
                            _sphere.centre.p[1], _sphere.centre.p[2]);
                }

                void
                setCentre (const Point3D& centre)
                {
                    _sphere.centre.p[0] = centre.x ();
                    _sphere.centre.p[1] = centre.y ();
                    _sphere.centre.p[2] = centre.z ();
                }

                const float &
                radius () const
                {
                    return _sphere.radius;
                }

                float &
                radius ()
                {
                    return _sphere.radius;
                }

            private:
                sal3d_geometric_sphere _sphere;
        };

        inline Plane
        getRobustTangentPlane (const COP &cop,
                float expectedInliersRatio, float &inliersRatio)
        {
            sal3d_geometric_plane plane;
            std::vector<sal3d_point3d> points (cop.height() *
                    cop.width());
            int size;

            Error e;
            sal3d_geometric_get_robust_tangent_plane(cop.c_cop(),
                    expectedInliersRatio, &plane, &points[0], &size,
                    &inliersRatio, &e.e);

            if (e.e.value < 0)
                throw e;

            return Plane (plane);
        }

        inline Plane
        getRobustTangentPlane (const COP &cop,
                float expectedInliersRatio,
                std::vector<Point3D> &inliers, float &inliersRatio)
        {
            sal3d_geometric_plane plane;
            std::vector<sal3d_point3d> points (cop.height() *
                    cop.width());

            int size;
            Error e;
            sal3d_geometric_get_robust_tangent_plane(cop.c_cop(),
                    expectedInliersRatio, &plane, &points[0], &size,
                    &inliersRatio, &e.e);

            if (e.e.value < 0)
                throw e;

            inliers.clear ();
            inliers.reserve (size);
            for (int i = 0; i < size; ++i)
            {
                Point3D p (points[i]);
                inliers.push_back (p);
            }

            return Plane (plane);
        }

        inline Plane
        getAveragePlane (const COP &cop)
        {
            sal3d_geometric_plane plane;

            sal3d_geometric_get_average_plane(cop.c_cop(), &plane);

            return Plane (plane);
        }

        inline float
        distanceToPlane(const Point3D& point, const Plane& plane,
                Point3D& intersectionPoint)
        {
            float distance;
            sal3d_point3d intersect;

            sal3d_geometric_get_distance_to_plane (point.c_point_3d (),
                    plane.c_plane (), &intersect ,
                    &distance);

            intersectionPoint = Point3D (intersect);

            return distance;
        }

        inline Sphere
        fitSphere (std::vector<Point3D> points)
        {
            if (points.size() < 4)
                throw InsufficientPoints();

            std::vector<sal3d_point3d> points_c(points.size());
            for (unsigned int i = 0; i < points.size (); ++i)
                points_c[i] = points[i].c_point_3d ();

            sal3d_geometric_sphere sphere;
            sal3d_geometric_fit_sphere (&points_c[0], points_c.size(), &sphere);

            return Sphere(sphere);
        }

        inline Sphere
        fitCOPToSphere (const COP &cop)
        {
            sal3d_geometric_sphere sphere;
            sal3d_geometric_fit_cop_to_sphere (cop.c_cop(), &sphere);

            return Sphere(sphere);
        }

    }   // !namespace Geometric
}   // !namespace sal3d

#endif // !AQSENSE_SAL3DPP_GEOMETRIC_HPP