More efficient point transformation.

The previous implementation called SVD which is unnecessary in case of Isometires...
moveit · Aug 9, 2019 · 9f2429b · 9f2429b
1 parent b6c5e98
commit 9f2429b
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/src/bodies.cpp b/src/bodies.cpp
@@ -630,8 +630,9 @@ bool bodies::Box::intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vect
   // Brian Smits. Efficient bounding box intersection. Ray tracing news 15(1), 2002
   float tmin, tmax, tymin, tymax, tzmin, tzmax;
   float divx, divy, divz;
-  const Eigen::Vector3d o(pose_.rotation().inverse() * (origin - center_) + center_);
-  const Eigen::Vector3d d(pose_.rotation().inverse() * dir);
+  const Eigen::Matrix3d invRot(box->pose_.linear().transpose());
+  const Eigen::Vector3d o(invRot * (origin - box->center_) + box->center_);
+  const Eigen::Vector3d d(invRot * dir);
 
   divx = 1 / d.x();
   if (divx >= 0)