[Test] Fix octree construction

  Position of octree is defined by resolution only. cell center coordinates are
  of the form:
    ((kx+.5)*resolution, (ky+.5)*resolution, (kz+.5)*resolution), where
    kx, ky, kz are integers.
  This commit fixes the construction of the environment octree in order to
  respect this constraint.
  In test_fcl_octree, envOctree contains envMesh.