From c5f64c7b4bcaadeaba9f1e52c669fe00d5721c96 Mon Sep 17 00:00:00 2001
From: Justin Carpentier <justin.carpentier@inria.fr>
Date: Fri, 15 Dec 2023 21:27:54 +0100
Subject: [PATCH] core: enhance quality for encompassing AABB in computeAABB
---
include/hpp/fcl/collision_object.h | 16 ++++++++++++----
1 file changed, 12 insertions(+), 4 deletions(-)
diff --git a/include/hpp/fcl/collision_object.h b/include/hpp/fcl/collision_object.h
index 7403f923..a8195f86 100644
--- a/include/hpp/fcl/collision_object.h
+++ b/include/hpp/fcl/collision_object.h
@@ -260,10 +260,18 @@ class HPP_FCL_DLLAPI CollisionObject {
if (t.getRotation().isIdentity()) {
aabb = translate(cgeom->aabb_local, t.getTranslation());
} else {
- Vec3f center(t.transform(cgeom->aabb_center));
- Vec3f delta(Vec3f::Constant(cgeom->aabb_radius));
- aabb.min_ = center - delta;
- aabb.max_ = center + delta;
+ aabb.min_ = aabb.max_ = t.getTranslation();
+
+ Vec3f min_world, max_world;
+ for (int k = 0; k < 3; ++k) {
+ min_world.array() = t.getRotation().row(k).array() *
+ cgeom->aabb_local.min_.transpose().array();
+ max_world.array() = t.getRotation().row(k).array() *
+ cgeom->aabb_local.max_.transpose().array();
+
+ aabb.min_[k] += min_world.array().min(max_world.array()).sum();
+ aabb.max_[k] += min_world.array().max(max_world.array()).sum();
+ }
}
}
--
GitLab