From 67635545e01a3d1cc3bff3e1389fe930d16ebd8e Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 10 Jun 2016 20:30:03 +0200
Subject: [PATCH] Update Quaternion3f API
---
include/hpp/fcl/math/transform.h | 40 ++++++++++++-------------
src/math/transform.cpp | 47 ++----------------------------
src/narrowphase/gjk_libccd.cpp | 4 +--
test/CMakeLists.txt | 8 ++---
test/test_fcl_box_box_distance.cpp | 11 +++----
test/test_fcl_capsule_box_1.cpp | 4 ++-
test/test_fcl_capsule_box_2.cpp | 4 ++-
test/test_fcl_capsule_capsule.cpp | 4 ++-
test/test_fcl_utility.cpp | 10 +++++++
test/test_fcl_utility.h | 2 ++
10 files changed, 53 insertions(+), 81 deletions(-)
diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h
index ebb19e16..2178a2a1 100644
--- a/include/hpp/fcl/math/transform.h
+++ b/include/hpp/fcl/math/transform.h
@@ -58,14 +58,6 @@ public:
data[3] = 0;
}
- Quaternion3f(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d)
- {
- data[0] = a;
- data[1] = b;
- data[2] = c;
- data[3] = d;
- }
-
/// @brief Whether the rotation is identity
bool isIdentity() const
{
@@ -127,20 +119,6 @@ public:
/// @brief rotate a vector
Vec3f transform(const Vec3f& v) const;
- inline const FCL_REAL& getW() const { return data[0]; }
- inline const FCL_REAL& getX() const { return data[1]; }
- inline const FCL_REAL& getY() const { return data[2]; }
- inline const FCL_REAL& getZ() const { return data[3]; }
-
- inline FCL_REAL& getW() { return data[0]; }
- inline FCL_REAL& getX() { return data[1]; }
- inline FCL_REAL& getY() { return data[2]; }
- inline FCL_REAL& getZ() { return data[3]; }
-
- Vec3f getColumn(std::size_t i) const;
-
- Vec3f getRow(std::size_t i) const;
-
bool operator == (const Quaternion3f& other) const
{
for(std::size_t i = 0; i < 4; ++i)
@@ -161,8 +139,26 @@ public:
return data[i];
}
+ inline FCL_REAL& w() { return data[0]; }
+ inline FCL_REAL& x() { return data[1]; }
+ inline FCL_REAL& y() { return data[2]; }
+ inline FCL_REAL& z() { return data[3]; }
+
+ inline const FCL_REAL& w() const { return data[0]; }
+ inline const FCL_REAL& x() const { return data[1]; }
+ inline const FCL_REAL& y() const { return data[2]; }
+ inline const FCL_REAL& z() const { return data[3]; }
+
private:
+ Quaternion3f(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d)
+ {
+ data[0] = a;
+ data[1] = b;
+ data[2] = c;
+ data[3] = d;
+ }
+
FCL_REAL data[4];
};
diff --git a/src/math/transform.cpp b/src/math/transform.cpp
index 279e0c50..f2163c40 100644
--- a/src/math/transform.cpp
+++ b/src/math/transform.cpp
@@ -316,8 +316,8 @@ Quaternion3f& Quaternion3f::inverse()
Vec3f Quaternion3f::transform(const Vec3f& v) const
{
- Vec3f u(getX(), getY(), getZ());
- double s = getW();
+ Vec3f u(x(), y(), z());
+ double s = w();
Vec3f vprime = 2*u.dot(v)*u + (s*s - u.dot(u))*v + 2*s*u.cross(v);
return vprime;
}
@@ -366,49 +366,6 @@ void Quaternion3f::toEuler(FCL_REAL& a, FCL_REAL& b, FCL_REAL& c) const
}
-Vec3f Quaternion3f::getColumn(std::size_t i) const
-{
- switch(i)
- {
- case 0:
- return Vec3f(data[0] * data[0] + data[1] * data[1] - data[2] * data[2] - data[3] * data[3],
- 2 * (- data[0] * data[3] + data[1] * data[2]),
- 2 * (data[1] * data[3] + data[0] * data[2]));
- case 1:
- return Vec3f(2 * (data[1] * data[2] + data[0] * data[3]),
- data[0] * data[0] - data[1] * data[1] + data[2] * data[2] - data[3] * data[3],
- 2 * (data[2] * data[3] - data[0] * data[1]));
- case 2:
- return Vec3f(2 * (data[1] * data[3] - data[0] * data[2]),
- 2 * (data[2] * data[3] + data[0] * data[1]),
- data[0] * data[0] - data[1] * data[1] - data[2] * data[2] + data[3] * data[3]);
- default:
- return Vec3f();
- }
-}
-
-Vec3f Quaternion3f::getRow(std::size_t i) const
-{
- switch(i)
- {
- case 0:
- return Vec3f(data[0] * data[0] + data[1] * data[1] - data[2] * data[2] - data[3] * data[3],
- 2 * (data[0] * data[3] + data[1] * data[2]),
- 2 * (data[1] * data[3] - data[0] * data[2]));
- case 1:
- return Vec3f(2 * (data[1] * data[2] - data[0] * data[3]),
- data[0] * data[0] - data[1] * data[1] + data[2] * data[2] - data[3] * data[3],
- 2 * (data[2] * data[3] + data[0] * data[1]));
- case 2:
- return Vec3f(2 * (data[1] * data[3] + data[0] * data[2]),
- 2 * (data[2] * data[3] - data[0] * data[1]),
- data[0] * data[0] - data[1] * data[1] - data[2] * data[2] + data[3] * data[3]);
- default:
- return Vec3f();
- }
-}
-
-
const Matrix3f& Transform3f::getRotationInternal() const
{
boost::mutex::scoped_lock slock(const_cast<boost::mutex&>(lock_));
diff --git a/src/narrowphase/gjk_libccd.cpp b/src/narrowphase/gjk_libccd.cpp
index caa207ba..b7bf46d9 100644
--- a/src/narrowphase/gjk_libccd.cpp
+++ b/src/narrowphase/gjk_libccd.cpp
@@ -509,7 +509,7 @@ static void shapeToGJK(const ShapeBase& s, const Transform3f& tf, ccd_obj_t* o)
const Quaternion3f& q = tf.getQuatRotation();
const Vec3f& T = tf.getTranslation();
ccdVec3Set(&o->pos, T[0], T[1], T[2]);
- ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
+ ccdQuatSet(&o->rot, q.x(), q.y(), q.z(), q.w());
ccdQuatInvert2(&o->rot_inv, &o->rot);
}
@@ -1016,7 +1016,7 @@ void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, cons
const Quaternion3f& q = tf.getQuatRotation();
const Vec3f& T = tf.getTranslation();
ccdVec3Set(&o->pos, T[0], T[1], T[2]);
- ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
+ ccdQuatSet(&o->rot, q.x(), q.y(), q.z(), q.w());
ccdQuatInvert2(&o->rot_inv, &o->rot);
return o;
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index d03acad6..1520f173 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -32,11 +32,11 @@ add_fcl_test(test_fcl_frontlist test_fcl_frontlist.cpp test_fcl_utility.cpp)
#add_fcl_test(test_fcl_math test_fcl_math.cpp test_fcl_utility.cpp)
add_fcl_test(test_fcl_sphere_capsule test_fcl_sphere_capsule.cpp)
-add_fcl_test(test_fcl_capsule_capsule test_fcl_capsule_capsule.cpp)
-add_fcl_test(test_fcl_box_box_distance test_fcl_box_box_distance.cpp)
+add_fcl_test(test_fcl_capsule_capsule test_fcl_capsule_capsule.cpp test_fcl_utility.cpp)
+add_fcl_test(test_fcl_box_box_distance test_fcl_box_box_distance.cpp test_fcl_utility.cpp)
add_fcl_test(test_fcl_simple test_fcl_simple.cpp)
-add_fcl_test(test_fcl_capsule_box_1 test_fcl_capsule_box_1.cpp)
-add_fcl_test(test_fcl_capsule_box_2 test_fcl_capsule_box_2.cpp)
+add_fcl_test(test_fcl_capsule_box_1 test_fcl_capsule_box_1.cpp test_fcl_utility.cpp)
+add_fcl_test(test_fcl_capsule_box_2 test_fcl_capsule_box_2.cpp test_fcl_utility.cpp)
#add_fcl_test(test_fcl_obb test_fcl_obb.cpp)
add_fcl_test(test_fcl_bvh_models test_fcl_bvh_models.cpp test_fcl_utility.cpp)
diff --git a/test/test_fcl_box_box_distance.cpp b/test/test_fcl_box_box_distance.cpp
index 5fd1bea4..c9dd9c45 100644
--- a/test/test_fcl_box_box_distance.cpp
+++ b/test/test_fcl_box_box_distance.cpp
@@ -46,10 +46,11 @@
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/shape/geometric_shapes.h>
+#include "test_fcl_utility.h"
+
typedef boost::shared_ptr <fcl::CollisionGeometry> CollisionGeometryPtr_t;
using fcl::Transform3f;
-using fcl::Quaternion3f;
using fcl::Vec3f;
using fcl::CollisionObject;
using fcl::DistanceResult;
@@ -104,7 +105,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_2)
CollisionGeometryPtr_t s2 (new fcl::Box (2, 2, 2));
static double pi = M_PI;
Transform3f tf1;
- Transform3f tf2 (Quaternion3f (cos (pi/8), sin(pi/8)/sqrt(3),
+ Transform3f tf2 (fcl::makeQuat (cos (pi/8), sin(pi/8)/sqrt(3),
sin(pi/8)/sqrt(3), sin(pi/8)/sqrt(3)),
Vec3f(0, 0, 10));
@@ -144,9 +145,9 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3)
CollisionGeometryPtr_t s1 (new fcl::Box (1, 1, 1));
CollisionGeometryPtr_t s2 (new fcl::Box (1, 1, 1));
static double pi = M_PI;
- Transform3f tf1 (Quaternion3f (cos (pi/8), 0, 0, sin (pi/8)),
+ Transform3f tf1 (fcl::makeQuat (cos (pi/8), 0, 0, sin (pi/8)),
Vec3f (-2, 1, .5));
- Transform3f tf2 (Quaternion3f (cos (pi/8), 0, sin(pi/8),0),
+ Transform3f tf2 (fcl::makeQuat (cos (pi/8), 0, sin(pi/8),0),
Vec3f (2, .5, .5));
CollisionObject o1 (s1, tf1);
@@ -182,7 +183,7 @@ BOOST_AUTO_TEST_CASE(distance_box_box_3)
BOOST_CHECK_CLOSE (p2 [2], p2Ref [2], 1e-4);
// Apply the same global transform to both objects and recompute
- Transform3f tf3 (Quaternion3f (0.435952844074,-0.718287018243,
+ Transform3f tf3 (fcl::makeQuat (0.435952844074,-0.718287018243,
0.310622451066, 0.444435113443),
Vec3f (4, 5, 6));
tf1 = tf3*tf1;
diff --git a/test/test_fcl_capsule_box_1.cpp b/test/test_fcl_capsule_box_1.cpp
index 16159ed1..acf11158 100644
--- a/test/test_fcl_capsule_box_1.cpp
+++ b/test/test_fcl_capsule_box_1.cpp
@@ -46,6 +46,8 @@
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/shape/geometric_shapes.h>
+#include "test_fcl_utility.h"
+
BOOST_AUTO_TEST_CASE(distance_capsule_box)
{
typedef boost::shared_ptr <fcl::CollisionGeometry> CollisionGeometryPtr_t;
@@ -96,7 +98,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
// Rotate capsule around y axis by pi/2 and move it behind box
tf1.setTranslation (fcl::Vec3f (-10., 0., 0.));
- tf1.setQuatRotation (fcl::Quaternion3f (sqrt(2)/2, 0, sqrt(2)/2, 0));
+ tf1.setQuatRotation (fcl::makeQuat (sqrt(2)/2, 0, sqrt(2)/2, 0));
capsule.setTransform (tf1);
// test distance
diff --git a/test/test_fcl_capsule_box_2.cpp b/test/test_fcl_capsule_box_2.cpp
index 81e355bc..b99dc5f0 100644
--- a/test/test_fcl_capsule_box_2.cpp
+++ b/test/test_fcl_capsule_box_2.cpp
@@ -39,6 +39,8 @@
#define CHECK_CLOSE_TO_0(x, eps) BOOST_CHECK_CLOSE ((x + 1.0), (1.0), (eps))
#include <boost/test/included/unit_test.hpp>
+#include "test_fcl_utility.h"
+
#include <cmath>
#include <hpp/fcl/distance.h>
#include <hpp/fcl/math/transform.h>
@@ -59,7 +61,7 @@ BOOST_AUTO_TEST_CASE(distance_capsule_box)
fcl::DistanceResult distanceResult;
// Rotate capsule around y axis by pi/2 and move it behind box
- fcl::Transform3f tf1 (fcl::Quaternion3f (sqrt(2)/2, 0, sqrt(2)/2, 0),
+ fcl::Transform3f tf1 (fcl::makeQuat (sqrt(2)/2, 0, sqrt(2)/2, 0),
fcl::Vec3f (-10., 0.8, 1.5));
fcl::Transform3f tf2;
fcl::CollisionObject capsule (capsuleGeometry, tf1);
diff --git a/test/test_fcl_capsule_capsule.cpp b/test/test_fcl_capsule_capsule.cpp
index 432e8ffe..f34e4c12 100644
--- a/test/test_fcl_capsule_capsule.cpp
+++ b/test/test_fcl_capsule_capsule.cpp
@@ -47,6 +47,8 @@
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/shape/geometric_shapes.h>
+#include "test_fcl_utility.h"
+
using namespace fcl;
typedef boost::shared_ptr <CollisionGeometry> CollisionGeometryPtr_t;
@@ -139,7 +141,7 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2)
CollisionGeometryPtr_t s2 (new Capsule (5, 10));
Transform3f tf1;
- Transform3f tf2 (Quaternion3f (sqrt (2)/2, 0, sqrt (2)/2, 0),
+ Transform3f tf2 (makeQuat (sqrt (2)/2, 0, sqrt (2)/2, 0),
Vec3f(0,0,25.1));
CollisionObject o1 (s1, tf1);
diff --git a/test/test_fcl_utility.cpp b/test/test_fcl_utility.cpp
index d01b6026..1dd723d6 100644
--- a/test/test_fcl_utility.cpp
+++ b/test/test_fcl_utility.cpp
@@ -454,4 +454,14 @@ std::string getGJKSolverName(GJKSolverType solver_type)
return std::string("invalid");
}
+Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z)
+{
+ Quaternion3f q;
+ q.w() = w;
+ q.x() = x;
+ q.y() = y;
+ q.z() = z;
+ return q;
+}
+
}
diff --git a/test/test_fcl_utility.h b/test/test_fcl_utility.h
index 65fd59ad..4385eab1 100644
--- a/test/test_fcl_utility.h
+++ b/test/test_fcl_utility.h
@@ -187,6 +187,8 @@ std::string getNodeTypeName(NODE_TYPE node_type);
std::string getGJKSolverName(GJKSolverType solver_type);
+Quaternion3f makeQuat(FCL_REAL w, FCL_REAL x, FCL_REAL y, FCL_REAL z);
+
}
#endif
--
GitLab