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

 *                                                                       *

 * 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"

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

// capped cylinder public API

dxCapsule::dxCapsule (dSpaceID space, dReal _radius, dReal _length) :

  dxGeom (space,1)

{

  type = dCapsuleClass;

  radius = _radius;

  lz = _length;

}

void dxCapsule::computeAABB()

{

  const dMatrix3& R = final_posr->R;

  const dVector3& pos = final_posr->pos;

  dReal xrange = dMUL(dFabs(dMUL(R[2],lz)),REAL(0.5)) + radius;

  dReal yrange = dMUL(dFabs(dMUL(R[6],lz)),REAL(0.5)) + radius;

  dReal zrange = dMUL(dFabs(dMUL(R[10],lz)),REAL(0.5)) + radius;

  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 dCreateCapsule (dSpaceID space, dReal radius, dReal length)

{

  return new dxCapsule (space,radius,length);

}

EXPORT_C void dGeomCapsuleSetParams (dGeomID g, dReal radius, dReal length)

{

  dxCapsule *c = (dxCapsule*) g;

  c->radius = radius;

  c->lz = length;

  dGeomMoved (g);

}

EXPORT_C void dGeomCapsuleGetParams (dGeomID g, dReal *radius, dReal *length)

{

  dxCapsule *c = (dxCapsule*) g;

  *radius = c->radius;

  *length = c->lz;

}

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

{

  g->recomputePosr();

  dxCapsule *c = (dxCapsule*) g;

  const dReal* R = g->final_posr->R;

  const dReal* pos = g->final_posr->pos;

  dVector3 a;

  a[0] = x - pos[0];

  a[1] = y - pos[1];

  a[2] = z - pos[2];

  dReal beta = dDOT14(a,R+2);

  dReal lz2 = dMUL(c->lz,REAL(0.5));

  if (beta < -lz2) beta = -lz2;

  else if (beta > lz2) beta = lz2;

  a[0] = c->final_posr->pos[0] + dMUL(beta,R[0*4+2]);

  a[1] = c->final_posr->pos[1] + dMUL(beta,R[1*4+2]);

  a[2] = c->final_posr->pos[2] + dMUL(beta,R[2*4+2]);

  return c->radius -

    dSqrt (dMUL((x-a[0]),(x-a[0])) + dMUL((y-a[1]),(y-a[1])) + dMUL((z-a[2]),(z-a[2])));

}

int dCollideCapsuleSphere (dxGeom *o1, dxGeom *o2, int /*flags*/,

			     dContactGeom *contact, int /*skip*/)

{

  dxCapsule *ccyl = (dxCapsule*) o1;

  dxSphere *sphere = (dxSphere*) o2;

  contact->g1 = o1;

  contact->g2 = o2;

  // find the point on the cylinder axis that is closest to the sphere

  dReal alpha = 

    dMUL(o1->final_posr->R[2],(o2->final_posr->pos[0] - o1->final_posr->pos[0])) +

    dMUL(o1->final_posr->R[6],(o2->final_posr->pos[1] - o1->final_posr->pos[1])) +

    dMUL(o1->final_posr->R[10],(o2->final_posr->pos[2] - o1->final_posr->pos[2]));

  dReal lz2 = dMUL(ccyl->lz,REAL(0.5));

  if (alpha > lz2) alpha = lz2;

  if (alpha < -lz2) alpha = -lz2;

  // collide the spheres

  dVector3 p;

  p[0] = o1->final_posr->pos[0] + dMUL(alpha,o1->final_posr->R[2]);

  p[1] = o1->final_posr->pos[1] + dMUL(alpha,o1->final_posr->R[6]);

  p[2] = o1->final_posr->pos[2] + dMUL(alpha,o1->final_posr->R[10]);

  return dCollideSpheres (p,ccyl->radius,o2->final_posr->pos,sphere->radius,contact);

}

int dCollideCapsuleBox (dxGeom *o1, dxGeom *o2, int /*flags*/,

			  dContactGeom *contact, int /*skip*/)

{

  dxCapsule *cyl = (dxCapsule*) o1;

  dxBox *box = (dxBox*) o2;

  contact->g1 = o1;

  contact->g2 = o2;

  // get p1,p2 = cylinder axis endpoints, get radius

  dVector3 p1,p2;

  dReal clen = dMUL(cyl->lz,REAL(0.5));

  p1[0] = o1->final_posr->pos[0] + dMUL(clen,o1->final_posr->R[2]);

  p1[1] = o1->final_posr->pos[1] + dMUL(clen,o1->final_posr->R[6]);

  p1[2] = o1->final_posr->pos[2] + dMUL(clen,o1->final_posr->R[10]);

  p2[0] = o1->final_posr->pos[0] - dMUL(clen,o1->final_posr->R[2]);

  p2[1] = o1->final_posr->pos[1] - dMUL(clen,o1->final_posr->R[6]);

  p2[2] = o1->final_posr->pos[2] - dMUL(clen,o1->final_posr->R[10]);

  dReal radius = cyl->radius;

  // copy out box center, rotation matrix, and side array

  dReal *c = o2->final_posr->pos;

  dReal *R = o2->final_posr->R;

  const dReal *side = box->side;

  // get the closest point between the cylinder axis and the box

  dVector3 pl,pb;

  dClosestLineBoxPoints (p1,p2,c,R,side,pl,pb);

  // generate contact point

  return dCollideSpheres (pl,radius,pb,0,contact);

}

int dCollideCapsuleCapsule (dxGeom *o1, dxGeom *o2,

				int flags, dContactGeom *contact, int skip)

{

  int i;

  const dReal tolerance = REAL(2e-5);

  dxCapsule *cyl1 = (dxCapsule*) o1;

  dxCapsule *cyl2 = (dxCapsule*) o2;

  contact->g1 = o1;

  contact->g2 = o2;

  // copy out some variables, for convenience

  dReal lz1 = dMUL(cyl1->lz,REAL(0.5));

  dReal lz2 = dMUL(cyl2->lz,REAL(0.5));

  dReal *pos1 = o1->final_posr->pos;

  dReal *pos2 = o2->final_posr->pos;

  dReal axis1[3],axis2[3];

  axis1[0] = o1->final_posr->R[2];

  axis1[1] = o1->final_posr->R[6];

  axis1[2] = o1->final_posr->R[10];

  axis2[0] = o2->final_posr->R[2];

  axis2[1] = o2->final_posr->R[6];

  axis2[2] = o2->final_posr->R[10];

  // if the cylinder axes are close to parallel, we'll try to detect up to

  // two contact points along the body of the cylinder. if we can't find any

  // points then we'll fall back to the closest-points algorithm. note that

  // we are not treating this special case for reasons of degeneracy, but

  // because we want two contact points in some situations. the closet-points

  // algorithm is robust in all casts, but it can return only one contact.

  dVector3 sphere1,sphere2;

  dReal a1a2 = dDOT (axis1,axis2);

  dReal det = REAL(1.0)-dMUL(a1a2,a1a2);

  if (det < tolerance) {

    // the cylinder axes (almost) parallel, so we will generate up to two

    // contacts. alpha1 and alpha2 (line position parameters) are related by:

    //       alpha2 =   alpha1 + (pos1-pos2)'*axis1   (if axis1==axis2)

    //    or alpha2 = -(alpha1 + (pos1-pos2)'*axis1)  (if axis1==-axis2)

    // first compute where the two cylinders overlap in alpha1 space:

    if (a1a2 < 0) {

      axis2[0] = -axis2[0];

      axis2[1] = -axis2[1];

      axis2[2] = -axis2[2];

    }

    dReal q[3];

    for (i=0; i<3; i++) q[i] = pos1[i]-pos2[i];

    dReal k = dDOT (axis1,q);

    dReal a1lo = -lz1;

    dReal a1hi = lz1;

    dReal a2lo = -lz2 - k;

    dReal a2hi = lz2 - k;

    dReal lo = (a1lo > a2lo) ? a1lo : a2lo;

    dReal hi = (a1hi < a2hi) ? a1hi : a2hi;

    if (lo <= hi) {

      int num_contacts = flags & NUMC_MASK;

      if (num_contacts >= 2 && lo < hi) {

	// generate up to two contacts. if one of those contacts is

	// not made, fall back on the one-contact strategy.

	for (i=0; i<3; i++) sphere1[i] = pos1[i] + dMUL(lo,axis1[i]);

	for (i=0; i<3; i++) sphere2[i] = pos2[i] + dMUL((lo+k),axis2[i]);

	int n1 = dCollideSpheres (sphere1,cyl1->radius,

				  sphere2,cyl2->radius,contact);

	if (n1) {

	  for (i=0; i<3; i++) sphere1[i] = pos1[i] + dMUL(hi,axis1[i]);

	  for (i=0; i<3; i++) sphere2[i] = pos2[i] + dMUL((hi+k),axis2[i]);

	  dContactGeom *c2 = CONTACT(contact,skip);

	  int n2 = dCollideSpheres (sphere1,cyl1->radius,

				    sphere2,cyl2->radius, c2);

	  if (n2) {

	    c2->g1 = o1;

	    c2->g2 = o2;

	    return 2;

	  }

	}

      }

      // just one contact to generate, so put it in the middle of

      // the range

      dReal alpha1 = dMUL((lo + hi),REAL(0.5));

      dReal alpha2 = alpha1 + k;

      for (i=0; i<3; i++) sphere1[i] = pos1[i] + dMUL(alpha1,axis1[i]);

      for (i=0; i<3; i++) sphere2[i] = pos2[i] + dMUL(alpha2,axis2[i]);

      return dCollideSpheres (sphere1,cyl1->radius,

			      sphere2,cyl2->radius,contact);

    }

  }

  // use the closest point algorithm

  dVector3 a1,a2,b1,b2;

  a1[0] = o1->final_posr->pos[0] + dMUL(axis1[0],lz1);

  a1[1] = o1->final_posr->pos[1] + dMUL(axis1[1],lz1);

  a1[2] = o1->final_posr->pos[2] + dMUL(axis1[2],lz1);

  a2[0] = o1->final_posr->pos[0] - dMUL(axis1[0],lz1);

  a2[1] = o1->final_posr->pos[1] - dMUL(axis1[1],lz1);

  a2[2] = o1->final_posr->pos[2] - dMUL(axis1[2],lz1);

  b1[0] = o2->final_posr->pos[0] + dMUL(axis2[0],lz2);

  b1[1] = o2->final_posr->pos[1] + dMUL(axis2[1],lz2);

  b1[2] = o2->final_posr->pos[2] + dMUL(axis2[2],lz2);

  b2[0] = o2->final_posr->pos[0] - dMUL(axis2[0],lz2);

  b2[1] = o2->final_posr->pos[1] - dMUL(axis2[1],lz2);

  b2[2] = o2->final_posr->pos[2] - dMUL(axis2[2],lz2);

  dClosestLineSegmentPoints (a1,a2,b1,b2,sphere1,sphere2);

  return dCollideSpheres (sphere1,cyl1->radius,sphere2,cyl2->radius,contact);

}

int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags,

			    dContactGeom *contact, int skip)

