diff -r 000000000000 -r 2f259fa3e83a ode/src/odemath.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ode/src/odemath.cpp	Tue Feb 02 01:00:49 2010 +0200
@@ -0,0 +1,202 @@
+/*************************************************************************
+ *                                                                       *
+ * 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;
+}
+