/*************************************************************************

 *                                                                       *

 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *

 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *

 *                                                                       *

 * This library is free software; you can redistribute it and/or         *

 * modify it under the terms of EITHER:                                  *

 *   (1) The GNU Lesser General Public License as published by the Free  *

 *       Software Foundation; either version 2.1 of the License, or (at  *

 *       your option) any later version. The text of the GNU Lesser      *

 *       General Public License is included with this library in the     *

 *       file LICENSE.TXT.                                               *

 *   (2) The BSD-style license that is included with this library in     *

 *       the file LICENSE-BSD.TXT.                                       *

 *                                                                       *

 * This library 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 files    *

 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *

 *                                                                       *

 *************************************************************************/

/*

standard ODE geometry primitives: public API and pairwise collision functions.

the rule is that only the low level primitive collision functions should set

dContactGeom::g1 and dContactGeom::g2.

*/

#include <ode/common.h>

#include <ode/collision.h>

#include <ode/matrix.h>

#include <ode/rotation.h>

#include <ode/odemath.h>

#include "collision_kernel.h"

#include "collision_std.h"

#include "collision_util.h"

//****************************************************************************

// box public API

dxBox::dxBox (dSpaceID space, dReal lx, dReal ly, dReal lz) : dxGeom (space,1)

{

  type = dBoxClass;

  side[0] = lx;

  side[1] = ly;

  side[2] = lz;

}

void dxBox::computeAABB()

{

  const dMatrix3& R = final_posr->R;

  const dVector3& pos = final_posr->pos;

  dReal xrange = dMUL(REAL(0.5),(dFabs (dMUL(R[0],side[0])) +

    dFabs (dMUL(R[1],side[1])) + dFabs (dMUL(R[2],side[2]))));

  dReal yrange = dMUL(REAL(0.5),(dFabs (dMUL(R[4],side[0])) +

    dFabs (dMUL(R[5],side[1])) + dFabs (dMUL(R[6],side[2]))));

  dReal zrange = dMUL(REAL(0.5),(dFabs (dMUL(R[8],side[0])) +

    dFabs (dMUL(R[9],side[1])) + dFabs (dMUL(R[10],side[2]))));

  aabb[0] = pos[0] - xrange;

  aabb[1] = pos[0] + xrange;

  aabb[2] = pos[1] - yrange;

  aabb[3] = pos[1] + yrange;

  aabb[4] = pos[2] - zrange;

  aabb[5] = pos[2] + zrange;

}

EXPORT_C dGeomID dCreateBox (dSpaceID space, dReal lx, dReal ly, dReal lz)

{

  return new dxBox (space,lx,ly,lz);

}

EXPORT_C void dGeomBoxSetLengths (dGeomID g, dReal lx, dReal ly, dReal lz)

{

  dxBox *b = (dxBox*) g;

  b->side[0] = lx;

  b->side[1] = ly;

  b->side[2] = lz;

  dGeomMoved (g);

}

EXPORT_C void dGeomBoxGetLengths (dGeomID g, dVector3 result)

{

  dxBox *b = (dxBox*) g;

  result[0] = b->side[0];

  result[1] = b->side[1];

  result[2] = b->side[2];

}

EXPORT_C dReal dGeomBoxPointDepth (dGeomID g, dReal x, dReal y, dReal z)

