#include <sphere.h>

Sphere::Sphere( const Vector3 &centre, double radius )
    : p( centre ), r( radius )
{
}

Sphere::~Sphere( void )
{
}

bool quadraticRoots( double *root1, double *root2, double a, double b, double c )
{
    /**
     * -b +/- sqrt( b^2 - 4ac )
     * -----------------------
     *          2a
     */
    double insqrt = ( b * b ) - ( 4 * a * c );
    if( insqrt < 0.0 ) return false;

    *root1 = ( -b + sqrt( insqrt ) ) / 2 * a;
    *root2 = ( -b - sqrt( insqrt ) ) / 2 * a;
    return true;
}

bool Sphere::closestIntersect( Vector3 *pos, Vector3 *norm,
                               const Vector3 &o,
                               const Vector3 &dir ) const
{
    double root1, root2;
    double diffx, diffy, diffz;

    diffx = o.x - p.x;
    diffy = o.y - p.y;
    diffz = o.z - p.z;

    if( quadraticRoots( &root1, &root2,
                        ( dir.x * dir.x ) + ( dir.y * dir.y ) + ( dir.z * dir.z ),
                        2 * ( ( dir.x * diffx ) + ( dir.y * diffy )
                                                + ( dir.z * diffz ) ),
                        ( diffx * diffx ) + ( diffy * diffy )
                                          + ( diffz * diffz ) - ( r * r ) ) ) {

        if( root1 < 0.0 && root2 < 0.0 ) return false;

        double nearest = root1;

        if( root1 < 0.0 ) {
            nearest = root2;
        } else if( root2 < 0.0 ) {
            nearest = root1;
        } else if( root2 < nearest ) {
            nearest = root2;
        }

        *pos = o.add( dir.scalarMult( nearest ) );
        *norm = (*pos).subtract( p ).normalize();

        return true;
    } else {
        return false;
    }
}