Index: trunk/src/lib/collision_detection/lin_alg.h
===================================================================
--- trunk/src/lib/collision_detection/lin_alg.h	(revision 5431)
+++ trunk/src/lib/collision_detection/lin_alg.h	(revision 5449)
@@ -32,6 +32,6 @@
   Z = (float *) calloc(100, 32);
 
-  for(ip=1; ip<=N; ip++) {  //initialize V to identity matrix
-    for (iq=1; iq<=N; iq++)  V[ip][iq]=0;
+  for(ip=0; ip<3; ip++) {  //initialize V to identity matrix
+    for (iq=0; iq<3; iq++)  V[ip][iq]=0;
     V[ip][ip]=1;
   }
@@ -44,6 +44,6 @@
   for (i=1; i<=50; i++) {
     sm=0;
-    for (ip=1; ip<N; ip++)    //sum off-diagonal elements
-      for (iq=ip+1; iq<=N; iq++)
+    for (ip=0; ip<2; ip++)    //sum off-diagonal elements
+      for (iq=ip+1; iq<3; iq++)
         sm=sm+fabs(A[ip][iq]);
     if (sm==0)
@@ -53,10 +53,7 @@
       return;       //normal return
     }
-    if (i < 4)
       tresh=0.2*sm*sm;
-    else
-      tresh=0;
-    for (ip=1; ip<N; ip++) {
-      for (iq=ip+1; iq<=N; iq++) {
+    for (ip=0; ip<2; ip++) {
+      for (iq=ip+1; iq<3; iq++) {
         g=100*fabs(A[ip][iq]);
 // after 4 sweeps, skip the rotation if the off-diagonal element is small
@@ -81,5 +78,5 @@
           D[iq] += h;
           A[ip][iq]=0;
-          for (j=1; j<ip; j++) {
+          for (j=0; j<ip; j++) {
             g=A[j][ip];
             h=A[j][iq];
@@ -99,5 +96,5 @@
             A[iq][j] = h+s*(g-h*tau);
           }
-          for (j=1; j<=N; j++) {
+          for (j=0; j<3; j++) {
             g=V[j][ip];
             h=V[j][iq];
@@ -109,5 +106,5 @@
       } // main iq loop
     } // main ip loop
-    for (ip=1; ip<=N; ip++) {
+    for (ip=0; ip<3; ip++) {
       B[ip] += Z[ip];
       D[ip]=B[ip];
Index: trunk/src/lib/collision_detection/obb_tree_node.cc
===================================================================
--- trunk/src/lib/collision_detection/obb_tree_node.cc	(revision 5431)
+++ trunk/src/lib/collision_detection/obb_tree_node.cc	(revision 5449)
@@ -360,7 +360,15 @@
   Vector*              axis = new Vector[3];                //!< the references to the obb axis
 
-  OBBTreeNode::coMat[1][1] = box->covarianceMatrix[0][0]; OBBTreeNode::coMat[1][2] = box->covarianceMatrix[0][1]; OBBTreeNode::coMat[1][3] = box->covarianceMatrix[0][2];
-  OBBTreeNode::coMat[2][1] = box->covarianceMatrix[1][0]; OBBTreeNode::coMat[2][2] = box->covarianceMatrix[1][1]; OBBTreeNode::coMat[2][3] = box->covarianceMatrix[1][2];
-  OBBTreeNode::coMat[3][1] = box->covarianceMatrix[2][0]; OBBTreeNode::coMat[3][2] = box->covarianceMatrix[2][1]; OBBTreeNode::coMat[3][3] = box->covarianceMatrix[2][2];
+  OBBTreeNode::coMat[0][0] = box->covarianceMatrix[0][0];
+  OBBTreeNode::coMat[0][1] = box->covarianceMatrix[0][1];
+  OBBTreeNode::coMat[0][2] = box->covarianceMatrix[0][2];
+
+  OBBTreeNode::coMat[1][0] = box->covarianceMatrix[1][0];
+  OBBTreeNode::coMat[1][1] = box->covarianceMatrix[1][1];
+  OBBTreeNode::coMat[1][2] = box->covarianceMatrix[1][2];
+
+  OBBTreeNode::coMat[2][0] = box->covarianceMatrix[2][0];
+  OBBTreeNode::coMat[2][1] = box->covarianceMatrix[2][1];
+  OBBTreeNode::coMat[2][2] = box->covarianceMatrix[2][2];
 
 //   OBBTreeNode::coMat[0][0] = box->covarianceMatrix[0][0];
@@ -393,7 +401,7 @@
 //   }
 
-  axis[0].x = OBBTreeNode::eigvMat[1][1]; axis[0].y = OBBTreeNode::eigvMat[2][1]; axis[0].z = OBBTreeNode::eigvMat[3][1];
-  axis[1].x = OBBTreeNode::eigvMat[1][2]; axis[1].y = OBBTreeNode::eigvMat[2][2]; axis[1].z = OBBTreeNode::eigvMat[3][2];
-  axis[2].x = OBBTreeNode::eigvMat[1][3]; axis[2].y = OBBTreeNode::eigvMat[2][3]; axis[2].z = OBBTreeNode::eigvMat[3][3];
+  axis[0].x = OBBTreeNode::eigvMat[0][0]; axis[0].y = OBBTreeNode::eigvMat[1][0]; axis[0].z = OBBTreeNode::eigvMat[2][0];
+  axis[1].x = OBBTreeNode::eigvMat[0][1]; axis[1].y = OBBTreeNode::eigvMat[1][1]; axis[1].z = OBBTreeNode::eigvMat[2][1];
+  axis[2].x = OBBTreeNode::eigvMat[0][2]; axis[2].y = OBBTreeNode::eigvMat[1][2]; axis[2].z = OBBTreeNode::eigvMat[2][2];
   axis[0].normalize();
   axis[1].normalize();