{

  g->recomputePosr();

  dxBox *b = (dxBox*) g;

  // Set p = (x,y,z) relative to box center

  //

  // This will be (0,0,0) if the point is at (side[0]/2,side[1]/2,side[2]/2)

  dVector3 p,q;

  p[0] = x - b->final_posr->pos[0];

  p[1] = y - b->final_posr->pos[1];

  p[2] = z - b->final_posr->pos[2];

  // Rotate p into box's coordinate frame, so we can

  // treat the OBB as an AABB

  dMULTIPLY1_331 (q,b->final_posr->R,p);

  // Record distance from point to each successive box side, and see

  // if the point is inside all six sides

  dReal dist[6];

  int   i;

  bool inside = true;

  for (i=0; i < 3; i++) {

    dReal side = dMUL(b->side[i],REAL(0.5));

    dist[i  ] = side - q[i];

    dist[i+3] = side + q[i];

    if ((dist[i] < 0) || (dist[i+3] < 0)) {

      inside = false;

    }

  }

  // If point is inside the box, the depth is the smallest positive distance

  // to any side

  if (inside) {

    dReal smallest_dist = (dReal) (unsigned) -1;

    for (i=0; i < 6; i++) {

      if (dist[i] < smallest_dist) smallest_dist = dist[i];

    }

    return smallest_dist;

  }

  // Otherwise, if point is outside the box, the depth is the largest

  // distance to any side.  This is an approximation to the 'proper'

  // solution (the proper solution may be larger in some cases).

  dReal largest_dist = 0;

  for (i=0; i < 6; i++) {

    if (dist[i] > largest_dist) largest_dist = dist[i];

  }

  return -largest_dist;

}

//****************************************************************************

// box-box collision utility

// find all the intersection points between the 2D rectangle with vertices

// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),

// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).

//

// the intersection points are returned as x,y pairs in the 'ret' array.

// the number of intersection points is returned by the function (this will

// be in the range 0 to 8).

static int intersectRectQuad (dReal h[2], dReal p[8], dReal ret[16])

{

  // q (and r) contain nq (and nr) coordinate points for the current (and

  // chopped) polygons

  int nq=4,nr;

  dReal buffer[16];

  dReal *q = p;

  dReal *r = ret;

  for (int dir=0; dir <= 1; dir++) {

    // direction notation: xy[0] = x axis, xy[1] = y axis

    for (int sign=-1; sign <= 1; sign += 2) {

      // chop q along the line xy[dir] = sign*h[dir]

      dReal *pq = q;

      dReal *pr = r;

      nr = 0;

      for (int i=nq; i > 0; i--) {

	// go through all points in q and all lines between adjacent points

	if (sign*pq[dir] < h[dir]) {

	  // this point is inside the chopping line

	  pr[0] = pq[0];

	  pr[1] = pq[1];

	  pr += 2;

	  nr++;

	  if (nr & 8) {

	    q = r;

	    goto done;

	  }

	}

	dReal *nextq = (i > 1) ? pq+2 : q;

	if ((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir])) {

	  // this line crosses the chopping line

	  pr[1-dir] = pq[1-dir] + dMUL(dDIV((nextq[1-dir]-pq[1-dir]),(nextq[dir]-pq[dir])),(sign*h[dir]-pq[dir]));

	  pr[dir] = sign*h[dir];

	  pr += 2;

	  nr++;

	  if (nr & 8) {

	    q = r;

	    goto done;

	  }

	}

	pq += 2;

      }

      q = r;

      r = (q==ret) ? buffer : ret;

      nq = nr;

    }

  }

 done:

  if (q != ret) memcpy (ret,q,nr*2*sizeof(dReal));

  return nr;

}

// given n points in the plane (array p, of size 2*n), generate m points that

// best represent the whole set. the definition of 'best' here is not

// predetermined - the idea is to select points that give good box-box

// collision detection behavior. the chosen point indexes are returned in the

// array iret (of size m). 'i0' is always the first entry in the array.

// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be

// in the range [0..n-1].

void cullPoints (int n, dReal p[], int m, int i0, int iret[])

