Menu
▾
▴
[Opende-svn] SF.net SVN: opende:[1705] trunk/ode/src
|
From: <ole...@us...> - 2009-10-25 17:44:17
|
Revision: 1705
http://opende.svn.sourceforge.net/opende/?rev=1705&view=rev
Author: oleh_derevenko
Date: 2009-10-25 17:44:06 +0000 (Sun, 25 Oct 2009)
Log Message:
-----------
Cosmetic: Dead code removed in step.cpp and lcp.cpp
Modified Paths:
--------------
trunk/ode/src/Makefile.am
trunk/ode/src/lcp.cpp
trunk/ode/src/step.cpp
Removed Paths:
-------------
trunk/ode/src/testing.cpp
trunk/ode/src/testing.h
Modified: trunk/ode/src/Makefile.am
===================================================================
--- trunk/ode/src/Makefile.am 2009-10-25 17:30:42 UTC (rev 1704)
+++ trunk/ode/src/Makefile.am 2009-10-25 17:44:06 UTC (rev 1705)
@@ -57,7 +57,6 @@
rotation.cpp \
sphere.cpp \
step.cpp step.h \
- testing.cpp testing.h \
timer.cpp \
util.cpp util.h
Modified: trunk/ode/src/lcp.cpp
===================================================================
--- trunk/ode/src/lcp.cpp 2009-10-25 17:30:42 UTC (rev 1704)
+++ trunk/ode/src/lcp.cpp 2009-10-25 17:44:06 UTC (rev 1705)
@@ -120,10 +120,9 @@
//***************************************************************************
// code generation parameters
-// LCP debugging (mosty for fast dLCP) - this slows things down a lot
+// LCP debugging (mostly for fast dLCP) - this slows things down a lot
//#define DEBUG_LCP
-//#define dLCP_SLOW // use slow dLCP object
#define dLCP_FAST // use fast dLCP object
// option 1 : matrix row pointers (less data copying)
@@ -335,367 +334,6 @@
//
// the index set C is special: solutions to A(C,C)\A(C,i) can be generated.
-/*
-#ifdef dLCP_SLOW
-
-// simple but slow implementation of dLCP, for testing the LCP drivers.
-
-#include "array.h"
-
-struct dLCP {
- int n,nub,nskip;
- dReal *Adata,*x,*b,*w,*lo,*hi; // LCP problem data
- ATYPE A; // A rows
- dArray<int> C,N; // index sets
- int last_i_for_solve1; // last i value given to solve1
-
- dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
- dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
- dReal *_Dell, dReal *_ell, dReal *_tmp,
- int *_state, int *_findex, int *_p, int *_C, dReal **Arows);
- // the constructor is given an initial problem description (A,x,b,w) and
- // space for other working data (which the caller may allocate on the stack).
- // some of this data is specific to the fast dLCP implementation.
- // the matrices A and L have size n*n, vectors have size n*1.
- // A represents a symmetric matrix but only the lower triangle is valid.
- // `nub' is the number of unbounded indexes at the start. all the indexes
- // 0..nub-1 will be put into C.
-
- ~dLCP();
-
- int getNub() { return nub; }
- // return the value of `nub'. the constructor may want to change it,
- // so the caller should find out its new value.
-
- // transfer functions: transfer index i to the given set (C or N). indexes
- // less than `nub' can never be given. A,x,b,w,etc may be permuted by these
- // functions, the caller must be robust to this.
-
- void transfer_i_to_C (int i);
- // this assumes C and N span 1:i-1. this also assumes that solve1() has
- // been recently called for the same i without any other transfer
- // functions in between (thereby allowing some data reuse for the fast
- // implementation).
- void transfer_i_to_N (int i);
- // this assumes C and N span 1:i-1.
- void transfer_i_from_N_to_C (int i);
- void transfer_i_from_C_to_N (int i);
-
- int numC();
- int numN();
- // return the number of indexes in set C/N
-
- int indexC (int i);
- int indexN (int i);
- // return index i in set C/N.
-
- // accessor and arithmetic functions. Aij translates as A(i,j), etc.
- // make sure that only the lower triangle of A is ever referenced.
-
- dReal Aii (int i);
- dReal AiC_times_qC (int i, dReal *q);
- dReal AiN_times_qN (int i, dReal *q); // for all Nj
- void pN_equals_ANC_times_qC (dReal *p, dReal *q); // for all Nj
- void pN_plusequals_ANi (dReal *p, int i, int sign=1);
- // for all Nj. sign = +1,-1. assumes i > maximum index in N.
- void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q);
- void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q); // for all Nj
- void solve1 (dReal *a, int i, int dir=1, int only_transfer=0);
- // get a(C) = - dir * A(C,C) \ A(C,i). dir must be +/- 1.
- // the fast version of this function computes some data that is needed by
- // transfer_i_to_C(). if only_transfer is nonzero then this function
- // *only* computes that data, it does not set a(C).
-
- void unpermute();
- // call this at the end of the LCP function. if the x/w values have been
- // permuted then this will unscramble them.
-};
-
-
-dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,
- dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,
- dReal *_Dell, dReal *_ell, dReal *_tmp,
- int *_state, int *_findex, int *_p, int *_C, dReal **Arows)
-{
- dUASSERT (_findex==0,"slow dLCP object does not support findex array");
-
- n = _n;
- nub = _nub;
- Adata = _Adata;
- A = 0;
- x = _x;
- b = _b;
- w = _w;
- lo = _lo;
- hi = _hi;
- nskip = dPAD(n);
- dSetZero (x,n);
- last_i_for_solve1 = -1;
-
- int i,j;
- C.setSize (n);
- N.setSize (n);
- for (i=0; i<n; i++) {
- C[i] = 0;
- N[i] = 0;
- }
-
-# ifdef ROWPTRS
- // make matrix row pointers
- A = Arows;
- for (i=0; i<n; i++) A[i] = Adata + i*nskip;
-# else
- A = Adata;
-# endif
-
- // lets make A symmetric
- for (i=0; i<n; i++) {
- for (j=i+1; j<n; j++) AROW(i)[j] = AROW(j)[i];
- }
-
- // if nub>0, put all indexes 0..nub-1 into C and solve for x
- if (nub > 0) {
- for (i=0; i<nub; i++) memcpy (_L+i*nskip,AROW(i),(i+1)*sizeof(dReal));
- dFactorLDLT (_L,_d,nub,nskip);
- memcpy (x,b,nub*sizeof(dReal));
- dSolveLDLT (_L,_d,x,nub,nskip);
- dSetZero (_w,nub);
- for (i=0; i<nub; i++) C[i] = 1;
- }
-}
-
-
-dLCP::~dLCP()
-{
-}
-
-
-void dLCP::transfer_i_to_C (int i)
-{
- if (i < nub) dDebug (0,"bad i");
- if (C[i]) dDebug (0,"i already in C");
- if (N[i]) dDebug (0,"i already in N");
- for (int k=0; k<i; k++) {
- if (!(C[k] ^ N[k])) dDebug (0,"assumptions for C and N violated");
- }
- for (int k=i; k<n; k++)
- if (C[k] || N[k]) dDebug (0,"assumptions for C and N violated");
- if (i != last_i_for_solve1) dDebug (0,"assumptions for i violated");
- last_i_for_solve1 = -1;
- C[i] = 1;
-}
-
-
-void dLCP::transfer_i_to_N (int i)
-{
- if (i < nub) dDebug (0,"bad i");
- if (C[i]) dDebug (0,"i already in C");
- if (N[i]) dDebug (0,"i already in N");
- for (int k=0; k<i; k++)
- if (!C[k] && !N[k]) dDebug (0,"assumptions for C and N violated");
- for (int k=i; k<n; k++)
- if (C[k] || N[k]) dDebug (0,"assumptions for C and N violated");
- last_i_for_solve1 = -1;
- N[i] = 1;
-}
-
-
-void dLCP::transfer_i_from_N_to_C (int i)
-{
- if (i < nub) dDebug (0,"bad i");
- if (C[i]) dDebug (0,"i already in C");
- if (!N[i]) dDebug (0,"i not in N");
- last_i_for_solve1 = -1;
- N[i] = 0;
- C[i] = 1;
-}
-
-
-void dLCP::transfer_i_from_C_to_N (int i)
-{
- if (i < nub) dDebug (0,"bad i");
- if (N[i]) dDebug (0,"i already in N");
- if (!C[i]) dDebug (0,"i not in C");
- last_i_for_solve1 = -1;
- C[i] = 0;
- N[i] = 1;
-}
-
-
-int dLCP::numC()
-{
- int i,count=0;
- for (i=0; i<n; i++) if (C[i]) count++;
- return count;
-}
-
-
-int dLCP::numN()
-{
- int i,count=0;
- for (i=0; i<n; i++) if (N[i]) count++;
- return count;
-}
-
-
-int dLCP::indexC (int i)
-{
- int k,count=0;
- for (k=0; k<n; k++) {
- if (C[k]) {
- if (count==i) return k;
- count++;
- }
- }
- dDebug (0,"bad index C (%d)",i);
- return 0;
-}
-
-
-int dLCP::indexN (int i)
-{
- int k,count=0;
- for (k=0; k<n; k++) {
- if (N[k]) {
- if (count==i) return k;
- count++;
- }
- }
- dDebug (0,"bad index into N");
- return 0;
-}
-
-
-dReal dLCP::Aii (int i)
-{
- return AROW(i)[i];
-}
-
-
-dReal dLCP::AiC_times_qC (int i, dReal *q)
-{
- dReal sum = 0;
- for (int k=0; k<n; k++) if (C[k]) sum += AROW(i)[k] * q[k];
- return sum;
-}
-
-
-dReal dLCP::AiN_times_qN (int i, dReal *q)
-{
- dReal sum = 0;
- for (int k=0; k<n; k++) if (N[k]) sum += AROW(i)[k] * q[k];
- return sum;
-}
-
-
-void dLCP::pN_equals_ANC_times_qC (dReal *p, dReal *q)
-{
- dReal sum;
- for (int ii=0; ii<n; ii++) if (N[ii]) {
- sum = 0;
- for (int jj=0; jj<n; jj++) if (C[jj]) sum += AROW(ii)[jj] * q[jj];
- p[ii] = sum;
- }
-}
-
-
-void dLCP::pN_plusequals_ANi (dReal *p, int i, int sign)
-{
- int k;
- for (k=0; k<n; k++) if (N[k] && k >= i) dDebug (0,"N assumption violated");
- if (sign > 0) {
- for (k=0; k<n; k++) if (N[k]) p[k] += AROW(i)[k];
- }
- else {
- for (k=0; k<n; k++) if (N[k]) p[k] -= AROW(i)[k];
- }
-}
-
-
-void dLCP::pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q)
-{
- for (int k=0; k<n; k++) if (C[k]) p[k] += s*q[k];
-}
-
-
-void dLCP::pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q)
-{
- for (int k=0; k<n; k++) if (N[k]) p[k] += s*q[k];
-}
-
-
-void dLCP::solve1 (dReal *a, int i, int dir, int only_transfer)
-{
-
- ALLOCA (dReal,AA,n*nskip*sizeof(dReal));
-#ifdef dUSE_MALLOC_FOR_ALLOCA
- if (AA == NULL) {
- dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
- return;
- }
-#endif
- ALLOCA (dReal,dd,n*sizeof(dReal));
-#ifdef dUSE_MALLOC_FOR_ALLOCA
- if (dd == NULL) {
- UNALLOCA(AA);
- dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
- return;
- }
-#endif
- ALLOCA (dReal,bb,n*sizeof(dReal));
-#ifdef dUSE_MALLOC_FOR_ALLOCA
- if (bb == NULL) {
- UNALLOCA(AA);
- UNALLOCA(dd);
- dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;
- return;
- }
-#endif
-
- int ii,jj,AAi,AAj;
-
- last_i_for_solve1 = i;
- AAi = 0;
- for (ii=0; ii<n; ii++) if (C[ii]) {
- AAj = 0;
- for (jj=0; jj<n; jj++) if (C[jj]) {
- AA[AAi*nskip+AAj] = AROW(ii)[jj];
- AAj++;
- }
- bb[AAi] = AROW(i)[ii];
- AAi++;
- }
- if (AAi==0) {
- UNALLOCA (AA);
- UNALLOCA (dd);
- UNALLOCA (bb);
- return;
- }
-
- dFactorLDLT (AA,dd,AAi,nskip);
- dSolveLDLT (AA,dd,bb,AAi,nskip);
-
- AAi=0;
- if (dir > 0) {
- for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = -bb[AAi++];
- }
- else {
- for (ii=0; ii<n; ii++) if (C[ii]) a[ii] = bb[AAi++];
- }
-
- UNALLOCA (AA);
- UNALLOCA (dd);
- UNALLOCA (bb);
-}
-
-
-void dLCP::unpermute()
-{
-}
-
-#endif // dLCP_SLOW
-*/
-
//***************************************************************************
// fast implementation of dLCP. see the above definition of dLCP for
// interface comments.
Modified: trunk/ode/src/step.cpp
===================================================================
--- trunk/ode/src/step.cpp 2009-10-25 17:30:42 UTC (rev 1704)
+++ trunk/ode/src/step.cpp 2009-10-25 17:44:06 UTC (rev 1705)
@@ -38,21 +38,6 @@
//#define TIMING
-//****************************************************************************
-// debugging - comparison of various vectors and matrices produced by the
-// slow and fast versions of the stepper.
-
-//#define COMPARE_METHODS
-
-#ifdef COMPARE_METHODS
-#include "testing.h"
-static dMatrixComparison comparator;
-#endif
-
-// undef to use the fast decomposition
-#define DIRECT_CHOLESKY
-#undef REPORT_ERROR
-
#ifdef TIMING
#define IFTIMING(x) x
#else
@@ -195,323 +180,7 @@
A[5] = sum5;
A[6] = sum6;
}
-/*
-//****************************************************************************
-// the slow, but sure way
-// note that this does not do any joint feedback!
-// given lists of bodies and joints that form an island, perform a first
-// order timestep.
-//
-// `body' is the body array, `nb' is the size of the array.
-// `_joint' is the body array, `nj' is the size of the array.
-
-void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int nb,
- dxJoint * const *_joint, int nj, dReal stepsize)
-{
- int i,j,k;
- int n6 = 6*nb;
-
- IFTIMING(dTimerStart("preprocessing"));
-
- // number all bodies in the body list - set their tag values
- for (i=0; i<nb; i++) body[i]->tag = i;
-
- // make a local copy of the joint array, because we might want to modify it.
- // (the "dxJoint *const*" declaration says we're allowed to modify the joints
- // but not the joint array, because the caller might need it unchanged).
- ALLOCA(dxJoint*,joint,nj*sizeof(dxJoint*));
- memcpy (joint,_joint,nj * sizeof(dxJoint*));
-
- // for all bodies, compute the inertia tensor and its inverse in the global
- // frame, and compute the rotational force and add it to the torque
- // accumulator.
- // @@@ check computation of rotational force.
- ALLOCA(dReal,I,3*nb*4*sizeof(dReal));
- ALLOCA(dReal,invI,3*nb*4*sizeof(dReal));
-
- //dSetZero (I,3*nb*4);
- //dSetZero (invI,3*nb*4);
- for (i=0; i<nb; i++) {
- dReal tmp[12];
- // compute inertia tensor in global frame
- dMultiply2_333 (tmp,body[i]->mass.I,body[i]->posr.R);
- dMultiply0_333 (I+i*12,body[i]->posr.R,tmp);
- // compute inverse inertia tensor in global frame
- dMultiply2_333 (tmp,body[i]->invI,body[i]->posr.R);
- dMultiply0_333 (invI+i*12,body[i]->posr.R,tmp);
- // compute rotational force
- dMultiply0_331 (tmp,I+i*12,body[i]->avel);
- dSubtractVectorCross3(body[i]->tacc,body[i]->avel,tmp);
- }
-
- // add the gravity force to all bodies
- for (i=0; i<nb; i++) {
- if ((body[i]->flags & dxBodyNoGravity)==0) {
- body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
- body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
- body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
- }
- }
-
- // get m = total constraint dimension, nub = number of unbounded variables.
- // create constraint offset array and number-of-rows array for all joints.
- // the constraints are re-ordered as follows: the purely unbounded
- // constraints, the mixed unbounded + LCP constraints, and last the purely
- // LCP constraints.
- //
- // joints with m=0 are inactive and are removed from the joints array
- // entirely, so that the code that follows does not consider them.
- int m = 0;
- ALLOCA(dxJoint::Info1,info,nj*sizeof(dxJoint::Info1));
- ALLOCA(int,ofs,nj*sizeof(int));
-
- for (i=0, j=0; j<nj; j++) { // i=dest, j=src
- joint[j]->getInfo1 (info+i);
- dIASSERT (info[i].m >= 0 && info[i].m <= 6 &&
- info[i].nub >= 0 && info[i].nub <= info[i].m);
- if (info[i].m > 0) {
- joint[i] = joint[j];
- i++;
- }
- }
- nj = i;
-
- // the purely unbounded constraints
- for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {
- ofs[i] = m;
- m += info[i].m;
- }
- //int nub = m;
- // the mixed unbounded + LCP constraints
- for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {
- ofs[i] = m;
- m += info[i].m;
- }
- // the purely LCP constraints
- for (i=0; i<nj; i++) if (info[i].nub == 0) {
- ofs[i] = m;
- m += info[i].m;
- }
-
- // create (6*nb,6*nb) inverse mass matrix `invM', and fill it with mass
- // parameters
- IFTIMING(dTimerNow ("create mass matrix"));
-
- int nskip = dPAD (n6);
- ALLOCA(dReal, invM, n6*nskip*sizeof(dReal));
-
- dSetZero (invM,n6*nskip);
- for (i=0; i<nb; i++) {
- dReal *MM = invM+(i*6)*nskip+(i*6);
- MM[0] = body[i]->invMass;
- MM[nskip+1] = body[i]->invMass;
- MM[2*nskip+2] = body[i]->invMass;
- MM += 3*nskip+3;
- for (j=0; j<3; j++) for (k=0; k<3; k++) {
- MM[j*nskip+k] = invI[i*12+j*4+k];
- }
- }
-
- // assemble some body vectors: fe = external forces, v = velocities
- ALLOCA(dReal,fe,n6*sizeof(dReal));
- ALLOCA(dReal,v,n6*sizeof(dReal));
-
- //dSetZero (fe,n6);
- //dSetZero (v,n6);
- for (i=0; i<nb; i++) {
- for (j=0; j<3; j++) fe[i*6+j] = body[i]->facc[j];
- for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j];
- for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j];
- for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j];
- }
-
- // this will be set to the velocity update
- ALLOCA(dReal,vnew,n6*sizeof(dReal));
- dSetZero (vnew,n6);
-
- // if there are constraints, compute cforce
- if (m > 0) {
- // create a constraint equation right hand side vector `c', a constraint
- // force mixing vector `cfm', and LCP low and high bound vectors, and an
- // 'findex' vector.
- ALLOCA(dReal,c,m*sizeof(dReal));
- ALLOCA(dReal,cfm,m*sizeof(dReal));
- ALLOCA(dReal,lo,m*sizeof(dReal));
- ALLOCA(dReal,hi,m*sizeof(dReal));
- ALLOCA(int,findex,m*sizeof(int));
- dSetZero (c,m);
- dSetValue (cfm,m,world->global_cfm);
- dSetValue (lo,m,-dInfinity);
- dSetValue (hi,m, dInfinity);
- for (i=0; i<m; i++) findex[i] = -1;
-
- // create (m,6*nb) jacobian mass matrix `J', and fill it with constraint
- // data. also fill the c vector.
- IFTIMING(dTimerNow ("create J"));
- ALLOCA(dReal,J,m*nskip*sizeof(dReal));
- dSetZero (J,m*nskip);
- dxJoint::Info2 Jinfo;
- Jinfo.rowskip = nskip;
- Jinfo.fps = dRecip(stepsize);
- Jinfo.erp = world->global_erp;
- for (i=0; i<nj; i++) {
- Jinfo.J1l = J + nskip*ofs[i] + 6*joint[i]->node[0].body->tag;
- Jinfo.J1a = Jinfo.J1l + 3;
- if (joint[i]->node[1].body) {
- Jinfo.J2l = J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag;
- Jinfo.J2a = Jinfo.J2l + 3;
- }
- else {
- Jinfo.J2l = 0;
- Jinfo.J2a = 0;
- }
- Jinfo.c = c + ofs[i];
- Jinfo.cfm = cfm + ofs[i];
- Jinfo.lo = lo + ofs[i];
- Jinfo.hi = hi + ofs[i];
- Jinfo.findex = findex + ofs[i];
- joint[i]->getInfo2 (&Jinfo);
- // adjust returned findex values for global index numbering
- for (j=0; j<info[i].m; j++) {
- if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
- }
- }
-
- // compute A = J*invM*J'
- IFTIMING(dTimerNow ("compute A"));
- ALLOCA(dReal,JinvM,m*nskip*sizeof(dReal));
- //dSetZero (JinvM,m*nskip);
- dMultiply0 (JinvM,J,invM,m,n6,n6);
- int mskip = dPAD(m);
- ALLOCA(dReal,A,m*mskip*sizeof(dReal));
- //dSetZero (A,m*mskip);
- dMultiply2 (A,JinvM,J,m,n6,m);
-
- // add cfm to the diagonal of A
- for (i=0; i<m; i++) A[i*mskip+i] += cfm[i] * Jinfo.fps;
-
-# ifdef COMPARE_METHODS
- comparator.nextMatrix (A,m,m,1,"A");
-# endif
-
- // compute `rhs', the right hand side of the equation J*a=c
- IFTIMING(dTimerNow ("compute rhs"));
- ALLOCA(dReal,tmp1,n6*sizeof(dReal));
- //dSetZero (tmp1,n6);
- dMultiply0 (tmp1,invM,fe,n6,n6,1);
- for (i=0; i<n6; i++) tmp1[i] += v[i]/stepsize;
- ALLOCA(dReal,rhs,m*sizeof(dReal));
- //dSetZero (rhs,m);
- dMultiply0 (rhs,J,tmp1,m,n6,1);
- for (i=0; i<m; i++) rhs[i] = c[i]/stepsize - rhs[i];
-
-# ifdef COMPARE_METHODS
- comparator.nextMatrix (c,m,1,0,"c");
- comparator.nextMatrix (rhs,m,1,0,"rhs");
-# endif
-
-
-
-
-
-#ifndef DIRECT_CHOLESKY
- // solve the LCP problem and get lambda.
- // this will destroy A but that's okay
- IFTIMING(dTimerNow ("solving LCP problem"));
- ALLOCA(dReal,lambda,m*sizeof(dReal));
- dSolveLCP (m,A,lambda,rhs,NULL,nub,lo,hi,findex);
-
-#ifdef dUSE_MALLOC_FOR_ALLOCA
- if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY)
- return;
-#endif
-
-
-#else
-
- // OLD WAY - direct factor and solve
-
- // factorize A (L*L'=A)
- IFTIMING(dTimerNow ("factorize A"));
- ALLOCA(dReal,L,m*mskip*sizeof(dReal));
- memcpy (L,A,m*mskip*sizeof(dReal));
- if (dFactorCholesky (L,m)==0) dDebug (0,"A is not positive definite");
-
- // compute lambda
- IFTIMING(dTimerNow ("compute lambda"));
- ALLOCA(dReal,lambda,m*sizeof(dReal));
- memcpy (lambda,rhs,m * sizeof(dReal));
- dSolveCholesky (L,lambda,m);
-#endif
-
-# ifdef COMPARE_METHODS
- comparator.nextMatrix (lambda,m,1,0,"lambda");
-# endif
-
- // compute the velocity update `vnew'
- IFTIMING(dTimerNow ("compute velocity update"));
- dMultiply1 (tmp1,J,lambda,n6,m,1);
- for (i=0; i<n6; i++) tmp1[i] += fe[i];
- dMultiply0 (vnew,invM,tmp1,n6,n6,1);
- for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
-
-#ifdef REPORT_ERROR
- // see if the constraint has worked: compute J*vnew and make sure it equals
- // `c' (to within a certain tolerance).
- IFTIMING(dTimerNow ("verify constraint equation"));
- dMultiply0 (tmp1,J,vnew,m,n6,1);
- dReal err = 0;
- for (i=0; i<m; i++) {
- err += dFabs(tmp1[i]-c[i]);
- }
- printf ("total constraint error=%.6e\n",err);
-#endif
-
- }
- else {
- // no constraints
- dMultiply0 (vnew,invM,fe,n6,n6,1);
- for (i=0; i<n6; i++) vnew[i] = v[i] + stepsize*vnew[i];
- }
-
-#ifdef COMPARE_METHODS
- comparator.nextMatrix (vnew,n6,1,0,"vnew");
-#endif
-
- // apply the velocity update to the bodies
- IFTIMING(dTimerNow ("update velocity"));
- for (i=0; i<nb; i++) {
- for (j=0; j<3; j++) body[i]->lvel[j] = vnew[i*6+j];
- for (j=0; j<3; j++) body[i]->avel[j] = vnew[i*6+3+j];
- }
-
- // update the position and orientation from the new linear/angular velocity
- // (over the given timestep)
- IFTIMING(dTimerNow ("update position"));
- for (i=0; i<nb; i++) dxStepBody (body[i],stepsize);
-
- IFTIMING(dTimerNow ("tidy up"));
-
- // zero all force accumulators
- for (i=0; i<nb; i++) {
- body[i]->facc[0] = 0;
- body[i]->facc[1] = 0;
- body[i]->facc[2] = 0;
- body[i]->facc[3] = 0;
- body[i]->tacc[0] = 0;
- body[i]->tacc[1] = 0;
- body[i]->tacc[2] = 0;
- body[i]->tacc[3] = 0;
- }
-
- IFTIMING(dTimerEnd());
- if (m > 0) IFTIMING(dTimerReport (stdout,1));
-
-}
-*/
-
-
//****************************************************************************
// an optimized version of dInternalStepIsland1()
@@ -990,10 +659,6 @@
} END_STATE_SAVE(context, cfmstate);
-# ifdef COMPARE_METHODS
- comparator.nextMatrix (A,m,m,1,"A");
-# endif
-
BEGIN_STATE_SAVE(context, tmp1state) {
// compute the right hand side `rhs'
IFTIMING(dTimerNow ("compute rhs"));
@@ -1014,10 +679,6 @@
}
}
-# ifdef COMPARE_METHODS
- comparator.nextMatrix (c,m,1,0,"c");
-# endif
-
{
// init rhs -- this erases 'c' as they reside in the same memory!!!
rhs = c;
@@ -1044,11 +705,6 @@
ofsi += infom;
}
}
-
-# ifdef COMPARE_METHODS
- comparator.nextMatrix (rhs,m,1,0,"rhs");
-# endif
-
} END_STATE_SAVE(context, tmp1state);
dReal *lambda = context->AllocateArray<dReal> (m);
@@ -1062,10 +718,6 @@
} END_STATE_SAVE(context, lcpstate);
-# ifdef COMPARE_METHODS
- comparator.nextMatrix (lambda,m,1,0,"lambda");
-# endif
-
{
IFTIMING(dTimerNow ("compute constraint force"));
@@ -1162,15 +814,6 @@
}
}
-#ifdef COMPARE_METHODS
- ALLOCA(dReal,tmp_vnew, nb*6*sizeof(dReal));
- for (i=0; i<nb; ++i) {
- for (j=0; j<3; ++j) tmp_vnew[i*6+j] = body[i]->lvel[j];
- for (j=0; j<3; ++j) tmp_vnew[i*6+3+j] = body[i]->avel[j];
- }
- comparator.nextMatrix (tmp_vnew,nb*6,1,0,"vnew");
-#endif
-
{
IFTIMING(dTimerNow ("tidy up"));
@@ -1200,34 +843,7 @@
dxWorld *world, dxBody * const *body, int nb,
dxJoint * const *joint, int nj, dReal stepsize)
{
-#ifndef COMPARE_METHODS
dInternalStepIsland_x2 (context,world,body,nb,joint,nj,stepsize);
-
-#endif
-
-#ifdef COMPARE_METHODS
- int i;
-
- // save body state
- ALLOCA(dxBody,state,nb*sizeof(dxBody));
-
- for (i=0; i<nb; i++) memcpy (state+i,body[i],sizeof(dxBody));
-
- // take slow step
- comparator.reset();
- dInternalStepIsland_x1 (context,world,body,nb,joint,nj,stepsize);
- comparator.end();
-
- // restore state
- for (i=0; i<nb; i++) memcpy (body[i],state+i,sizeof(dxBody));
-
- // take fast step
- dInternalStepIsland_x2 (context,world,body,nb,joint,nj,stepsize);
- comparator.end();
-
- //comparator.dump();
- //_exit (1);
-#endif
}
size_t dxEstimateStepMemoryRequirements (dxBody * const *body, int nb, dxJoint * const *_joint, int _nj)
Deleted: trunk/ode/src/testing.cpp
===================================================================
--- trunk/ode/src/testing.cpp 2009-10-25 17:30:42 UTC (rev 1704)
+++ trunk/ode/src/testing.cpp 2009-10-25 17:44:06 UTC (rev 1705)
@@ -1,244 +0,0 @@
-/*************************************************************************
- * *
- * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
- * All rights reserved. Email: ru...@q1... 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/odeconfig.h>
-#include "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,
- const 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);
-}
Deleted: trunk/ode/src/testing.h
===================================================================
--- trunk/ode/src/testing.h 2009-10-25 17:30:42 UTC (rev 1704)
+++ trunk/ode/src/testing.h 2009-10-25 17:44:06 UTC (rev 1705)
@@ -1,65 +0,0 @@
-/*************************************************************************
- * *
- * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. *
- * All rights reserved. Email: ru...@q1... 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. *
- * *
- *************************************************************************/
-
-/* stuff used for testing */
-
-#ifndef _ODE_TESTING_H_
-#define _ODE_TESTING_H_
-
-#include <ode/common.h>
-#include "array.h"
-
-
-// compare a sequence of named matrices/vectors, i.e. to make sure that two
-// different pieces of code are giving the same results.
-
-class dMatrixComparison {
- struct dMatInfo;
- dArray<dMatInfo*> mat;
- int afterfirst,index;
-
-public:
- dMatrixComparison();
- ~dMatrixComparison();
-
- dReal nextMatrix (dReal *A, int n, int m, int lower_tri, const char *name, ...);
- // add a new n*m matrix A to the sequence. the name of the matrix is given
- // by the printf-style arguments (name,...). if this is the first sequence
- // then this object will simply record the matrices and return 0.
- // if this the second or subsequent sequence then this object will compare
- // the matrices with the first sequence, and report any differences.
- // the matrix error will be returned. if `lower_tri' is 1 then only the
- // lower triangle of the matrix (including the diagonal) will be compared
- // (the matrix must be square).
-
- void end();
- // end a sequence.
-
- void reset();
- // restarts the object, so the next sequence will be the first sequence.
-
- void dump();
- // print out info about all the matrices in the sequence
-};
-
-
-#endif
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|