/***************************************************************************
 *   Copyright (C) 2007 by A.J. Tavakoli                                   *
 *                                                                         *
 *   This program 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 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program 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, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

#include <cmath>
#include <cstddef>

namespace Math3D {


// tests sphere with center c0 and radius r0 against sphere with center r1 and
// radius r0.  returns true if there is an intersection, false otherwise.
template<class Real>
bool intersectSphereSphere(const Point3<Real> &c0, Real r0,
                           const Point3<Real> &c1, Real r1) {
  Vector3<Real> diff = c1 - c0;
  Real radiusSum = r0 + r1;
  Real sqrDist = diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2];
  
  // if distance between center of two spheres is less than sum of radii,
  // then there is an intersection
  if ( sqrDist < radiusSum*radiusSum )
    return true; // spheres intersect
  
  return false;
}


// tests a sphere for intersection against an axis-aligned bounding-box
template<class Real>
bool intersectSphereAABB(const Point3<Real> &c, Real r, const AABB<Real> &aabb) {
  // to store squared distance from sphere center to box
  Real d = 0;

  const Point3<Real> &min = aabb.getMin(),
                      &max = aabb.getMax();

  for( int i=0; i < 3; i++ ) {
    if( c[i] < min[i] ) {
      Real s = c[i] - min[i];
      d += s*s;
    }
    else if ( c[i] > max[i] ) {
      Real s = c[i] - max[i];
      d += s*s;
    }
  }

  return d <= r*r;
}


// tests a sphere for intersection against an axis-aligned bounding-box, and if
// there is an intersection then the intersection point is stored in p.
template<class Real>
bool intersectSphereAABB(const Point3<Real> &c, Real r, const AABB<Real> &aabb,
                         Point3<Real> &p) {
  // to store squared distance from sphere center to box
  Real d = 0;

  const Point3<Real> &min = aabb.getMin(),
                      &max = aabb.getMax();
  Vector3<Real> s;

  for( int i=0; i < 3; i++ ) {
    if( c[i] < min[i] ) {
      s[i] = c[i] - min[i];
      d += s[i]*s[i];
    }
    else if ( c[i] > max[i] ) {
      s[i] = c[i] - max[i];
      d += s[i]*s[i];
    }
  }

  // if squared distance is less than squared radius then there is an
  // intersection
  if ( d <= r*r ) {
    // store intersection point
    p = c + s;
    return true;
  }

  return false;
}


template<class Real>
bool intersectAABBAABB(const AABB<Real> &box1, const AABB<Real> &box2,
                       AABB<Real> *overlap) {
  const Point3<Real> &b1min = box1.getMin(),
                     &b1max = box1.getMax(),
                     &b2min = box2.getMin(),
                     &b2max = box2.getMax();

  if (b1min[0] > b2max[0]) return false;
  if (b1max[0] < b2min[0]) return false;
  if (b1min[1] > b2max[1]) return false;
  if (b1max[1] < b2min[1]) return false;
  if (b1min[2] > b2max[2]) return false;
  if (b1max[2] < b2min[2]) return false;

  // store overlap of boxes, if it is requested
  if ( overlap != NULL ) {
    Point3<Real> min, max;
    min[0] = b1min[0] > b2min[0] ? b1min[0] : b2min[0];
    max[0] = b1max[0] > b2max[0] ? b2max[0] : b2max[0];
    min[1] = b1min[1] > b2min[1] ? b1min[1] : b2min[1];
    max[1] = b1max[1] > b2max[1] ? b2max[1] : b2max[1];
    min[2] = b1min[2] > b2min[2] ? b1min[2] : b2min[2];
    max[2] = b1max[2] > b2max[2] ? b2max[2] : b2max[2];
    (*overlap) = AABB<Real>(min, max);
  }

  return true;
}


} // namespace Math3D
