--- /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;
+}
+