Commit 67635545 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Update Quaternion3f API

parent 405a4b83
......@@ -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];
};
......
......@@ -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_));
......
......@@ -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;
......
......@@ -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)
......
......@@ -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;
......
......@@ -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
......
......@@ -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);
......
......@@ -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);
......
......@@ -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;
}
}
......@@ -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
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment