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

//****************************************************************************
// plane public API

static void make_sure_plane_normal_has_unit_length (dxPlane *g)
{
  dReal l = dMUL(g->p[0],g->p[0]) + dMUL(g->p[1],g->p[1]) + dMUL(g->p[2],g->p[2]);
  if (l > 0) {
    l = dRecipSqrt(l);
    g->p[0] = dMUL(g->p[0],l);
    g->p[1] = dMUL(g->p[1],l);
    g->p[2] = dMUL(g->p[2],l);
    g->p[3] = dMUL(g->p[3],l);
  }
  else {
    g->p[0] = REAL(1.0);
    g->p[1] = 0;
    g->p[2] = 0;
    g->p[3] = 0;
  }
}


dxPlane::dxPlane (dSpaceID space, dReal a, dReal b, dReal c, dReal d) :
  dxGeom (space,0)
{
  type = dPlaneClass;
  p[0] = a;
  p[1] = b;
  p[2] = c;
  p[3] = d;
  make_sure_plane_normal_has_unit_length (this);
}


void dxPlane::computeAABB()
{
	aabb[0] = -dInfinity;
	aabb[1] = dInfinity;
	aabb[2] = -dInfinity;
	aabb[3] = dInfinity;
	aabb[4] = -dInfinity;
	aabb[5] = dInfinity;

	// Planes that have normal vectors aligned along an axis can use a
	// less comprehensive (half space) bounding box.

	if ( p[1] == REAL(0.0f) && p[2] == REAL(0.0f) ) {
		// normal aligned with x-axis
		aabb[0] = (p[0] > 0) ? -dInfinity : -p[3];
		aabb[1] = (p[0] > 0) ? p[3] : dInfinity;
	} else
	if ( p[0] == REAL(0.0f) && p[2] == REAL(0.0f) ) {
		// normal aligned with y-axis
		aabb[2] = (p[1] > 0) ? -dInfinity : -p[3];
		aabb[3] = (p[1] > 0) ? p[3] : dInfinity;
	} else
	if ( p[0] == REAL(0.0f) && p[1] == REAL(0.0f) ) {
		// normal aligned with z-axis
		aabb[4] = (p[2] > 0) ? -dInfinity : -p[3];
		aabb[5] = (p[2] > 0) ? p[3] : dInfinity;
	}
}


EXPORT_C dGeomID dCreatePlane (dSpaceID space,
		      dReal a, dReal b, dReal c, dReal d)
{
  return new dxPlane (space,a,b,c,d);
}


EXPORT_C void dGeomPlaneSetParams (dGeomID g, dReal a, dReal b, dReal c, dReal d)
{
  dxPlane *p = (dxPlane*) g;
  p->p[0] = a;
  p->p[1] = b;
  p->p[2] = c;
  p->p[3] = d;
  make_sure_plane_normal_has_unit_length (p);
  dGeomMoved (g);
}


EXPORT_C void dGeomPlaneGetParams (dGeomID g, dVector4 result)
{
  dxPlane *p = (dxPlane*) g;
  result[0] = p->p[0];
  result[1] = p->p[1];
  result[2] = p->p[2];
  result[3] = p->p[3];
}


EXPORT_C dReal dGeomPlanePointDepth (dGeomID g, dReal x, dReal y, dReal z)
{
  dxPlane *p = (dxPlane*) g;
  return p->p[3] - dMUL(p->p[0],x) - dMUL(p->p[1],y) - dMUL(p->p[2],z);
}