#include <float.h>
#include "geom.h"
#include "geom_tree.h"
#include "collider.h"

Geom::Geom(GeomTree* geomtree) {
  m_geomtree = geomtree;
  m_orient[0] = matrix4x4d::Identity();
  m_orient[1] = matrix4x4d::Identity();
  m_invOrient = matrix4x4d::Identity();
  m_orientIdx = 0;
  m_active = true;
  m_data = 0;
}

void Geom::MoveTo(const matrix4x4d& m) {
  m_orientIdx = !m_orientIdx;
  m_orient[m_orientIdx] = m;
  m_invOrient = m.InverseOf();
}

void Geom::MoveTo(const matrix4x4d& m, const vector3d pos) {
  m_orientIdx = !m_orientIdx;
  m_orient[m_orientIdx] = m;
  m_orient[m_orientIdx][12] = pos.x;
  m_orient[m_orientIdx][13] = pos.y;
  m_orient[m_orientIdx][14] = pos.z;
  m_invOrient = m_orient[m_orientIdx].InverseOf();
}

void Geom::Collide(Geom* b, void(*callback)(CollisionContact*)) {
  for(int i = 0; i < m_geomtree->m_numVertices; i++) {
    vector3d v(&m_geomtree->m_vertices[3*i]);
    vector3d from = m_orient[!m_orientIdx] * v;
    vector3d to = m_orient[m_orientIdx] * v;
    from = b->m_invOrient * from;
    to = b->m_invOrient * to;
    vector3d dir = to - from;
    const double len = dir.Length();
    dir *= 1.0f/len;

    vector3f _from(from.x, from.y, from.z);
    vector3f _dir(dir.x, dir.y, dir.z);

    isect_t isect;
    isect.dist = len;
    isect.triIdx = -1;
    _dir.Normalize();
    b->m_geomtree->TraceRay(_from, _dir, &isect);
    if(isect.triIdx != -1) {
      CollisionContact c;
      /* In world coords. */
      c.pos = b->GetTransform() * (from + dir*isect.dist);
      vector3f n = b->m_geomtree->GetTriNormal(isect.triIdx);
      c.normal = vector3d(n.x, n.y, n.z);
      c.normal = b->GetTransform().ApplyRotationOnly(c.normal);

      c.depth = len - isect.dist;
      c.triIdx = isect.triIdx;
      c.g1 = this;
      c.g2 = b;
      (*callback)(&c);
    }
  }
}