/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 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.                     *
 *                                                                       *
 *************************************************************************/

#include <ode/common.h>
#include <ode/odemath.h>


// this may be called for vectors `a' with extremely small magnitude, for
// example the result of a cross product on two nearly perpendicular vectors.
// we must be robust to these small vectors. to prevent numerical error,
// first find the component a[i] with the largest magnitude and then scale
// all the components by 1/a[i]. then we can compute the length of `a' and
// scale the components by 1/l. this has been verified to work with vectors
// containing the smallest representable numbers.

EXPORT_C void dNormalize3 (dVector3 a)
{
  dReal a0,a1,a2,aa0,aa1,aa2,l;
  a0 = a[0];
  a1 = a[1];
  a2 = a[2];
  aa0 = dFabs(a0);
  aa1 = dFabs(a1);
  aa2 = dFabs(a2);
  if (aa1 > aa0) {
    if (aa2 > aa1) {
      goto aa2_largest;
    }
    else {		// aa1 is largest
      a0 = dDIV(a0,aa1);
      a2 = dDIV(a2,aa1);
      l = dRecipSqrt (dMUL(a0,a0) + dMUL(a2,a2) + REAL(1.0));
      a[0] = dMUL(a0,l);
      a[1] = dCopySign(l,a1);
      a[2] = dMUL(a2,l);
    }
  }
  else {
    if (aa2 > aa0) {
      aa2_largest:	// aa2 is largest
      a0 = dDIV(a0,aa2);
      a1 = dDIV(a1,aa2);
      l = dRecipSqrt (dMUL(a0,a0) + dMUL(a1,a1) + REAL(1.0));
      a[0] = dMUL(a0,l);
      a[1] = dMUL(a1,l);
      a[2] = dCopySign(l,a2);
    }
    else {		// aa0 is largest
      if (aa0 <= 0) {
	a[0] = REAL(1.0);	// if all a's are zero, this is where we'll end up.
	a[1] = 0;	// return a default unit length vector.
	a[2] = 0;
	return;
      }
      a1 = dDIV(a1,aa0);
      a2 = dDIV(a2,aa0);
      l = dRecipSqrt (dMUL(a1,a1) + dMUL(a2,a2) + REAL(1.0));
      a[0] = dCopySign(l,a0);
      a[1] = dMUL(a1,l);
      a[2] = dMUL(a2,l);
    }
  }
}


/* OLD VERSION */
/*
void dNormalize3 (dVector3 a)
{

  dReal l = dDOT(a,a);
  if (l > 0) {
    l = dRecipSqrt(l);
    a[0] *= l;
    a[1] *= l;
    a[2] *= l;
  }
  else {
    a[0] = 1;
    a[1] = 0;
    a[2] = 0;
  }
}
*/


EXPORT_C void dNormalize4 (dVector4 a)
{

  dReal l = dDOT(a,a)+dMUL(a[3],a[3]);
  if (l > 0) {
    l = dRecipSqrt(l);
    a[0] = dMUL(a[0],l);
    a[1] = dMUL(a[1],l);
    a[2] = dMUL(a[2],l);
    a[3] = dMUL(a[3],l);
  }
  else {
    a[0] = REAL(1.0);
    a[1] = 0;
    a[2] = 0;
    a[3] = 0;
  }
}


EXPORT_C void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q)
{
  if (dFabs(n[2]) > dSQRT1_2) {
    // choose p in y-z plane
    dReal a = dMUL(n[1],n[1]) + dMUL(n[2],n[2]);
    dReal k = dRecipSqrt (a);
    p[0] = 0;
    p[1] = -dMUL(n[2],k);
    p[2] = dMUL(n[1],k);
    // set q = n x p
    q[0] = dMUL(a,k);
    q[1] = -dMUL(n[0],p[2]);
    q[2] = dMUL(n[0],p[1]);
  }
  else {
    // choose p in x-y plane
    dReal a = dMUL(n[0],n[0]) + dMUL(n[1],n[1]);
    dReal k = dRecipSqrt (a);
    p[0] = -dMUL(n[1],k);
    p[1] = dMUL(n[0],k);
    p[2] = 0;
    // set q = n x p
    q[0] = -dMUL(n[2],p[1]);
    q[1] = dMUL(n[2],p[0]);
    q[2] = dMUL(a,k);
  }
}

EXPORT_C dReal dArcTan2(const dReal y, const dReal x)
{
	dReal coeff_1 = dPI/4;
  	dReal coeff_2 = 3*coeff_1;
	dReal abs_y = dFabs(y) + dEpsilon;      // kludge to prevent 0/0 condition
	dReal angle;
   if (x>=0)
   {
      dReal r = dDIV((x - abs_y),(x + abs_y));
      //angle = coeff_1 - dMUL(coeff_1,r);
      angle = dMUL(REAL(0.1963),dMUL(r,dMUL(r,r))) - dMUL(REAL(0.9817),r) + coeff_1;
   }
   else
   {
      dReal r = dDIV((x + abs_y),(abs_y - x));
      //angle = coeff_2 - dMUL(coeff_1,r);
      angle = dMUL(REAL(0.1963),dMUL(r,dMUL(r,r))) - dMUL(REAL(0.9817),r) + coeff_2;
   }
   if (y < 0)
   return(-angle);     // negate if in quad III or IV
   else
   return(angle);
}

EXPORT_C dReal dArcSin(dReal arg)
{
	dReal temp;
	int sign;

	sign = 0;
	if(arg < 0)
	{
		arg = -arg;
		sign++;
	}
	if(arg > REAL(1.0)) {
		arg = REAL(1.0);
		//return dInfinity;
	}
	temp = dSqrt(REAL(1.0) - dMUL(arg,arg));
	if(arg > REAL(0.7))
		temp = dPI/2 - dArcTan2(temp,arg);
	else
		temp = dArcTan2(arg,temp);
	if(sign > 0)
		temp = -temp;
	return temp;
}