/*
 * Copyright (c) 2007, Insomniac Games
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Insomniac Games nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY <copyright holder> ``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 <copyright holder> 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.
 *
 * -----------------------------------------------------------------------------
 *
 * a simple root finding algorithm is used to find the value of 't' that
 * satisfies:
 *              d|D(t)|^2/dt = 0
 * where:
 *              |D(t)| = |p(t)-b(t)|
 * where p(t) is a point on the line parameterized by t:
 *              p(t) = p1 + t*(p2-p1)
 * and b(t) is that same point clipped to the boundary of the box. in box-
 * relative coordinates d|D(t)|^2/dt is the sum of three x,y,z components
 * each of which looks like this:
 *
 *          t_lo     /
 *            ______/    -->t
 *           /     t_hi
 *          /
 *
 * t_lo and t_hi are the t values where the line passes through the planes
 * corresponding to the sides of the box. the algorithm computes d|D(t)|^2/dt
 * in a piecewise fashion from t=0 to t=1, stopping at the point where
 * d|D(t)|^2/dt crosses from negative to positive.
*/

float DistanceLA(const PrimLineseg *linesegA, const PrimAABB *aabbB, Result *result)
{
  i32 i, domain[3], solution = false;
  float t_anchor[3], t, t_next, deriv, deriv_next, derivs;
 
  vec4f line_vector_mirrored;
  vec4f line_v0_offset_mirrored;
  vec4f sign;
 
  // find the line segment point first end point in the coordinate frame of the AABB
  vec4f line_v0_offset;
  VecSub3(line_v0_offset, linesegA->m_vert0, aabbB->m_center);
 
  // find the line segment vector from the first point to the last
  vec4f line_vector;
  VecSub3(line_vector, linesegA->m_vert1, linesegA->m_vert0);
 
  // mirror the line vector so that it has all components >= 0
  for(i=0;i<3;i++)
  {
    if(line_vector[i] < 0.0f)
    {
      // mirror needed
      line_v0_offset_mirrored.SetElem(i, -line_v0_offset[i]);
      line_vector_mirrored.SetElem(i, -line_vector[i]);
      sign.SetElem(i, -1.0f);
    }
    else
    {
      // no mirror needed
      line_v0_offset_mirrored.SetElem(i, line_v0_offset[i]);
      line_vector_mirrored.SetElem(i, line_vector[i]);
      sign.SetElem(i, 1.0f);
    }
  }
 
  // domain (odd calls this region) is -1,0,+1 depending on which side of the box planes each
  // coordinate is on. t_anchor (ode calls this tanchor) is the next t value at which there is a
  // transition, or the last one if there are no more.
  for(i=0;i<3;i++)
  {
    if(line_vector_mirrored[i] > 0.0f)
    {
      if(line_v0_offset_mirrored[i] < -aabbB->m_radii[i])
      {
        domain[i] = -1;
        t_anchor[i] = ((-aabbB->m_radii[i] - line_v0_offset_mirrored[i]) / line_vector_mirrored[i]);
      }
      else
      {
        domain[i] = (line_v0_offset_mirrored[i] > aabbB->m_radii[i]);
        t_anchor[i] = ((aabbB->m_radii[i] - line_v0_offset_mirrored[i]) / line_vector_mirrored[i]);
      }
    }
    else
    {
      domain[i] = 0;
      t_anchor[i] = 2.0f;
    }
  }
 
  // square the line segment vector
  vec4f line_vector_mirrored_2;
  for(i=0;i<3; i++) 
  {
    line_vector_mirrored_2.SetElem(i, line_vector_mirrored[i] * line_vector_mirrored[i]);
  }
 
  // compute deriv, the solution to d|d|^2/dt for t=0. if it's >= 0 then p1 is the closest point
  deriv=0.0f; // set initial derivative value
  for(i=0;i<3;i++)
  {
    if(domain[i])
      deriv -= line_vector_mirrored_2[i] * t_anchor[i];
  }
  if(deriv >= 0.0f)
    solution = true;
 
  t = 0.0f; // set initial t value
  if(!solution)
  {
    do
    {
      // t_next is the next t value, next_t in ode
      // find the point on the line that is at the next clip plane boundary
      t_next = 1.0f;
      for(i=0;i<3;i++)
      {
        if((t_anchor[i] > t) && (t_anchor[i] < 1.0f) && (t_anchor[i] < t_next))
          t_next = t_anchor[i];
      }
 
      // compute the deriv = d|d|^2/dt for the next t
      deriv_next = 0.0f;
      for(i=0;i<3;i++)
      {
        if(domain[i])
          deriv_next += line_vector_mirrored_2[i] * (t_next - t_anchor[i]);
      }
 
      // if the sign of d|d|^2/dt has changed, solution = the crossover point
      if(deriv_next >= 0.0f)
      {
        derivs = (deriv_next - deriv) / (t_next - t);
        t -= deriv / derivs;
        solution = true;
        break;
      }
 
      // advance to the next anchor point / region
      for(i=0;i<3;i++)
      {
        if(t_anchor[i] == t_next)
        {
          t_anchor[i] = ((aabbB->m_radii[i] - line_v0_offset_mirrored[i]) / line_vector_mirrored[i]);
          domain[i]++;
        }
      }
 
      t = t_next;
      deriv = deriv_next;
 
    } while(t < 1.0f);
  }
 
  // p1 is the closest point
  if(!solution)
    t = 1.0f;
 
  // compute closest point on the line
  // note: line_vector=p2-p1
  for(i=0; i<3; i++)
    result->m_ipA.SetElem(i, (linesegA->m_vert0[i] + line_vector[i] * t));
 
  // compute closest point on the box
  for (i=0; i<3; i++) 
  {
    line_v0_offset_mirrored.SetElem(i, sign[i] * (line_v0_offset_mirrored[i] + t*line_vector_mirrored[i]));
    if (line_v0_offset_mirrored[i] < -aabbB->m_radii[i]) 
      line_v0_offset_mirrored.SetElem(i,-aabbB->m_radii[i]);
    else if (line_v0_offset_mirrored[i] > aabbB->m_radii[i]) 
      line_v0_offset_mirrored.SetElem(i, aabbB->m_radii[i]);
  }
  VecAdd3(result->m_ipB, line_v0_offset_mirrored, aabbB->m_center);
 
  // return the distance between the closest points
  return VecDist3(result->m_ipA, result->m_ipB);
}