From 7d4395259a72a88e73200247c1093ee12080d640 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Mon, 13 Jun 2016 19:24:06 +0200
Subject: [PATCH] Avoid some copy
---
src/distance_capsule_capsule.cpp | 8 ++++----
src/shape/geometric_shapes_utility.cpp | 12 ++----------
2 files changed, 6 insertions(+), 14 deletions(-)
diff --git a/src/distance_capsule_capsule.cpp b/src/distance_capsule_capsule.cpp
index 00ee5b9b..457e565d 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 e3843373..19e59cbe 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;
}
--
GitLab