Merge pull request #264 from nmansard/topic/worldGeoms

[Minor] avoid re-placing world geoms.

src/algorithm/geometry.hxx

@@ -43,7 +43,8 @@ namespace se3
     for (GeometryData::GeomIndex i=0; i < (GeometryData::GeomIndex) data_geom.model_geom.ngeoms; ++i)
     {
       const Model::JointIndex & parent = model_geom.geometryObjects[i].parent;
-      data_geom.oMg[i] =  (data.oMi[parent] * model_geom.geometryObjects[i].placement);
+      if (parent>0) data_geom.oMg[i] =  (data.oMi[parent] * model_geom.geometryObjects[i].placement);
+      else          data_geom.oMg[i] =  model_geom.geometryObjects[i].placement;
       data_geom.oMg_fcl[i] =  toFclTransform3f(data_geom.oMg[i]);
     }
   }