From d62d959c1a17907521e0e82510033e0d4628a1cd Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 3 Jun 2016 20:13:00 +0200
Subject: [PATCH] Remove more fcl math classes specific method to make it
compliant with eigen.
---
include/hpp/fcl/BV/AABB.h | 8 ++--
include/hpp/fcl/BV/BV.h | 18 +++----
include/hpp/fcl/eigen/math_details.h | 23 ++-------
.../fcl/eigen/plugins/ccd/interval-vector.h | 7 +++
include/hpp/fcl/eigen/vec_3fx.h | 6 ---
include/hpp/fcl/math/math_details.h | 5 --
include/hpp/fcl/math/vec_3fx.h | 1 -
include/hpp/fcl/simd/math_simd_details.h | 7 ---
src/ccd/interval_matrix.cpp | 18 +++----
src/ccd/taylor_matrix.cpp | 18 +++----
src/distance_capsule_capsule.cpp | 4 +-
src/narrowphase/narrowphase.cpp | 48 +++++++++----------
src/shape/geometric_shapes_utility.cpp | 24 +++++-----
test/test_fcl_eigen.cpp | 6 ---
test/test_fcl_math.cpp | 24 ----------
15 files changed, 80 insertions(+), 137 deletions(-)
diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h
index 68d158b4..d0ffca34 100644
--- a/include/hpp/fcl/BV/AABB.h
+++ b/include/hpp/fcl/BV/AABB.h
@@ -143,16 +143,16 @@ public:
/// @brief Merge the AABB and a point
inline AABB& operator += (const Vec3f& p)
{
- min_.ubound(p);
- max_.lbound(p);
+ min_.array().min(p.array());
+ max_.array().max(p.array());
return *this;
}
/// @brief Merge the AABB and another AABB
inline AABB& operator += (const AABB& other)
{
- min_.ubound(other.min_);
- max_.lbound(other.max_);
+ min_.array().min(other.min_.array());
+ max_.array().max(other.max_.array());
return *this;
}
diff --git a/include/hpp/fcl/BV/BV.h b/include/hpp/fcl/BV/BV.h
index 76e12fd9..c9f4f025 100644
--- a/include/hpp/fcl/BV/BV.h
+++ b/include/hpp/fcl/BV/BV.h
@@ -120,17 +120,17 @@ public:
bv2.extent = Vec3f(extent[id[0]], extent[id[1]], extent[id[2]]);
const Matrix3f& R = tf1.getRotation();
bool left_hand = (id[0] == (id[1] + 1) % 3);
- bv2.axis[0] = left_hand ? -R.getColumn(id[0]) : R.getColumn(id[0]);
- bv2.axis[1] = R.getColumn(id[1]);
- bv2.axis[2] = R.getColumn(id[2]);
+ bv2.axis[0] = left_hand ? -R.col(id[0]) : R.col(id[0]);
+ bv2.axis[1] = R.col(id[1]);
+ bv2.axis[2] = R.col(id[2]);
*/
bv2.To = tf1.transform(bv1.center());
bv2.extent = (bv1.max_ - bv1.min_) * 0.5;
const Matrix3f& R = tf1.getRotation();
- bv2.axis[0] = R.getColumn(0);
- bv2.axis[1] = R.getColumn(1);
- bv2.axis[2] = R.getColumn(2);
+ bv2.axis[0] = R.col(0);
+ bv2.axis[1] = R.col(1);
+ bv2.axis[2] = R.col(2);
}
};
@@ -283,9 +283,9 @@ public:
const Matrix3f& R = tf1.getRotation();
bool left_hand = (id[0] == (id[1] + 1) % 3);
- if (left_hand) bv2.axis[0] = -R.getColumn(id[0]); else bv2.axis[0] = R.getColumn(id[0]);
- bv2.axis[1] = R.getColumn(id[1]);
- bv2.axis[2] = R.getColumn(id[2]);
+ if (left_hand) bv2.axis[0] = -R.col(id[0]); else bv2.axis[0] = R.col(id[0]);
+ bv2.axis[1] = R.col(id[1]);
+ bv2.axis[2] = R.col(id[2]);
}
};
diff --git a/include/hpp/fcl/eigen/math_details.h b/include/hpp/fcl/eigen/math_details.h
index c1ed7fd5..0f2437da 100644
--- a/include/hpp/fcl/eigen/math_details.h
+++ b/include/hpp/fcl/eigen/math_details.h
@@ -84,11 +84,6 @@ struct eigen_wrapper_v3
v.setConstant (x);
}
- inline void negate()
- {
- v *= -1;
- }
-
inline eigen_wrapper_v3<T>& ubound(const eigen_wrapper_v3<T>& u)
{
v = v.cwiseMin (u.v);
@@ -229,11 +224,6 @@ struct eigen_wrapper_v4
v[3] = 0;
}
- inline void negate()
- {
- v *= -1;
- }
-
inline eigen_wrapper_v4<T>& ubound(const eigen_wrapper_v4<T>& u)
{
v = v.cwiseMin (u.v);
@@ -352,11 +342,6 @@ struct eigen_v3 :
this->setConstant (x);
}
- inline void negate()
- {
- this->operator*= (-1);
- }
-
template<typename OtherDerived>
inline eigen_v3<T>& ubound(const Eigen::MatrixBase<OtherDerived>& u)
{
@@ -649,10 +634,10 @@ struct eigen_m3 :
this->row(3) = v3;
}
- inline ColXpr getColumn(size_t i) { return this->col (i); }
- inline RowXpr getRow(size_t i) { return this->row (i); }
- inline ConstColXpr getColumn(size_t i) const { return this->col (i); }
- inline ConstRowXpr getRow(size_t i) const { return this->row (i); }
+ inline ColXpr col(size_t i) { return this->Base::col (i); }
+ inline RowXpr row(size_t i) { return this->Base::row (i); }
+ inline ConstColXpr col(size_t i) const { return this->Base::col (i); }
+ inline ConstRowXpr row(size_t i) const { return this->Base::row (i); }
inline eigen_m3<T>& operator *= (const eigen_m3<T>& other) { return static_cast<eigen_m3<T>&>(this->operator*=(other)); }
inline eigen_m3<T>& operator += (const eigen_m3<T>& other) { return static_cast<eigen_m3<T>&>(this->operator+=(other)); }
diff --git a/include/hpp/fcl/eigen/plugins/ccd/interval-vector.h b/include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
index fda38f99..1675c876 100644
--- a/include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
+++ b/include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
@@ -5,3 +5,10 @@ IVector3& operator=(const FclType<Derived>& other)
setValue (tmp);
return *this;
}
+template <typename Derived>
+IVector3& operator=(const Eigen::MatrixBase<Derived>& other)
+{
+ const Vec3f& tmp (other);
+ setValue (tmp);
+ return *this;
+}
diff --git a/include/hpp/fcl/eigen/vec_3fx.h b/include/hpp/fcl/eigen/vec_3fx.h
index 2fe02cdc..0edb1db8 100644
--- a/include/hpp/fcl/eigen/vec_3fx.h
+++ b/include/hpp/fcl/eigen/vec_3fx.h
@@ -343,7 +343,6 @@ public:
{
return ((*this - other).cwiseAbs().array () < epsilon).all();
}
- inline FclMatrix& negate() { *this = -*this; return *this; }
bool operator == (const FclMatrix& other) const
{
@@ -550,11 +549,6 @@ public:
return ((*this - other).cwiseAbs().array () < epsilon).all();
}
- /*
- inline const typename Eigen::CwiseUnaryOp<Eigen::internal::scalar_opposite_op<T>, const EigenOp>
- negate() { return this->operator-(); }
- // */
-
template <typename Derived>
bool operator == (const Eigen::MatrixBase<Derived>& other) const
{
diff --git a/include/hpp/fcl/math/math_details.h b/include/hpp/fcl/math/math_details.h
index 391efd1f..50586708 100644
--- a/include/hpp/fcl/math/math_details.h
+++ b/include/hpp/fcl/math/math_details.h
@@ -83,11 +83,6 @@ struct Vec3Data
vs[0] = x; vs[1] = x; vs[2] = x;
}
- inline void negate()
- {
- vs[0] = -vs[0]; vs[1] = -vs[1]; vs[2] = -vs[2];
- }
-
inline Vec3Data<T>& ubound(const Vec3Data<T>& u)
{
vs[0] = std::min(vs[0], u.vs[0]);
diff --git a/include/hpp/fcl/math/vec_3fx.h b/include/hpp/fcl/math/vec_3fx.h
index f7dee084..adbd4a28 100644
--- a/include/hpp/fcl/math/vec_3fx.h
+++ b/include/hpp/fcl/math/vec_3fx.h
@@ -133,7 +133,6 @@ public:
inline void setValue(U x) { data.setValue(x); }
inline void setZero () {data.setValue (0); }
inline bool equal(const Vec3fX& other, U epsilon = std::numeric_limits<U>::epsilon() * 100) const { return details::equal(data, other.data, epsilon); }
- inline Vec3fX<T>& negate() { data.negate(); return *this; }
bool operator == (const Vec3fX& other) const
{
diff --git a/include/hpp/fcl/simd/math_simd_details.h b/include/hpp/fcl/simd/math_simd_details.h
index 13346b63..d6dec69f 100644
--- a/include/hpp/fcl/simd/math_simd_details.h
+++ b/include/hpp/fcl/simd/math_simd_details.h
@@ -103,7 +103,6 @@ struct sse_meta_f4
inline void setValue(float x, float y, float z, float w = 1) { v = _mm_setr_ps(x, y, z, w); }
inline void setValue(float x) { v = _mm_set1_ps(x); }
inline void setValue(__m128 x) { v = x; }
- inline void negate() { v = _mm_sub_ps(xmms_0, v); }
inline sse_meta_f4& ubound(const sse_meta_f4& u)
{
@@ -195,12 +194,6 @@ struct sse_meta_d4
v[1] = y;
}
- inline void negate()
- {
- v[0] = _mm_sub_pd(xmmd_0, v[0]);
- v[1] = _mm_sub_pd(xmmd_0, v[1]);
- }
-
inline sse_meta_d4& ubound(const sse_meta_d4& u)
{
v[0] = _mm_min_pd(v[0], u.v[0]);
diff --git a/src/ccd/interval_matrix.cpp b/src/ccd/interval_matrix.cpp
index 0c03f3bf..927afec2 100644
--- a/src/ccd/interval_matrix.cpp
+++ b/src/ccd/interval_matrix.cpp
@@ -52,9 +52,9 @@ IMatrix3::IMatrix3(FCL_REAL v)
IMatrix3::IMatrix3(const Matrix3f& m)
{
- v_[0] = m.getRow(0);
- v_[1] = m.getRow(1);
- v_[2] = m.getRow(2);
+ v_[0] = m.row(0);
+ v_[1] = m.row(1);
+ v_[2] = m.row(2);
}
IMatrix3::IMatrix3(FCL_REAL m[3][3][2])
@@ -138,9 +138,9 @@ Matrix3f IMatrix3::getHigh() const
IMatrix3 IMatrix3::operator * (const Matrix3f& m) const
{
- const Vec3f& mc0 = m.getColumn(0);
- const Vec3f& mc1 = m.getColumn(1);
- const Vec3f& mc2 = m.getColumn(2);
+ const Vec3f& mc0 = m.col(0);
+ const Vec3f& mc1 = m.col(1);
+ const Vec3f& mc2 = m.col(2);
return IMatrix3(IVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
IVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
@@ -170,9 +170,9 @@ IMatrix3 IMatrix3::operator * (const IMatrix3& m) const
IMatrix3& IMatrix3::operator *= (const Matrix3f& m)
{
- const Vec3f& mc0 = m.getColumn(0);
- const Vec3f& mc1 = m.getColumn(1);
- const Vec3f& mc2 = m.getColumn(2);
+ const Vec3f& mc0 = m.col(0);
+ const Vec3f& mc1 = m.col(1);
+ const Vec3f& mc2 = m.col(2);
v_[0].setValue(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
v_[1].setValue(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
diff --git a/src/ccd/taylor_matrix.cpp b/src/ccd/taylor_matrix.cpp
index ecde6afd..e4d435de 100644
--- a/src/ccd/taylor_matrix.cpp
+++ b/src/ccd/taylor_matrix.cpp
@@ -66,9 +66,9 @@ TMatrix3::TMatrix3(const TVector3& v1, const TVector3& v2, const TVector3& v3)
TMatrix3::TMatrix3(const Matrix3f& m, const boost::shared_ptr<TimeInterval>& time_interval)
{
- v_[0] = TVector3(m.getRow(0), time_interval);
- v_[1] = TVector3(m.getRow(1), time_interval);
- v_[2] = TVector3(m.getRow(2), time_interval);
+ v_[0] = TVector3(m.row(0), time_interval);
+ v_[1] = TVector3(m.row(1), time_interval);
+ v_[2] = TVector3(m.row(2), time_interval);
}
void TMatrix3::setIdentity()
@@ -109,9 +109,9 @@ TaylorModel& TMatrix3::operator () (size_t i, size_t j)
TMatrix3 TMatrix3::operator * (const Matrix3f& m) const
{
- const Vec3f& mc0 = m.getColumn(0);
- const Vec3f& mc1 = m.getColumn(1);
- const Vec3f& mc2 = m.getColumn(2);
+ const Vec3f& mc0 = m.col(0);
+ const Vec3f& mc1 = m.col(1);
+ const Vec3f& mc2 = m.col(2);
return TMatrix3(TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2)),
TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2)),
@@ -153,9 +153,9 @@ TMatrix3 TMatrix3::operator * (FCL_REAL d) const
TMatrix3& TMatrix3::operator *= (const Matrix3f& m)
{
- const Vec3f& mc0 = m.getColumn(0);
- const Vec3f& mc1 = m.getColumn(1);
- const Vec3f& mc2 = m.getColumn(2);
+ const Vec3f& mc0 = m.col(0);
+ const Vec3f& mc1 = m.col(1);
+ const Vec3f& mc2 = m.col(2);
v_[0] = TVector3(v_[0].dot(mc0), v_[0].dot(mc1), v_[0].dot(mc2));
v_[1] = TVector3(v_[1].dot(mc0), v_[1].dot(mc1), v_[1].dot(mc2));
diff --git a/src/distance_capsule_capsule.cpp b/src/distance_capsule_capsule.cpp
index efb63895..00ee5b9b 100644
--- a/src/distance_capsule_capsule.cpp
+++ b/src/distance_capsule_capsule.cpp
@@ -38,8 +38,8 @@ namespace fcl {
fcl::Vec3f center1 = tf1.getTranslation ();
fcl::Vec3f center2 = tf2.getTranslation ();
// We assume that capsules are oriented along z-axis.
- fcl::Vec3f direction1 = tf1.getRotation ().getColumn (2);
- fcl::Vec3f direction2 = tf2.getRotation ().getColumn (2);
+ fcl::Vec3f direction1 = tf1.getRotation ().col (2);
+ fcl::Vec3f direction2 = tf2.getRotation ().col (2);
FCL_REAL halfLength1 = 0.5*c1->lz;
FCL_REAL halfLength2 = 0.5*c2->lz;
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 69b27476..3e1601ec 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -1078,12 +1078,12 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// if we get to this point, the boxes interpenetrate. compute the normal
// in global coordinates.
if(best_col_id != -1)
- normal = normalR->getColumn(best_col_id);
+ normal = normalR->col(best_col_id);
else
normal = R1 * normalC;
if(invert_normal)
- normal.negate();
+ normal = -normal;
*depth = -s; // s is negative when the boxes are in collision
@@ -1099,7 +1099,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;
- pa += R1.getColumn(j) * (A[j] * sign);
+ pa += R1.col(j) * (A[j] * sign);
}
// find a point pb on the intersecting edge of box 2
@@ -1108,12 +1108,12 @@ 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;
- pb += R2.getColumn(j) * (B[j] * sign);
+ pb += R2.col(j) * (B[j] * sign);
}
FCL_REAL alpha, beta;
- Vec3f ua(R1.getColumn((code-7)/3));
- Vec3f ub(R2.getColumn((code-7)%3));
+ Vec3f ua(R1.col((code-7)/3));
+ Vec3f ub(R2.col((code-7)%3));
lineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
pa += ua * alpha;
@@ -1204,9 +1204,9 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// compute center point of incident face, in reference-face coordinates
Vec3f center;
if(nr[lanr] < 0)
- center = (*pb) - (*pa) + Rb->getColumn(lanr) * ((*Sb)[lanr]);
+ center = (*pb) - (*pa) + Rb->col(lanr) * ((*Sb)[lanr]);
else
- center = (*pb) - (*pa) - Rb->getColumn(lanr) * ((*Sb)[lanr]);
+ center = (*pb) - (*pa) - Rb->col(lanr) * ((*Sb)[lanr]);
// find the normal and non-normal axis numbers of the reference box
int codeN, code1, code2;
@@ -1238,10 +1238,10 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
// 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->getColumn(code1);
+ Vec3f tempRac = Ra->col(code1);
m11 = Rb->transposeDot(a1, tempRac);
m12 = Rb->transposeDot(a2, tempRac);
- tempRac = Ra->getColumn(code2);
+ tempRac = Ra->col(code2);
m21 = Rb->transposeDot(a1, tempRac);
m22 = Rb->transposeDot(a2, tempRac);
@@ -1284,7 +1284,7 @@ int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
{
FCL_REAL k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
FCL_REAL k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
- points[cnum] = center + Rb->getColumn(a1) * k1 + Rb->getColumn(a2) * k2;
+ points[cnum] = center + Rb->col(a1) * k1 + Rb->col(a2) * k2;
dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]);
if(dep[cnum] >= 0)
{
@@ -1463,9 +1463,9 @@ bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1,
if(depth < 0) return false;
Vec3f axis[3];
- axis[0] = R.getColumn(0);
- axis[1] = R.getColumn(1);
- axis[2] = R.getColumn(2);
+ axis[0] = R.col(0);
+ axis[1] = R.col(1);
+ axis[2] = R.col(2);
/// find deepest point
Vec3f p(T);
@@ -1513,7 +1513,7 @@ bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3f& tf1,
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
- Vec3f dir_z = R.getColumn(2);
+ Vec3f dir_z = R.col(2);
FCL_REAL cosa = dir_z.dot(new_s2.n);
if(std::abs(cosa) < halfspaceIntersectTolerance<FCL_REAL>())
@@ -1560,7 +1560,7 @@ bool cylinderHalfspaceIntersect(const Cylinder& s1, const Transform3f& tf1,
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
- Vec3f dir_z = R.getColumn(2);
+ Vec3f dir_z = R.col(2);
FCL_REAL cosa = dir_z.dot(new_s2.n);
if(cosa < halfspaceIntersectTolerance<FCL_REAL>())
@@ -1612,7 +1612,7 @@ bool coneHalfspaceIntersect(const Cone& s1, const Transform3f& tf1,
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
- Vec3f dir_z = R.getColumn(2);
+ Vec3f dir_z = R.col(2);
FCL_REAL cosa = dir_z.dot(new_s2.n);
if(cosa < halfspaceIntersectTolerance<FCL_REAL>())
@@ -1917,9 +1917,9 @@ bool boxPlaneIntersect(const Box& s1, const Transform3f& tf1,
if(depth < 0) return false;
Vec3f axis[3];
- axis[0] = R.getColumn(0);
- axis[1] = R.getColumn(1);
- axis[2] = R.getColumn(2);
+ axis[0] = R.col(0);
+ axis[1] = R.col(1);
+ axis[2] = R.col(2);
// find the deepest point
Vec3f p = T;
@@ -1973,7 +1973,7 @@ bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1,
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
- Vec3f dir_z = R.getColumn(2);
+ Vec3f dir_z = R.col(2);
Vec3f p1 = T + dir_z * (0.5 * s1.lz);
Vec3f p2 = T - dir_z * (0.5 * s1.lz);
@@ -2003,7 +2003,7 @@ bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1,
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
- Vec3f dir_z = R.getColumn(2);
+ Vec3f dir_z = R.col(2);
Vec3f p1 = T + dir_z * (0.5 * s1.lz);
@@ -2106,7 +2106,7 @@ bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1,
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
- Vec3f dir_z = R.getColumn(2);
+ Vec3f dir_z = R.col(2);
FCL_REAL cosa = dir_z.dot(new_s2.n);
if(std::abs(cosa) < planeIntersectTolerance<FCL_REAL>())
@@ -2186,7 +2186,7 @@ bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1,
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
- Vec3f dir_z = R.getColumn(2);
+ Vec3f dir_z = R.col(2);
FCL_REAL cosa = dir_z.dot(new_s2.n);
if(std::abs(cosa) < planeIntersectTolerance<FCL_REAL>())
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index efb09790..d6f6e69c 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -412,9 +412,9 @@ void computeBV<OBB, Box>(const Box& s, const Transform3f& tf, OBB& bv)
const Vec3f& T = tf.getTranslation();
bv.To = T;
- bv.axis[0] = R.getColumn(0);
- bv.axis[1] = R.getColumn(1);
- bv.axis[2] = R.getColumn(2);
+ bv.axis[0] = R.col(0);
+ bv.axis[1] = R.col(1);
+ bv.axis[2] = R.col(2);
bv.extent = s.side * (FCL_REAL)0.5;
}
@@ -437,9 +437,9 @@ void computeBV<OBB, Capsule>(const Capsule& s, const Transform3f& tf, OBB& bv)
const Vec3f& T = tf.getTranslation();
bv.To = T;
- bv.axis[0] = R.getColumn(0);
- bv.axis[1] = R.getColumn(1);
- bv.axis[2] = R.getColumn(2);
+ bv.axis[0] = R.col(0);
+ bv.axis[1] = R.col(1);
+ bv.axis[2] = R.col(2);
bv.extent.setValue(s.radius, s.radius, s.lz / 2 + s.radius);
}
@@ -450,9 +450,9 @@ void computeBV<OBB, Cone>(const Cone& s, const Transform3f& tf, OBB& bv)
const Vec3f& T = tf.getTranslation();
bv.To = T;
- bv.axis[0] = R.getColumn(0);
- bv.axis[1] = R.getColumn(1);
- bv.axis[2] = R.getColumn(2);
+ bv.axis[0] = R.col(0);
+ bv.axis[1] = R.col(1);
+ bv.axis[2] = R.col(2);
bv.extent.setValue(s.radius, s.radius, s.lz / 2);
}
@@ -463,9 +463,9 @@ void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv)
const Vec3f& T = tf.getTranslation();
bv.To = T;
- bv.axis[0] = R.getColumn(0);
- bv.axis[1] = R.getColumn(1);
- bv.axis[2] = R.getColumn(2);
+ bv.axis[0] = R.col(0);
+ bv.axis[1] = R.col(1);
+ bv.axis[2] = R.col(2);
bv.extent.setValue(s.radius, s.radius, s.lz / 2);
}
diff --git a/test/test_fcl_eigen.cpp b/test/test_fcl_eigen.cpp
index 70223d03..329e32a8 100644
--- a/test/test_fcl_eigen.cpp
+++ b/test/test_fcl_eigen.cpp
@@ -86,12 +86,6 @@ BOOST_AUTO_TEST_CASE(fcl_eigen_vec3fx)
PRINT_VECTOR(b)
PRINT_VECTOR(min(a,b))
PRINT_VECTOR(max(a,b))
- a.lbound(b);
- PRINT_VECTOR(a)
- std::cout << (a+1).lbound(b) << std::endl;
- std::cout << (a+1).ubound(b) << std::endl;
-
- std::cout << a.getRow(1) << std::endl;
}
BOOST_AUTO_TEST_CASE(fcl_eigen_matrix3fx)
diff --git a/test/test_fcl_math.cpp b/test/test_fcl_math.cpp
index fb1dbd4a..2262b579 100644
--- a/test/test_fcl_math.cpp
+++ b/test/test_fcl_math.cpp
@@ -461,12 +461,6 @@ BOOST_AUTO_TEST_CASE(vec_test_eigen_vec32_consistent)
BOOST_CHECK(std::abs(v1.norm() - v3.norm()) < 1e-5);
BOOST_CHECK(std::abs(v1.squaredNorm() - v3.squaredNorm()) < 1e-5);
- v12 = v1; v12.negate();
- v34 = v3; v34.negate();
- BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
- BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
- BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
-
v12 = v1; v12.normalize();
v34 = v3; v34.normalize();
BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
@@ -609,12 +603,6 @@ BOOST_AUTO_TEST_CASE(vec_test_eigen_vec64_consistent)
BOOST_CHECK(std::abs(v1.norm() - v3.norm()) < 1e-5);
BOOST_CHECK(std::abs(v1.squaredNorm() - v3.squaredNorm()) < 1e-5);
- v12 = v1; v12.negate();
- v34 = v3; v34.negate();
- BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
- BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
- BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
-
v12 = v1; v12.normalize();
v34 = v3; v34.normalize();
BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
@@ -925,12 +913,6 @@ BOOST_AUTO_TEST_CASE(vec_test_sse_vec32_consistent)
BOOST_CHECK(std::abs(v1.norm() - v3.norm()) < 1e-5);
BOOST_CHECK(std::abs(v1.squaredNorm() - v3.squaredNorm()) < 1e-5);
- v12 = v1; v12.negate();
- v34 = v3; v34.negate();
- BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
- BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
- BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
-
v12 = v1; v12.normalize();
v34 = v3; v34.normalize();
BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
@@ -1073,12 +1055,6 @@ BOOST_AUTO_TEST_CASE(vec_test_sse_vec64_consistent)
BOOST_CHECK(std::abs(v1.norm() - v3.norm()) < 1e-5);
BOOST_CHECK(std::abs(v1.squaredNorm() - v3.squaredNorm()) < 1e-5);
- v12 = v1; v12.negate();
- v34 = v3; v34.negate();
- BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
- BOOST_CHECK(std::abs(v12[1] - v34[1]) < 1e-5);
- BOOST_CHECK(std::abs(v12[2] - v34[2]) < 1e-5);
-
v12 = v1; v12.normalize();
v34 = v3; v34.normalize();
BOOST_CHECK(std::abs(v12[0] - v34[0]) < 1e-5);
--
GitLab