diff --git a/src/distance_capsule_capsule.cpp b/src/distance_capsule_capsule.cpp
index 00ee5b9b8610e98b6d2387f7a013f68a7fbd863c..457e565d3e79a749453e53978532ce1a2724f8a6 100644
--- a/src/distance_capsule_capsule.cpp
+++ b/src/distance_capsule_capsule.cpp
@@ -35,11 +35,11 @@ namespace fcl {
     const Capsule* c2 = static_cast <const Capsule*> (o2);
 
     // We assume that capsules are centered at the origin.
-    fcl::Vec3f center1 = tf1.getTranslation ();
-    fcl::Vec3f center2 = tf2.getTranslation ();
+    const fcl::Vec3f& center1 = tf1.getTranslation ();
+    const fcl::Vec3f& center2 = tf2.getTranslation ();
     // We assume that capsules are oriented along z-axis.
-    fcl::Vec3f direction1 = tf1.getRotation ().col (2);
-    fcl::Vec3f direction2 = tf2.getRotation ().col (2);
+    Matrix3f::ConstColXpr direction1 = tf1.getRotation ().col (2);
+    Matrix3f::ConstColXpr direction2 = tf2.getRotation ().col (2);
     FCL_REAL halfLength1 = 0.5*c1->lz;
     FCL_REAL halfLength2 = 0.5*c2->lz;
 
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index e38433730e7e080dd3b556be14d18f7f14ac550b..19e59cbe77edef159d56d475cf39089fcf674642 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -253,11 +253,7 @@ void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv)
   const Matrix3f& R = tf.getRotation();
   const Vec3f& T = tf.getTranslation();
 
-  FCL_REAL x_range = 0.5 * (fabs(R(0, 0) * s.side[0]) + fabs(R(0, 1) * s.side[1]) + fabs(R(0, 2) * s.side[2]));
-  FCL_REAL y_range = 0.5 * (fabs(R(1, 0) * s.side[0]) + fabs(R(1, 1) * s.side[1]) + fabs(R(1, 2) * s.side[2]));
-  FCL_REAL z_range = 0.5 * (fabs(R(2, 0) * s.side[0]) + fabs(R(2, 1) * s.side[1]) + fabs(R(2, 2) * s.side[2]));
-
-  Vec3f v_delta(x_range, y_range, z_range);
+  Vec3f v_delta (0.5 * R.cwiseAbs() * s.side);
   bv.max_ = T + v_delta;
   bv.min_ = T - v_delta;
 }
@@ -278,11 +274,7 @@ void computeBV<AABB, Capsule>(const Capsule& s, const Transform3f& tf, AABB& bv)
   const Matrix3f& R = tf.getRotation();
   const Vec3f& T = tf.getTranslation();
 
-  FCL_REAL x_range = 0.5 * fabs(R(0, 2) * s.lz) + s.radius;
-  FCL_REAL y_range = 0.5 * fabs(R(1, 2) * s.lz) + s.radius;
-  FCL_REAL z_range = 0.5 * fabs(R(2, 2) * s.lz) + s.radius;
-
-  Vec3f v_delta(x_range, y_range, z_range);
+  Vec3f v_delta(0.5 * (R.col(2)*s.lz).cwiseAbs() + Vec3f::Constant(s.radius));
   bv.max_ = T + v_delta;
   bv.min_ = T - v_delta;
 }