[ODE] Stepfast bug fix

[Joe Ante]

Hi,

Probably a bit late now that quickstep is here...
Can someone check this into cvs.

Step fast didn¹t save the acceleration/torque when gravity was disabled.
I also removed the unneccessary assigments of zero values. And only setting
the body tag one time instead of 3 times inside the loop.


--- /Users/joachima/Desktop/Ode/oldode-040513/ode/src/stepfast.cpp      Sun
May 16 16:33:25 2004
+++ /Users/joachima/Desktop/unity/External/ode/ode/src/stepfast.cpp     Wed
May  5 22:26:49 2004
@@ -777,13 +777,13 @@
        dReal *globalInvI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal));
        for (b = 0; b < nb; b++)
        {
-               for (i = 0; i < 3; i++)
+               for (i = 0; i < 4; i++)
                {
                        saveFacc[b * 4 + i] = bodies[b]->facc[i];
                        saveTacc[b * 4 + i] = bodies[b]->tacc[i];
-               }
                bodies[b]->tag = b;
        }
+       }
 
        for (iter = 0; iter < maxiterations; iter++)
        {
@@ -820,13 +820,9 @@
                                body->facc[0] = saveFacc[b * 4 + 0] +
body->mass.mass * world->gravity[0];
                                body->facc[1] = saveFacc[b * 4 + 1] +
body->mass.mass * world->gravity[1];
                                body->facc[2] = saveFacc[b * 4 + 2] +
body->mass.mass * world->gravity[2];
+                               body->facc[3] = 0;
                        }
-                       else
-                       {
-                               body->facc[0] = saveFacc[b * 4 + 0];
-                               body->facc[1] = saveFacc[b * 4 + 1];
-                               body->facc[2] = saveFacc[b * 4 + 2];
-                       }
+
                }
 
 #ifdef RANDOM_JOINT_ORDER
@@ -899,7 +895,7 @@
                {
                        body = bodies[b];
 
-                       for (i = 0; i < 3; i++)
+                       for (i = 0; i < 4; i++)
                        {
                                body->facc[i] *= ministep;
                                body->tacc[i] *= ministep;
@@ -917,7 +913,7 @@
                }
        }
        for (b = 0; b < nb; b++)
-               for (j = 0; j < 3; j++)
+               for (j = 0; j < 4; j++)
                        bodies[b]->facc[j] = bodies[b]->tacc[j] = 0;
 }