src/GeometryToolbox.hh

/*
 * Copyright Staffan Gimåker 2007-2010.
 *
 * ---
 *
 * This file is part of peekabot.
 *
 * peekabot is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * peekabot 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/>.
 */

#ifndef PEEKABOT_GEOM_GEOMETRY_TOOLBOX_HH_INCLUDED
#define PEEKABOT_GEOM_GEOMETRY_TOOLBOX_HH_INCLUDED


#include <cmath>
#include <cassert>
#include <cstddef>
#include <vector>
#include <Eigen/Core>


namespace peekabot
{
    namespace geom
    {
        template<class FwdIt, class VertexType>
        void calc_optimal_bounding_sphere(
            FwdIt begin, FwdIt end,
            VertexType &midp, float &r,
            float x_scale_factor,
            float y_scale_factor,
            float z_scale_factor) throw()
        {
            float max_sq_dist = 0;
            midp = VertexType(0,0,0);

            size_t count = 0;

            for( FwdIt it = begin; it < end; )
            {
                ++count;

                const VertexType &v1 = *it;
                FwdIt next_it = ++it;

                for( FwdIt jt = next_it; jt < end; ++jt )
                {
                    VertexType diff = v1 - (*jt);
                    diff(0) *= x_scale_factor;
                    diff(1) *= y_scale_factor;
                    diff(2) *= z_scale_factor;
                    float sq_dist = diff.dot(diff);

                    if( sq_dist > max_sq_dist )
                    {
                        max_sq_dist = sq_dist;
                        midp = *jt;
                        midp(0) *= x_scale_factor;
                        midp(1) *= y_scale_factor;
                        midp(2) *= z_scale_factor;
                        midp += diff/2;
                    }
                }
            }

            // Special case for when there's only one vertex
            if( count == 1 )
                midp = *begin;

            r = sqrtf(max_sq_dist)/2;
        }


        template<typename FwdIt, typename VertexType>
        void calc_approx_bounding_sphere_naive(
            FwdIt begin, FwdIt end,
            VertexType &midp, float &r,
            float x_scale_factor,
            float y_scale_factor,
            float z_scale_factor) throw()
        {
            // Special case: no coordinates
            if( begin == end )
            {
                midp = VertexType(0, 0, 0);
                r = 0;
                return;
            }

            const VertexType sv(
                x_scale_factor, y_scale_factor, z_scale_factor);

            midp = VertexType(0, 0, 0);
            for( FwdIt it = begin; it < end; ++it )
                midp += (*it).cwise() * sv;
            midp /= std::distance(begin, end);

            float sq_r = 0;
            for( FwdIt it = begin; it < end; ++it )
                sq_r = std::max(sq_r, ((*it).cwise() * sv - midp).squaredNorm());
            r = sqrtf(sq_r);
        }


        inline void calc_approx_bounding_sphere_naive(
            const std::vector<Eigen::Vector3f> &pts,
            Eigen::Vector3f &midp, float &r,
            float x_scale_factor,
            float y_scale_factor,
            float z_scale_factor) throw()
        {
            typedef Eigen::Vector3f VertexType;

            // Special case: no coordinates
            if( pts.empty() )
            {
                midp = VertexType(0, 0, 0);
                r = 0;
                return;
            }

            const VertexType sv(
                x_scale_factor, y_scale_factor, z_scale_factor);

            midp = VertexType(0, 0, 0);
            for( std::size_t i = 0; i < pts.size(); ++i )
                midp += pts[i].cwise() * sv;
            midp /= pts.size();

            float sq_r = 0;
            for( std::size_t i = 0; i < pts.size(); ++i )
                sq_r = std::max(sq_r, (pts[i].cwise() * sv - midp).squaredNorm());
            r = sqrtf(sq_r);
        }


