/*
 * Collision.cpp
 *
 * Copyright (C) 2009 Matthias Schiffer <matthias@gamezock.de>
 *
 * This program is free software: you can redistribute it and/or modify it
 * under the terms of the GNU Lesser General Public License as published by the
 * Free Software Foundation, either version 3 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 Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License along
 * with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#include "Collision.h"
#include <cmath>
#include <limits>

namespace Zoom {

bool Collision::test(const Triangle &t, const MathUtil::Ray &ray, float *distance) {
  vmml::vec3f edge1 = t.getVertex(1) - t.getVertex(0);
  vmml::vec3f edge2 = t.getVertex(2) - t.getVertex(0);

  vmml::vec3f pvec = ray.getDirection().cross(edge2);

  float det = vmml::dot(edge1, pvec);
  if(det < MathUtil::EPSILON)
    return false;

  vmml::vec3f tvec = ray.getVertex() - t.getVertex(0);
  float u = vmml::dot(pvec, tvec);

  if(u < 0 || u > det)
    return false;

  vmml::vec3f qvec = tvec.cross(edge1);
  float v = vmml::dot(ray.getDirection(), qvec);
  if(v < 0 || u+v > det)
    return false;

  if(distance)
    *distance = vmml::dot(edge2, qvec) / det;

  return true;
}

vmml::vec3f Collision::projectToEdge(const vmml::vec3f& p, const vmml::vec3f& v1, const vmml::vec3f& v2) {
  vmml::vec3f pVec = p - v1;
  vmml::vec3f edge = v2 - v1;

  float lengthSq = edge.squared_length();
  float edgeProj = vmml::dot(edge, pVec);

  if(edgeProj < 0) return v1;
  if(edgeProj > lengthSq) return v2;

  return v1 + (edgeProj/lengthSq)*edge;
}


bool Collision::testEdge(const vmml::vec3f &v1, const vmml::vec3f &v2, const vmml::vec3f &m, float r, const vmml::vec3f &move, float *distance) {
  vmml::vec3f edge = v2 - v1;

  vmml::vec3f avec = vmml::dot(edge, move)*edge / edge.squared_length() - move;
  vmml::vec3f cvec = v1 + vmml::dot(edge, m-v1)*edge / edge.squared_length() - m;

  float a = avec.squared_length();
  float b_2 = vmml::dot(avec, cvec);
  float c = cvec.squared_length() - r*r;

  float rootSq = b_2*b_2 - a*c;
  if(rootSq < 0)
    return false;

  float minRoot = -a - b_2;
  float maxRoot = a*MathUtil::EPSILON - b_2;

  if(minRoot < 0)
    minRoot = 0;
  if(maxRoot < 0)
    return false;

  if(rootSq < minRoot*minRoot || rootSq >= maxRoot*maxRoot)
    return false;

  minRoot = a*(edge.dot(m - v1) - edge.squared_length())/edge.dot(move) - b_2;
  maxRoot = a*edge.dot(m - v1)/edge.dot(move) - b_2;

  if(minRoot < 0)
    minRoot = 0;
  if(maxRoot < 0)
    return false;

  if(rootSq < minRoot*minRoot || rootSq > maxRoot*maxRoot)
    return false;

  *distance = -(b_2 + std::sqrt(rootSq))/a;

  return true;
}

bool Collision::testVertex(const vmml::vec3f &v, const vmml::vec3f &m, float r, const vmml::vec3f &move, float *distance) {
  float a = move.squared_length();
  float b_2 = vmml::dot(m-v, move);
  float c = (m-v).squared_length() - r*r;

  float rootSq = b_2*b_2 - a*c;
  if(rootSq < 0)
    return false;

  float minRoot = -a - b_2;
  float maxRoot = a*MathUtil::EPSILON - b_2;

  if(minRoot < 0)
    minRoot = 0;
  if(maxRoot < 0)
    return false;

  if(rootSq <= minRoot*minRoot || rootSq >= maxRoot*maxRoot)
    return false;

  *distance = -(b_2 + std::sqrt(rootSq))/a;

  return true;
}

bool Collision::test(const Triangle &t, const vmml::vec3f &m, float r, const vmml::vec3f &move, float *distance, vmml::vec3f *normal) {
  if(move.squared_length() == 0)
    return false;

  float d;

  vmml::vec3f triangleNormal = t.getNormal();
  if(test(t, MathUtil::Ray(m - r*triangleNormal, move), &d) && d > -MathUtil::EPSILON) {
    if(d < 1) {
      if(distance)
        *distance = d;
      if(normal)
        *normal = triangleNormal;

      return true;
    }
    else {
      return false;
    }
  }

  bool collision = false;
  float minDistance;

  for(int i = 0; i < 3; ++i) {
    if(testEdge(t.getVertex(i), t.getVertex((i+1)%3), m, r, move, &d)) {
      if(!collision || d < minDistance) {
        collision = true;
        minDistance = d;

        if(normal) {
          vmml::vec3f p = m + move*d;

          *normal = (p - projectToEdge(p, t.getVertex(i), t.getVertex((i+1)%3)))/r;
        }
      }
    }
  }

  if(collision) {
    if(distance)
      *distance = minDistance;

    return true;
  }

  collision = false;
  for(int i = 0; i < 3; ++i) {
    if(testVertex(t.getVertex(i), m, r, move, &d)) {
      if(!collision || d < minDistance) {
        collision = true;
        minDistance = d;
      }

      if(normal) {
        vmml::vec3f p = m + move*d;

        *normal = (p - t.getVertex(i))/r;
      }
    }
  }

  if(collision) {
    if(distance)
      *distance = minDistance;

    return true;
  }
  else {
    return false;
  }
}

}