/*
This code provided under the terms of the Id Software 
LIMITED USE SOFTWARE LICENSE AGREEMENT, a copy of which is included with the
GtkRadiant sources (see LICENSE_ID). If you did not receive a copy of 
LICENSE_ID, please contact Id Software immediately at info@idsoftware.com.

All changes and additions to the original source which have been developed by
other contributors (see CONTRIBUTORS) are provided under the terms of the
license the contributors choose (see LICENSE), to the extent permitted by the
LICENSE_ID. If you did not receive a copy of the contributor license,
please contact the GtkRadiant maintainers at info@gtkradiant.com immediately.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <float.h>

#include "mathlib.h"

const aabb_t g_aabb_null = {
  { 0, 0, 0, },
  { -FLT_MAX, -FLT_MAX, -FLT_MAX, },
};

void aabb_construct_for_vec3(aabb_t *aabb, const vec3_t min, const vec3_t max)
{
  VectorMid(min, max, aabb->origin);
  VectorSubtract(max, aabb->origin, aabb->extents);
}

void aabb_clear(aabb_t *aabb)
{
  VectorCopy(g_aabb_null.origin, aabb->origin);
  VectorCopy(g_aabb_null.extents, aabb->extents);
}

void aabb_extend_by_point(aabb_t *aabb, const vec3_t point)
{
#if 1
  int i;
  vec_t min, max, displacement;
  for(i=0; i<3; i++)
  {
    displacement = point[i] - aabb->origin[i];
    if(fabs(displacement) > aabb->extents[i])
    {
      if(aabb->extents[i] < 0) // degenerate
      {
        min = max = point[i];
      }
      else if(displacement > 0)
      {
        min = aabb->origin[i] - aabb->extents[i];
        max = aabb->origin[i] + displacement;
      }
      else
      {
        max = aabb->origin[i] + aabb->extents[i];
        min = aabb->origin[i] + displacement;
      }
      aabb->origin[i] = (min + max) * 0.5f;
      aabb->extents[i] = max - aabb->origin[i];
    }
  }
#else
  unsigned int i;
  for(i=0; i<3; ++i)
  {
    if(aabb->extents[i] < 0) // degenerate
    {
      aabb->origin[i] = point[i];
      aabb->extents[i] = 0;
    }
    else
    {
      vec_t displacement = point[i] - aabb->origin[i];
      vec_t increment = (vec_t)fabs(displacement) - aabb->extents[i];
      if(increment > 0)
      {
        increment *= (vec_t)((displacement > 0) ? 0.5 : -0.5);
        aabb->extents[i] += increment;
        aabb->origin[i] += increment;
      }
    }
  }
#endif
}

void aabb_extend_by_aabb(aabb_t *aabb, const aabb_t *aabb_src)
{
  int i;
  vec_t min, max, displacement, difference;
  for(i=0; i<3; i++)
  {
    displacement = aabb_src->origin[i] - aabb->origin[i];
    difference = aabb_src->extents[i] - aabb->extents[i];
    if(aabb->extents[i] < 0
      || difference >= fabs(displacement))
    {
      // 2nd contains 1st
      aabb->extents[i] = aabb_src->extents[i];
      aabb->origin[i] = aabb_src->origin[i];
    }
    else if(aabb_src->extents[i] < 0
      || -difference >= fabs(displacement))
    {
      // 1st contains 2nd
      continue;
    }
    else
    {
      // not contained
      if(displacement > 0)
      {
        min = aabb->origin[i] - aabb->extents[i];
        max = aabb_src->origin[i] + aabb_src->extents[i];
      }
      else
      {
        min = aabb_src->origin[i] - aabb_src->extents[i];
        max = aabb->origin[i] + aabb->extents[i];
      }
      aabb->origin[i] = (min + max) * 0.5f;
      aabb->extents[i] = max - aabb->origin[i];
    }
  }
}

void aabb_extend_by_vec3(aabb_t *aabb, vec3_t extension)
{
  VectorAdd(aabb->extents, extension, aabb->extents);
}

int aabb_test_point(const aabb_t *aabb, const vec3_t point)
{
  int i;
  for(i=0; i<3; i++)
    if(fabs(point[i] - aabb->origin[i]) >= aabb->extents[i])
      return 0;
  return 1;
}

int aabb_test_aabb(const aabb_t *aabb, const aabb_t *aabb_src)
{
  int i;
  for (i=0; i<3; i++)
    if ( fabs(aabb_src->origin[i] - aabb->origin[i]) > (fabs(aabb->extents[i]) + fabs(aabb_src->extents[i])) )
      return 0;
  return 1;
}

int aabb_test_plane(const aabb_t *aabb, const float *plane)
{
  float fDist, fIntersect;

  // calc distance of origin from plane
  fDist = DotProduct(plane, aabb->origin) + plane[3];
  
   // calc extents distance relative to plane normal
  fIntersect = (vec_t)(fabs(plane[0] * aabb->extents[0]) + fabs(plane[1] * aabb->extents[1]) + fabs(plane[2] * aabb->extents[2]));
  // accept if origin is less than or equal to this distance
  if (fabs(fDist) < fIntersect) return 1; // partially inside
  else if (fDist < 0) return 2; // totally inside
  return 0; // totally outside
}

/* 
Fast Ray-Box Intersection
by Andrew Woo
from "Graphics Gems", Academic Press, 1990
*/

