/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield 
 *
 * This library is open source and may be redistributed and/or modified under  
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or 
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 * 
 * This library 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 
 * OpenSceneGraph Public License for more details.
*/

#ifndef OSG_BOUNDINGSPHERE
#define OSG_BOUNDINGSPHERE 1

#include <osg/Config>
#include <osg/Export>
#include <osg/Vec3f>
#include <osg/Vec3d>

namespace osg {

template<typename VT>
class BoundingBoxImpl;

/** General purpose bounding sphere class for enclosing nodes/objects/vertices.
  * Bounds internal osg::Nodes in the scene, assists in view frustum culling,
  * etc. Similar in function to BoundingBox, it's quicker for evaluating
  * culling but generally will not cull as aggressively because it encloses a
  * greater volume.
*/
template<typename VT>
class BoundingSphereImpl
{
    public:
        typedef VT vec_type;
        typedef typename vec_type::value_type value_type;        

        vec_type    _center;
        value_type  _radius;

        /** Construct a default bounding sphere with radius to -1.0f, representing an invalid/unset bounding sphere.*/ 
        BoundingSphereImpl() : _center(0.0,0.0,0.0),_radius(-1.0) {}
    
        /** Creates a bounding sphere initialized to the given extents. */
        BoundingSphereImpl(const vec_type& center, value_type radius) : _center(center),_radius(radius) {}

        /** Creates a bounding sphere initialized to the given extents. */
        BoundingSphereImpl(const BoundingSphereImpl& bs) : _center(bs._center),_radius(bs._radius) {}

        /** Creates a bounding sphere initialized to the given extents. */
        BoundingSphereImpl(const BoundingBoxImpl<VT>& bb) : _center(0.0,0.0,0.0),_radius(-1.0) { expandBy(bb); }

        /** Clear the bounding sphere. Reset to default values. */
        inline void init()
        {
            _center.set(0.0,0.0,0.0);
            _radius = -1.0;
        }

        /** Returns true of the bounding sphere extents are valid, false
          * otherwise. */
        inline bool valid() const { return _radius>=0.0; }

        /** Set the bounding sphere to the given center/radius using floats. */ 
        inline void set(const vec_type& center,value_type radius)
        {
            _center = center;
            _radius = radius;
        }

        /** Returns the center of the bounding sphere. */
        inline vec_type& center() { return _center; }
        
        /** Returns the const center of the bounding sphere. */
        inline const vec_type& center() const { return _center; }

        /** Returns the radius of the bounding sphere. */
        inline value_type& radius() { return _radius; }
        /** Returns the const radius of the bounding sphere. */
        inline value_type radius() const { return _radius; }
        
        /** Returns the squared length of the radius. Note, For performance
          * reasons, the calling method is responsible for checking to make
          * sure the sphere is valid. */
        inline value_type radius2() const { return _radius*_radius; }

        /** Expands the sphere to encompass the given point. Repositions the
          * sphere center to minimize the radius increase. If the sphere is
          * uninitialized, set its center to v and radius to zero. */
        template<typename vector_type>
        void expandBy(const vector_type& v);

        /** Expands the sphere to encompass the given point. Does not
          * reposition the sphere center. If the sphere is
          * uninitialized, set its center to v and radius to zero. */
        template<typename vector_type>
        void expandRadiusBy(const vector_type& v);

        /** Expands the sphere to encompass the given sphere. Repositions the
          * sphere center to minimize the radius increase. If the sphere is
          * uninitialized, set its center and radius to match sh. */
        void expandBy(const BoundingSphereImpl& sh);

        /** Expands the sphere to encompass the given sphere. Does not
          * repositions the sphere center. If the sphere is
          * uninitialized, set its center and radius to match sh. */
        void expandRadiusBy(const BoundingSphereImpl& sh);

        /** Expands the sphere to encompass the given box. Repositions the
          * sphere center to minimize the radius increase. */
        void expandBy(const BoundingBoxImpl<VT>& bb);

        /** Expands the sphere to encompass the given box. Does not
          * repositions the sphere center. */
        void expandRadiusBy(const BoundingBoxImpl<VT>& bb);

        /** Returns true if v is within the sphere. */
        inline bool contains(const vec_type& v) const
        {
            return valid() && ((v-_center).length2()<=radius2());
        }


