From 9f2429b5076e4a3bdd0b62c153d7992519499e0e Mon Sep 17 00:00:00 2001
From: Martin Pecka <peci1@seznam.cz>
Date: Tue, 18 Jun 2019 15:46:07 +0200
Subject: [PATCH] More efficient point transformation.

The previous implementation called SVD which is unnecessary in case of Isometires...
---
 src/bodies.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/bodies.cpp b/src/bodies.cpp
index f4501427..36dabb68 100644
--- 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)