[ODE] Ray-trimesh bugfix

[Joe Ante]

> This pair of ray fixes looks good to me, but since I don't use
> rays I'll wait for feedback from the list before committing.
> (In the meantime it'd be nice if you could resubmit the patches
> as attachments so that they don't get damaged by line-wrapping etc.)
> Thanks,
> --Adam

-------------- next part --------------
--- /Users/joachima/Desktop/Ode/ode-040520/ode/src/stepfast.cpp Wed May  5 22:26:49 2004
+++ /Users/joachima/Desktop/Ode/oldode-040513/ode/src/stepfast.cpp      Sun May 16 16:33:25 2004
@@ -777,12 +777,12 @@
        dReal *globalInvI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal));
        for (b = 0; b < nb; b++)
        {
-               for (i = 0; i < 4; i++)
+               for (i = 0; i < 3; i++)
                {
                        saveFacc[b * 4 + i] = bodies[b]->facc[i];
                        saveTacc[b * 4 + i] = bodies[b]->tacc[i];
-                       bodies[b]->tag = b;
                }
+               bodies[b]->tag = b;
        }
 
        for (iter = 0; iter < maxiterations; iter++)
@@ -820,9 +820,13 @@
                                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
@@ -836,7 +840,7 @@
                        joint = joints[j];
                        dxJoint::Info1 i1 = info[j];
                        dxJoint::Info2 i2 = Jinfo[j];
-                        const int r = dRandInt(j+1);
+                       const int r = dRandInt(j+1);
                        joints[j] = joints[r];
                        info[j] = info[r];
                        Jinfo[j] = Jinfo[r];
@@ -895,7 +899,7 @@
                {
                        body = bodies[b];
 
-                       for (i = 0; i < 4; i++)
+                       for (i = 0; i < 3; i++)
                        {
                                body->facc[i] *= ministep;
                                body->tacc[i] *= ministep;
@@ -913,7 +917,7 @@
                }
        }
        for (b = 0; b < nb; b++)
-               for (j = 0; j < 4; j++)
+               for (j = 0; j < 3; j++)
                        bodies[b]->facc[j] = bodies[b]->tacc[j] = 0;
 }
 
@@ -983,7 +987,7 @@
        // nothing to do if no bodies
        if (world->nb <= 0)
                return;
-
+
        dInternalHandleAutoDisabling (world,stepsize);
 
        // make arrays for body and joint lists (for a single island) to go into
-------------- next part --------------
--- /Users/joachima/Desktop/Ode/ode-040520/ode/src/collision_trimesh_ray.cpp    Mon Apr 19 23:48:49 2004
+++ /Users/joachima/Desktop/Ode/oldode-040513/ode/src/collision_trimesh_ray.cpp Sun May 16 16:28:24 2004
@@ -64,12 +64,13 @@
 
        /* Intersect */
        Matrix4x4 amatrix;
-       Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix));
-
-       /* Retrieve data */
-       int TriCount = TriMesh->Faces.GetNbFaces();
+       int TriCount = 0;
+       if (Collider.Collide(WorldRay, TriMesh->Data->BVTree, &MakeMatrix(TLPosition, TLRotation, amatrix)))
+               TriCount = TriMesh->Faces.GetNbFaces();
+
+       if (TriCount == 0)
+               return 0;
 
-       if (TriCount != 0){
                const CollisionFace* Faces = TriMesh->Faces.GetFaces();
 
                int OutTriCount = 0;
@@ -118,6 +119,4 @@
                        }
                }
                return OutTriCount;
-       }
-       else return 0;
 }
-------------- next part --------------
--- /Users/joachima/Desktop/Ode/ode-040520/ode/src/collision_std.cpp    Mon Apr 19 20:42:11 2004
+++ /Users/joachima/Desktop/Ode/oldode-040513/ode/src/collision_std.cpp Sat May 15 16:11:40 2004
@@ -437,15 +437,20 @@
   return r->length;
 }
 
-
 void dGeomRaySet (dGeomID g, dReal px, dReal py, dReal pz,
                  dReal dx, dReal dy, dReal dz)
 {
   dUASSERT (g && g->type == dRayClass,"argument not a ray");
-  dGeomSetPosition (g,px,py,pz);
-  dMatrix3 R;
-  dRFromZAxis (R,dx,dy,dz);
-  dGeomSetRotation (g,R);
+  dReal* rot = g->R;
+  dReal* pos = g->pos;
+  pos[0] = px;
+  pos[1] = py;
+  pos[2] = pz;
+  
+  rot[0*4+2] = dx;
+  rot[1*4+2] = dy;
+  rot[2*4+2] = dz;
+  dGeomMoved (g);
 }