        /** Returns true if there is a non-empty intersection with the given
          * bounding sphere. */
        inline bool intersects( const BoundingSphereImpl& bs ) const
        {
            return valid() && bs.valid() &&
                   ((_center - bs._center).length2() <= (_radius + bs._radius)*(_radius + bs._radius));
        }
        
};


template<typename VT>
 template<typename vector_type>
void BoundingSphereImpl<VT>::expandBy(const vector_type& v)
{
    if (valid())
    {
        vec_type dv = v-_center;
        value_type r = dv.length();
        if (r>_radius)
        {
            value_type dr = (r-_radius)*0.5;
            _center += dv*(dr/r);
            _radius += dr;
        } // else do nothing as vertex is within sphere.
    }
    else
    {
        _center = v;
        _radius = 0.0;
    }
}

template<typename VT>
 template<typename vector_type>
void BoundingSphereImpl<VT>::expandRadiusBy(const vector_type& v)
{
    if (valid())
    {
        value_type r = (v-_center).length();
        if (r>_radius) _radius = r;
        // else do nothing as vertex is within sphere.
    }
    else
    {
        _center = v;
        _radius = 0.0;
    }
}

template<typename VT>
void BoundingSphereImpl<VT>::expandBy(const BoundingSphereImpl& sh)
{
    // ignore operation if incomming BoundingSphere is invalid.
    if (!sh.valid()) return;

    // This sphere is not set so use the inbound sphere
    if (!valid())
    {
        _center = sh._center;
        _radius = sh._radius;

        return;
    }
    
    
    // Calculate d == The distance between the sphere centers   
    double d = ( _center - sh.center() ).length();

    // New sphere is already inside this one
    if ( d + sh.radius() <= _radius )  
    {
        return;
    }

    //  New sphere completely contains this one 
    if ( d + _radius <= sh.radius() )  
    {
        _center = sh._center;
        _radius = sh._radius;
        return;
    }

    
    // Build a new sphere that completely contains the other two:
    //
    // The center point lies halfway along the line between the furthest
    // points on the edges of the two spheres.
    //
    // Computing those two points is ugly - so we'll use similar triangles
    double new_radius = (_radius + d + sh.radius() ) * 0.5;
    double ratio = ( new_radius - _radius ) / d ;

    _center[0] += ( sh.center()[0] - _center[0] ) * ratio;
    _center[1] += ( sh.center()[1] - _center[1] ) * ratio;
    _center[2] += ( sh.center()[2] - _center[2] ) * ratio;

    _radius = new_radius;

}

template<typename VT>
void BoundingSphereImpl<VT>::expandRadiusBy(const BoundingSphereImpl& sh)
{
    if (sh.valid())
    {
        if (valid())
        {
            value_type r = (sh._center-_center).length()+sh._radius;
            if (r>_radius) _radius = r;
            // else do nothing as vertex is within sphere.
        }
        else
        {
            _center = sh._center;
            _radius = sh._radius;
        }
    }
}

template<typename VT>        
void BoundingSphereImpl<VT>::expandBy(const BoundingBoxImpl<VT>& bb)
{
    if (bb.valid())
    {
        if (valid())
        {
            BoundingBoxImpl<vec_type> newbb(bb);

            for(unsigned int c=0;c<8;++c)
            {
                vec_type v = bb.corner(c)-_center; // get the direction vector from corner
                v.normalize(); // normalise it.
                v *= -_radius; // move the vector in the opposite direction distance radius.
                v += _center; // move to absolute position.
                newbb.expandBy(v); // add it into the new bounding box.
            }
            
            _center = newbb.center();
            _radius = newbb.radius();
            
        }
        else
        {
            _center = bb.center();
            _radius = bb.radius();
        }
    }
}

template<typename VT>
void BoundingSphereImpl<VT>::expandRadiusBy(const BoundingBoxImpl<VT>& bb)
{
    if (bb.valid())
    {
        if (valid())
        {
            for(unsigned int c=0;c<8;++c)
            {
                expandRadiusBy(bb.corner(c));
            }
        }
        else
        {
            _center = bb.center();
            _radius = bb.radius();
        }
    }
}

typedef BoundingSphereImpl<Vec3f> BoundingSpheref;
typedef BoundingSphereImpl<Vec3d> BoundingSphered;

#ifdef OSG_USE_FLOAT_BOUNDINGSPHERE
        typedef BoundingSpheref BoundingSphere;
#else
        typedef BoundingSphered BoundingSphere;        
#endif
}

#endif