diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node-inl.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node-inl.h index 6c09fe80e..44ddde5fe 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node-inl.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node-inl.h @@ -621,7 +621,7 @@ bool meshConservativeAdvancementOrientedNodeCanStop( // n is in local frame of c1, so we need to turn n into the global frame Vector3 n_transformed = getBVAxis(model1->getBV(c1).bv, 0) * n[0] + - getBVAxis(model1->getBV(c1).bv, 1) * n[2] + // TODO(JS): not n[1]? + getBVAxis(model1->getBV(c1).bv, 1) * n[1] + getBVAxis(model1->getBV(c1).bv, 2) * n[2]; Quaternion R0; motion1->getCurrentRotation(R0);