        template<typename FwdIt, typename VertexType>
        void calc_approx_bounding_sphere_ritter(
            FwdIt begin, FwdIt end,
            VertexType &midp, float &r,
            float x_scale_factor,
            float y_scale_factor,
            float z_scale_factor) throw()
        {
            // Special case: no coordinates
            if( begin == end )
            {
                midp = VertexType(0,0,0);
                r = 0;
                return;
            }

            VertexType min[3], max[3];
            for( int i = 0; i < 3; ++i )
                min[i] = max[i] = *begin;

            FwdIt it = begin;
            for( ++it; it < end; ++it )
            {
                for( int i = 0; i < 3; ++i )
                {
                    if( (*it)(i) < min[i](i) )
                        min[i] = *it;
                    else if( (*it)(i) > max[i](i) )
                        max[i] = *it;
                }
            }

            const VertexType sv(
                x_scale_factor, y_scale_factor, z_scale_factor);

            midp = VertexType(0, 0, 0);
            r = 0; // Note: r is actually used as 4r^2 here for a while

            for( int i = 0; i < 3; ++i )
            {
                max[i] = max[i].cwise() * sv;
                min[i] = min[i].cwise() * sv;

                float sq_d = (max[i]-min[i]).squaredNorm();
                if( sq_d > r )
                {
                    r = sq_d;
                    midp = min[i] + (max[i]-min[i])/2;
                }
            }

            r = sqrt(r/4); // Change r back to actually being r

            for( FwdIt it = begin; it < end; ++it )
            {
                VertexType diff = (*it).cwise() * sv - midp;
                float sq_d = diff.squaredNorm();

                if( sq_d > r*r+0.0000000001 )
                {
                    float d = sqrtf(sq_d);
                    r = (d + r)/2;
                    midp += (d-r)/d * diff;
                }
            }
        }


        inline void calc_approx_bounding_sphere_ritter(
            const std::vector<Eigen::Vector3f> &pts,
            Eigen::Vector3f &midp, float &r,
            float x_scale_factor,
            float y_scale_factor,
            float z_scale_factor) throw()
        {
            typedef Eigen::Vector3f VertexType;

            // Special case: no coordinates
            if( pts.empty() )
            {
                midp = VertexType(0,0,0);
                r = 0;
                return;
            }

            VertexType min[3], max[3];
            for( int i = 0; i < 3; ++i )
                min[i] = max[i] = pts[0];

            for( std::size_t j = 1; j < pts.size(); ++j )
            {
                for( int i = 0; i < 3; ++i )
                {
                    if( pts[j](i) < min[i](i) )
                        min[i] = pts[j];
                    else if( pts[j](i) > max[i](i) )
                        max[i] = pts[j];
                }
            }

            const VertexType sv(
                x_scale_factor, y_scale_factor, z_scale_factor);

            midp = VertexType(0, 0, 0);
            r = 0; // Note: r is actually used as 4r^2 here for a while

            for( int i = 0; i < 3; ++i )
            {
                max[i] = max[i].cwise() * sv;
                min[i] = min[i].cwise() * sv;

                float sq_d = (max[i]-min[i]).squaredNorm();
                if( sq_d > r )
                {
                    r = sq_d;
                    midp = min[i] + (max[i]-min[i])/2;
                }
            }

            r = sqrt(r/4); // Change r back to actually being r

            for( std::size_t i = 0; i < pts.size(); ++i )
            {
                VertexType diff = pts[i].cwise() * sv - midp;
                float sq_d = diff.squaredNorm();

                if( sq_d > r*r+0.0000000001 )
                {
                    float d = sqrtf(sq_d);
                    r = (d + r)/2;
                    midp += (d-r)/d * diff;
                }
            }
        }


