[ODE] Stepfast - auto-disable stuff

Adam D. Moss aspirin at ntlworld.com
Mon Jul 14 12:19:01 2003


This is a multi-part message in MIME format.
--------------060704000106050703090806
Content-Type: text/plain; charset=us-ascii; format=flowed
Content-Transfer-Encoding: 7bit

Here's the same again, but against current ODE CVS HEAD.

It's not a very big diff, but there's a scattering of odd cruft
included (the change in the first hunk is a particular puzzle, for
example) that probably says that you should apply as-is only with
caution

Regards,
--Adam
-- 
Adam D. Moss   . ,,^^   adam@gimp.org   http://www.foxbox.org/   co:3
"Tell people something they know already and they will thank you for
it.  Tell them something new and they will hate you for it."

--------------060704000106050703090806
Content-Type: text/plain;
 name="newaraz.patch"
Content-Transfer-Encoding: 7bit
Content-Disposition: inline;
 filename="newaraz.patch"

? newaraz.patch
Index: ode/src/collision_std.cpp
===================================================================
RCS file: /cvsroot/opende/ode/ode/src/collision_std.cpp,v
retrieving revision 1.9
diff -u -b -r1.9 collision_std.cpp
--- ode/src/collision_std.cpp	11 Jul 2003 21:35:34 -0000	1.9
+++ ode/src/collision_std.cpp	14 Jul 2003 19:13:20 -0000
@@ -1108,7 +1108,7 @@
     int maxi = 0;
     for (int i=1; i<3; i++) {
       dReal tt = dFabs(t[i]);
-      if (tt > max) {
+      if (tt < max) { // Araz: was >
 	max = tt;
 	maxi = i;
       }
Index: ode/src/error.cpp
===================================================================
RCS file: /cvsroot/opende/ode/ode/src/error.cpp,v
retrieving revision 1.9
diff -u -b -r1.9 error.cpp
--- ode/src/error.cpp	25 Jun 2002 23:46:18 -0000	1.9
+++ ode/src/error.cpp	14 Jul 2003 19:13:20 -0000
@@ -1,4 +1,4 @@
-/*************************************************************************
+ /*************************************************************************
  *                                                                       *
  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
@@ -65,6 +65,11 @@
 }
 
 
+//****************************************************************************
+// unix
+
+#ifndef WIN32
+
 static void printMessage (int num, const char *msg1, const char *msg2,
 			  va_list ap)
 {
@@ -77,11 +82,6 @@
   fflush (stderr);
 }
 
-//****************************************************************************
-// unix
-
-#ifndef WIN32
-
 extern "C" void dError (int num, const char *msg, ...)
 {
   va_list ap;
@@ -111,6 +111,7 @@
   else printMessage (num,"ODE Message",msg,ap);
 }
 
+
 #endif
 
 //****************************************************************************
@@ -127,6 +128,19 @@
 
 #include "windows.h"
 
+static void printMessage (int num, const char *msg1, const char *msg2,
+			  va_list ap)
+{
+	// Araz
+	char buf[1000];
+	if (num) sprintf(buf,"\n%s %d: ",msg1,num);
+	else sprintf (buf,"\n%s: ",msg1);
+	OutputDebugString( buf );
+	vsprintf(buf,msg2,ap);
+	OutputDebugString( buf );
+	OutputDebugString( "\n" );
+}
+
 
 extern "C" void dError (int num, const char *msg, ...)
 {
Index: ode/src/objects.h
===================================================================
RCS file: /cvsroot/opende/ode/ode/src/objects.h,v
retrieving revision 1.10
diff -u -b -r1.10 objects.h
--- ode/src/objects.h	10 Nov 2002 23:15:59 -0000	1.10
+++ ode/src/objects.h	14 Jul 2003 19:13:20 -0000
@@ -76,6 +76,8 @@
   dVector3 lvel,avel;		// linear and angular velocity of POR
   dVector3 facc,tacc;		// force and torque accululators
   dVector3 finite_rot_axis;	// finite rotation axis, unit length or 0=none
+  // Araz:
+  int mDisableSteps; // auto-disable counter
 };
 
 
@@ -86,6 +88,9 @@
   dVector3 gravity;		// gravity vector (m/s/s)
   dReal global_erp;		// global error reduction parameter
   dReal global_cfm;		// global costraint force mixing parameter
+  // Araz: auto-disabling stuff
+  dReal	mAutoDisableThreshold;
+  int	mAutoDisableSteps;
 };
 
 
Index: ode/src/ode.cpp
===================================================================
RCS file: /cvsroot/opende/ode/ode/src/ode.cpp,v
retrieving revision 1.39
diff -u -b -r1.39 ode.cpp
--- ode/src/ode.cpp	11 Jul 2003 21:35:34 -0000	1.39
+++ ode/src/ode.cpp	14 Jul 2003 19:13:22 -0000
@@ -353,6 +353,8 @@
   dSetZero (b->facc,4);
   dSetZero (b->tacc,4);
   dSetZero (b->finite_rot_axis,4);
+  // Araz: auto-disable
+  b->mDisableSteps = 0;
   addObjectToList (b,(dObject **) &w->firstbody);
   w->nb++;
   return b;
@@ -1168,6 +1170,9 @@
 #else
   #error dSINGLE or dDOUBLE must be defined
 #endif
+  // Araz: auto-disabling stuff
+  w->mAutoDisableSteps = 10;
+  w->mAutoDisableThreshold = REAL(0.008);
   return w;
 }
 
@@ -1202,6 +1207,17 @@
   delete w;
 }
 
+// Araz: auto-disable stuff
+void dWorldSetAutoDisableSteps( dWorldID w, int steps )
+{
+	dAASSERT(w);
+	w->mAutoDisableSteps = steps;
+}
+void dWorldSetAutoDisableThreshold( dWorldID w, dReal threshold )
+{
+	dAASSERT(w);
+	w->mAutoDisableThreshold = threshold;
+}
 
 void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z)
 {
Index: ode/src/stepfast.cpp
===================================================================
RCS file: /cvsroot/opende/ode/ode/src/stepfast.cpp,v
retrieving revision 1.1
diff -u -b -r1.1 stepfast.cpp
--- ode/src/stepfast.cpp	11 Jul 2003 04:36:58 -0000	1.1
+++ ode/src/stepfast.cpp	14 Jul 2003 19:13:22 -0000
@@ -96,7 +96,8 @@
 		A += Askip + 1;
 		C += 8;
 	}
-
}
+
+}
 
 static void
 MultiplyAdd2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip)
@@ -127,7 +128,8 @@
 		A += Askip + 1;
 		C += 8;
 	}
-
}
+
+}
 
 
 // this assumes the 4th and 8th rows of B are zero.
@@ -983,6 +985,30 @@
 #ifdef TIMING
 		dTimerNow ("Island Processing");
 #endif
+		// Araz: auto-disabling stuff
+		if( !(bb->flags&dxBodyDisabled) ) {
+			bool disable = true;
+			const dReal *lvel = bb->lvel;;
+			dReal lspeed = dDOT(lvel,lvel);
+			if( lspeed > world->mAutoDisableThreshold ) {
+				disable = false;
+			} else {
+				const dReal *avel = bb->avel;
+				dReal aspeed = dDOT(avel,avel);
+				if( aspeed > world->mAutoDisableThreshold ) {
+					disable = false;
+				}
+			}
+			if( disable )
+				++bb->mDisableSteps;
+			else
+				bb->mDisableSteps = 0;
+			
+			if( bb->mDisableSteps > world->mAutoDisableSteps ) {
+				bb->flags |= dxBodyDisabled;
+			}
+		}
+
 		// get bb = the next enabled, untagged body, and tag it
 		if (bb->tag || (bb->flags & dxBodyDisabled))
 			continue;
@@ -1019,6 +1045,9 @@
 						if (thisDepth < 0)
 							continue;
 						n->body->flags &= ~dxBodyDisabled;
+						// Araz: auto disable stuff
+						n->body->mDisableSteps = 0;
+						// /Araz
 						n->body->tag = 1;
 						autostack[stacksize] = thisDepth;
 						stack[stacksize++] = n->body;
@@ -1040,6 +1069,9 @@
 		{
 			body[i]->tag = 1;
 			body[i]->flags &= ~dxBodyDisabled;
+			// Araz: auto disable stuff
+			//body[i]->mDisableSteps = 0;
+			// /Araz
 		}
 		for (i = 0; i < jcount; i++)
 			joint[i]->tag = 1;

--------------060704000106050703090806--