Commit 7f3e1fd4 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Use Eigen Matrix without FclMatrix

parent fdd9b54d
......@@ -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.
......
......@@ -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();
}
......
......@@ -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:
......
......@@ -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)
{
}
......
......@@ -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];
......
......@@ -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]);
......
......@@ -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;
}
......
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);
......
......@@ -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;
}
......
......@@ -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];
......
/*
* 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
......@@ -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
......
......@@ -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()) {}
};