diff --git a/include/hpp/fcl/ccd/motion.h b/include/hpp/fcl/ccd/motion.h
index 6277abb6dcb018a6652ebfb02f460e820f8313a0..3cd3d97164a2dbfc7b4d4ce0a1ffcbbde7866cc2 100644
--- a/include/hpp/fcl/ccd/motion.h
+++ b/include/hpp/fcl/ccd/motion.h
@@ -354,7 +354,7 @@ public:
protected:
void computeScrewParameter()
{
- Quaternion3f deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
+ Quaternion3f deltaq = tf2.getQuatRotation() * tf1.getQuatRotation().inverse();
deltaq.toAxisAngle(axis, angular_vel);
if(angular_vel < 0)
{
diff --git a/include/hpp/fcl/ccd/taylor_matrix.h b/include/hpp/fcl/ccd/taylor_matrix.h
index 18819387fd9629bbcc3825392369a3a2292ed552..3602e90a85e2196e2f82303cc4aea5bb7ea7a8c4 100644
--- a/include/hpp/fcl/ccd/taylor_matrix.h
+++ b/include/hpp/fcl/ccd/taylor_matrix.h
@@ -49,6 +49,8 @@ namespace fcl
class TMatrix3
{
TVector3 v_[3];
+
+ template<int Rows> struct product_return_type { };
public:
TMatrix3();
@@ -63,9 +65,13 @@ public:
const TaylorModel& operator () (size_t i, size_t j) const;
TaylorModel& operator () (size_t i, size_t j);
- TVector3 operator * (const Vec3f& v) const;
+ template<typename Derived>
+ typename product_return_type<Derived::ColsAtCompileTime>::type
+ operator * (const Eigen::MatrixBase<Derived>& rhs) const
+ {
+ return product_return_type<Derived::ColsAtCompileTime>::run(*this, rhs);
+ }
TVector3 operator * (const TVector3& v) const;
- TMatrix3 operator * (const Matrix3f& m) const;
TMatrix3 operator * (const TMatrix3& m) const;
TMatrix3 operator * (const TaylorModel& d) const;
TMatrix3 operator * (FCL_REAL d) const;
@@ -107,6 +113,25 @@ public:
TMatrix3& rotationConstrain();
};
+template<> struct TMatrix3::product_return_type <1> {
+ typedef TVector3 type;
+ template<typename Derived>
+ static type run (const TMatrix3& tm, const Eigen::MatrixBase<Derived>& rhs) {
+ return TVector3(tm.v_[0].dot(rhs), tm.v_[1].dot(rhs), tm.v_[2].dot(rhs));
+ }
+};
+template<> struct TMatrix3::product_return_type <3> {
+ typedef TMatrix3 type;
+ template<typename Derived>
+ static type run (const TMatrix3& tm, const Eigen::MatrixBase<Derived>& rhs)
+ {
+ return TMatrix3(
+ TVector3(tm.v_[0].dot(rhs.col(0)), tm.v_[0].dot(rhs.col(1)), tm.v_[0].dot(rhs.col(2))),
+ TVector3(tm.v_[1].dot(rhs.col(0)), tm.v_[1].dot(rhs.col(1)), tm.v_[1].dot(rhs.col(2))),
+ TVector3(tm.v_[2].dot(rhs.col(0)), tm.v_[2].dot(rhs.col(1)), tm.v_[2].dot(rhs.col(2))));
+ }
+};
+
TMatrix3 rotationConstrain(const TMatrix3& m);
TMatrix3 operator * (const Matrix3f& m, const TaylorModel& a);
diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h
index f7a229577f5b03b2f4a15902d8b8bb3c3045897e..1cda961077f1b11f0b79b9fa794841d286c28ac7 100644
--- a/include/hpp/fcl/math/transform.h
+++ b/include/hpp/fcl/math/transform.h
@@ -48,6 +48,10 @@ namespace fcl
/// @brief Quaternion used locally by InterpMotion
class Quaternion3f
{
+private:
+ typedef Eigen::Matrix<FCL_REAL, 4, 1> Vec4f;
+ typedef typename Vec4f:: FixedSegmentReturnType<3>::Type XYZ_t;
+ typedef typename Vec4f::ConstFixedSegmentReturnType<3>::Type XYZConst_t;
public:
enum {
W = 0,
@@ -58,16 +62,22 @@ public:
/// @brief Default quaternion is identity rotation
Quaternion3f()
{
- data[0] = 1;
- data[1] = 0;
- data[2] = 0;
- data[3] = 0;
+ data[W] = 1;
+ data[X] = 0;
+ data[Y] = 0;
+ data[Z] = 0;
}
+ /// @brief Construct quaternion from 4D vector
+ template <typename Derived>
+ Quaternion3f(const Eigen::MatrixBase<Derived>& other) :
+ data (other.derived())
+ {}
+
/// @brief Whether the rotation is identity
bool isIdentity() const
{
- return (data[0] == 1) && (data[1] == 0) && (data[2] == 0) && (data[3] == 0);
+ return (data[W] == 1) && (data[X] == 0) && (data[Y] == 0) && (data[Z] == 0);
}
/// @brief Matrix to quaternion
@@ -89,24 +99,50 @@ public:
void toAxes(Matrix3f& axis) const;
/// @brief Axis and angle to quaternion
- void fromAxisAngle(const Vec3f& axis, FCL_REAL angle);
+ template<typename Derived>
+ inline void fromAxisAngle(const Eigen::MatrixBase<Derived>& axis, FCL_REAL angle)
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
+ FCL_REAL half_angle = 0.5 * angle;
+ FCL_REAL sn = sin((double)half_angle);
+ data[W] = cos((double)half_angle);
+ data.segment<3>(X).noalias() = sn*axis;
+ }
/// @brief Quaternion to axis and angle
void toAxisAngle(Vec3f& axis, FCL_REAL& angle) const;
/// @brief Dot product between quaternions
- FCL_REAL dot(const Quaternion3f& other) const;
+ inline FCL_REAL dot(const Quaternion3f& other) const
+ {
+ return data.dot(other.data);
+ }
/// @brief addition
- Quaternion3f operator + (const Quaternion3f& other) const;
- const Quaternion3f& operator += (const Quaternion3f& other);
+ inline const Eigen::CwiseBinaryOp<
+ Eigen::internal::scalar_sum_op <FCL_REAL>, const Vec4f, const Vec4f>
+ operator + (const Quaternion3f& other) const
+ {
+ return Eigen::CwiseBinaryOp< Eigen::internal::scalar_sum_op <FCL_REAL>,
+ const Vec4f, const Vec4f>
+ (data, other.data);
+ }
+ inline const Quaternion3f& operator += (const Quaternion3f& other)
+ {
+ data += other.data;
+ return *this;
+ }
/// @brief minus
Quaternion3f operator - (const Quaternion3f& other) const;
const Quaternion3f& operator -= (const Quaternion3f& other);
/// @brief multiplication
- Quaternion3f operator * (const Quaternion3f& other) const;
+ inline Quaternion3f operator * (const Quaternion3f& other) const
+ {
+ return Quaternion3f(w() * other.w() - vec().dot(other.vec()),
+ w() * other.vec() + other.w() * vec() + vec().cross(other.vec()));
+ }
const Quaternion3f& operator *= (const Quaternion3f& other);
/// @brief division
@@ -117,22 +153,54 @@ public:
const Quaternion3f& operator *= (FCL_REAL t);
/// @brief conjugate
- Quaternion3f& conj();
+ inline Quaternion3f conj() const
+ {
+ return Quaternion3f (w(), -vec());
+ }
/// @brief inverse
- Quaternion3f& inverse();
+ inline Quaternion3f inverse() const {
+ return conj();
+ }
+
+ inline void normalize () {
+ FCL_REAL n = data.squaredNorm();
+ if (n > 0) data *= 1/sqrt(n);
+ }
+
+ template<typename Derived> struct Transform {
+ typedef typename XYZConst_t::cross_product_return_type<Derived>::type Cross_t;
+ typedef Eigen::CwiseBinaryOp <
+ Eigen::internal::scalar_sum_op <FCL_REAL>,
+ const typename Derived::ScalarMultipleReturnType,
+ const typename Cross_t::ScalarMultipleReturnType >
+ RightSum_t;
+ typedef Eigen::CwiseBinaryOp <
+ Eigen::internal::scalar_sum_op <FCL_REAL>,
+ const typename XYZConst_t::ScalarMultipleReturnType,
+ const RightSum_t >
+ Type;
+ };
/// @brief rotate a vector
- Vec3f transform(const Vec3f& v) const;
+ template<typename Derived>
+ /*inline Vec3f transform(const Eigen::MatrixBase<Derived>& v) const*/
+ inline typename Transform<Derived>::Type
+ transform(const Eigen::MatrixBase<Derived>& v) const
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
+ Vec4f::ConstFixedSegmentReturnType<3>::Type u (data.segment<3>(X));
+ const FCL_REAL& s = w();
+ /*return 2*u.dot(v)*u + (s*s - u.dot(u))*v + 2*s*u.cross(v);*/
+ const FCL_REAL uDv = u.dot(v);
+ const FCL_REAL uDu = u.dot(u);
+ const typename Transform<Derived>::RightSum_t rhs((s*s - uDu)*v, 2*s*u.cross(v.derived()));
+ return typename Transform<Derived>::Type (2*uDv*u, rhs);
+ }
bool operator == (const Quaternion3f& other) const
{
- for(std::size_t i = 0; i < 4; ++i)
- {
- if(data[i] != other[i])
- return false;
- }
- return true;
+ return (data == other.data);
}
bool operator != (const Quaternion3f& other) const
@@ -160,19 +228,24 @@ private:
data[Z] = z;
}
+ template<typename Derived>
+ Quaternion3f(FCL_REAL _w, const Eigen::MatrixBase<Derived>& _vec)
+ {
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
+ w() = _w;
+ vec().noalias() = _vec;
+ }
+
FCL_REAL operator [] (std::size_t i) const
{
return data[i];
}
- FCL_REAL data[4];
-};
-
-/// @brief conjugate of quaternion
-Quaternion3f conj(const Quaternion3f& q);
+ inline Vec4f:: FixedSegmentReturnType<3>::Type vec() { return data.segment<3>(X); }
+ inline Vec4f::ConstFixedSegmentReturnType<3>::Type vec() const { return data.segment<3>(X); }
-/// @brief inverse of quaternion
-Quaternion3f inverse(const Quaternion3f& q);
+ Vec4f data;
+};
static inline std::ostream& operator << (std::ostream& o, const Quaternion3f& q)
{
@@ -296,17 +369,19 @@ public:
}
/// @brief set transform from rotation
- inline void setRotation(const Matrix3f& R_)
+ template<typename Derived>
+ inline void setRotation(const Eigen::MatrixBase<Derived>& R_)
{
- R = R_;
+ R.noalias() = R_;
matrix_set = true;
- q.fromRotation(R_);
+ q.fromRotation(R);
}
/// @brief set transform from translation
- inline void setTranslation(const Vec3f& T_)
+ template<typename Derived>
+ inline void setTranslation(const Eigen::MatrixBase<Derived>& T_)
{
- T = T_;
+ T.noalias() = T_;
}
/// @brief set transform from rotation
@@ -317,8 +392,10 @@ public:
}
/// @brief transform a given vector by the transform
- inline Vec3f transform(const Vec3f& v) const
+ template <typename Derived>
+ inline Vec3f transform(const Eigen::MatrixBase<Derived>& v) const
{
+ EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3);
return q.transform(v) + T;
}
@@ -334,7 +411,7 @@ public:
/// @brief inverse the transform and multiply with another
inline Transform3f inverseTimes(const Transform3f& other) const
{
- const Quaternion3f& q_inv = fcl::conj(q);
+ const Quaternion3f& q_inv = q.conj();
return Transform3f(q_inv * other.q, q_inv.transform(other.T - T));
}
diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h
index 7703561aa6908b4624b5cfb03fa4e84318e446d1..19cb43de5d5a9973e2aad29709c995b644134c6c 100644
--- a/include/hpp/fcl/shape/geometric_shapes.h
+++ b/include/hpp/fcl/shape/geometric_shapes.h
@@ -351,7 +351,7 @@ public:
int* index = polygons + 1;
for(int i = 0; i < num_planes; ++i)
{
- Vec3f plane_center;
+ Vec3f plane_center(0,0,0);
// compute the center of the polygon
for(int j = 0; j < *points_in_poly; ++j)
@@ -379,13 +379,13 @@ public:
Vec3f computeCOM() const
{
- Vec3f com;
+ Vec3f com(0,0,0);
FCL_REAL vol = 0;
int* points_in_poly = polygons;
int* index = polygons + 1;
for(int i = 0; i < num_planes; ++i)
{
- Vec3f plane_center;
+ Vec3f plane_center(0,0,0);
// compute the center of the polygon
for(int j = 0; j < *points_in_poly; ++j)
@@ -419,7 +419,7 @@ public:
int* index = polygons + 1;
for(int i = 0; i < num_planes; ++i)
{
- Vec3f plane_center;
+ Vec3f plane_center(0,0,0);
// compute the center of the polygon
for(int j = 0; j < *points_in_poly; ++j)
diff --git a/src/ccd/motion.cpp b/src/ccd/motion.cpp
index 554131ae6b2d8e5fefce95db18b7af2c7d1e667c..a6cc367daa173ef526692f0ce8d5c64049194e26 100644
--- a/src/ccd/motion.cpp
+++ b/src/ccd/motion.cpp
@@ -503,7 +503,7 @@ bool InterpMotion::integrate(double dt) const
void InterpMotion::computeVelocity()
{
linear_vel = tf2.transform(reference_p) - tf1.transform(reference_p);
- Quaternion3f deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
+ Quaternion3f deltaq = tf2.getQuatRotation() * tf1.getQuatRotation().inverse();
deltaq.toAxisAngle(angular_axis, angular_vel);
if(angular_vel < 0)
{
diff --git a/src/ccd/taylor_matrix.cpp b/src/ccd/taylor_matrix.cpp
index e4d435de59bffc65638854d397cefcbb7796372e..c8f5f826c0fd01658a11939bf8b2be942f24a1a2 100644
--- a/src/ccd/taylor_matrix.cpp
+++ b/src/ccd/taylor_matrix.cpp
@@ -107,23 +107,6 @@ TaylorModel& TMatrix3::operator () (size_t i, size_t j)
return v_[i][j];
}
-TMatrix3 TMatrix3::operator * (const Matrix3f& m) const
-{
- 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)),
- TVector3(v_[2].dot(mc0), v_[2].dot(mc1), v_[2].dot(mc2)));
-}
-
-
-TVector3 TMatrix3::operator * (const Vec3f& v) const
-{
- return TVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
-}
-
TVector3 TMatrix3::operator * (const TVector3& v) const
{
return TVector3(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
diff --git a/src/math/transform.cpp b/src/math/transform.cpp
index 4c0596f3135831dd71b50d67a64d2bfff62f6b54..5ee6d9baadf1085b710767791ffdb9b6a62a82e9 100644
--- a/src/math/transform.cpp
+++ b/src/math/transform.cpp
@@ -103,9 +103,15 @@ void Quaternion3f::toRotation(Matrix3f& R) const
FCL_REAL twoYZ = twoZ*data[Y];
FCL_REAL twoZZ = twoZ*data[Z];
- R << 1.0 - (twoYY + twoZZ), twoXY - twoWZ, twoXZ + twoWY,
- twoXY + twoWZ, 1.0 - (twoXX + twoZZ), twoYZ - twoWX,
- twoXZ - twoWY, twoYZ + twoWX, 1.0 - (twoXX + twoYY);
+ R(0,0) = 1.0 - (twoYY + twoZZ);
+ R(0,1) = twoXY - twoWZ;
+ R(0,2) = twoXZ + twoWY;
+ R(1,0) = twoXY + twoWZ;
+ R(1,1) = 1.0 - (twoXX + twoZZ);
+ R(1,2) = twoYZ - twoWX;
+ R(2,0) = twoXZ - twoWY;
+ R(2,1) = twoYZ + twoWX;
+ R(2,2) = 1.0 - (twoXX + twoYY);
}
void Quaternion3f::fromAxes(const Matrix3f& axes)
@@ -173,27 +179,14 @@ void Quaternion3f::toAxes(Matrix3f& axes) const
twoXZ + twoWY, twoYZ - twoWX, 1.0 - (twoXX + twoYY);
}
-
-void Quaternion3f::fromAxisAngle(const Vec3f& axis, FCL_REAL angle)
-{
- FCL_REAL half_angle = 0.5 * angle;
- FCL_REAL sn = sin((double)half_angle);
- data[W] = cos((double)half_angle);
- data[X] = sn * axis[0];
- data[Y] = sn * axis[1];
- data[Z] = sn * axis[2];
-}
-
void Quaternion3f::toAxisAngle(Vec3f& axis, FCL_REAL& angle) const
{
- double sqr_length = data[X] * data[X] + data[Y] * data[Y] + data[Z] * data[Z];
+ FCL_REAL sqr_length = data.squaredNorm();
if(sqr_length > 0)
{
angle = 2.0 * acos((double)data[W]);
double inv_length = 1.0 / sqrt(sqr_length);
- axis[0] = inv_length * data[X];
- axis[1] = inv_length * data[Y];
- axis[2] = inv_length * data[Z];
+ axis.noalias() = inv_length * data.segment<3>(X);
}
else
{
@@ -204,52 +197,6 @@ void Quaternion3f::toAxisAngle(Vec3f& axis, FCL_REAL& angle) const
}
}
-FCL_REAL Quaternion3f::dot(const Quaternion3f& other) const
-{
- return data[W] * other.data[W] + data[X] * other.data[X] + data[Y] * other.data[Y] + data[Z] * other.data[Z];
-}
-
-Quaternion3f Quaternion3f::operator + (const Quaternion3f& other) const
-{
- return Quaternion3f(data[W] + other.data[W], data[X] + other.data[X],
- data[Y] + other.data[Y], data[Z] + other.data[Z]);
-}
-
-const Quaternion3f& Quaternion3f::operator += (const Quaternion3f& other)
-{
- data[W] += other.data[W];
- data[X] += other.data[X];
- data[Y] += other.data[Y];
- data[Z] += other.data[Z];
-
- return *this;
-}
-
-Quaternion3f Quaternion3f::operator - (const Quaternion3f& other) const
-{
- return Quaternion3f(data[W] - other.data[W], data[X] - other.data[X],
- data[Y] - other.data[Y], data[Z] - other.data[Z]);
-}
-
-const Quaternion3f& Quaternion3f::operator -= (const Quaternion3f& other)
-{
- data[W] -= other.data[W];
- data[X] -= other.data[X];
- data[Y] -= other.data[Y];
- data[Z] -= other.data[Z];
-
- return *this;
-}
-
-Quaternion3f Quaternion3f::operator * (const Quaternion3f& other) const
-{
- return Quaternion3f(data[W] * other.data[W] - data[X] * other.data[X] - data[Y] * other.data[Y] - data[Z] * other.data[Z],
- data[W] * other.data[X] + data[X] * other.data[W] + data[Y] * other.data[Z] - data[Z] * other.data[Y],
- data[W] * other.data[Y] - data[X] * other.data[Z] + data[Y] * other.data[W] + data[Z] * other.data[X],
- data[W] * other.data[Z] + data[X] * other.data[Y] - data[Y] * other.data[X] + data[Z] * other.data[W]);
-}
-
-
const Quaternion3f& Quaternion3f::operator *= (const Quaternion3f& other)
{
FCL_REAL a = data[W] * other.data[W] - data[X] * other.data[X] - data[Y] * other.data[Y] - data[Z] * other.data[Z];
@@ -284,56 +231,6 @@ const Quaternion3f& Quaternion3f::operator *= (FCL_REAL t)
return *this;
}
-
-Quaternion3f& Quaternion3f::conj()
-{
- data[X] = -data[X];
- data[Y] = -data[Y];
- data[Z] = -data[Z];
- return *this;
-}
-
-Quaternion3f& Quaternion3f::inverse()
-{
- FCL_REAL sqr_length = data[W] * data[W] + data[X] * data[X] + data[Y] * data[Y] + data[Z] * data[Z];
- if(sqr_length > 0)
- {
- FCL_REAL inv_length = 1 / std::sqrt(sqr_length);
- data[W] *= inv_length;
- data[X] *= (-inv_length);
- data[Y] *= (-inv_length);
- data[Z] *= (-inv_length);
- }
- else
- {
- data[X] = -data[X];
- data[Y] = -data[Y];
- data[Z] = -data[Z];
- }
-
- return *this;
-}
-
-Vec3f Quaternion3f::transform(const Vec3f& v) const
-{
- Vec3f u(x(), y(), z());
- double s = w();
- Vec3f vprime = 2*u.dot(v)*u + (s*s - u.dot(u))*v + 2*s*u.cross(v);
- return vprime;
-}
-
-Quaternion3f conj(const Quaternion3f& q)
-{
- Quaternion3f r(q);
- return r.conj();
-}
-
-Quaternion3f inverse(const Quaternion3f& q)
-{
- Quaternion3f res(q);
- return res.inverse();
-}
-
void Quaternion3f::fromEuler(FCL_REAL a, FCL_REAL b, FCL_REAL c)
{
Matrix3f R;
@@ -388,14 +285,14 @@ Transform3f inverse(const Transform3f& tf)
void relativeTransform(const Transform3f& tf1, const Transform3f& tf2,
Transform3f& tf)
{
- const Quaternion3f& q1_inv = fcl::conj(tf1.getQuatRotation());
+ const Quaternion3f& q1_inv = tf1.getQuatRotation().conj();
tf = Transform3f(q1_inv * tf2.getQuatRotation(), q1_inv.transform(tf2.getTranslation() - tf1.getTranslation()));
}
void relativeTransform2(const Transform3f& tf1, const Transform3f& tf2,
Transform3f& tf)
{
- const Quaternion3f& q1inv = fcl::conj(tf1.getQuatRotation());
+ const Quaternion3f& q1inv = tf1.getQuatRotation().conj();
const Quaternion3f& q2_q1inv = tf2.getQuatRotation() * q1inv;
tf = Transform3f(q2_q1inv, tf2.getTranslation() - q2_q1inv.transform(tf1.getTranslation()));
}
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index 84a30982ef5c78149558ad12201a0aabb9293e26..e38433730e7e080dd3b556be14d18f7f14ac550b 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -267,7 +267,7 @@ void computeBV<AABB, Sphere>(const Sphere& s, const Transform3f& tf, AABB& bv)
{
const Vec3f& T = tf.getTranslation();
- Vec3f v_delta(s.radius);
+ Vec3f v_delta(Vec3f::Constant(s.radius));
bv.max_ = T + v_delta;
bv.min_ = T - v_delta;
}
@@ -348,8 +348,8 @@ void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf, AABB&
const FCL_REAL& d = new_s.d;
AABB bv_;
- bv_.min_ = Vec3f(-std::numeric_limits<FCL_REAL>::max());
- bv_.max_ = Vec3f(std::numeric_limits<FCL_REAL>::max());
+ bv_.min_ = Vec3f::Constant(-std::numeric_limits<FCL_REAL>::max());
+ bv_.max_ = Vec3f::Constant(std::numeric_limits<FCL_REAL>::max());
if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
{
// normal aligned with x axis
@@ -380,8 +380,8 @@ void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv)
const FCL_REAL& d = new_s.d;
AABB bv_;
- bv_.min_ = Vec3f(-std::numeric_limits<FCL_REAL>::max());
- bv_.max_ = Vec3f(std::numeric_limits<FCL_REAL>::max());
+ bv_.min_ = Vec3f::Constant(-std::numeric_limits<FCL_REAL>::max());
+ bv_.max_ = Vec3f::Constant(std::numeric_limits<FCL_REAL>::max());
if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
{
// normal aligned with x axis