From 7f3e1fd41ad7685fc58c63279e39fd20dc211a41 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 10 Jun 2016 12:09:27 +0200
Subject: [PATCH] Use Eigen Matrix without FclMatrix
---
include/hpp/fcl/BV/AABB.h | 14 +-
include/hpp/fcl/BV/BV_node.h | 14 +-
include/hpp/fcl/BVH/BVH_model.h | 19 +-
include/hpp/fcl/BVH/BV_splitter.h | 2 +-
include/hpp/fcl/broadphase/broadphase_SaP.h | 4 +-
include/hpp/fcl/ccd/motion.h | 7 +-
include/hpp/fcl/collision_object.h | 20 +-
.../fcl/eigen/plugins/ccd/interval-vector.h | 7 -
include/hpp/fcl/eigen/taylor_operator.h | 4 +-
include/hpp/fcl/math/matrix_3f.h | 6 +-
include/hpp/fcl/math/tools.h | 268 ++++++++++++++++++
include/hpp/fcl/math/vec_3f.h | 7 +-
include/hpp/fcl/narrowphase/gjk.h | 4 +
include/hpp/fcl/narrowphase/narrowphase.h | 20 +-
include/hpp/fcl/shape/geometric_shapes.h | 48 ++--
src/BV/AABB.cpp | 4 +-
src/BV/OBB.cpp | 106 ++++---
src/BV/RSS.cpp | 46 +--
src/BVH/BVH_utility.cpp | 4 +-
src/BVH/BV_fitter.cpp | 16 +-
src/broadphase/broadphase_SSaP.cpp | 2 +-
src/broadphase/broadphase_SaP.cpp | 18 +-
src/broadphase/hierarchy_tree.cpp | 2 +-
src/ccd/interval_matrix.cpp | 12 +-
src/collision_node.cpp | 4 +-
src/intersect.cpp | 4 +-
src/math/transform.cpp | 3 +-
src/narrowphase/gjk.cpp | 18 +-
src/narrowphase/narrowphase.cpp | 66 ++---
src/shape/geometric_shapes_utility.cpp | 34 +--
test/test_fcl_geometric_shapes.cpp | 48 ++--
test/test_fcl_sphere_capsule.cpp | 14 +-
32 files changed, 563 insertions(+), 282 deletions(-)
create mode 100644 include/hpp/fcl/math/tools.h
diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h
index f71d1a0c..522c47f4 100644
--- a/include/hpp/fcl/BV/AABB.h
+++ b/include/hpp/fcl/BV/AABB.h
@@ -62,8 +62,8 @@ public:
}
/// @brief Creating an AABB with two endpoints a and b
- AABB(const Vec3f& a, const Vec3f&b) : min_(min(a, b)),
- max_(max(a, b))
+ AABB(const Vec3f& a, const Vec3f&b) : min_(a.cwiseMin(b)),
+ max_(a.cwiseMax(b))
{
}
@@ -74,8 +74,8 @@ public:
}
/// @brief Creating an AABB contains three points
- AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) : min_(min(min(a, b), c)),
- max_(max(max(a, b), c))
+ AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) : min_(a.cwiseMin(b).cwiseMin(c)),
+ max_(a.cwiseMax(b).cwiseMax(c))
{
}
@@ -124,8 +124,8 @@ public:
return false;
}
- overlap_part.min_ = max(min_, other.min_);
- overlap_part.max_ = min(max_, other.max_);
+ overlap_part.min_ = min_.cwiseMax(other.min_);
+ overlap_part.max_ = max_.cwiseMin(other.max_);
return true;
}
@@ -214,7 +214,7 @@ public:
/// @brief whether two AABB are equal
inline bool equal(const AABB& other) const
{
- return min_.equal(other.min_) && max_.equal(other.max_);
+ return isEqual(min_, other.min_) && isEqual(max_, other.max_);
}
/// @brief expand the half size of the AABB by delta, and keep the center unchanged.
diff --git a/include/hpp/fcl/BV/BV_node.h b/include/hpp/fcl/BV/BV_node.h
index 9d2f5cff..52a40baa 100644
--- a/include/hpp/fcl/BV/BV_node.h
+++ b/include/hpp/fcl/BV/BV_node.h
@@ -105,31 +105,31 @@ struct BVNode : public BVNodeBase
Vec3f getCenter() const { return bv.center(); }
/// @brief Access the orientation of the BV
- Matrix3f getOrientation() const { return Matrix3f::getIdentity(); }
+ Matrix3f getOrientation() const { return Matrix3f::Identity(); }
};
template<>
inline Matrix3f BVNode<OBB>::getOrientation() const
{
- return Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ return (Matrix3f() << bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
- bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]);
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]).finished();
}
template<>
inline Matrix3f BVNode<RSS>::getOrientation() const
{
- return Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ return (Matrix3f() << bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
- bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]);
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]).finished();
}
template<>
inline Matrix3f BVNode<OBBRSS>::getOrientation() const
{
- return Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
+ return (Matrix3f() << bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
- bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]);
+ bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]).finished();
}
diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h
index cb85a70a..5794850c 100644
--- a/include/hpp/fcl/BVH/BVH_model.h
+++ b/include/hpp/fcl/BVH/BVH_model.h
@@ -222,13 +222,12 @@ public:
Matrix3f computeMomentofInertia() const
{
- Matrix3f C(0, 0, 0,
- 0, 0, 0,
- 0, 0, 0);
+ Matrix3f C = Matrix3f::Zero();
- Matrix3f C_canonical(1/60.0, 1/120.0, 1/120.0,
- 1/120.0, 1/60.0, 1/120.0,
- 1/120.0, 1/120.0, 1/60.0);
+ Matrix3f C_canonical;
+ C_canonical << 1/60.0, 1/120.0, 1/120.0,
+ 1/120.0, 1/60.0, 1/120.0,
+ 1/120.0, 1/120.0, 1/60.0;
for(int i = 0; i < num_tris; ++i)
{
@@ -236,15 +235,11 @@ public:
const Vec3f& v1 = vertices[tri[0]];
const Vec3f& v2 = vertices[tri[1]];
const Vec3f& v3 = vertices[tri[2]];
- Matrix3f A(v1, v2, v3);
+ Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose();
C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
}
- FCL_REAL trace_C = C(0, 0) + C(1, 1) + C(2, 2);
-
- return Matrix3f(trace_C - C(0, 0), -C(0, 1), -C(0, 2),
- -C(1, 0), trace_C - C(1, 1), -C(1, 2),
- -C(2, 0), -C(2, 1), trace_C - C(2, 2));
+ return C.trace() * Matrix3f::Identity() - C;
}
public:
diff --git a/include/hpp/fcl/BVH/BV_splitter.h b/include/hpp/fcl/BVH/BV_splitter.h
index c10670fd..c8178c88 100644
--- a/include/hpp/fcl/BVH/BV_splitter.h
+++ b/include/hpp/fcl/BVH/BV_splitter.h
@@ -76,7 +76,7 @@ class BVSplitter : public BVSplitterBase<BV>
{
public:
- BVSplitter(SplitMethodType method) : split_method(method)
+ BVSplitter(SplitMethodType method) : split_vector(0,0,0), split_method(method)
{
}
diff --git a/include/hpp/fcl/broadphase/broadphase_SaP.h b/include/hpp/fcl/broadphase/broadphase_SaP.h
index 21b07bb6..b0b23076 100644
--- a/include/hpp/fcl/broadphase/broadphase_SaP.h
+++ b/include/hpp/fcl/broadphase/broadphase_SaP.h
@@ -164,13 +164,13 @@ protected:
else return aabb->cached.min_;
}
- inline Vec3f::U getVal(size_t i) const
+ inline Vec3f::Scalar getVal(size_t i) const
{
if(minmax) return aabb->cached.max_[i];
else return aabb->cached.min_[i];
}
- inline Vec3f::U& getVal(size_t i)
+ inline Vec3f::Scalar& getVal(size_t i)
{
if(minmax) return aabb->cached.max_[i];
else return aabb->cached.min_[i];
diff --git a/include/hpp/fcl/ccd/motion.h b/include/hpp/fcl/ccd/motion.h
index c9ddc388..6277abb6 100644
--- a/include/hpp/fcl/ccd/motion.h
+++ b/include/hpp/fcl/ccd/motion.h
@@ -166,7 +166,6 @@ public:
}
// set tm
- Matrix3f I(1, 0, 0, 0, 1, 0, 0, 0, 1);
// R(t) = R(t0) + R'(t0) (t-t0) + 1/2 R''(t0)(t-t0)^2 + 1 / 6 R'''(t0) (t-t0)^3 + 1 / 24 R''''(l)(t-t0)^4; t0 = 0.5
/// 1. compute M(1/2)
Vec3f Rt0 = (Rd[0] + Rd[1] * 23 + Rd[2] * 23 + Rd[3]) * (1 / 48.0);
@@ -182,7 +181,7 @@ public:
Matrix3f hatWt0;
hat(hatWt0, Wt0);
Matrix3f hatWt0_sqr = hatWt0 * hatWt0;
- Matrix3f Mt0 = I + hatWt0 * sintheta0 + hatWt0_sqr * (1 - costheta0);
+ Matrix3f Mt0 = Matrix3f::Identity() + hatWt0 * sintheta0 + hatWt0_sqr * (1 - costheta0);
/// 2. compute M'(1/2)
Vec3f dRt0 = (-Rd[0] - Rd[1] * 5 + Rd[2] * 5 + Rd[3]) * (1 / 8.0);
@@ -340,7 +339,7 @@ public:
TaylorModel sin_model(getTimeInterval());
generateTaylorModelForSinFunc(sin_model, angular_vel, 0);
- TMatrix3 delta_R = hat_axis * sin_model - hat_axis * hat_axis * (cos_model - 1) + Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1);
+ TMatrix3 delta_R = hat_axis * sin_model - hat_axis * hat_axis * (cos_model - 1) + Matrix3f::Identity();
TaylorModel a(getTimeInterval()), b(getTimeInterval()), c(getTimeInterval());
generateTaylorModelForLinearFunc(a, 0, linear_vel * axis[0]);
@@ -494,7 +493,7 @@ public:
TaylorModel sin_model(getTimeInterval());
generateTaylorModelForSinFunc(sin_model, angular_vel, 0);
- TMatrix3 delta_R = hat_angular_axis * sin_model - hat_angular_axis * hat_angular_axis * (cos_model - 1) + Matrix3f(1, 0, 0, 0, 1, 0, 0, 0, 1);
+ TMatrix3 delta_R = hat_angular_axis * sin_model - hat_angular_axis * hat_angular_axis * (cos_model - 1) + Matrix3f::Identity();
TaylorModel a(getTimeInterval()), b(getTimeInterval()), c(getTimeInterval());
generateTaylorModelForLinearFunc(a, 0, linear_vel[0]);
diff --git a/include/hpp/fcl/collision_object.h b/include/hpp/fcl/collision_object.h
index fb140008..ec9ecdce 100644
--- a/include/hpp/fcl/collision_object.h
+++ b/include/hpp/fcl/collision_object.h
@@ -134,15 +134,15 @@ public:
Vec3f com = computeCOM();
FCL_REAL V = computeVolume();
- return Matrix3f(C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
- C(0, 1) + V * com[0] * com[1],
- C(0, 2) + V * com[0] * com[2],
- C(1, 0) + V * com[1] * com[0],
- C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]),
- C(1, 2) + V * com[1] * com[2],
- C(2, 0) + V * com[2] * com[0],
- C(2, 1) + V * com[2] * com[1],
- C(2, 2) - V * (com[0] * com[0] + com[1] * com[1]));
+ return (Matrix3f() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
+ C(0, 1) + V * com[0] * com[1],
+ C(0, 2) + V * com[0] * com[2],
+ C(1, 0) + V * com[1] * com[0],
+ C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]),
+ C(1, 2) + V * com[1] * com[2],
+ C(2, 0) + V * com[2] * com[0],
+ C(2, 1) + V * com[2] * com[1],
+ C(2, 2) - V * (com[0] * com[0] + com[1] * com[1])).finished();
}
};
@@ -207,7 +207,7 @@ public:
else
{
Vec3f center = t.transform(cgeom->aabb_center);
- Vec3f delta(cgeom->aabb_radius);
+ Vec3f delta(Vec3f::Constant(cgeom->aabb_radius));
aabb.min_ = center - delta;
aabb.max_ = center + delta;
}
diff --git a/include/hpp/fcl/eigen/plugins/ccd/interval-vector.h b/include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
index 1675c876..9ec6217f 100644
--- a/include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
+++ b/include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
@@ -1,11 +1,4 @@
template <typename Derived>
-IVector3& operator=(const FclType<Derived>& other)
-{
- const Vec3f& tmp = other.fcl();
- setValue (tmp);
- return *this;
-}
-template <typename Derived>
IVector3& operator=(const Eigen::MatrixBase<Derived>& other)
{
const Vec3f& tmp (other);
diff --git a/include/hpp/fcl/eigen/taylor_operator.h b/include/hpp/fcl/eigen/taylor_operator.h
index fa02f2b7..1de50b31 100644
--- a/include/hpp/fcl/eigen/taylor_operator.h
+++ b/include/hpp/fcl/eigen/taylor_operator.h
@@ -14,9 +14,9 @@ template<> struct TaylorReturnType<1> { typedef TVector3 type; typedef Vec3f
template<> struct TaylorReturnType<3> { typedef TMatrix3 type; typedef Matrix3f eigen_type; };
template<typename Derived>
-typename TaylorReturnType<Derived::ColsAtCompileTime>::type operator * (const FclType<Derived>& v, const TaylorModel& a)
+typename TaylorReturnType<Derived::ColsAtCompileTime>::type operator * (const Eigen::MatrixBase<Derived>& v, const TaylorModel& a)
{
- const typename TaylorReturnType<Derived::ColsAtCompileTime>::eigen_type b = v.fcl();
+ const typename TaylorReturnType<Derived::ColsAtCompileTime>::eigen_type b = v.derived();
return b * a;
}
diff --git a/include/hpp/fcl/math/matrix_3f.h b/include/hpp/fcl/math/matrix_3f.h
index b04d816b..5226b537 100644
--- a/include/hpp/fcl/math/matrix_3f.h
+++ b/include/hpp/fcl/math/matrix_3f.h
@@ -48,13 +48,14 @@ namespace fcl
{
#if FCL_HAVE_EIGEN
- typedef Eigen::FclMatrix<FCL_REAL, 3> Matrix3f;
+ typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
#elif FCL_HAVE_SSE
typedef Matrix3fX<details::sse_meta_f12> Matrix3f;
#else
typedef Matrix3fX<details::Matrix3Data<FCL_REAL> > Matrix3f;
#endif
+#if ! FCL_HAVE_EIGEN
static inline std::ostream& operator << (std::ostream& o, const Matrix3f& m)
{
o << "[(" << m(0, 0) << " " << m(0, 1) << " " << m(0, 2) << ")("
@@ -62,6 +63,7 @@ static inline std::ostream& operator << (std::ostream& o, const Matrix3f& m)
<< m(2, 0) << " " << m(2, 1) << " " << m(2, 2) << ")]";
return o;
}
+#endif
@@ -73,7 +75,7 @@ public:
Matrix3f Sigma;
/// @brief Variations along the eign axes
- Matrix3f::U sigma[3];
+ Matrix3f::Scalar sigma[3];
/// @brief Eigen axes of the variation matrix
Vec3f axis[3];
diff --git a/include/hpp/fcl/math/tools.h b/include/hpp/fcl/math/tools.h
new file mode 100644
index 00000000..77cbfe62
--- /dev/null
+++ b/include/hpp/fcl/math/tools.h
@@ -0,0 +1,268 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Joseph Mirabel */
+
+#ifndef FCL_MATH_TOOLS_H
+#define FCL_MATH_TOOLS_H
+
+#include <hpp/fcl/deprecated.hh>
+#include <hpp/fcl/config.hh>
+#include <hpp/fcl/config-fcl.hh>
+
+#include <Eigen/Dense>
+#include <Eigen/Geometry>
+
+#include <cmath>
+#include <iostream>
+#include <limits>
+
+#define FCL_CCD_INTERVALVECTOR_PLUGIN <hpp/fcl/eigen/plugins/ccd/interval-vector.h>
+#define FCL_CCD_MATRIXVECTOR_PLUGIN <hpp/fcl/eigen/plugins/ccd/interval-matrix.h>
+
+namespace fcl
+{
+
+template<typename Derived>
+static inline typename Derived::Scalar triple(
+ const Eigen::MatrixBase<Derived>& x,
+ const Eigen::MatrixBase<Derived>& y,
+ const Eigen::MatrixBase<Derived>& z)
+{
+ return x.derived().dot(y.derived().cross(z.derived()));
+}
+
+
+/*
+template<typename Derived, typename OtherDerived>
+static inline const typename Eigen::BinaryReturnType<const Derived, const OtherDerived>::Min
+ min(const Eigen::MatrixBase<Derived>& x, const Eigen::MatrixBase<OtherDerived>& y)
+{
+ return typename Eigen::BinaryReturnType<const Derived, const OtherDerived>::Min (x.derived(), y.derived());
+}
+
+template<typename Derived, typename OtherDerived>
+static inline const typename Eigen::BinaryReturnType<const Derived, const OtherDerived>::Max
+ max(const Eigen::MatrixBase<Derived>& x, const Eigen::MatrixBase<OtherDerived>& y)
+{
+ return typename Eigen::BinaryReturnType<const Derived, const OtherDerived>::Max (x.derived(), y.derived());
+}
+
+template<typename Derived>
+static inline const typename Eigen::UnaryReturnType<const Derived>::Abs
+abs(const Eigen::MatrixBase<Derived>& x)
+{
+ return typename Eigen::UnaryReturnType<const Derived>::Abs (x.derived());
+} */
+
+template<typename Derived1, typename Derived2, typename Derived3>
+void generateCoordinateSystem(
+ const Eigen::MatrixBase<Derived1>& _w,
+ const Eigen::MatrixBase<Derived2>& _u,
+ const Eigen::MatrixBase<Derived3>& _v)
+{
+ typedef typename Derived1::Scalar T;
+
+ Eigen::MatrixBase<Derived1>& w = const_cast < Eigen::MatrixBase<Derived1>& > (_w);
+ Eigen::MatrixBase<Derived2>& u = const_cast < Eigen::MatrixBase<Derived2>& > (_u);
+ Eigen::MatrixBase<Derived3>& v = const_cast < Eigen::MatrixBase<Derived3>& > (_v);
+
+ T inv_length;
+ if(std::abs(w[0]) >= std::abs(w[1]))
+ {
+ inv_length = (T)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
+ u[0] = -w[2] * inv_length;
+ u[1] = (T)0;
+ u[2] = w[0] * inv_length;
+ v[0] = w[1] * u[2];
+ v[1] = w[2] * u[0] - w[0] * u[2];
+ v[2] = -w[1] * u[0];
+ }
+ else
+ {
+ inv_length = (T)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
+ u[0] = (T)0;
+ u[1] = w[2] * inv_length;
+ u[2] = -w[1] * inv_length;
+ v[0] = w[1] * u[2] - w[2] * u[1];
+ v[1] = -w[0] * u[2];
+ v[2] = w[0] * u[1];
+ }
+}
+
+/* ----- Start Matrices ------ */
+template<typename Derived, typename OtherDerived>
+void hat(const Eigen::MatrixBase<Derived>& mat, const Eigen::MatrixBase<OtherDerived>& vec) HPP_FCL_DEPRECATED;
+
+template<typename Derived, typename OtherDerived>
+void hat(const Eigen::MatrixBase<Derived>& mat, const Eigen::MatrixBase<OtherDerived>& vec)
+{
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3);
+ EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(OtherDerived, 3, 1);
+ const_cast< Eigen::MatrixBase<Derived>& >(mat) << 0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0;
+}
+
+template<typename Derived, typename OtherDerived>
+void relativeTransform(const Eigen::MatrixBase<Derived>& R1, const Eigen::MatrixBase<OtherDerived>& t1,
+ const Eigen::MatrixBase<Derived>& R2, const Eigen::MatrixBase<OtherDerived>& t2,
+ const Eigen::MatrixBase<Derived>& R , const Eigen::MatrixBase<OtherDerived>& t)
+{
+ const_cast< Eigen::MatrixBase<Derived>& >(R) = R1.transpose() * R2;
+ const_cast< Eigen::MatrixBase<OtherDerived>& >(t) = R1.transpose() * (t2 - t1);
+}
+
+/// @brief compute the eigen vector and eigen vector of a matrix. dout is the eigen values, vout is the eigen vectors
+template<typename Derived, typename Vector>
+void eigen(const Eigen::MatrixBase<Derived>& m, typename Derived::Scalar dout[3], Vector* vout)
+{
+ typedef typename Derived::Scalar Scalar;
+ Derived R(m.derived());
+ int n = 3;
+ int j, iq, ip, i;
+ Scalar tresh, theta, tau, t, sm, s, h, g, c;
+ int nrot;
+ Scalar b[3];
+ Scalar z[3];
+ Scalar v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+ Scalar d[3];
+
+ for(ip = 0; ip < n; ++ip)
+ {
+ b[ip] = d[ip] = R(ip, ip);
+ z[ip] = 0;
+ }
+
+ nrot = 0;
+
+ for(i = 0; i < 50; ++i)
+ {
+ sm = 0;
+ for(ip = 0; ip < n; ++ip)
+ for(iq = ip + 1; iq < n; ++iq)
+ sm += std::abs(R(ip, iq));
+ if(sm == 0.0)
+ {
+ vout[0] << v[0][0], v[0][1], v[0][2];
+ vout[1] << v[1][0], v[1][1], v[1][2];
+ vout[2] << v[2][0], v[2][1], v[2][2];
+ dout[0] = d[0]; dout[1] = d[1]; dout[2] = d[2];
+ return;
+ }
+
+ if(i < 3) tresh = 0.2 * sm / (n * n);
+ else tresh = 0.0;
+
+ for(ip = 0; ip < n; ++ip)
+ {
+ for(iq= ip + 1; iq < n; ++iq)
+ {
+ g = 100.0 * std::abs(R(ip, iq));
+ if(i > 3 &&
+ std::abs(d[ip]) + g == std::abs(d[ip]) &&
+ std::abs(d[iq]) + g == std::abs(d[iq]))
+ R(ip, iq) = 0.0;
+ else if(std::abs(R(ip, iq)) > tresh)
+ {
+ h = d[iq] - d[ip];
+ if(std::abs(h) + g == std::abs(h)) t = (R(ip, iq)) / h;
+ else
+ {
+ theta = 0.5 * h / (R(ip, iq));
+ t = 1.0 /(std::abs(theta) + std::sqrt(1.0 + theta * theta));
+ if(theta < 0.0) t = -t;
+ }
+ c = 1.0 / std::sqrt(1 + t * t);
+ s = t * c;
+ tau = s / (1.0 + c);
+ h = t * R(ip, iq);
+ z[ip] -= h;
+ z[iq] += h;
+ d[ip] -= h;
+ d[iq] += h;
+ R(ip, iq) = 0.0;
+ for(j = 0; j < ip; ++j) { g = R(j, ip); h = R(j, iq); R(j, ip) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
+ for(j = ip + 1; j < iq; ++j) { g = R(ip, j); h = R(j, iq); R(ip, j) = g - s * (h + g * tau); R(j, iq) = h + s * (g - h * tau); }
+ for(j = iq + 1; j < n; ++j) { g = R(ip, j); h = R(iq, j); R(ip, j) = g - s * (h + g * tau); R(iq, j) = h + s * (g - h * tau); }
+ for(j = 0; j < n; ++j) { g = v[j][ip]; h = v[j][iq]; v[j][ip] = g - s * (h + g * tau); v[j][iq] = h + s * (g - h * tau); }
+ nrot++;
+ }
+ }
+ }
+ for(ip = 0; ip < n; ++ip)
+ {
+ b[ip] += z[ip];
+ d[ip] = b[ip];
+ z[ip] = 0.0;
+ }
+ }
+
+ std::cerr << "eigen: too many iterations in Jacobi transform." << std::endl;
+
+ return;
+}
+
+template<typename Derived, typename OtherDerived>
+typename Derived::Scalar quadraticForm(const Eigen::MatrixBase<Derived>& R, const Eigen::MatrixBase<OtherDerived>& v)
+{
+ return v.dot(R * v);
+}
+
+template<typename Derived, typename OtherDerived>
+bool isEqual(const Eigen::MatrixBase<Derived>& lhs, const Eigen::MatrixBase<OtherDerived>& rhs, const FCL_REAL tol = std::numeric_limits<FCL_REAL>::epsilon()*100)
+{
+ return ((lhs - rhs).array().abs() < tol).all();
+}
+
+template <typename Derived>
+inline Derived& setEulerZYX(const Eigen::MatrixBase<Derived>& R, FCL_REAL eulerX, FCL_REAL eulerY, FCL_REAL eulerZ)
+{
+ const_cast< Eigen::MatrixBase<Derived>& >(R).noalias() = (
+ Eigen::AngleAxisd (eulerZ, Eigen::Vector3d::UnitZ()) *
+ Eigen::AngleAxisd (eulerY, Eigen::Vector3d::UnitY()) *
+ Eigen::AngleAxisd (eulerX, Eigen::Vector3d::UnitX())
+ ).toRotationMatrix();
+ return const_cast< Eigen::MatrixBase<Derived>& >(R).derived();
+}
+
+template <typename Derived>
+inline Derived& setEulerYPR(const Eigen::MatrixBase<Derived>& R, FCL_REAL yaw, FCL_REAL pitch, FCL_REAL roll)
+{
+ return setEulerZYX(R, roll, pitch, yaw);
+}
+
+}
+
+
+#endif
diff --git a/include/hpp/fcl/math/vec_3f.h b/include/hpp/fcl/math/vec_3f.h
index f81e6da1..1ecc46b1 100644
--- a/include/hpp/fcl/math/vec_3f.h
+++ b/include/hpp/fcl/math/vec_3f.h
@@ -42,7 +42,9 @@
#include <hpp/fcl/data_types.h>
#if FCL_HAVE_EIGEN
-# include <hpp/fcl/eigen/math_details.h>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <hpp/fcl/math/tools.h>
#elif FCL_HAVE_SSE
# include <hpp/fcl/simd/math_simd_details.h>
#else
@@ -50,7 +52,6 @@
#endif
#if FCL_HAVE_EIGEN
-# include <hpp/fcl/eigen/vec_3fx.h>
#else
# include <hpp/fcl/math/vec_3fx.h>
#endif
@@ -64,7 +65,7 @@ namespace fcl
{
#if FCL_HAVE_EIGEN
- typedef Eigen::FclMatrix<FCL_REAL, 1> Vec3f;
+ typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
#elif FCL_HAVE_SSE
typedef Vec3fX<details::sse_meta_f4> Vec3f;
#else
diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h
index bd592408..8ad919ae 100644
--- a/include/hpp/fcl/narrowphase/gjk.h
+++ b/include/hpp/fcl/narrowphase/gjk.h
@@ -130,6 +130,8 @@ struct GJK
Vec3f d;
/// @brieg support vector (i.e., the furthest point on the shape along the support direction)
Vec3f w;
+
+ SimplexV () : d(Vec3f::Zero()), w(Vec3f::Zero()) {}
};
struct Simplex
@@ -220,6 +222,8 @@ private:
SimplexF* l[2]; // the pre and post faces in the list
size_t e[3];
size_t pass;
+
+ SimplexF () : n(Vec3f::Zero()) {};
};
struct SimplexList
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index 482afc76..2929408d 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -453,7 +453,7 @@ struct GJKSolver_indep
details::MinkowskiDiff shape;
shape.shapes[0] = &s1;
shape.shapes[1] = &s2;
- shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
+ shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
shape.toshape0 = tf1.inverseTimes(tf2);
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
@@ -468,7 +468,7 @@ struct GJKSolver_indep
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
- Vec3f w0;
+ Vec3f w0 (Vec3f::Zero());
for(size_t i = 0; i < epa.result.rank; ++i)
{
w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
@@ -517,7 +517,7 @@ struct GJKSolver_indep
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
- Vec3f w0;
+ Vec3f w0 (Vec3f::Zero());
for(size_t i = 0; i < epa.result.rank; ++i)
{
w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
@@ -551,7 +551,7 @@ struct GJKSolver_indep
details::MinkowskiDiff shape;
shape.shapes[0] = &s;
shape.shapes[1] = &tri;
- shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
+ shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
shape.toshape0 = tf1.inverseTimes(tf2);
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
@@ -566,7 +566,7 @@ struct GJKSolver_indep
details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
if(epa_status != details::EPA::Failed)
{
- Vec3f w0;
+ Vec3f w0 (Vec3f::Zero());
for(size_t i = 0; i < epa.result.rank; ++i)
{
w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
@@ -598,7 +598,7 @@ struct GJKSolver_indep
details::MinkowskiDiff shape;
shape.shapes[0] = &s1;
shape.shapes[1] = &s2;
- shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
+ shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
shape.toshape0 = tf1.inverseTimes(tf2);
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
@@ -607,7 +607,7 @@ struct GJKSolver_indep
if(gjk_status == details::GJK::Valid)
{
- Vec3f w0, w1;
+ Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
FCL_REAL p = gjk.getSimplex()->p[i];
@@ -659,7 +659,7 @@ struct GJKSolver_indep
if(gjk_status == details::GJK::Valid)
{
- Vec3f w0, w1;
+ Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
FCL_REAL p = gjk.getSimplex()->p[i];
@@ -700,7 +700,7 @@ struct GJKSolver_indep
details::MinkowskiDiff shape;
shape.shapes[0] = &s;
shape.shapes[1] = &tri;
- shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
+ shape.toshape1 = tf2.getRotation().transpose() * tf1.getRotation();
shape.toshape0 = tf1.inverseTimes(tf2);
details::GJK gjk(gjk_max_iterations, gjk_tolerance);
@@ -709,7 +709,7 @@ struct GJKSolver_indep
if(gjk_status == details::GJK::Valid)
{
- Vec3f w0, w1;
+ Vec3f w0 (Vec3f::Zero()), w1 (Vec3f::Zero());
for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
{
FCL_REAL p = gjk.getSimplex()->p[i];
diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h
index 9c76ec11..7703561a 100644
--- a/include/hpp/fcl/shape/geometric_shapes.h
+++ b/include/hpp/fcl/shape/geometric_shapes.h
@@ -106,9 +106,9 @@ public:
FCL_REAL a2 = side[0] * side[0] * V;
FCL_REAL b2 = side[1] * side[1] * V;
FCL_REAL c2 = side[2] * side[2] * V;
- return Matrix3f((b2 + c2) / 12, 0, 0,
- 0, (a2 + c2) / 12, 0,
- 0, 0, (a2 + b2) / 12);
+ return (Matrix3f() << (b2 + c2) / 12, 0, 0,
+ 0, (a2 + c2) / 12, 0,
+ 0, 0, (a2 + b2) / 12).finished();
}
};
@@ -132,9 +132,7 @@ public:
Matrix3f computeMomentofInertia() const
{
FCL_REAL I = 0.4 * radius * radius * computeVolume();
- return Matrix3f(I, 0, 0,
- 0, I, 0,
- 0, 0, I);
+ return I * Matrix3f::Identity();
}
FCL_REAL computeVolume() const
@@ -176,9 +174,9 @@ public:
FCL_REAL ix = v_cyl * lz * lz / 12.0 + 0.25 * v_cyl * radius + 0.4 * v_sph * radius * radius + 0.25 * v_sph * lz * lz;
FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;
- return Matrix3f(ix, 0, 0,
- 0, ix, 0,
- 0, 0, iz);
+ return (Matrix3f() << ix, 0, 0,
+ 0, ix, 0,
+ 0, 0, iz).finished();
}
};
@@ -214,9 +212,9 @@ public:
FCL_REAL ix = V * (0.1 * lz * lz + 3 * radius * radius / 20);
FCL_REAL iz = 0.3 * V * radius * radius;
- return Matrix3f(ix, 0, 0,
- 0, ix, 0,
- 0, 0, iz);
+ return (Matrix3f() << ix, 0, 0,
+ 0, ix, 0,
+ 0, 0, iz).finished();
}
Vec3f computeCOM() const
@@ -256,9 +254,9 @@ public:
FCL_REAL V = computeVolume();
FCL_REAL ix = V * (3 * radius * radius + lz * lz) / 12;
FCL_REAL iz = V * radius * radius / 2;
- return Matrix3f(ix, 0, 0,
- 0, ix, 0,
- 0, 0, iz);
+ return (Matrix3f() << ix, 0, 0,
+ 0, ix, 0,
+ 0, 0, iz).finished();
}
};
@@ -342,13 +340,12 @@ public:
Matrix3f computeMomentofInertia() const
{
- Matrix3f C(0, 0, 0,
- 0, 0, 0,
- 0, 0, 0);
+ Matrix3f C = Matrix3f::Zero();
- Matrix3f C_canonical(1/60.0, 1/120.0, 1/120.0,
- 1/120.0, 1/60.0, 1/120.0,
- 1/120.0, 1/120.0, 1/60.0);
+ Matrix3f C_canonical;
+ C_canonical << 1/60.0, 1/120.0, 1/120.0,
+ 1/120.0, 1/60.0, 1/120.0,
+ 1/120.0, 1/120.0, 1/60.0;
int* points_in_poly = polygons;
int* index = polygons + 1;
@@ -369,7 +366,7 @@ public:
int e_second = index[(j+1)%*points_in_poly];
const Vec3f& v1 = points[e_first];
const Vec3f& v2 = points[e_second];
- Matrix3f A(v1, v2, v3); // this is A' in the original document
+ Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose(); // this is A' in the original document
C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
}
@@ -377,12 +374,7 @@ public:
index = points_in_poly + 1;
}
- FCL_REAL trace_C = C(0, 0) + C(1, 1) + C(2, 2);
-
- return Matrix3f(trace_C - C(0, 0), -C(0, 1), -C(0, 2),
- -C(1, 0), trace_C - C(1, 1), -C(1, 2),
- -C(2, 0), -C(2, 1), trace_C - C(2, 2));
-
+ return C.trace() * Matrix3f::Identity() - C;
}
Vec3f computeCOM() const
diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp
index 0f66b459..82f71598 100644
--- a/src/BV/AABB.cpp
+++ b/src/BV/AABB.cpp
@@ -43,8 +43,8 @@
namespace fcl
{
-AABB::AABB() : min_(std::numeric_limits<FCL_REAL>::max()),
- max_(-std::numeric_limits<FCL_REAL>::max())
+AABB::AABB() : min_(Vec3f::Constant(std::numeric_limits<FCL_REAL>::max())),
+ max_(Vec3f::Constant(-std::numeric_limits<FCL_REAL>::max()))
{
}
diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp
index d69b4cf6..bf045f80 100644
--- a/src/BV/OBB.cpp
+++ b/src/BV/OBB.cpp
@@ -75,7 +75,7 @@ inline OBB merge_largedist(const OBB& b1, const OBB& b2)
computeVertices(b2, vertex + 8);
Matrix3f M;
Vec3f E[3];
- Matrix3f::U s[3] = {0, 0, 0};
+ Matrix3f::Scalar s[3] = {0, 0, 0};
// obb axes
Vec3f& R0 = b.axis[0];
@@ -178,48 +178,57 @@ bool obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const Vec3f&
register FCL_REAL t, s;
const FCL_REAL reps = 1e-6;
- Matrix3f Bf = abs(B);
- Bf += reps;
+ Matrix3f Bf (B.array().abs() + reps);
+ // Bf += reps;
// if any of these tests are one-sided, then the polyhedra are disjoint
// A1 x A2 = A0
t = ((T[0] < 0.0) ? -T[0] : T[0]);
- if(t > (a[0] + Bf.dotX(b)))
+ // if(t > (a[0] + Bf.dotX(b)))
+ if(t > (a[0] + Bf.row(0).dot(b)))
return true;
// B1 x B2 = B0
- s = B.transposeDotX(T);
+ // s = B.transposeDotX(T);
+ s = B.col(0).dot(T);
t = ((s < 0.0) ? -s : s);
- if(t > (b[0] + Bf.transposeDotX(a)))
+ // if(t > (b[0] + Bf.transposeDotX(a)))
+ if(t > (b[0] + Bf.col(0).dot(a)))
return true;
// A2 x A0 = A1
t = ((T[1] < 0.0) ? -T[1] : T[1]);
- if(t > (a[1] + Bf.dotY(b)))
+ // if(t > (a[1] + Bf.dotY(b)))
+ if(t > (a[1] + Bf.row(1).dot(b)))
return true;
// A0 x A1 = A2
t =((T[2] < 0.0) ? -T[2] : T[2]);
- if(t > (a[2] + Bf.dotZ(b)))
+ // if(t > (a[2] + Bf.dotZ(b)))
+ if(t > (a[2] + Bf.row(2).dot(b)))
return true;
// B2 x B0 = B1
- s = B.transposeDotY(T);
+ // s = B.transposeDotY(T);
+ s = B.col(1).dot(T);
t = ((s < 0.0) ? -s : s);
- if(t > (b[1] + Bf.transposeDotY(a)))
+ // if(t > (b[1] + Bf.transposeDotY(a)))
+ if(t > (b[1] + Bf.col(1).dot(a)))
return true;
// B0 x B1 = B2
- s = B.transposeDotZ(T);
+ // s = B.transposeDotZ(T);
+ s = B.col(2).dot(T);
t = ((s < 0.0) ? -s : s);
- if(t > (b[2] + Bf.transposeDotZ(a)))
+ // if(t > (b[2] + Bf.transposeDotZ(a)))
+ if(t > (b[2] + Bf.col(2).dot(a)))
return true;
// A0 x B0
@@ -312,8 +321,9 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
b [0] + b [1] + b [2]);
FCL_REAL breakDistance2 = breakDistance * breakDistance;
- Matrix3f Bf = abs(B);
- Bf += reps;
+ // Matrix3f Bf = abs(B);
+ // Bf += reps;
+ Matrix3f Bf (B.array().abs() + reps);
squaredLowerBoundDistance = 0;
// if any of these tests are one-sided, then the polyhedra are disjoint
@@ -321,7 +331,7 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
// A1 x A2 = A0
t = ((T[0] < 0.0) ? -T[0] : T[0]);
- diff = t - (a[0] + Bf.dotX(b));
+ diff = t - (a[0] + Bf.row(0).dot(b));
if (diff > 0) {
squaredLowerBoundDistance += diff*diff;
}
@@ -329,7 +339,7 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
// A2 x A0 = A1
t = ((T[1] < 0.0) ? -T[1] : T[1]);
- diff = t - (a[1] + Bf.dotY(b));
+ diff = t - (a[1] + Bf.row(1).dot(b));
if (diff > 0) {
squaredLowerBoundDistance += diff*diff;
}
@@ -337,7 +347,7 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
// A0 x A1 = A2
t =((T[2] < 0.0) ? -T[2] : T[2]);
- diff = t - (a[2] + Bf.dotZ(b));
+ diff = t - (a[2] + Bf.row(2).dot(b));
if (diff > 0) {
squaredLowerBoundDistance += diff*diff;
}
@@ -346,28 +356,28 @@ bool obbDisjointAndLowerBoundDistance (const Matrix3f& B, const Vec3f& T,
return true;
// B1 x B2 = B0
- s = B.transposeDotX(T);
+ s = B.col(0).dot(T);
t = ((s < 0.0) ? -s : s);
- diff = t - (b[0] + Bf.transposeDotX(a));
+ diff = t - (b[0] + Bf.col(0).dot(a));
if (diff > 0) {
squaredLowerBoundDistance += diff*diff;
}
// B2 x B0 = B1
- s = B.transposeDotY(T);
+ s = B.col(1).dot(T);
t = ((s < 0.0) ? -s : s);
- diff = t - (b[1] + Bf.transposeDotY(a));
+ diff = t - (b[1] + Bf.col(1).dot(a));
if (diff > 0) {
squaredLowerBoundDistance += diff*diff;
}
// B0 x B1 = B2
- s = B.transposeDotZ(T);
+ s = B.col(2).dot(T);
t = ((s < 0.0) ? -s : s);
- diff = t - (b[2] + Bf.transposeDotZ(a));
+ diff = t - (b[2] + Bf.col(2).dot(a));
if (diff > 0) {
squaredLowerBoundDistance += diff*diff;
}
@@ -537,9 +547,10 @@ bool OBB::overlap(const OBB& other) const
/// First compute the rotation part, then translation part
Vec3f t = other.To - To; // T2 - T1
Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
- Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
- axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
- axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
+ Matrix3f R;
+ R << axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
+ axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
+ axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]);
return !obbDisjoint(R, T, extent, other.extent);
}
@@ -551,12 +562,13 @@ bool OBB::overlap(const OBB& other) const
/// First compute the rotation part, then translation part
Vec3f t = other.To - To; // T2 - T1
Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
- Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]),
- axis[0].dot(other.axis[2]),
- axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]),
- axis[1].dot(other.axis[2]),
- axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]),
- axis[2].dot(other.axis[2]));
+ Matrix3f R;
+ R << axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]),
+ axis[0].dot(other.axis[2]),
+ axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]),
+ axis[1].dot(other.axis[2]),
+ axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]),
+ axis[2].dot(other.axis[2]);
return !obbDisjointAndLowerBoundDistance
(R, T, extent, other.extent, sqrDistLowerBound);
@@ -619,13 +631,15 @@ FCL_REAL OBB::distance(const OBB& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2)
{
- Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
- R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
- R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
+ Matrix3f R0b2;
+ R0b2 << R0.row(0).dot(b2.axis[0]), R0.row(0).dot(b2.axis[1]), R0.row(0).dot(b2.axis[2]),
+ R0.row(1).dot(b2.axis[0]), R0.row(1).dot(b2.axis[1]), R0.row(1).dot(b2.axis[2]),
+ R0.row(2).dot(b2.axis[0]), R0.row(2).dot(b2.axis[1]), R0.row(2).dot(b2.axis[2]);
- Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
- R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
- R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
+ Matrix3f R;
+ R << R0b2.col(0).dot(b1.axis[0]), R0b2.col(1).dot(b1.axis[0]), R0b2.col(2).dot(b1.axis[0]),
+ R0b2.col(0).dot(b1.axis[1]), R0b2.col(1).dot(b1.axis[1]), R0b2.col(2).dot(b1.axis[1]),
+ R0b2.col(0).dot(b1.axis[2]), R0b2.col(1).dot(b1.axis[2]), R0b2.col(2).dot(b1.axis[2]);
Vec3f Ttemp = R0 * b2.To + T0 - b1.To;
Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
@@ -636,13 +650,15 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2)
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBB& b1, const OBB& b2,
FCL_REAL& sqrDistLowerBound)
{
- Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
- R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
- R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
-
- Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
- R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
- R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
+ Matrix3f R0b2;
+ R0b2 << R0.row(0).dot(b2.axis[0]), R0.row(0).dot(b2.axis[1]), R0.row(0).dot(b2.axis[2]),
+ R0.row(1).dot(b2.axis[0]), R0.row(1).dot(b2.axis[1]), R0.row(1).dot(b2.axis[2]),
+ R0.row(2).dot(b2.axis[0]), R0.row(2).dot(b2.axis[1]), R0.row(2).dot(b2.axis[2]);
+
+ Matrix3f R;
+ R << R0b2.col(0).dot(b1.axis[0]), R0b2.col(1).dot(b1.axis[0]), R0b2.col(2).dot(b1.axis[0]),
+ R0b2.col(0).dot(b1.axis[1]), R0b2.col(1).dot(b1.axis[1]), R0b2.col(2).dot(b1.axis[1]),
+ R0b2.col(0).dot(b1.axis[2]), R0b2.col(1).dot(b1.axis[2]), R0b2.col(2).dot(b1.axis[2]);
Vec3f Ttemp = R0 * b2.To + T0 - b1.To;
Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp
index 607d5cbc..435a1b2a 100644
--- a/src/BV/RSS.cpp
+++ b/src/BV/RSS.cpp
@@ -135,7 +135,7 @@ FCL_REAL rectDistance(const Matrix3f& Rab, Vec3f const& Tab, const FCL_REAL a[2]
bA0_dot_B1 = b[1] * A0_dot_B1;
bA1_dot_B1 = b[1] * A1_dot_B1;
- Vec3f Tba = Rab.transposeTimes(Tab);
+ Vec3f Tba (Rab.transpose() * Tab);
Vec3f S;
FCL_REAL t, u;
@@ -842,9 +842,10 @@ bool RSS::overlap(const RSS& other) const
Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2]));
/// Now compute R1'R2
- Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
+ Matrix3f R;
+ R << axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
- axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
+ axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]);
FCL_REAL dist = rectDistance(R, T, l, other.l);
return (dist <= (r + other.r));
@@ -854,15 +855,17 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
{
// ROb2 = R0 . b2
// where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
- Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
- R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
- R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
+ Matrix3f R0b2;
+ R0b2 << R0.row(0).dot(b2.axis[0]), R0.row(0).dot(b2.axis[1]), R0.row(0).dot(b2.axis[2]),
+ R0.row(1).dot(b2.axis[0]), R0.row(1).dot(b2.axis[1]), R0.row(1).dot(b2.axis[2]),
+ R0.row(2).dot(b2.axis[0]), R0.row(2).dot(b2.axis[1]), R0.row(2).dot(b2.axis[2]);
// (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
// R = b2^T RO^T b1
- Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
- R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
- R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
+ Matrix3f R;
+ R << R0b2.col(0).dot(b1.axis[0]), R0b2.col(1).dot(b1.axis[0]), R0b2.col(2).dot(b1.axis[0]),
+ R0b2.col(0).dot(b1.axis[1]), R0b2.col(1).dot(b1.axis[1]), R0b2.col(2).dot(b1.axis[1]),
+ R0b2.col(0).dot(b1.axis[2]), R0b2.col(1).dot(b1.axis[2]), R0b2.col(2).dot(b1.axis[2]);
Vec3f Ttemp = R0 * b2.Tr + T0 - b1.Tr;
Vec3f T(Ttemp.dot(b1.axis[0]), Ttemp.dot(b1.axis[1]), Ttemp.dot(b1.axis[2]));
@@ -1081,7 +1084,7 @@ RSS RSS::operator + (const RSS& other) const
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- Matrix3f::U s[3] = {0, 0, 0};
+ Matrix3f::Scalar s[3] = {0, 0, 0};
getCovariance(v, NULL, NULL, NULL, 16, M);
eigen(M, s, E);
@@ -1113,9 +1116,10 @@ FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
// First compute the rotation part, then translation part
Vec3f t = other.Tr - Tr; // T2 - T1
Vec3f T(t.dot(axis[0]), t.dot(axis[1]), t.dot(axis[2])); // R1'(T2-T1)
- Matrix3f R(axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
- axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
- axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]));
+ Matrix3f R;
+ R << axis[0].dot(other.axis[0]), axis[0].dot(other.axis[1]), axis[0].dot(other.axis[2]),
+ axis[1].dot(other.axis[0]), axis[1].dot(other.axis[1]), axis[1].dot(other.axis[2]),
+ axis[2].dot(other.axis[0]), axis[2].dot(other.axis[1]), axis[2].dot(other.axis[2]);
FCL_REAL dist = rectDistance(R, T, l, other.l, P, Q);
dist -= (r + other.r);
@@ -1124,13 +1128,15 @@ FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, Vec3f* P, Vec3f* Q)
{
- Matrix3f R0b2(R0.dotX(b2.axis[0]), R0.dotX(b2.axis[1]), R0.dotX(b2.axis[2]),
- R0.dotY(b2.axis[0]), R0.dotY(b2.axis[1]), R0.dotY(b2.axis[2]),
- R0.dotZ(b2.axis[0]), R0.dotZ(b2.axis[1]), R0.dotZ(b2.axis[2]));
-
- Matrix3f R(R0b2.transposeDotX(b1.axis[0]), R0b2.transposeDotY(b1.axis[0]), R0b2.transposeDotZ(b1.axis[0]),
- R0b2.transposeDotX(b1.axis[1]), R0b2.transposeDotY(b1.axis[1]), R0b2.transposeDotZ(b1.axis[1]),
- R0b2.transposeDotX(b1.axis[2]), R0b2.transposeDotY(b1.axis[2]), R0b2.transposeDotZ(b1.axis[2]));
+ Matrix3f R0b2;
+ R0b2 << R0.row(0).dot(b2.axis[0]), R0.row(0).dot(b2.axis[1]), R0.row(0).dot(b2.axis[2]),
+ R0.row(1).dot(b2.axis[0]), R0.row(1).dot(b2.axis[1]), R0.row(1).dot(b2.axis[2]),
+ R0.row(2).dot(b2.axis[0]), R0.row(2).dot(b2.axis[1]), R0.row(2).dot(b2.axis[2]);
+
+ Matrix3f R;
+ R << R0b2.col(0).dot(b1.axis[0]), R0b2.col(1).dot(b1.axis[0]), R0b2.col(2).dot(b1.axis[0]),
+ R0b2.col(0).dot(b1.axis[1]), R0b2.col(1).dot(b1.axis[1]), R0b2.col(2).dot(b1.axis[1]),
+ R0b2.col(0).dot(b1.axis[2]), R0b2.col(1).dot(b1.axis[2]), R0b2.col(2).dot(b1.axis[2]);
Vec3f Ttemp = R0 * b2.Tr + T0 - b1.Tr;
diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp
index c748d022..e06b1a6e 100644
--- a/src/BVH/BVH_utility.cpp
+++ b/src/BVH/BVH_utility.cpp
@@ -106,8 +106,8 @@ void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r = 1.0)
void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M)
{
- Vec3f S1;
- Vec3f S2[3];
+ Vec3f S1 (Vec3f::Zero());
+ Vec3f S2[3] = { Vec3f::Zero(), Vec3f::Zero(), Vec3f::Zero() };
if(ts)
{
diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp
index 6eda96e4..548bc4f4 100644
--- a/src/BVH/BV_fitter.cpp
+++ b/src/BVH/BV_fitter.cpp
@@ -49,7 +49,7 @@ static const double invCosA = 2.0 / sqrt(3.0);
static const double sinA = 0.5;
static const double cosA = sqrt(3.0) / 2.0;
-static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::U eigenS[3], Vec3f axis[3])
+static inline void axisFromEigen(Vec3f eigenV[3], Matrix3f::Scalar eigenS[3], Vec3f axis[3])
{
int min, mid, max;
if(eigenS[0] > eigenS[1]) { max = 0; min = 1; }
@@ -138,7 +138,7 @@ void fitn(Vec3f* ps, int n, OBB& bv)
{
Matrix3f M;
Vec3f E[3];
- Matrix3f::U s[3] = {0, 0, 0}; // three eigen values
+ Matrix3f::Scalar s[3] = {0, 0, 0}; // three eigen values
getCovariance(ps, NULL, NULL, NULL, n, M);
eigen(M, s, E);
@@ -224,7 +224,7 @@ void fitn(Vec3f* ps, int n, RSS& bv)
{
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- Matrix3f::U s[3] = {0, 0, 0};
+ Matrix3f::Scalar s[3] = {0, 0, 0};
getCovariance(ps, NULL, NULL, NULL, n, M);
eigen(M, s, E);
@@ -341,7 +341,7 @@ void fitn(Vec3f* ps, int n, kIOS& bv)
{
Matrix3f M;
Vec3f E[3];
- Matrix3f::U s[3] = {0, 0, 0}; // three eigen values;
+ Matrix3f::Scalar s[3] = {0, 0, 0}; // three eigen values;
getCovariance(ps, NULL, NULL, NULL, n, M);
eigen(M, s, E);
@@ -523,7 +523,7 @@ OBB BVFitter<OBB>::fit(unsigned int* primitive_indices, int num_primitives)
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- Matrix3f::U s[3]; // three eigen values
+ Matrix3f::Scalar s[3]; // three eigen values
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
@@ -541,7 +541,7 @@ OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices, int num_primitives
OBBRSS bv;
Matrix3f M;
Vec3f E[3];
- Matrix3f::U s[3];
+ Matrix3f::Scalar s[3];
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
@@ -572,7 +572,7 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- Matrix3f::U s[3]; // three eigen values
+ Matrix3f::Scalar s[3]; // three eigen values
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
axisFromEigen(E, s, bv.axis);
@@ -600,7 +600,7 @@ kIOS BVFitter<kIOS>::fit(unsigned int* primitive_indices, int num_primitives)
Matrix3f M; // row first matrix
Vec3f E[3]; // row first eigen-vectors
- Matrix3f::U s[3];
+ Matrix3f::Scalar s[3];
getCovariance(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, M);
eigen(M, s, E);
diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp
index a08bf5a9..4a7cee14 100644
--- a/src/broadphase/broadphase_SSaP.cpp
+++ b/src/broadphase/broadphase_SSaP.cpp
@@ -353,7 +353,7 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, Distance
}
else // need more loop
{
- if(dummy_vector.equal(obj->getAABB().max_))
+ if(isEqual(dummy_vector, obj->getAABB().max_))
dummy_vector = dummy_vector + delta;
else
dummy_vector = dummy_vector * 2 - obj->getAABB().max_;
diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp
index e60c0a35..0fb886ea 100644
--- a/src/broadphase/broadphase_SaP.cpp
+++ b/src/broadphase/broadphase_SaP.cpp
@@ -120,9 +120,9 @@ void SaPCollisionManager::registerObjects(const std::vector<CollisionObject*>& o
for(size_t coord = 0; coord < 3; ++coord)
{
std::sort(endpoints.begin(), endpoints.end(),
- boost::bind(std::less<Vec3f::U>(),
- boost::bind(static_cast<Vec3f::U (EndPoint::*)(size_t) const >(&EndPoint::getVal), _1, coord),
- boost::bind(static_cast<Vec3f::U (EndPoint::*)(size_t) const >(&EndPoint::getVal), _2, coord)));
+ boost::bind(std::less<Vec3f::Scalar>(),
+ boost::bind(static_cast<Vec3f::Scalar (EndPoint::*)(size_t) const >(&EndPoint::getVal), _1, coord),
+ boost::bind(static_cast<Vec3f::Scalar (EndPoint::*)(size_t) const >(&EndPoint::getVal), _2, coord)));
endpoints[0]->prev[coord] = NULL;
endpoints[0]->next[coord] = endpoints[1];
@@ -513,9 +513,9 @@ bool SaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionC
// compute stop_pos by binary search, this is cheaper than check it in while iteration linearly
std::vector<EndPoint*>::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
- boost::bind(std::less<Vec3f::U>(),
- boost::bind(static_cast<Vec3f::U (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
- boost::bind(static_cast<Vec3f::U (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
+ boost::bind(std::less<Vec3f::Scalar>(),
+ boost::bind(static_cast<Vec3f::Scalar (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
+ boost::bind(static_cast<Vec3f::Scalar (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
EndPoint* end_pos = NULL;
if(res_it != velist[axis].end())
@@ -579,9 +579,9 @@ bool SaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceC
std::vector<EndPoint*>::const_iterator res_it = std::upper_bound(velist[axis].begin(), velist[axis].end(), &dummy,
- boost::bind(std::less<Vec3f::U>(),
- boost::bind(static_cast<Vec3f::U (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
- boost::bind(static_cast<Vec3f::U (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
+ boost::bind(std::less<Vec3f::Scalar>(),
+ boost::bind(static_cast<Vec3f::Scalar (EndPoint::*)(size_t) const>(&EndPoint::getVal), _1, axis),
+ boost::bind(static_cast<Vec3f::Scalar (EndPoint::*)(size_t) const>(&EndPoint::getVal), _2, axis)));
EndPoint* end_pos = NULL;
if(res_it != velist[axis].end())
diff --git a/src/broadphase/hierarchy_tree.cpp b/src/broadphase/hierarchy_tree.cpp
index e1d48d98..c738be79 100644
--- a/src/broadphase/hierarchy_tree.cpp
+++ b/src/broadphase/hierarchy_tree.cpp
@@ -73,7 +73,7 @@ bool HierarchyTree<AABB>::update(NodeBase<AABB>* leaf, const AABB& bv_, const Ve
{
AABB bv(bv_);
if(leaf->bv.contain(bv)) return false;
- Vec3f marginv(margin);
+ Vec3f marginv(Vec3f::Constant(margin));
bv.min_ -= marginv;
bv.max_ += marginv;
if(vel[0] > 0) bv.max_[0] += vel[0];
diff --git a/src/ccd/interval_matrix.cpp b/src/ccd/interval_matrix.cpp
index 927afec2..a2ad5811 100644
--- a/src/ccd/interval_matrix.cpp
+++ b/src/ccd/interval_matrix.cpp
@@ -124,16 +124,16 @@ Vec3f IMatrix3::getRowHigh(size_t i) const
Matrix3f IMatrix3::getLow() const
{
- return Matrix3f(v_[0][0][0], v_[0][1][0], v_[0][2][0],
- v_[1][0][0], v_[1][1][0], v_[1][2][0],
- v_[2][0][0], v_[2][1][0], v_[2][2][0]);
+ return (Matrix3f() << v_[0][0][0], v_[0][1][0], v_[0][2][0],
+ v_[1][0][0], v_[1][1][0], v_[1][2][0],
+ v_[2][0][0], v_[2][1][0], v_[2][2][0]).finished();
}
Matrix3f IMatrix3::getHigh() const
{
- return Matrix3f(v_[0][0][1], v_[0][1][1], v_[0][2][1],
- v_[1][0][1], v_[1][1][1], v_[1][2][1],
- v_[2][0][1], v_[2][1][1], v_[2][2][1]);
+ return (Matrix3f() << v_[0][0][1], v_[0][1][1], v_[0][2][1],
+ v_[1][0][1], v_[1][1][1], v_[1][2][1],
+ v_[2][0][1], v_[2][1][1], v_[2][2][1]).finished();
}
IMatrix3 IMatrix3::operator * (const Matrix3f& m) const
diff --git a/src/collision_node.cpp b/src/collision_node.cpp
index 02be38e0..5d1bddb0 100644
--- a/src/collision_node.cpp
+++ b/src/collision_node.cpp
@@ -67,10 +67,10 @@ void collide2(MeshCollisionTraversalNodeOBB* node, BVHFrontList* front_list)
Matrix3f Rtemp, R;
Vec3f Ttemp, T;
Rtemp = node->R * node->model2->getBV(0).getOrientation();
- R = node->model1->getBV(0).getOrientation().transposeTimes(Rtemp);
+ R = node->model1->getBV(0).getOrientation().transpose() * Rtemp;
Ttemp = node->R * node->model2->getBV(0).getCenter() + node->T;
Ttemp -= node->model1->getBV(0).getCenter();
- T = node->model1->getBV(0).getOrientation().transposeTimes(Ttemp);
+ T = node->model1->getBV(0).getOrientation().transpose() * Ttemp;
collisionRecurse(node, 0, 0, R, T, front_list);
}
diff --git a/src/intersect.cpp b/src/intersect.cpp
index bcf3d10d..7991ae4a 100644
--- a/src/intersect.cpp
+++ b/src/intersect.cpp
@@ -1112,7 +1112,7 @@ bool Intersect::buildTrianglePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f
{
Vec3f n_ = (v2 - v1).cross(v3 - v1);
FCL_REAL norm2 = n_.squaredNorm();
- if (n > 0)
+ if (norm2 > 0)
{
*n = n_ / sqrt(norm2);
*t = n_.dot(v1);
@@ -1126,7 +1126,7 @@ bool Intersect::buildEdgePlane(const Vec3f& v1, const Vec3f& v2, const Vec3f& tn
{
Vec3f n_ = (v2 - v1).cross(tn);
FCL_REAL norm2 = n_.squaredNorm();
- if (n > 0)
+ if (norm2 > 0)
{
*n = n_ / sqrt(norm2);
*t = n_.dot(v1);
diff --git a/src/math/transform.cpp b/src/math/transform.cpp
index 5fb31596..c5f406d2 100644
--- a/src/math/transform.cpp
+++ b/src/math/transform.cpp
@@ -338,7 +338,8 @@ Quaternion3f inverse(const Quaternion3f& q)
void Quaternion3f::fromEuler(FCL_REAL a, FCL_REAL b, FCL_REAL c)
{
Matrix3f R;
- R.setEulerYPR(a, b, c);
+ // R.setEulerYPR(a, b, c);
+ setEulerYPR(R, a, b, c);
fromRotation(R);
}
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 6e01e202..fc65947a 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -142,7 +142,7 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
const Convex* convex = static_cast<const Convex*>(shape);
FCL_REAL maxdot = - std::numeric_limits<FCL_REAL>::max();
Vec3f* curp = convex->points;
- Vec3f bestv;
+ Vec3f bestv(0,0,0);
for(int i = 0; i < convex->num_points; ++i, curp+=1)
{
FCL_REAL dot = dir.dot(*curp);
@@ -166,7 +166,7 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
void GJK::initialize()
{
- ray = Vec3f();
+ ray = Vec3f::Zero();
nfree = 0;
status = Failed;
current = 0;
@@ -201,7 +201,9 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
simplices[0].rank = 0;
ray = guess;
- appendVertex(simplices[0], (ray.squaredNorm() > 0) ? -ray : Vec3f(1, 0, 0));
+ // appendVertex(simplices[0], (ray.squaredNorm() > 0) ? -ray : Vec3f(1, 0, 0));
+ if (ray.squaredNorm() > 0) appendVertex(simplices[0], -ray);
+ else appendVertex(simplices[0], Vec3f(1, 0, 0));
simplices[0].p[0] = 1;
ray = simplices[0].c[0]->w;
lastw[0] = lastw[1] = lastw[2] = lastw[3] = ray; // cache previous support points, the new support point will compare with it to avoid too close support points
@@ -266,7 +268,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
if(project_res.sqr_distance >= 0)
{
next_simplex.rank = 0;
- ray = Vec3f();
+ ray = Vec3f(0,0,0);
current = next;
for(size_t i = 0; i < curr_simplex.rank; ++i)
{
@@ -303,13 +305,13 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
void GJK::getSupport(const Vec3f& d, SimplexV& sv) const
{
- sv.d = normalize(d);
+ sv.d = d.normalized();
sv.w = shape.support(sv.d);
}
void GJK::getSupport(const Vec3f& d, const Vec3f& v, SimplexV& sv) const
{
- sv.d = normalize(d);
+ sv.d = d.normalized();
sv.w = shape.support(sv.d, v);
}
@@ -333,7 +335,7 @@ bool GJK::encloseOrigin()
{
for(size_t i = 0; i < 3; ++i)
{
- Vec3f axis;
+ Vec3f axis(Vec3f::Zero());
axis[i] = 1;
appendVertex(*simplex, axis);
if(encloseOrigin()) return true;
@@ -349,7 +351,7 @@ bool GJK::encloseOrigin()
Vec3f d = simplex->c[1]->w - simplex->c[0]->w;
for(size_t i = 0; i < 3; ++i)
{
- Vec3f axis;
+ Vec3f axis(0,0,0);
axis[i] = 1;
Vec3f p = d.cross(axis);
if(p.squaredNorm() > 0)
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 3e1601ec..befa6090 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.normalize() * - FCL_REAL(1);
+ Vec3f normal = diff.normalized() * - FCL_REAL(1);
if (distance < 0 && penetration_depth)
*penetration_depth = -distance;
@@ -304,7 +304,7 @@ bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf,
if(distance_sqr > 0)
{
FCL_REAL distance = std::sqrt(distance_sqr);
- if(normal_) *normal_ = normalize(contact_to_center);
+ if(normal_) *normal_ = contact_to_center.normalized();
if(contact_points) *contact_points = contact_point;
if(penetration_depth) *penetration_depth = -(radius - distance);
}
@@ -795,16 +795,16 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
FCL_REAL s, s2, l;
int invert_normal, code;
- Vec3f p = T2 - T1; // get vector from centers of box 1 to box 2, relative to box 1
- Vec3f pp = R1.transposeTimes(p); // get pp = p relative to body 1
+ Vec3f p (T2 - T1); // get vector from centers of box 1 to box 2, relative to box 1
+ 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;
// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
- Matrix3f R = R1.transposeTimes(R2);
- Matrix3f Q = abs(R);
+ Matrix3f R (R1.transpose() * R2);
+ Matrix3f Q (R.cwiseAbs());
// for all 15 possible separating axes:
@@ -827,7 +827,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// separating axis = u1, u2, u3
tmp = pp[0];
- s2 = std::abs(tmp) - (Q.dotX(B) + A[0]);
+ s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]);
if(s2 > 0) { *return_code = 0; return 0; }
if(s2 > s)
{
@@ -839,7 +839,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
tmp = pp[1];
- s2 = std::abs(tmp) - (Q.dotY(B) + A[1]);
+ s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]);
if(s2 > 0) { *return_code = 0; return 0; }
if(s2 > s)
{
@@ -851,7 +851,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
tmp = pp[2];
- s2 = std::abs(tmp) - (Q.dotZ(B) + A[2]);
+ s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]);
if(s2 > 0) { *return_code = 0; return 0; }
if(s2 > s)
{
@@ -863,8 +863,8 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
}
// separating axis = v1, v2, v3
- tmp = R2.transposeDotX(p);
- s2 = std::abs(tmp) - (Q.transposeDotX(A) + B[0]);
+ tmp = R2.col(0).dot(p);
+ s2 = std::abs(tmp) - (Q.col(0).dot(A) + B[0]);
if(s2 > 0) { *return_code = 0; return 0; }
if(s2 > s)
{
@@ -875,8 +875,8 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
code = 4;
}
- tmp = R2.transposeDotY(p);
- s2 = std::abs(tmp) - (Q.transposeDotY(A) + B[1]);
+ tmp = R2.col(1).dot(p);
+ s2 = std::abs(tmp) - (Q.col(1).dot(A) + B[1]);
if(s2 > 0) { *return_code = 0; return 0; }
if(s2 > s)
{
@@ -887,8 +887,8 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
code = 5;
}
- tmp = R2.transposeDotZ(p);
- s2 = std::abs(tmp) - (Q.transposeDotZ(A) + B[2]);
+ tmp = R2.col(2).dot(p);
+ s2 = std::abs(tmp) - (Q.col(2).dot(A) + B[2]);
if(s2 > 0) { *return_code = 0; return 0; }
if(s2 > s)
{
@@ -901,7 +901,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
FCL_REAL fudge2(1.0e-6);
- Q += fudge2;
+ Q.array() += fudge2;
Vec3f n;
FCL_REAL eps = std::numeric_limits<FCL_REAL>::epsilon();
@@ -1098,7 +1098,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
for(int j = 0; j < 3; ++j)
{
- sign = (R1.transposeDot(j, normal) > 0) ? 1 : -1;
+ sign = (R1.col(j).dot(normal) > 0) ? 1 : -1;
pa += R1.col(j) * (A[j] * sign);
}
@@ -1107,7 +1107,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
for(int j = 0; j < 3; ++j)
{
- sign = (R2.transposeDot(j, normal) > 0) ? -1 : 1;
+ sign = (R2.col(j).dot(normal) > 0) ? -1 : 1;
pb += R2.col(j) * (B[j] * sign);
}
@@ -1163,8 +1163,8 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
else
normal2 = -normal;
- nr = Rb->transposeTimes(normal2);
- anr = abs(nr);
+ nr = Rb->transpose() * normal2;
+ anr = nr.cwiseAbs();
// find the largest compontent of anr: this corresponds to the normal
// for the indident face. the other axis numbers of the indicent face
@@ -1233,17 +1233,17 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// find the four corners of the incident face, in reference-face coordinates
FCL_REAL quad[8]; // 2D coordinate of incident face (x,y pairs)
FCL_REAL c1, c2, m11, m12, m21, m22;
- c1 = Ra->transposeDot(code1, center);
- c2 = Ra->transposeDot(code2, center);
+ c1 = Ra->col(code1).dot(center);
+ c2 = Ra->col(code2).dot(center);
// optimize this? - we have already computed this data above, but it is not
// stored in an easy-to-index format. for now it's quicker just to recompute
// the four dot products.
Vec3f tempRac = Ra->col(code1);
- m11 = Rb->transposeDot(a1, tempRac);
- m12 = Rb->transposeDot(a2, tempRac);
+ m11 = Rb->col(a1).dot(tempRac);
+ m12 = Rb->col(a2).dot(tempRac);
tempRac = Ra->col(code2);
- m21 = Rb->transposeDot(a1, tempRac);
- m22 = Rb->transposeDot(a2, tempRac);
+ m21 = Rb->col(a1).dot(tempRac);
+ m22 = Rb->col(a2).dot(tempRac);
FCL_REAL k1 = m11 * (*Sb)[a1];
FCL_REAL k2 = m21 * (*Sb)[a1];
@@ -1433,9 +1433,9 @@ bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1,
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
- Vec3f Q = R.transposeTimes(new_s2.n);
+ Vec3f Q = R.transpose() * new_s2.n;
Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
- Vec3f B = abs(A);
+ Vec3f B = A.cwiseAbs();
FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T);
return (depth >= 0);
@@ -1455,9 +1455,9 @@ bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1,
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
- Vec3f Q = R.transposeTimes(new_s2.n);
+ Vec3f Q = R.transpose() * new_s2.n;
Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
- Vec3f B = abs(A);
+ Vec3f B = A.cwiseAbs();
FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T);
if(depth < 0) return false;
@@ -1908,9 +1908,9 @@ bool boxPlaneIntersect(const Box& s1, const Transform3f& tf1,
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
- Vec3f Q = R.transposeTimes(new_s2.n);
+ Vec3f Q = R.transpose() * new_s2.n;
Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
- Vec3f B = abs(A);
+ Vec3f B = A.cwiseAbs();
FCL_REAL signed_dist = new_s2.signedDistance(T);
FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - std::abs(signed_dist);
@@ -2081,7 +2081,7 @@ bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1,
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
- Vec3f Q = R.transposeTimes(new_s2.n);
+ Vec3f Q = R.transpose() * new_s2.n;
FCL_REAL term = std::abs(Q[2]) * s1.lz + s1.radius * std::sqrt(Q[0] * Q[0] + Q[1] * Q[1]);
FCL_REAL dist = new_s2.distance(T);
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index 3cc09c6e..058a5908 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -945,33 +945,33 @@ void constructBox(const AABB& bv, Box& box, Transform3f& tf)
void constructBox(const OBB& bv, Box& box, Transform3f& tf)
{
box = Box(bv.extent * 2);
- tf = Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
- bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
- bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
+ tf = Transform3f((Matrix3f() << bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]).finished(), bv.To);
}
void constructBox(const OBBRSS& bv, Box& box, Transform3f& tf)
{
box = Box(bv.obb.extent * 2);
- tf = Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
+ tf = Transform3f((Matrix3f() << bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
- bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
+ bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]).finished(), bv.obb.To);
}
void constructBox(const kIOS& bv, Box& box, Transform3f& tf)
{
box = Box(bv.obb.extent * 2);
- tf = Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
+ tf = Transform3f((Matrix3f() << bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
- bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
+ bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]).finished(), bv.obb.To);
}
void constructBox(const RSS& bv, Box& box, Transform3f& tf)
{
box = Box(bv.width(), bv.height(), bv.depth());
- tf = Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ tf = Transform3f((Matrix3f() << bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
- bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]).finished(), bv.Tr);
}
void constructBox(const KDOP<16>& bv, Box& box, Transform3f& tf)
@@ -1003,33 +1003,33 @@ void constructBox(const AABB& bv, const Transform3f& tf_bv, Box& box, Transform3
void constructBox(const OBB& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.extent * 2);
- tf = tf_bv *Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ tf = tf_bv *Transform3f((Matrix3f() << bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
- bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]).finished(), bv.To);
}
void constructBox(const OBBRSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.obb.extent * 2);
- tf = tf_bv * Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
+ tf = tf_bv * Transform3f((Matrix3f() << bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
- bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
+ bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]).finished(), bv.obb.To);
}
void constructBox(const kIOS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.obb.extent * 2);
- tf = tf_bv * Transform3f(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
+ tf = tf_bv * Transform3f((Matrix3f() << bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
- bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
+ bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]).finished(), bv.obb.To);
}
void constructBox(const RSS& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
{
box = Box(bv.width(), bv.height(), bv.depth());
- tf = tf_bv * Transform3f(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ tf = tf_bv * Transform3f((Matrix3f() << bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
- bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]).finished(), bv.Tr);
}
void constructBox(const KDOP<16>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf)
diff --git a/test/test_fcl_geometric_shapes.cpp b/test/test_fcl_geometric_shapes.cpp
index 7199b307..fa8bae9e 100644
--- a/test/test_fcl_geometric_shapes.cpp
+++ b/test/test_fcl_geometric_shapes.cpp
@@ -179,7 +179,7 @@ void compareContact(const S1& s1, const Transform3f& tf1,
{
if (expected_point)
{
- bool contact_equal = contact.equal(*expected_point, tol);
+ bool contact_equal = isEqual(contact, *expected_point, tol);
BOOST_CHECK(contact_equal);
if (!contact_equal)
printComparisonError("contact", s1, tf1, s2, tf2, solver_type, contact, *expected_point, false, tol);
@@ -195,10 +195,10 @@ void compareContact(const S1& s1, const Transform3f& tf1,
if (expected_normal)
{
- bool normal_equal = normal.equal(*expected_normal, tol);
+ bool normal_equal = isEqual(normal, *expected_normal, tol);
if (!normal_equal && check_opposite_normal)
- normal_equal = normal.equal(-(*expected_normal), tol);
+ normal_equal = isEqual(normal, -(*expected_normal), tol);
BOOST_CHECK(normal_equal);
if (!normal_equal)
@@ -407,7 +407,7 @@ void testBoxBoxContactPoints(const Matrix3f& R)
std::sort(vertices.begin(), vertices.end(), compareContactPoints);
// The lowest vertex along z-axis should be the contact point
- BOOST_CHECK(vertices[0].equal(point));
+ BOOST_CHECK(isEqual(vertices[0], point));
}
BOOST_AUTO_TEST_CASE(shapeIntersection_boxbox)
@@ -761,11 +761,11 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_spheretriangle)
res = solver1.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
+ BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
res = solver1.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
+ BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
}
BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacetriangle)
@@ -802,11 +802,11 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacetriangle)
res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
+ BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
+ BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
}
BOOST_AUTO_TEST_CASE(shapeIntersection_planetriangle)
@@ -843,11 +843,11 @@ BOOST_AUTO_TEST_CASE(shapeIntersection_planetriangle)
res = solver1.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
+ BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
res = solver1.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
+ BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
}
BOOST_AUTO_TEST_CASE(shapeIntersection_halfspacesphere)
@@ -3126,11 +3126,11 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_spheretriangle)
res = solver2.shapeTriangleIntersect(s, Transform3f(), t[0], t[1], t[2], NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
+ BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
res = solver2.shapeTriangleIntersect(s, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
+ BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
}
BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_halfspacetriangle)
@@ -3167,11 +3167,11 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_halfspacetriangle)
res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
+ BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
+ BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
}
BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_planetriangle)
@@ -3208,11 +3208,11 @@ BOOST_AUTO_TEST_CASE(shapeIntersectionGJK_planetriangle)
res = solver2.shapeTriangleIntersect(hs, Transform3f(), t[0], t[1], t[2], Transform3f(), NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(normal.equal(Vec3f(1, 0, 0), 1e-9));
+ BOOST_CHECK(isEqual(normal, Vec3f(1, 0, 0), 1e-9));
res = solver2.shapeTriangleIntersect(hs, transform, t[0], t[1], t[2], transform, NULL, NULL, &normal);
BOOST_CHECK(res);
- BOOST_CHECK(normal.equal(transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
+ BOOST_CHECK(isEqual(normal, transform.getRotation() * Vec3f(1, 0, 0), 1e-9));
}
@@ -3451,8 +3451,8 @@ void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distan
BOOST_CHECK(resA);
BOOST_CHECK(resB);
- BOOST_CHECK(contactA.equal(contactB, tol)); // contact point should be same
- BOOST_CHECK(normalA.equal(-normalB, tol)); // normal should be opposite
+ BOOST_CHECK(isEqual(contactA, contactB, tol)); // contact point should be same
+ BOOST_CHECK(isEqual(normalA, -normalB, tol)); // normal should be opposite
BOOST_CHECK_CLOSE(depthA, depthB, tol); // penetration depth should be same
resA = solver2.shapeIntersect(s1, tf1, s2, tf2, &contactA, &depthA, &normalA);
@@ -3460,8 +3460,8 @@ void testReversibleShapeIntersection(const S1& s1, const S2& s2, FCL_REAL distan
BOOST_CHECK(resA);
BOOST_CHECK(resB);
- BOOST_CHECK(contactA.equal(contactB, tol));
- BOOST_CHECK(normalA.equal(-normalB, tol));
+ BOOST_CHECK(isEqual(contactA, contactB, tol));
+ BOOST_CHECK(isEqual(normalA, -normalB, tol));
BOOST_CHECK_CLOSE(depthA, depthB, tol);
}
@@ -3539,8 +3539,8 @@ void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance)
BOOST_CHECK(resA);
BOOST_CHECK(resB);
BOOST_CHECK_CLOSE(distA, distB, tol); // distances should be same
- BOOST_CHECK(p1A.equal(p2B, tol)); // closest points should in reverse order
- BOOST_CHECK(p2A.equal(p1B, tol));
+ BOOST_CHECK(isEqual(p1A, p2B, tol)); // closest points should in reverse order
+ BOOST_CHECK(isEqual(p2A, p1B, tol));
resA = solver2.shapeDistance(s1, tf1, s2, tf2, &distA, &p1A, &p2A);
resB = solver2.shapeDistance(s2, tf2, s1, tf1, &distB, &p1B, &p2B);
@@ -3548,8 +3548,8 @@ void testReversibleShapeDistance(const S1& s1, const S2& s2, FCL_REAL distance)
BOOST_CHECK(resA);
BOOST_CHECK(resB);
BOOST_CHECK_CLOSE(distA, distB, tol);
- BOOST_CHECK(p1A.equal(p2B, tol));
- BOOST_CHECK(p2A.equal(p1B, tol));
+ BOOST_CHECK(isEqual(p1A, p2B, tol));
+ BOOST_CHECK(isEqual(p2A, p1B, tol));
}
BOOST_AUTO_TEST_CASE(reversibleShapeDistance_allshapes)
diff --git a/test/test_fcl_sphere_capsule.cpp b/test/test_fcl_sphere_capsule.cpp
index 4e0763aa..c74e41a9 100644
--- a/test/test_fcl_sphere_capsule.cpp
+++ b/test/test_fcl_sphere_capsule.cpp
@@ -100,7 +100,8 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_separated_capsule_rotated)
Capsule capsule (50, 200.);
Matrix3f rotation;
- rotation.setEulerZYX (M_PI * 0.5, 0., 0.);
+ // rotation.setEulerZYX (M_PI * 0.5, 0., 0.);
+ setEulerZYX(rotation, M_PI * 0.5, 0., 0.);
Transform3f capsule_transform (rotation, Vec3f (150., 0., 0.));
BOOST_CHECK (!solver.shapeIntersect(sphere1, sphere1_transform, capsule, capsule_transform, NULL, NULL, NULL));
@@ -125,8 +126,8 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z)
BOOST_CHECK (is_intersecting);
BOOST_CHECK (penetration == 25.);
- BOOST_CHECK (Vec3f (0., 0., 1.).equal(normal));
- BOOST_CHECK (Vec3f (0., 0., 0.).equal(contact_point));
+ BOOST_CHECK (isEqual(Vec3f (0., 0., 1.), normal));
+ BOOST_CHECK (isEqual(Vec3f (0., 0., 0.), contact_point));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z_rotated)
@@ -139,7 +140,8 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z_rotated)
Capsule capsule (50, 200.);
Matrix3f rotation;
- rotation.setEulerZYX (M_PI * 0.5, 0., 0.);
+ // rotation.setEulerZYX (M_PI * 0.5, 0., 0.);
+ setEulerZYX(rotation, M_PI * 0.5, 0., 0.);
Transform3f capsule_transform (rotation, Vec3f (0., 50., 75));
FCL_REAL penetration = 0.;
@@ -150,8 +152,8 @@ BOOST_AUTO_TEST_CASE(Sphere_Capsule_Intersect_test_penetration_z_rotated)
BOOST_CHECK (is_intersecting);
BOOST_CHECK_CLOSE (25, penetration, solver.collision_tolerance);
- BOOST_CHECK (Vec3f (0., 0., 1.).equal(normal));
- BOOST_CHECK (Vec3f (0., 0., 50.).equal(contact_point, solver.collision_tolerance));
+ BOOST_CHECK (isEqual(Vec3f (0., 0., 1.),normal));
+ BOOST_CHECK (isEqual(Vec3f (0., 0., 50.), contact_point, solver.collision_tolerance));
}
BOOST_AUTO_TEST_CASE(Sphere_Capsule_Distance_test_collision)
--
GitLab