{

  dxCapsule *ccyl = (dxCapsule*) o1;

  dxPlane *plane = (dxPlane*) o2;

  // collide the deepest capping sphere with the plane

  dReal sign = (dDOT14 (plane->p,o1->final_posr->R+2) > 0) ? REAL(-1.0) : REAL(1.0);

  dVector3 p;

  p[0] = o1->final_posr->pos[0] + dMUL(o1->final_posr->R[2],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));

  p[1] = o1->final_posr->pos[1] + dMUL(o1->final_posr->R[6],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));

  p[2] = o1->final_posr->pos[2] + dMUL(o1->final_posr->R[10],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));

  dReal k = dDOT (p,plane->p);

  dReal depth = plane->p[3] - k + ccyl->radius;

  if (depth < 0) return 0;

  contact->normal[0] = plane->p[0];

  contact->normal[1] = plane->p[1];

  contact->normal[2] = plane->p[2];

  contact->pos[0] = p[0] - dMUL(plane->p[0],ccyl->radius);

  contact->pos[1] = p[1] - dMUL(plane->p[1],ccyl->radius);

  contact->pos[2] = p[2] - dMUL(plane->p[2],ccyl->radius);

  contact->depth = depth;

  int ncontacts = 1;

  if ((flags & NUMC_MASK) >= 2) {

    // collide the other capping sphere with the plane

    p[0] = o1->final_posr->pos[0] - dMUL(o1->final_posr->R[2],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));

    p[1] = o1->final_posr->pos[1] - dMUL(o1->final_posr->R[6],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));

    p[2] = o1->final_posr->pos[2] - dMUL(o1->final_posr->R[10],dMUL(ccyl->lz,dMUL(REAL(0.5),sign)));

    k = dDOT (p,plane->p);

    depth = plane->p[3] - k + ccyl->radius;

    if (depth >= 0) {

      dContactGeom *c2 = CONTACT(contact,skip);

      c2->normal[0] = plane->p[0];

      c2->normal[1] = plane->p[1];

      c2->normal[2] = plane->p[2];

      c2->pos[0] = p[0] - dMUL(plane->p[0],ccyl->radius);

      c2->pos[1] = p[1] - dMUL(plane->p[1],ccyl->radius);

      c2->pos[2] = p[2] - dMUL(plane->p[2],ccyl->radius);

      c2->depth = depth;

      ncontacts = 2;

    }

  }

  for (int i=0; i < ncontacts; i++) {

    CONTACT(contact,i*skip)->g1 = o1;

    CONTACT(contact,i*skip)->g2 = o2;

  }

  return ncontacts;

}