one more for the gipper

1 files changed, 243 insertions, 0 deletions

diff --git a/libraries/ode-0.9/ode/src/testing.cpp b/libraries/ode-0.9/ode/src/testing.cpp
new file mode 100644
index 0000000..a22f865
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/testing.cpp
@@ -0,0 +1,243 @@
+/*************************************************************************
+ *                                                                       *
+ * 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/config.h>
+#include <ode/misc.h>
+#include <ode/memory.h>
+#include "testing.h"
+
+#ifdef dDOUBLE
+static const dReal tol = 1.0e-9;
+#else
+static const dReal tol = 1.0e-5f;
+#endif
+
+
+// matrix header on the stack
+
+struct dMatrixComparison::dMatInfo {
+  int n,m;              // size of matrix
+  char name[128];       // name of the matrix
+  dReal *data;          // matrix data
+  int size;             // size of `data'
+};
+
+
+
+dMatrixComparison::dMatrixComparison()
+{
+  afterfirst = 0;
+  index = 0;
+}
+
+
+dMatrixComparison::~dMatrixComparison()
+{
+  reset();
+}
+
+
+dReal dMatrixComparison::nextMatrix (dReal *A, int n, int m, int lower_tri,
+                                     char *name, ...)
+{
+  if (A==0 || n < 1 || m < 1 || name==0) dDebug (0,"bad args to nextMatrix");
+  int num = n*dPAD(m);
+
+  if (afterfirst==0) {
+    dMatInfo *mi = (dMatInfo*) dAlloc (sizeof(dMatInfo));
+    mi->n = n;
+    mi->m = m;
+    mi->size = num * sizeof(dReal);
+    mi->data = (dReal*) dAlloc (mi->size);
+    memcpy (mi->data,A,mi->size);
+
+    va_list ap;
+    va_start (ap,name);
+    vsprintf (mi->name,name,ap);
+    if (strlen(mi->name) >= sizeof (mi->name)) dDebug (0,"name too long");
+
+    mat.push (mi);
+    return 0;
+  }
+  else {
+    if (lower_tri && n != m)
+      dDebug (0,"dMatrixComparison, lower triangular matrix must be square");
+    if (index >= mat.size()) dDebug (0,"dMatrixComparison, too many matrices");
+    dMatInfo *mp = mat[index];
+    index++;
+
+    dMatInfo mi;
+    va_list ap;
+    va_start (ap,name);
+    vsprintf (mi.name,name,ap);
+    if (strlen(mi.name) >= sizeof (mi.name)) dDebug (0,"name too long");
+
+    if (strcmp(mp->name,mi.name) != 0)
+      dDebug (0,"dMatrixComparison, name mismatch (\"%s\" and \"%s\")",
+              mp->name,mi.name);
+    if (mp->n != n || mp->m != m)
+      dDebug (0,"dMatrixComparison, size mismatch (%dx%d and %dx%d)",
+              mp->n,mp->m,n,m);
+
+    dReal maxdiff;
+    if (lower_tri) {
+      maxdiff = dMaxDifferenceLowerTriangle (A,mp->data,n);
+    }
+    else {
+      maxdiff = dMaxDifference (A,mp->data,n,m);
+    }
+    if (maxdiff > tol)
+      dDebug (0,"dMatrixComparison, matrix error (size=%dx%d, name=\"%s\", "
+              "error=%.4e)",n,m,mi.name,maxdiff);
+    return maxdiff;
+  }
+}
+
+
+void dMatrixComparison::end()
+{
+  if (mat.size() <= 0) dDebug (0,"no matrices in sequence");
+  afterfirst = 1;
+  index = 0;
+}
+
+
+void dMatrixComparison::reset()
+{
+  for (int i=0; i<mat.size(); i++) {
+    dFree (mat[i]->data,mat[i]->size);
+    dFree (mat[i],sizeof(dMatInfo));
+  }
+  mat.setSize (0);
+  afterfirst = 0;
+  index = 0;
+}
+
+
+void dMatrixComparison::dump()
+{
+  for (int i=0; i<mat.size(); i++)
+    printf ("%d: %s (%dx%d)\n",i,mat[i]->name,mat[i]->n,mat[i]->m);
+}
+
+//****************************************************************************
+// unit test
+
+#include <setjmp.h>
+
+static jmp_buf jump_buffer;
+
+static void myDebug (int num, const char *msg, va_list ap)
+{
+  // printf ("(Error %d: ",num);
+  // vprintf (msg,ap);
+  // printf (")\n");
+  longjmp (jump_buffer,1);
+}
+
+
+extern "C" ODE_API void dTestMatrixComparison()
+{
+  volatile int i;
+  printf ("dTestMatrixComparison()\n");
+  dMessageFunction *orig_debug = dGetDebugHandler();
+
+  dMatrixComparison mc;
+  dReal A[50*50];
+
+  // make first sequence
+  unsigned long seed = dRandGetSeed();
+  for (i=1; i<49; i++) {
+    dMakeRandomMatrix (A,i,i+1,1.0);
+    mc.nextMatrix (A,i,i+1,0,"A%d",i);
+  }
+  mc.end();
+
+  //mc.dump();
+
+  // test identical sequence
+  dSetDebugHandler (&myDebug);
+  dRandSetSeed (seed);
+  if (setjmp (jump_buffer)) {
+    printf ("\tFAILED (1)\n");
+  }
+  else {
+    for (i=1; i<49; i++) {
+      dMakeRandomMatrix (A,i,i+1,1.0);
+      mc.nextMatrix (A,i,i+1,0,"A%d",i);
+    }
+    mc.end();
+    printf ("\tpassed (1)\n");
+  }
+  dSetDebugHandler (orig_debug);
+
+  // test broken sequences (with matrix error)
+  dRandSetSeed (seed);
+  volatile int passcount = 0;
+  for (i=1; i<49; i++) {
+    if (setjmp (jump_buffer)) {
+      passcount++;
+    }
+    else {
+      dSetDebugHandler (&myDebug);
+      dMakeRandomMatrix (A,i,i+1,1.0);
+      A[(i-1)*dPAD(i+1)+i] += REAL(0.01);
+      mc.nextMatrix (A,i,i+1,0,"A%d",i);
+      dSetDebugHandler (orig_debug);
+    }
+  }
+  mc.end();
+  printf ("\t%s (2)\n",(passcount == 48) ? "passed" : "FAILED");
+
+  // test broken sequences (with name error)
+  dRandSetSeed (seed);
+  passcount = 0;
+  for (i=1; i<49; i++) {
+    if (setjmp (jump_buffer)) {
+      passcount++;
+    }
+    else {
+      dSetDebugHandler (&myDebug);
+      dMakeRandomMatrix (A,i,i+1,1.0);
+      mc.nextMatrix (A,i,i+1,0,"B%d",i);
+      dSetDebugHandler (orig_debug);
+    }
+  }
+  mc.end();
+  printf ("\t%s (3)\n",(passcount == 48) ? "passed" : "FAILED");
+
+  // test identical sequence again
+  dSetDebugHandler (&myDebug);
+  dRandSetSeed (seed);
+  if (setjmp (jump_buffer)) {
+    printf ("\tFAILED (4)\n");
+  }
+  else {
+    for (i=1; i<49; i++) {
+      dMakeRandomMatrix (A,i,i+1,1.0);
+      mc.nextMatrix (A,i,i+1,0,"A%d",i);
+    }
+    mc.end();
+    printf ("\tpassed (4)\n");
+  }
+  dSetDebugHandler (orig_debug);
+}