#define NUMDIM	3
#define RIGHT	0
#define LEFT	1
#define MIDDLE	2

int aabb_intersect_ray(const aabb_t *aabb, const ray_t *ray, vec3_t intersection)
{
	int inside = 1;
	char quadrant[NUMDIM];
	register int i;
	int whichPlane;
	double maxT[NUMDIM];
	double candidatePlane[NUMDIM];
  
  const float *origin = ray->origin;
  const float *direction = ray->direction;

	/* Find candidate planes; this loop can be avoided if
   	rays cast all from the eye(assume perpsective view) */
	for (i=0; i<NUMDIM; i++)
  {
		if(origin[i] < (aabb->origin[i] - aabb->extents[i]))
    {
			quadrant[i] = LEFT;
			candidatePlane[i] = (aabb->origin[i] - aabb->extents[i]);
			inside = 0;
		}
    else if (origin[i] > (aabb->origin[i] + aabb->extents[i]))
    {
			quadrant[i] = RIGHT;
			candidatePlane[i] = (aabb->origin[i] + aabb->extents[i]);
			inside = 0;
		}
    else
    {
			quadrant[i] = MIDDLE;
    }
  }

	/* Ray origin inside bounding box */
	if(inside == 1)
  {
		VectorCopy(ray->origin, intersection);
		return 1;
	}


	/* Calculate T distances to candidate planes */
	for (i = 0; i < NUMDIM; i++)
  {
		if (quadrant[i] != MIDDLE && direction[i] !=0.)
			maxT[i] = (candidatePlane[i] - origin[i]) / direction[i];
		else
			maxT[i] = -1.;
  }

	/* Get largest of the maxT's for final choice of intersection */
	whichPlane = 0;
	for (i = 1; i < NUMDIM; i++)
		if (maxT[whichPlane] < maxT[i])
			whichPlane = i;

	/* Check final candidate actually inside box */
	if (maxT[whichPlane] < 0.)
    return 0;
	for (i = 0; i < NUMDIM; i++)
  {
		if (whichPlane != i)
    {
			intersection[i] = (vec_t)(origin[i] + maxT[whichPlane] * direction[i]);
			if (fabs(intersection[i] - aabb->origin[i]) > aabb->extents[i])
				return 0;
		}
    else
    {
			intersection[i] = (vec_t)candidatePlane[i];
		}
  }

	return 1;				/* ray hits box */
}

