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

// get some math functions under windows
#ifdef WIN32
#include <float.h>
#ifndef CYGWIN			// added by andy for cygwin
#undef copysign
#define copysign(a,b) ((dReal)_copysign(a,b))
#endif				// added by andy for cygwin
#endif

#undef dNormalize3
#undef dNormalize4


// 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.

int dSafeNormalize3 (dVector3 a)
{
  dReal a0,a1,a2,aa0,aa1,aa2,l;
  dAASSERT (a);
  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 /= aa1;
      a2 /= aa1;
      l = dRecipSqrt (a0*a0 + a2*a2 + 1);
      a[0] = a0*l;
      a[1] = dCopySign(l,a1);
      a[2] = a2*l;
    }
  }
  else {
    if (aa2 > aa0) {
      aa2_largest:	// aa2 is largest
      a0 /= aa2;
      a1 /= aa2;
      l = dRecipSqrt (a0*a0 + a1*a1 + 1);
      a[0] = a0*l;
      a[1] = a1*l;
      a[2] = dCopySign(l,a2);
    }
    else {		// aa0 is largest
      if (aa0 <= 0) {
	a[0] = 1;	// 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 0;
      }
      a1 /= aa0;
      a2 /= aa0;
      l = dRecipSqrt (a1*a1 + a2*a2 + 1);
      a[0] = dCopySign(l,a0);
      a[1] = a1*l;
      a[2] = a2*l;
    }
  }
  return 1;
}

/* OLD VERSION */
/*
void dNormalize3 (dVector3 a)
{
  dIASSERT (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;
  }
}
*/

void dNormalize3(dVector3 a)
{
	_dNormalize3(a);
}


int dSafeNormalize4 (dVector4 a)
{
  dAASSERT (a);
  dReal l = dDOT(a,a)+a[3]*a[3];
  if (l > 0) {
    l = dRecipSqrt(l);
    a[0] *= l;
    a[1] *= l;
    a[2] *= l;
    a[3] *= l;
	return 1;
  }
  else {
    a[0] = 1;
    a[1] = 0;
    a[2] = 0;
    a[3] = 0;
    return 0;
  }
}

void dNormalize4(dVector4 a)
{
	_dNormalize4(a);
}


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