        template<typename FwdIt, typename VertexType>
        void calc_approx_bounding_sphere_aabb_center(
            FwdIt begin, FwdIt end,
            VertexType &midp, float &r,
            float x_scale_factor,
            float y_scale_factor,
            float z_scale_factor) throw()
        {
            // Special case: no coordinates
            if( begin == end )
            {
                midp = VertexType(0, 0, 0);
                r = 0;
                return;
            }

            Eigen::Vector3f min(
                std::numeric_limits<float>::max(),
                std::numeric_limits<float>::max(),
                std::numeric_limits<float>::max());

            Eigen::Vector3f max(
                -std::numeric_limits<float>::max(),
                -std::numeric_limits<float>::max(),
                -std::numeric_limits<float>::max());

            for( FwdIt it = begin; it < end; ++it )
            {
                for( std::size_t i = 0; i < 3; ++i )
                {
                    min(i) = std::min(min(i), (*it)(i));
                    max(i) = std::max(max(i), (*it)(i));
                }
            }

            const VertexType sv(
                x_scale_factor, y_scale_factor, z_scale_factor);

            min = min.cwise() * sv;
            max = max.cwise() * sv;
            midp = min/2 + max/2;

            float sq_r = 0;
            for( FwdIt it = begin; it < end; ++it )
                sq_r = std::max(sq_r, ((*it).cwise() * sv - midp).squaredNorm());
            r = sqrtf(sq_r);
        }


        inline void calc_approx_bounding_sphere_aabb_center(
            const std::vector<Eigen::Vector3f> &pts,
            Eigen::Vector3f &midp, float &r,
            float x_scale_factor,
            float y_scale_factor,
            float z_scale_factor) throw()
        {
            // Special case: no coordinates
            if( pts.empty() )
            {
                midp = Eigen::Vector3f(0, 0, 0);
                r = 0;
                return;
            }

            Eigen::Vector3f min(
                std::numeric_limits<float>::max(),
                std::numeric_limits<float>::max(),
                std::numeric_limits<float>::max());

            Eigen::Vector3f max(
                -std::numeric_limits<float>::max(),
                -std::numeric_limits<float>::max(),
                -std::numeric_limits<float>::max());

            for( std::size_t j = 0; j < pts.size(); ++j )
            {
                for( std::size_t i = 0; i < 3; ++i )
                {
                    min(i) = std::min(min(i), pts[j](i));
                    max(i) = std::max(max(i), pts[j](i));
                }
            }

            const Eigen::Vector3f sv(
                x_scale_factor, y_scale_factor, z_scale_factor);

            min = min.cwise() * sv;
            max = max.cwise() * sv;
            midp = min/2 + max/2;

            float sq_r = 0;
            for( std::size_t i = 0; i < pts.size(); ++i )
                sq_r = std::max(sq_r, (pts[i].cwise() * sv - midp).squaredNorm());
            r = sqrtf(sq_r);
        }


        template<typename FwdIt, typename VertexType>
        void calc_approx_bounding_sphere(
            FwdIt begin, FwdIt end,
            VertexType &midp, float &r,
            float x_scale_factor,
            float y_scale_factor,
            float z_scale_factor) throw()
        {
            calc_approx_bounding_sphere_aabb_center(
                begin, end, midp, r,
                x_scale_factor, y_scale_factor, z_scale_factor);
        }


        inline void calc_approx_bounding_sphere(
            const std::vector<Eigen::Vector3f> &pts,
            Eigen::Vector3f &midp, float &r,
            float x_scale_factor,
            float y_scale_factor,
            float z_scale_factor) throw()
        {
            calc_approx_bounding_sphere_aabb_center(
                pts, midp, r,
                x_scale_factor, y_scale_factor, z_scale_factor);
        }


        // Extend an existing bounding sphere using Ritter's algorithm
        inline bool extend_bounding_sphere(
            const std::vector<Eigen::Vector3f> &pts,
            Eigen::Vector3f &midp, float &r,
            float bail_out_thr) throw()
        {
            const float old_r = r;

            for( std::size_t i = 0; i < pts.size(); ++i )
            {
                Eigen::Vector3f diff = pts[i] - midp;
                float sq_d = diff.squaredNorm();

                if( sq_d > r*r+0.0000000001 )
                {
                    float d = sqrtf(sq_d);
                    r = (d + r)/2;
                    midp += (d-r)/d * diff;

                    if( r > bail_out_thr*old_r )
                    {
                        // It's grown too much, bail out
                        return true;
                    }
                }
            }

            return false;
        }
    }
}


#endif // PEEKABOT_GEOM_GEOMETRY_TOOLBOX_HH_INCLUDED