{

  // compute the centroid of the polygon in cx,cy

  int i,j;

  dReal a,cx,cy,q;

  if (n==1) {

    cx = p[0];

    cy = p[1];

  }

  else if (n==2) {

    cx = dMUL(REAL(0.5),(p[0] + p[2]));

    cy = dMUL(REAL(0.5),(p[1] + p[3]));

  }

  else {

    a = 0;

    cx = 0;

    cy = 0;

    for (i=0; i<(n-1); i++) {

      q = dMUL(p[i*2],p[i*2+3]) - dMUL(p[i*2+2],p[i*2+1]);

      a += q;

      cx += dMUL(q,(p[i*2]+p[i*2+2]));

      cy += dMUL(q,(p[i*2+1]+p[i*2+3]));

    }

    q = dMUL(p[n*2-2],p[1]) - dMUL(p[0],p[n*2-1]);

    a = dRecip(dMUL(REAL(3.0),(a+q)));

    cx = dMUL(a,(cx + dMUL(q,(p[n*2-2]+p[0]))));

    cy = dMUL(a,(cy + dMUL(q,(p[n*2-1]+p[1]))));

  }

  // compute the angle of each point w.r.t. the centroid

  dReal A[8];

  for (i=0; i<n; i++) A[i] = dArcTan2(p[i*2+1]-cy,p[i*2]-cx);

  // search for points that have angles closest to A[i0] + i*(2*pi/m).

  int avail[8];

  for (i=0; i<n; i++) avail[i] = 1;

  avail[i0] = 0;

  iret[0] = i0;

  iret++;

  for (j=1; j<m; j++) {

    a = dMUL(REAL(j),(2*dPI/m)) + A[i0];

    if (a > dPI) a -= 2*dPI;

    dReal maxdiff = REAL(1e9);

    dReal diff;

#ifndef dNODEBUG

    *iret = i0;			// iret is not allowed to keep this value

#endif

    for (i=0; i<n; i++) {

      if (avail[i]) {

	diff = dFabs (A[i]-a);

	if (diff > dPI) diff = 2*dPI - diff;

	if (diff < maxdiff) {

	  maxdiff = diff;

	  *iret = i;

	}

      }

    }

    avail[*iret] = 0;

    iret++;

  }

}

// given two boxes (p1,R1,side1) and (p2,R2,side2), collide them together and

// generate contact points. this returns 0 if there is no contact otherwise

// it returns the number of contacts generated.

// `normal' returns the contact normal.

// `depth' returns the maximum penetration depth along that normal.

// `return_code' returns a number indicating the type of contact that was

// detected:

//        1,2,3 = box 2 intersects with a face of box 1

//        4,5,6 = box 1 intersects with a face of box 2

//        7..15 = edge-edge contact

// `maxc' is the maximum number of contacts allowed to be generated, i.e.

// the size of the `contact' array.

// `contact' and `skip' are the contact array information provided to the

// collision functions. this function only fills in the position and depth

// fields.

EXPORT_C int dBoxBox (const dVector3 p1, const dMatrix3 R1,

	     const dVector3 side1, const dVector3 p2,

	     const dMatrix3 R2, const dVector3 side2,

	     dVector3 normal, dReal *depth, int *return_code,

	     int maxc, dContactGeom *contact, int skip)

{

  const dReal fudge_factor = REAL(1.05);

  dVector3 p,pp,normalC;

  const dReal *normalR = 0;

  dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,

    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;

  int i,j,invert_normal,code;

  // get vector from centers of box 1 to box 2, relative to box 1

  p[0] = p2[0] - p1[0];

  p[1] = p2[1] - p1[1];

  p[2] = p2[2] - p1[2];

  dMULTIPLY1_331 (pp,R1,p);		// get pp = p relative to body 1

  // get side lengths / 2

  A[0] = dMUL(side1[0],REAL(0.5));

  A[1] = dMUL(side1[1],REAL(0.5));

  A[2] = dMUL(side1[2],REAL(0.5));

  B[0] = dMUL(side2[0],REAL(0.5));

  B[1] = dMUL(side2[1],REAL(0.5));

  B[2] = dMUL(side2[2],REAL(0.5));

  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2

  R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);

  R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);

  R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);

  Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);

  Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);

  Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);

  // for all 15 possible separating axes:

  //   * see if the axis separates the boxes. if so, return 0.

  //   * find the depth of the penetration along the separating axis (s2)

  //   * if this is the largest depth so far, record it.

  // the normal vector will be set to the separating axis with the smallest

  // depth. note: normalR is set to point to a column of R1 or R2 if that is

  // the smallest depth normal so far. otherwise normalR is 0 and normalC is

  // set to a vector relative to body 1. invert_normal is 1 if the sign of

  // the normal should be flipped.