int aabb_test_ray(const aabb_t* aabb, const ray_t* ray)
{
 vec3_t displacement, ray_absolute;
 vec_t f;
 
 displacement[0] = ray->origin[0] - aabb->origin[0];
 if(fabs(displacement[0]) > aabb->extents[0] && displacement[0] * ray->direction[0] >= 0.0f)
   return 0;
 
 displacement[1] = ray->origin[1] - aabb->origin[1];
 if(fabs(displacement[1]) > aabb->extents[1] && displacement[1] * ray->direction[1] >= 0.0f)
   return 0;
 
 displacement[2] = ray->origin[2] - aabb->origin[2];
 if(fabs(displacement[2]) > aabb->extents[2] && displacement[2] * ray->direction[2] >= 0.0f)
   return 0;
 
 ray_absolute[0] = (float)fabs(ray->direction[0]);
 ray_absolute[1] = (float)fabs(ray->direction[1]);
 ray_absolute[2] = (float)fabs(ray->direction[2]);

 f = ray->direction[1] * displacement[2] - ray->direction[2] * displacement[1];
 if((float)fabs(f) > aabb->extents[1] * ray_absolute[2] + aabb->extents[2] * ray_absolute[1])
   return 0;

 f = ray->direction[2] * displacement[0] - ray->direction[0] * displacement[2];
 if((float)fabs(f) > aabb->extents[0] * ray_absolute[2] + aabb->extents[2] * ray_absolute[0])
   return 0;

 f = ray->direction[0] * displacement[1] - ray->direction[1] * displacement[0];
 if((float)fabs(f) > aabb->extents[0] * ray_absolute[1] + aabb->extents[1] * ray_absolute[0])
   return 0;
 
 return 1;
}

void aabb_orthogonal_transform(aabb_t* dst, const aabb_t* src, const m4x4_t transform)
{
  VectorCopy(src->origin, dst->origin);
  m4x4_transform_point(transform, dst->origin);

  dst->extents[0] = (vec_t)(fabs(transform[0]  * src->extents[0])
                          + fabs(transform[4]  * src->extents[1])
                          + fabs(transform[8]  * src->extents[2]));
  dst->extents[1] = (vec_t)(fabs(transform[1]  * src->extents[0])
                          + fabs(transform[5]  * src->extents[1])
                          + fabs(transform[9]  * src->extents[2]));
  dst->extents[2] = (vec_t)(fabs(transform[2]  * src->extents[0])
                          + fabs(transform[6]  * src->extents[1])
                          + fabs(transform[10] * src->extents[2]));
}

void aabb_for_bbox(aabb_t *aabb, const bbox_t *bbox)
{
	int i;
	vec3_t temp[3];

  VectorCopy(bbox->aabb.origin, aabb->origin);

	// calculate the AABB extents in local coord space from the OBB extents and axes
	VectorScale(bbox->axes[0], bbox->aabb.extents[0], temp[0]);
	VectorScale(bbox->axes[1], bbox->aabb.extents[1], temp[1]);
	VectorScale(bbox->axes[2], bbox->aabb.extents[2], temp[2]);
	for(i=0;i<3;i++) aabb->extents[i] = (vec_t)(fabs(temp[0][i]) + fabs(temp[1][i]) + fabs(temp[2][i]));
}

void aabb_for_area(aabb_t *aabb, vec3_t area_tl, vec3_t area_br, int axis)
{
  aabb_clear(aabb);
  aabb->extents[axis] = FLT_MAX;
  aabb_extend_by_point(aabb, area_tl);
  aabb_extend_by_point(aabb, area_br);
}

int aabb_oriented_intersect_plane(const aabb_t *aabb, const m4x4_t transform, const vec_t* plane)
{
  vec_t fDist, fIntersect;

  // calc distance of origin from plane
  fDist = DotProduct(plane, aabb->origin) + plane[3];

  // calc extents distance relative to plane normal
  fIntersect = (vec_t)(fabs(aabb->extents[0] * DotProduct(plane, transform))
    + fabs(aabb->extents[1] * DotProduct(plane, transform+4))
    + fabs(aabb->extents[2] * DotProduct(plane, transform+8)));
  // accept if origin is less than this distance
  if (fabs(fDist) < fIntersect) return 1; // partially inside
  else if (fDist < 0) return 2; // totally inside
  return 0; // totally outside
}

void aabb_corners(const aabb_t* aabb, vec3_t corners[8])
{
  vec3_t min, max;
  VectorSubtract(aabb->origin, aabb->extents, min);
  VectorAdd(aabb->origin, aabb->extents, max);
  VectorSet(corners[0], min[0], max[1], max[2]);
  VectorSet(corners[1], max[0], max[1], max[2]);
  VectorSet(corners[2], max[0], min[1], max[2]);
  VectorSet(corners[3], min[0], min[1], max[2]);
  VectorSet(corners[4], min[0], max[1], min[2]);
  VectorSet(corners[5], max[0], max[1], min[2]);
  VectorSet(corners[6], max[0], min[1], min[2]);
  VectorSet(corners[7], min[0], min[1], min[2]);
}


