From 3bbfe56d58340871dda528768916660dfb6912a8 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 10 Jun 2016 18:19:58 +0200
Subject: [PATCH] Minor optimizations (mostly noalias)
---
src/BVH/BV_fitter.cpp | 7 +++----
src/narrowphase/gjk.cpp | 8 ++++----
src/narrowphase/narrowphase.cpp | 6 +++---
src/shape/geometric_shapes_utility.cpp | 12 ++++++------
4 files changed, 16 insertions(+), 17 deletions(-)
diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp
index a303d8a5..419c38eb 100644
--- a/src/BVH/BV_fitter.cpp
+++ b/src/BVH/BV_fitter.cpp
@@ -156,11 +156,10 @@ void fit2(Vec3f* ps, RSS& bv)
{
const Vec3f& p1 = ps[0];
const Vec3f& p2 = ps[1];
- Vec3f p1p2 = p1 - p2;
- FCL_REAL len_p1p2 = p1p2.norm();
- p1p2.normalize();
+ bv.axes.col(0).noalias() = p1 - p2;
+ FCL_REAL len_p1p2 = bv.axes.col(0).norm();
+ bv.axes.col(0) /= len_p1p2;
- bv.axes.col(0) = p1p2;
generateCoordinateSystem(bv.axes.col(0), bv.axes.col(1), bv.axes.col(2));
bv.l[0] = len_p1p2;
bv.l[1] = 0;
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index fc65947a..1a705c61 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -305,14 +305,14 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
void GJK::getSupport(const Vec3f& d, SimplexV& sv) const
{
- sv.d = d.normalized();
- sv.w = shape.support(sv.d);
+ sv.d.noalias() = d.normalized();
+ sv.w.noalias() = shape.support(sv.d);
}
void GJK::getSupport(const Vec3f& d, const Vec3f& v, SimplexV& sv) const
{
- sv.d = d.normalized();
- sv.w = shape.support(sv.d, v);
+ sv.d.noalias() = d.normalized();
+ sv.w.noalias() = shape.support(sv.d, v);
}
void GJK::removeVertex(Simplex& simplex)
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index befa6090..662f7e19 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -89,7 +89,7 @@ bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1,
if (distance > 0)
return false;
- Vec3f normal = diff.normalized() * - FCL_REAL(1);
+ Vec3f normal (-diff.normalized());
if (distance < 0 && penetration_depth)
*penetration_depth = -distance;
@@ -799,8 +799,8 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
Vec3f pp (R1.transpose() * p); // get pp = p relative to body 1
// get side lengths / 2
- Vec3f A = side1 * 0.5;
- Vec3f B = side2 * 0.5;
+ Vec3f A (side1 * 0.5);
+ Vec3f B (side2 * 0.5);
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
Matrix3f R (R1.transpose() * R2);
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index 97d0a8d5..84a30982 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -411,7 +411,7 @@ void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv)
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- bv.To = T;
+ bv.To.noalias() = T;
bv.axes.noalias() = R;
bv.extent = s.side * (FCL_REAL)0.5;
}
@@ -421,7 +421,7 @@ void computeBV<OBB, Sphere>(const Sphere& s, const Transform3f& tf, OBB& bv)
{
const Vec3f& T = tf.getTranslation();
- bv.To = T;
+ bv.To.noalias() = T;
bv.axes.setIdentity();
bv.extent.setConstant(s.radius);
}
@@ -432,7 +432,7 @@ void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv)
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- bv.To = T;
+ bv.To.noalias() = T;
bv.axes.noalias() = R;
bv.extent << s.radius, s.radius, s.lz / 2 + s.radius;
}
@@ -443,7 +443,7 @@ void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv)
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- bv.To = T;
+ bv.To.noalias() = T;
bv.axes.noalias() = R;
bv.extent << s.radius, s.radius, s.lz / 2;
}
@@ -454,7 +454,7 @@ void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv)
const Matrix3f& R = tf.getRotation();
const Vec3f& T = tf.getTranslation();
- bv.To = T;
+ bv.To.noalias() = T;
bv.axes.noalias() = R;
bv.extent << s.radius, s.radius, s.lz / 2;
}
@@ -467,7 +467,7 @@ void computeBV<OBB, Convex>(const Convex& s, const Transform3f& tf, OBB& bv)
fit(s.points, s.num_points, bv);
- bv.axes = R * bv.axes;
+ bv.axes.applyOnTheLeft(R);
bv.To = R * bv.To + T;
}
--
GitLab