#define TST(expr1,expr2,norm,cc) \

  s2 = dFabs(expr1) - (expr2); \

  if (s2 > 0) return 0; \

  if (s2 > s) { \

    s = s2; \

    normalR = norm; \

    invert_normal = ((expr1) < 0); \

    code = (cc); \

  }

  s = -dInfinity;

  invert_normal = 0;

  code = 0;

  // separating axis = u1,u2,u3

  TST (pp[0],(A[0] + dMUL(B[0],Q11) + dMUL(B[1],Q12) + dMUL(B[2],Q13)),R1+0,1);

  TST (pp[1],(A[1] + dMUL(B[0],Q21) + dMUL(B[1],Q22) + dMUL(B[2],Q23)),R1+1,2);

  TST (pp[2],(A[2] + dMUL(B[0],Q31) + dMUL(B[1],Q32) + dMUL(B[2],Q33)),R1+2,3);

  // separating axis = v1,v2,v3

  TST (dDOT41(R2+0,p),(dMUL(A[0],Q11) + dMUL(A[1],Q21) + dMUL(A[2],Q31) + B[0]),R2+0,4);

  TST (dDOT41(R2+1,p),(dMUL(A[0],Q12) + dMUL(A[1],Q22) + dMUL(A[2],Q32) + B[1]),R2+1,5);

  TST (dDOT41(R2+2,p),(dMUL(A[0],Q13) + dMUL(A[1],Q23) + dMUL(A[2],Q33) + B[2]),R2+2,6);

  // note: cross product axes need to be scaled when s is computed.

  // normal (n1,n2,n3) is relative to box 1.

#undef TST

#define TST(expr1,expr2,n1,n2,n3,cc) \

  s2 = dFabs(expr1) - (expr2); \

  if (s2 > 0) return 0; \

  l = dSqrt (dMUL((n1),(n1)) + dMUL((n2),(n2)) + dMUL((n3),(n3))); \

  if (l > 0) { \

    s2 = dDIV(s2,l); \

    if (dMUL(s2,fudge_factor) > s) { \

      s = s2; \

      normalR = 0; \

      normalC[0] = dDIV((n1),l); normalC[1] = dDIV((n2),l); normalC[2] = dDIV((n3),l); \

      invert_normal = ((expr1) < 0); \

      code = (cc); \

    } \

  }

  // separating axis = u1 x (v1,v2,v3)

  TST((dMUL(pp[2],R21)-dMUL(pp[1],R31)),(dMUL(A[1],Q31)+dMUL(A[2],Q21)+dMUL(B[1],Q13)+dMUL(B[2],Q12)),0,-R31,R21,7);

  TST((dMUL(pp[2],R22)-dMUL(pp[1],R32)),(dMUL(A[1],Q32)+dMUL(A[2],Q22)+dMUL(B[0],Q13)+dMUL(B[2],Q11)),0,-R32,R22,8);

  TST((dMUL(pp[2],R23)-dMUL(pp[1],R33)),(dMUL(A[1],Q33)+dMUL(A[2],Q23)+dMUL(B[0],Q12)+dMUL(B[1],Q11)),0,-R33,R23,9);

  // separating axis = u2 x (v1,v2,v3)

  TST((dMUL(pp[0],R31)-dMUL(pp[2],R11)),(dMUL(A[0],Q31)+dMUL(A[2],Q11)+dMUL(B[1],Q23)+dMUL(B[2],Q22)),R31,0,-R11,10);

  TST((dMUL(pp[0],R32)-dMUL(pp[2],R12)),(dMUL(A[0],Q32)+dMUL(A[2],Q12)+dMUL(B[0],Q23)+dMUL(B[2],Q21)),R32,0,-R12,11);

  TST((dMUL(pp[0],R33)-dMUL(pp[2],R13)),(dMUL(A[0],Q33)+dMUL(A[2],Q13)+dMUL(B[0],Q22)+dMUL(B[1],Q21)),R33,0,-R13,12);

  // separating axis = u3 x (v1,v2,v3)

  TST((dMUL(pp[1],R11)-dMUL(pp[0],R21)),(dMUL(A[0],Q21)+dMUL(A[1],Q11)+dMUL(B[1],Q33)+dMUL(B[2],Q32)),-R21,R11,0,13);

  TST((dMUL(pp[1],R12)-dMUL(pp[0],R22)),(dMUL(A[0],Q22)+dMUL(A[1],Q12)+dMUL(B[0],Q33)+dMUL(B[2],Q31)),-R22,R12,0,14);

  TST((dMUL(pp[1],R13)-dMUL(pp[0],R23)),(dMUL(A[0],Q23)+dMUL(A[1],Q13)+dMUL(B[0],Q32)+dMUL(B[1],Q31)),-R23,R13,0,15);