void bbox_update_radius(bbox_t *bbox)
{
  bbox->radius = VectorLength(bbox->aabb.extents);
}

void aabb_for_transformed_aabb(aabb_t* dst, const aabb_t* src, const m4x4_t transform)
{
  if(src->extents[0] < 0
    || src->extents[1] < 0
    || src->extents[2] < 0)
  {
    aabb_clear(dst);
    return;
  }

  VectorCopy(src->origin, dst->origin);
  m4x4_transform_point(transform, dst->origin);

  dst->extents[0] = (vec_t)(fabs(transform[0]  * src->extents[0])
                          + fabs(transform[4]  * src->extents[1])
                          + fabs(transform[8]  * src->extents[2]));
  dst->extents[1] = (vec_t)(fabs(transform[1]  * src->extents[0])
                          + fabs(transform[5]  * src->extents[1])
                          + fabs(transform[9]  * src->extents[2]));
  dst->extents[2] = (vec_t)(fabs(transform[2]  * src->extents[0])
                          + fabs(transform[6]  * src->extents[1])
                          + fabs(transform[10] * src->extents[2]));
}

void bbox_for_oriented_aabb(bbox_t *bbox, const aabb_t *aabb, const m4x4_t matrix, const vec3_t euler, const vec3_t scale)
{
	double rad[3];
	double pi_180 = Q_PI / 180;
  double A, B, C, D, E, F, AD, BD;
  
	VectorCopy(aabb->origin, bbox->aabb.origin);
	
  m4x4_transform_point(matrix, bbox->aabb.origin);

	bbox->aabb.extents[0] = aabb->extents[0] * scale[0];
	bbox->aabb.extents[1] = aabb->extents[1] * scale[1];
	bbox->aabb.extents[2] = aabb->extents[2] * scale[2];

  rad[0] = euler[0] * pi_180;
	rad[1] = euler[1] * pi_180;
	rad[2] = euler[2] * pi_180;

  A       = cos(rad[0]);
  B       = sin(rad[0]);
  C       = cos(rad[1]);
  D       = sin(rad[1]);
  E       = cos(rad[2]);
  F       = sin(rad[2]);

  AD      =   A * -D;
  BD      =   B * -D;

	bbox->axes[0][0] = (vec_t)(C*E);
	bbox->axes[0][1] = (vec_t)(-BD*E + A*F);
	bbox->axes[0][2] = (vec_t)(AD*E + B*F);
	bbox->axes[1][0] = (vec_t)(-C*F);
	bbox->axes[1][1] = (vec_t)(BD*F + A*E);
	bbox->axes[1][2] = (vec_t)(-AD*F + B*E);
	bbox->axes[2][0] = (vec_t)D;
	bbox->axes[2][1] = (vec_t)(-B*C);
	bbox->axes[2][2] = (vec_t)(A*C);

  bbox_update_radius(bbox);
}

int bbox_intersect_plane(const bbox_t *bbox, const vec_t* plane)
{
  vec_t fDist, fIntersect;

  // calc distance of origin from plane
  fDist = DotProduct(plane, bbox->aabb.origin) + plane[3];

	// trivial accept/reject using bounding sphere
	if (fabs(fDist) > bbox->radius)
	{
		if (fDist < 0)
			return 2; // totally inside
		else
			return 0; // totally outside
	}

  // calc extents distance relative to plane normal
  fIntersect = (vec_t)(fabs(bbox->aabb.extents[0] * DotProduct(plane, bbox->axes[0]))
             + fabs(bbox->aabb.extents[1] * DotProduct(plane, bbox->axes[1]))
             + fabs(bbox->aabb.extents[2] * DotProduct(plane, bbox->axes[2])));
  // accept if origin is less than this distance
  if (fabs(fDist) < fIntersect) return 1; // partially inside
  else if (fDist < 0) return 2; // totally inside
  return 0; // totally outside
}