
#include "Intersection.h"

int bboxIntersect(double boxmin[3], double boxmax[3],double box2min[3], double box2max[3])
{
	if ( boxmin[0] > box2max[0] || box2min[0] > boxmax[0]
      ||boxmin[1] > box2max[1] || box2min[1] > boxmax[1]
      ||boxmin[2] > box2max[2] || box2min[2] > boxmax[2] ) 
   {  
        return 0;
   }
   
   return 1;
}

int bSphereIntersect(CVector3 &pos1, CVector3 &pos2, double rad1, double rad2)
{

  CVector3	dp	= pos1;
  dp -= pos2;

  double r = rad1 + rad2;

  double pp = dp[0] * dp[0] + dp[1] * dp[1] + dp[2] * dp[2];
  return pp <= r*r;
}

int bSphereSweepIntersect(CVector3 &vel1, CVector3 &vel2, CVector3 &pos1, CVector3 &pos2, double rad1, double rad2)
{
  //Initialize the return value
  //*t = 0.0f;

  // Relative velocity
  CVector3	dv = vel2;
  dv-=vel1;
  // Relative position
  CVector3	dp	= pos2;
  dp -= pos1;
  //Minimal distance squared
  double r = rad1 + rad2;
  //dP^2-r^2
  double pp = dp[0] * dp[0] + dp[1] * dp[1] + dp[2] * dp[2] - r*r;
  
   //dP*dV
  double pv	= dp[0] * dv[0] + dp[1] * dv[1] + dp[2] * dv[2];
//FIRST CHECK IF THEY R MOVING AWAY, cause if you do pp test first, they wont move if they are intersecting!!
  //(2)Check if the spheres are moving away from each other
  if ( pv >= 0 ) return 0; 
  
  //(1)Check if the spheres are already intersecting
 if ( pp < 0 ) return 1;



  //dV^2
  double vv = dv[0] * dv[0] + dv[1] * dv[1] + dv[2] * dv[2];
  //(3)Check if the spheres can intersect within 1 frame
  if ( (pv + vv) <= 0 && (vv + 2 * pv + pp) >= 0 ) return 0;

  //tmin = -dP*dV/dV*2 
  //the time when the distance between the spheres is minimal
  double tmin = -pv/vv;

  //Discriminant/(4*dV^2) = -(dp^2-r^2+dP*dV*tmin)
  return ( pp + pv * tmin > 0 );

}