#undef TST

  if (!code) return 0;

  // if we get to this point, the boxes interpenetrate. compute the normal

  // in global coordinates.

  if (normalR) {

    normal[0] = normalR[0];

    normal[1] = normalR[4];

    normal[2] = normalR[8];

  }

  else {

    dMULTIPLY0_331 (normal,R1,normalC);

  }

  if (invert_normal) {

    normal[0] = -normal[0];

    normal[1] = -normal[1];

    normal[2] = -normal[2];

  }

  *depth = -s;

  // compute contact point(s)

  if (code > 6) {

    // an edge from box 1 touches an edge from box 2.

    // find a point pa on the intersecting edge of box 1

    dVector3 pa;

    dReal sign;

    for (i=0; i<3; i++) pa[i] = p1[i];

    for (j=0; j<3; j++) {

      sign = (dDOT14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0);

      for (i=0; i<3; i++) pa[i] += dMUL(sign,dMUL(A[j],R1[i*4+j]));

    }

    // find a point pb on the intersecting edge of box 2

    dVector3 pb;

    for (i=0; i<3; i++) pb[i] = p2[i];

    for (j=0; j<3; j++) {

      sign = (dDOT14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0);

      for (i=0; i<3; i++) pb[i] += dMUL(sign,dMUL(B[j],R2[i*4+j]));

    }

    dReal alpha,beta;

    dVector3 ua,ub;

    for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];

    for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];

    dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);

    for (i=0; i<3; i++) pa[i] += dMUL(ua[i],alpha);

    for (i=0; i<3; i++) pb[i] += dMUL(ub[i],beta);

    for (i=0; i<3; i++) contact[0].pos[i] = dMUL(REAL(0.5),(pa[i]+pb[i]));

    contact[0].depth = *depth;

    *return_code = code;

    return 1;

  }

  // okay, we have a face-something intersection (because the separating

  // axis is perpendicular to a face). define face 'a' to be the reference

  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be

  // the incident face (the closest face of the other box).

  const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;

  if (code <= 3) {

    Ra = R1;

    Rb = R2;

    pa = p1;

    pb = p2;

    Sa = A;

    Sb = B;

  }

  else {

    Ra = R2;

    Rb = R1;

    pa = p2;

    pb = p1;

    Sa = B;

    Sb = A;

  }

  // nr = normal vector of reference face dotted with axes of incident box.

  // anr = absolute values of nr.

  dVector3 normal2,nr,anr;

  if (code <= 3) {

    normal2[0] = normal[0];

    normal2[1] = normal[1];

    normal2[2] = normal[2];

  }

  else {

    normal2[0] = -normal[0];

    normal2[1] = -normal[1];

    normal2[2] = -normal[2];

  }

  dMULTIPLY1_331 (nr,Rb,normal2);

  anr[0] = dFabs (nr[0]);

  anr[1] = dFabs (nr[1]);

  anr[2] = dFabs (nr[2]);

  // find the largest compontent of anr: this corresponds to the normal

  // for the indident face. the other axis numbers of the indicent face

  // are stored in a1,a2.

  int lanr,a1,a2;

  if (anr[1] > anr[0]) {

    if (anr[1] > anr[2]) {

      a1 = 0;

      lanr = 1;

      a2 = 2;

    }

    else {

      a1 = 0;