diff --git a/CMakeLists.txt b/CMakeLists.txt
index 80adcfdf123ee6db15f378ab6dff8979a0504f87..38bb394cb4ed2dfe60f7a2864f67b005333cbb18 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -48,6 +48,7 @@ setup_project()
set(FCL_HAVE_SSE FALSE CACHE BOOL "Enable SSE vectorization")
set(FCL_HAVE_EIGEN FALSE CACHE BOOL "Use eigen wrappers")
+set(FCL_USE_NATIVE_EIGEN FALSE CACHE BOOL "Use native eigen matrix type when possible")
add_optional_dependency("eigen3 >= 3.0.0")
if (EIGEN3_FOUND)
diff --git a/include/hpp/fcl/config-fcl.hh.in b/include/hpp/fcl/config-fcl.hh.in
index fa302b1b802558eb6c5a2676397b43d404444024..0b1506c71d9d091626037585156e5ab61da6decd 100644
--- a/include/hpp/fcl/config-fcl.hh.in
+++ b/include/hpp/fcl/config-fcl.hh.in
@@ -38,6 +38,8 @@
# include "config.h"
#cmakedefine01 FCL_HAVE_EIGEN
+#cmakedefine01 FCL_USE_NATIVE_EIGEN
+
#cmakedefine01 FCL_HAVE_SSE
#cmakedefine01 FCL_HAVE_OCTOMAP
#cmakedefine01 FCL_HAVE_FLANN
diff --git a/include/hpp/fcl/eigen/math_details.h b/include/hpp/fcl/eigen/math_details.h
index 3867583ddf733d01db6103caae9a868d216a8395..207e797d4e5045e814405fd9ae6755bb04ed029f 100644
--- a/include/hpp/fcl/eigen/math_details.h
+++ b/include/hpp/fcl/eigen/math_details.h
@@ -33,6 +33,8 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
+/** \author Joseph Mirabel */
+
#ifndef FCL_EIGEN_DETAILS_H
#define FCL_EIGEN_DETAILS_H
diff --git a/include/hpp/fcl/eigen/matrix_3fx.h b/include/hpp/fcl/eigen/matrix_3fx.h
new file mode 100644
index 0000000000000000000000000000000000000000..8e9356ce94043b8f9ed709ff4ebbbc8998add19e
--- /dev/null
+++ b/include/hpp/fcl/eigen/matrix_3fx.h
@@ -0,0 +1,469 @@
+/*
+ * 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 Jia Pan */
+
+#ifndef FCL_EIGEN_MATRIX_3F_H
+#define FCL_EIGEN_MATRIX_3F_H
+
+namespace fcl
+{
+
+/// @brief Matrix2 class wrapper. the core data is in the template parameter class
+template<typename T>
+class Matrix3fX
+ : Eigen::Matrix <T, 3, 1>
+{
+public:
+ // typedef typename T::vector_type S;
+
+ Matrix3fX() {}
+ Matrix3fX(U xx, U xy, U xz,
+ U yx, U yy, U yz,
+ U zx, U zy, U zz) : data(xx, xy, xz, yx, yy, yz, zx, zy, zz)
+ {}
+
+ Matrix3fX(const Vec3fX<S>& v1, const Vec3fX<S>& v2, const Vec3fX<S>& v3)
+ : data(v1.data, v2.data, v3.data) {}
+
+ Matrix3fX(const Matrix3fX<T>& other) : data(other.data) {}
+
+ Matrix3fX(const T& data_) : data(data_) {}
+
+ inline Vec3fX<S> getColumn(size_t i) const
+ {
+ return Vec3fX<S>(data.getColumn(i));
+ }
+
+ inline Vec3fX<S> getRow(size_t i) const
+ {
+ return Vec3fX<S>(data.getRow(i));
+ }
+
+ inline U operator () (size_t i, size_t j) const
+ {
+ return data(i, j);
+ }
+
+ inline U& operator () (size_t i, size_t j)
+ {
+ return data(i, j);
+ }
+
+ inline Vec3fX<S> operator * (const Vec3fX<S>& v) const
+ {
+ return Vec3fX<S>(data * v.data);
+ }
+
+ inline Matrix3fX<T> operator * (const Matrix3fX<T>& m) const
+ {
+ return Matrix3fX<T>(data * m.data);
+ }
+
+ inline Matrix3fX<T> operator + (const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data + other.data);
+ }
+
+ inline Matrix3fX<T> operator - (const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data - other.data);
+ }
+
+ inline Matrix3fX<T> operator + (U c) const
+ {
+ return Matrix3fX<T>(data + c);
+ }
+
+ inline Matrix3fX<T> operator - (U c) const
+ {
+ return Matrix3fX<T>(data - c);
+ }
+
+ inline Matrix3fX<T> operator * (U c) const
+ {
+ return Matrix3fX<T>(data * c);
+ }
+
+ inline Matrix3fX<T> operator / (U c) const
+ {
+ return Matrix3fX<T>(data / c);
+ }
+
+ inline Matrix3fX<T>& operator *= (const Matrix3fX<T>& other)
+ {
+ data *= other.data;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator += (const Matrix3fX<T>& other)
+ {
+ data += other.data;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator -= (const Matrix3fX<T>& other)
+ {
+ data -= other.data;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator += (U c)
+ {
+ data += c;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator -= (U c)
+ {
+ data -= c;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator *= (U c)
+ {
+ data *= c;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator /= (U c)
+ {
+ data /= c;
+ return *this;
+ }
+
+ inline void setIdentity()
+ {
+ data.setIdentity();
+ }
+
+ inline bool isIdentity () const
+ {
+ return
+ data (0,0) == 1 && data (0,1) == 0 && data (0,2) == 0 &&
+ data (1,0) == 0 && data (1,1) == 1 && data (1,2) == 0 &&
+ data (2,0) == 0 && data (2,1) == 0 && data (2,2) == 1;
+ }
+
+ inline void setZero()
+ {
+ data.setZero();
+ }
+
+ /// @brief Set the matrix from euler angles YPR around ZYX axes
+ /// @param eulerX Roll about X axis
+ /// @param eulerY Pitch around Y axis
+ /// @param eulerZ Yaw aboud Z axis
+ ///
+ /// These angles are used to produce a rotation matrix. The euler
+ /// angles are applied in ZYX order. I.e a vector is first rotated
+ /// about X then Y and then Z
+ inline void setEulerZYX(FCL_REAL eulerX, FCL_REAL eulerY, FCL_REAL eulerZ)
+ {
+ FCL_REAL ci(cos(eulerX));
+ FCL_REAL cj(cos(eulerY));
+ FCL_REAL ch(cos(eulerZ));
+ FCL_REAL si(sin(eulerX));
+ FCL_REAL sj(sin(eulerY));
+ FCL_REAL sh(sin(eulerZ));
+ FCL_REAL cc = ci * ch;
+ FCL_REAL cs = ci * sh;
+ FCL_REAL sc = si * ch;
+ FCL_REAL ss = si * sh;
+
+ setValue(cj * ch, sj * sc - cs, sj * cc + ss,
+ cj * sh, sj * ss + cc, sj * cs - sc,
+ -sj, cj * si, cj * ci);
+
+ }
+
+ /// @brief Set the matrix from euler angles using YPR around YXZ respectively
+ /// @param yaw Yaw about Y axis
+ /// @param pitch Pitch about X axis
+ /// @param roll Roll about Z axis
+ void setEulerYPR(FCL_REAL yaw, FCL_REAL pitch, FCL_REAL roll)
+ {
+ setEulerZYX(roll, pitch, yaw);
+ }
+
+ inline U determinant() const
+ {
+ return data.determinant();
+ }
+
+ Matrix3fX<T>& transpose()
+ {
+ data.transpose();
+ return *this;
+ }
+
+ Matrix3fX<T>& inverse()
+ {
+ data.inverse();
+ return *this;
+ }
+
+ Matrix3fX<T>& abs()
+ {
+ data.abs();
+ return *this;
+ }
+
+ static const Matrix3fX<T>& getIdentity()
+ {
+ static const Matrix3fX<T> I((U)1, (U)0, (U)0,
+ (U)0, (U)1, (U)0,
+ (U)0, (U)0, (U)1);
+ return I;
+ }
+
+ Matrix3fX<T> transposeTimes(const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data.transposeTimes(other.data));
+ }
+
+ Matrix3fX<T> timesTranspose(const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data.timesTranspose(other.data));
+ }
+
+ Vec3fX<S> transposeTimes(const Vec3fX<S>& v) const
+ {
+ return Vec3fX<S>(data.transposeTimes(v.data));
+ }
+
+ Matrix3fX<T> tensorTransform(const Matrix3fX<T>& m) const
+ {
+ Matrix3fX<T> res(*this);
+ res *= m;
+ return res.timesTranspose(*this);
+ }
+
+ // (1 0 0)^T (*this)^T v
+ inline U transposeDotX(const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(0, v.data);
+ }
+
+ // (0 1 0)^T (*this)^T v
+ inline U transposeDotY(const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(1, v.data);
+ }
+
+ // (0 0 1)^T (*this)^T v
+ inline U transposeDotZ(const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(2, v.data);
+ }
+
+ // (\delta_{i3})^T (*this)^T v
+ inline U transposeDot(size_t i, const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(i, v.data);
+ }
+
+ // (1 0 0)^T (*this) v
+ inline U dotX(const Vec3fX<S>& v) const
+ {
+ return data.dot(0, v.data);
+ }
+
+ // (0 1 0)^T (*this) v
+ inline U dotY(const Vec3fX<S>& v) const
+ {
+ return data.dot(1, v.data);
+ }
+
+ // (0 0 1)^T (*this) v
+ inline U dotZ(const Vec3fX<S>& v) const
+ {
+ return data.dot(2, v.data);
+ }
+
+ // (\delta_{i3})^T (*this) v
+ inline U dot(size_t i, const Vec3fX<S>& v) const
+ {
+ return data.dot(i, v.data);
+ }
+
+ inline void setValue(U xx, U xy, U xz,
+ U yx, U yy, U yz,
+ U zx, U zy, U zz)
+ {
+ data.setValue(xx, xy, xz,
+ yx, yy, yz,
+ zx, zy, zz);
+ }
+
+ inline void setValue(U x)
+ {
+ data.setValue(x);
+ }
+};
+
+template<typename T>
+void hat(Matrix3fX<T>& mat, const Vec3fX<typename T::vector_type>& vec)
+{
+ mat.setValue(0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0);
+}
+
+template<typename T>
+void relativeTransform(const Matrix3fX<T>& R1, const Vec3fX<typename T::vector_type>& t1,
+ const Matrix3fX<T>& R2, const Vec3fX<typename T::vector_type>& t2,
+ Matrix3fX<T>& R, Vec3fX<typename T::vector_type>& t)
+{
+ R = R1.transposeTimes(R2);
+ t = R1.transposeTimes(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 T>
+void eigen(const Matrix3fX<T>& m, typename T::meta_type dout[3], Vec3fX<typename T::vector_type> vout[3])
+{
+ Matrix3fX<T> R(m);
+ int n = 3;
+ int j, iq, ip, i;
+ typename T::meta_type tresh, theta, tau, t, sm, s, h, g, c;
+ int nrot;
+ typename T::meta_type b[3];
+ typename T::meta_type z[3];
+ typename T::meta_type v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+ typename T::meta_type 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].setValue(v[0][0], v[0][1], v[0][2]);
+ vout[1].setValue(v[1][0], v[1][1], v[1][2]);
+ vout[2].setValue(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 T>
+Matrix3fX<T> abs(const Matrix3fX<T>& R)
+{
+ return Matrix3fX<T>(abs(R.data));
+}
+
+template<typename T>
+Matrix3fX<T> transpose(const Matrix3fX<T>& R)
+{
+ return Matrix3fX<T>(transpose(R.data));
+}
+
+template<typename T>
+Matrix3fX<T> inverse(const Matrix3fX<T>& R)
+{
+ return Matrix3fX<T>(inverse(R.data));
+}
+
+template<typename T>
+typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename T::vector_type>& v)
+{
+ return v.dot(R * v);
+}
+
+}
+
+
+
+#endif
diff --git a/include/hpp/fcl/eigen/vec_3fx.h b/include/hpp/fcl/eigen/vec_3fx.h
new file mode 100644
index 0000000000000000000000000000000000000000..da68282678a4cc779b9eacdeee353f2a3aeedfda
--- /dev/null
+++ b/include/hpp/fcl/eigen/vec_3fx.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_EIGEN_VEC_3F_H
+#define FCL_EIGEN_VEC_3F_H
+
+#include <hpp/fcl/config-fcl.hh>
+#include <hpp/fcl/data_types.h>
+
+#include <hpp/fcl/eigen/math_details.h>
+
+#include <cmath>
+#include <iostream>
+#include <limits>
+
+
+namespace fcl
+{
+
+/// @brief Vector3 class wrapper. The core data is in the template parameter class.
+template <typename T>
+class Vec3fX :
+ Eigen::Matrix <T, 3, 1>
+{
+public:
+ typedef Eigen::Matrix <T, 3, 1> Base;
+
+ Vec3fX(void): Base() {}
+
+ // This constructor allows you to construct MyVectorType from Eigen expressions
+ template<typename OtherDerived>
+ Vec3fX(const Eigen::MatrixBase<OtherDerived>& other)
+ : Base(other)
+ {}
+
+ // This method allows you to assign Eigen expressions to MyVectorType
+ template<typename OtherDerived>
+ Vec3fX& operator=(const Eigen::MatrixBase <OtherDerived>& other)
+ {
+ this->Base::operator=(other);
+ return *this;
+ }
+
+ /// @brief create Vector (x, y, z)
+ Vec3fX(T x, T y, T z) : Base(x, y, z) {}
+
+ /// @brief create vector (x, x, x)
+ Vec3fX(U x) : Base(Base::Constant (x)) {}
+
+ /// @brief create vector using the internal data type
+ // Vec3fX(const T& data_) : data(data_) {}
+
+ // inline U operator [] (size_t i) const { return data[i]; }
+ // inline U& operator [] (size_t i) { return data[i]; }
+
+ // inline Vec3fX operator + (const Vec3fX& other) const { return Vec3fX(data + other.data); }
+ // inline Vec3fX operator - (const Vec3fX& other) const { return Vec3fX(data - other.data); }
+ // inline Vec3fX operator * (const Vec3fX& other) const { return Vec3fX(data * other.data); }
+ // inline Vec3fX operator / (const Vec3fX& other) const { return Vec3fX(data / other.data); }
+ // inline Vec3fX& operator += (const Vec3fX& other) { data += other.data; return *this; }
+ // inline Vec3fX& operator -= (const Vec3fX& other) { data -= other.data; return *this; }
+ // inline Vec3fX& operator *= (const Vec3fX& other) { data *= other.data; return *this; }
+ // inline Vec3fX& operator /= (const Vec3fX& other) { data /= other.data; return *this; }
+ // inline Vec3fX operator + (U t) const { return Vec3fX(data + t); }
+ // inline Vec3fX operator - (U t) const { return Vec3fX(data - t); }
+ // inline Vec3fX operator * (U t) const { return Vec3fX(data * t); }
+ // inline Vec3fX operator / (U t) const { return Vec3fX(data / t); }
+ // inline Vec3fX& operator += (U t) { data += t; return *this; }
+ // inline Vec3fX& operator -= (U t) { data -= t; return *this; }
+ // inline Vec3fX& operator *= (U t) { data *= t; return *this; }
+ // inline Vec3fX& operator /= (U t) { data /= t; return *this; }
+ // inline Vec3fX operator - () const { return Vec3fX(-data); }
+ // inline Vec3fX cross(const Vec3fX& other) const { return Vec3fX(details::cross_prod(data, other.data)); }
+ // inline U dot(const Vec3fX& other) const { return details::dot_prod3(data, other.data); }
+ inline Vec3fX& normalize()
+ {
+ T sqr_length = this->squaredNorm();
+ if(sqr_length > 0) this->operator/= ((T)sqrt(sqr_length));
+ return *this;
+ }
+
+ inline Vec3fX& normalize(bool* signal)
+ {
+ T sqr_length = this->squaredNorm();
+ if(sqr_length > 0)
+ {
+ this->operator/= ((T)sqrt(sqr_length));
+ *signal = true;
+ }
+ else
+ *signal = false;
+ return *this;
+ }
+
+ inline Vec3fX& abs()
+ {
+ *this = this->cwiseAbs ();
+ return *this;
+ }
+
+ inline T length() const { return this->norm(); }
+ // inline T norm() const { return sqrt(details::dot_prod3(data, data)); }
+ inline T sqrLength() const { return this->squaredNorm(); }
+ // inline T squaredNorm() const { return details::dot_prod3(data, data); }
+ inline void setValue(T x, T y, T z) { m_storage.data() = { x, y, z } }
+ inline void setValue(T x) { this->setConstant (x); }
+ // inline void setZero () {data.setValue (0); }
+ inline bool equal(const Vec3fX& other, T epsilon = std::numeric_limits<U>::epsilon() * 100) const
+ {
+ return ((*this - other).cwiseAbs().array () < epsilon).all();
+ }
+ inline Vec3fX<T>& negate() { *this = -*this; return *this; }
+
+ bool operator == (const Vec3fX& other) const
+ {
+ return equal(other, 0);
+ }
+
+ bool operator != (const Vec3fX& other) const
+ {
+ return !(*this == other);
+ }
+
+
+ inline Vec3fX<T>& ubound(const Vec3fX<T>& u)
+ {
+ *this = this->cwiseMin (u);
+ return *this;
+ }
+
+ inline Vec3fX<T>& lbound(const Vec3fX<T>& l)
+ {
+ *this = this->cwiseMax (l);
+ return *this;
+ }
+
+ bool isZero() const
+ {
+ return (m_storage.data()[0] == 0)
+ && (m_storage.data()[1] == 0)
+ && (m_storage.data()[2] == 0);
+ }
+
+};
+
+template<typename T>
+static inline Vec3fX<T> normalize(const Vec3fX<T>& v)
+{
+ typename T::meta_type sqr_length = v.squaredNorm ();
+ if(sqr_length > 0)
+ return v / (typename T::meta_type)sqrt(sqr_length);
+ else
+ return v;
+}
+
+template <typename T>
+static inline typename T::meta_type triple(const Vec3fX<T>& x, const Vec3fX<T>& y, const Vec3fX<T>& z)
+{
+ return x.dot(y.cross(z));
+}
+
+template <typename T>
+std::ostream& operator << (std::ostream& out, const Vec3fX<T>& x)
+{
+ out << x[0] << " " << x[1] << " " << x[2];
+ return out;
+}
+
+template <typename T>
+// static inline Vec3fX<T> min(const Vec3fX<T>& x, const Vec3fX<T>& y)
+static inline
+const Eigen::CwiseBinaryOp <Eigen::internal::scalar_min_op<T>, const Vec3fX<T>, const Vec3fX<T> >
+ min(const Vec3fX<T>& x, const Vec3fX<T>& y)
+{
+ return x.cwiseMin (y);
+}
+
+template <typename T>
+// static inline Vec3fX<T> max(const Vec3fX<T>& x, const Vec3fX<T>& y)
+static inline
+const Eigen::CwiseBinaryOp<Eigen::internal::scalar_max_op<T>, const Vec3fX<T>, const Vec3fX<T> >
+max(const Vec3fX<T>& x, const Vec3fX<T>& y)
+{
+ return x.cwiseMax (y);
+}
+
+template <typename T>
+// static inline Vec3fX<T> abs(const Vec3fX<T>& x)
+static inline
+const Eigen::CwiseUnaryOp<Eigen::internal::scalar_abs_op<T>, const Vec3fX<T> >
+abs(const Vec3fX<T>& x)
+{
+ return x.cwiseAbs ();
+}
+
+template <typename T>
+void generateCoordinateSystem(const Vec3fX<T>& w, Vec3fX<T>& u, Vec3fX<T>& v)
+{
+ typedef typename T::meta_type U;
+ U inv_length;
+ if(std::abs(w[0]) >= std::abs(w[1]))
+ {
+ inv_length = (U)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
+ u[0] = -w[2] * inv_length;
+ u[1] = (U)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 = (U)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
+ u[0] = (U)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];
+ }
+}
+
+ // template <typename T>
+ // inline Vec3fX <T> operator * (const typename Vec3fX <T>::U& t,
+ // const Vec3fX <T>& v)
+ // {
+ // return Vec3fX <T> (v.data * t);
+ // }
+
+
+}
+
+
+#endif
diff --git a/include/hpp/fcl/math/matrix_3f.h b/include/hpp/fcl/math/matrix_3f.h
index 6a679bc09b4dfc18420982ea999ef9da58ebc6b9..03f562cab45652e3c56226e7c9d27a573650b930 100644
--- a/include/hpp/fcl/math/matrix_3f.h
+++ b/include/hpp/fcl/math/matrix_3f.h
@@ -40,434 +40,21 @@
#include <hpp/fcl/math/vec_3f.h>
-namespace fcl
-{
-
-/// @brief Matrix2 class wrapper. the core data is in the template parameter class
-template<typename T>
-class Matrix3fX
-{
-public:
- typedef typename T::meta_type U;
- typedef typename T::vector_type S;
- T data;
-
- Matrix3fX() {}
- Matrix3fX(U xx, U xy, U xz,
- U yx, U yy, U yz,
- U zx, U zy, U zz) : data(xx, xy, xz, yx, yy, yz, zx, zy, zz)
- {}
-
- Matrix3fX(const Vec3fX<S>& v1, const Vec3fX<S>& v2, const Vec3fX<S>& v3)
- : data(v1.data, v2.data, v3.data) {}
-
- Matrix3fX(const Matrix3fX<T>& other) : data(other.data) {}
-
- Matrix3fX(const T& data_) : data(data_) {}
-
- inline Vec3fX<S> getColumn(size_t i) const
- {
- return Vec3fX<S>(data.getColumn(i));
- }
-
- inline Vec3fX<S> getRow(size_t i) const
- {
- return Vec3fX<S>(data.getRow(i));
- }
-
- inline U operator () (size_t i, size_t j) const
- {
- return data(i, j);
- }
-
- inline U& operator () (size_t i, size_t j)
- {
- return data(i, j);
- }
-
- inline Vec3fX<S> operator * (const Vec3fX<S>& v) const
- {
- return Vec3fX<S>(data * v.data);
- }
-
- inline Matrix3fX<T> operator * (const Matrix3fX<T>& m) const
- {
- return Matrix3fX<T>(data * m.data);
- }
-
- inline Matrix3fX<T> operator + (const Matrix3fX<T>& other) const
- {
- return Matrix3fX<T>(data + other.data);
- }
-
- inline Matrix3fX<T> operator - (const Matrix3fX<T>& other) const
- {
- return Matrix3fX<T>(data - other.data);
- }
-
- inline Matrix3fX<T> operator + (U c) const
- {
- return Matrix3fX<T>(data + c);
- }
-
- inline Matrix3fX<T> operator - (U c) const
- {
- return Matrix3fX<T>(data - c);
- }
-
- inline Matrix3fX<T> operator * (U c) const
- {
- return Matrix3fX<T>(data * c);
- }
-
- inline Matrix3fX<T> operator / (U c) const
- {
- return Matrix3fX<T>(data / c);
- }
-
- inline Matrix3fX<T>& operator *= (const Matrix3fX<T>& other)
- {
- data *= other.data;
- return *this;
- }
-
- inline Matrix3fX<T>& operator += (const Matrix3fX<T>& other)
- {
- data += other.data;
- return *this;
- }
-
- inline Matrix3fX<T>& operator -= (const Matrix3fX<T>& other)
- {
- data -= other.data;
- return *this;
- }
-
- inline Matrix3fX<T>& operator += (U c)
- {
- data += c;
- return *this;
- }
-
- inline Matrix3fX<T>& operator -= (U c)
- {
- data -= c;
- return *this;
- }
-
- inline Matrix3fX<T>& operator *= (U c)
- {
- data *= c;
- return *this;
- }
-
- inline Matrix3fX<T>& operator /= (U c)
- {
- data /= c;
- return *this;
- }
-
- inline void setIdentity()
- {
- data.setIdentity();
- }
-
- inline bool isIdentity () const
- {
- return
- data (0,0) == 1 && data (0,1) == 0 && data (0,2) == 0 &&
- data (1,0) == 0 && data (1,1) == 1 && data (1,2) == 0 &&
- data (2,0) == 0 && data (2,1) == 0 && data (2,2) == 1;
- }
-
- inline void setZero()
- {
- data.setZero();
- }
-
- /// @brief Set the matrix from euler angles YPR around ZYX axes
- /// @param eulerX Roll about X axis
- /// @param eulerY Pitch around Y axis
- /// @param eulerZ Yaw aboud Z axis
- ///
- /// These angles are used to produce a rotation matrix. The euler
- /// angles are applied in ZYX order. I.e a vector is first rotated
- /// about X then Y and then Z
- inline void setEulerZYX(FCL_REAL eulerX, FCL_REAL eulerY, FCL_REAL eulerZ)
- {
- FCL_REAL ci(cos(eulerX));
- FCL_REAL cj(cos(eulerY));
- FCL_REAL ch(cos(eulerZ));
- FCL_REAL si(sin(eulerX));
- FCL_REAL sj(sin(eulerY));
- FCL_REAL sh(sin(eulerZ));
- FCL_REAL cc = ci * ch;
- FCL_REAL cs = ci * sh;
- FCL_REAL sc = si * ch;
- FCL_REAL ss = si * sh;
-
- setValue(cj * ch, sj * sc - cs, sj * cc + ss,
- cj * sh, sj * ss + cc, sj * cs - sc,
- -sj, cj * si, cj * ci);
-
- }
-
- /// @brief Set the matrix from euler angles using YPR around YXZ respectively
- /// @param yaw Yaw about Y axis
- /// @param pitch Pitch about X axis
- /// @param roll Roll about Z axis
- void setEulerYPR(FCL_REAL yaw, FCL_REAL pitch, FCL_REAL roll)
- {
- setEulerZYX(roll, pitch, yaw);
- }
-
- inline U determinant() const
- {
- return data.determinant();
- }
-
- Matrix3fX<T>& transpose()
- {
- data.transpose();
- return *this;
- }
-
- Matrix3fX<T>& inverse()
- {
- data.inverse();
- return *this;
- }
-
- Matrix3fX<T>& abs()
- {
- data.abs();
- return *this;
- }
-
- static const Matrix3fX<T>& getIdentity()
- {
- static const Matrix3fX<T> I((U)1, (U)0, (U)0,
- (U)0, (U)1, (U)0,
- (U)0, (U)0, (U)1);
- return I;
- }
-
- Matrix3fX<T> transposeTimes(const Matrix3fX<T>& other) const
- {
- return Matrix3fX<T>(data.transposeTimes(other.data));
- }
-
- Matrix3fX<T> timesTranspose(const Matrix3fX<T>& other) const
- {
- return Matrix3fX<T>(data.timesTranspose(other.data));
- }
-
- Vec3fX<S> transposeTimes(const Vec3fX<S>& v) const
- {
- return Vec3fX<S>(data.transposeTimes(v.data));
- }
-
- Matrix3fX<T> tensorTransform(const Matrix3fX<T>& m) const
- {
- Matrix3fX<T> res(*this);
- res *= m;
- return res.timesTranspose(*this);
- }
-
- // (1 0 0)^T (*this)^T v
- inline U transposeDotX(const Vec3fX<S>& v) const
- {
- return data.transposeDot(0, v.data);
- }
-
- // (0 1 0)^T (*this)^T v
- inline U transposeDotY(const Vec3fX<S>& v) const
- {
- return data.transposeDot(1, v.data);
- }
-
- // (0 0 1)^T (*this)^T v
- inline U transposeDotZ(const Vec3fX<S>& v) const
- {
- return data.transposeDot(2, v.data);
- }
-
- // (\delta_{i3})^T (*this)^T v
- inline U transposeDot(size_t i, const Vec3fX<S>& v) const
- {
- return data.transposeDot(i, v.data);
- }
-
- // (1 0 0)^T (*this) v
- inline U dotX(const Vec3fX<S>& v) const
- {
- return data.dot(0, v.data);
- }
-
- // (0 1 0)^T (*this) v
- inline U dotY(const Vec3fX<S>& v) const
- {
- return data.dot(1, v.data);
- }
-
- // (0 0 1)^T (*this) v
- inline U dotZ(const Vec3fX<S>& v) const
- {
- return data.dot(2, v.data);
- }
-
- // (\delta_{i3})^T (*this) v
- inline U dot(size_t i, const Vec3fX<S>& v) const
- {
- return data.dot(i, v.data);
- }
-
- inline void setValue(U xx, U xy, U xz,
- U yx, U yy, U yz,
- U zx, U zy, U zz)
- {
- data.setValue(xx, xy, xz,
- yx, yy, yz,
- zx, zy, zz);
- }
-
- inline void setValue(U x)
- {
- data.setValue(x);
- }
-};
-
-template<typename T>
-void hat(Matrix3fX<T>& mat, const Vec3fX<typename T::vector_type>& vec)
-{
- mat.setValue(0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0);
-}
-
-template<typename T>
-void relativeTransform(const Matrix3fX<T>& R1, const Vec3fX<typename T::vector_type>& t1,
- const Matrix3fX<T>& R2, const Vec3fX<typename T::vector_type>& t2,
- Matrix3fX<T>& R, Vec3fX<typename T::vector_type>& t)
-{
- R = R1.transposeTimes(R2);
- t = R1.transposeTimes(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 T>
-void eigen(const Matrix3fX<T>& m, typename T::meta_type dout[3], Vec3fX<typename T::vector_type> vout[3])
-{
- Matrix3fX<T> R(m);
- int n = 3;
- int j, iq, ip, i;
- typename T::meta_type tresh, theta, tau, t, sm, s, h, g, c;
- int nrot;
- typename T::meta_type b[3];
- typename T::meta_type z[3];
- typename T::meta_type v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
- typename T::meta_type 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].setValue(v[0][0], v[0][1], v[0][2]);
- vout[1].setValue(v[1][0], v[1][1], v[1][2]);
- vout[2].setValue(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 T>
-Matrix3fX<T> abs(const Matrix3fX<T>& R)
-{
- return Matrix3fX<T>(abs(R.data));
-}
-
-template<typename T>
-Matrix3fX<T> transpose(const Matrix3fX<T>& R)
-{
- return Matrix3fX<T>(transpose(R.data));
-}
-
-template<typename T>
-Matrix3fX<T> inverse(const Matrix3fX<T>& R)
-{
- return Matrix3fX<T>(inverse(R.data));
-}
+#if FCL_USE_NATIVE_EIGEN
+# include <hpp/fcl/eigen/matrix_3fx.h>
+#else
+# include <hpp/fcl/math/matrix_3fx.h>
+#endif
-template<typename T>
-typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename T::vector_type>& v)
+namespace fcl
{
- return v.dot(R * v);
-}
-
#if FCL_HAVE_EIGEN
-typedef Matrix3fX<details::eigen_m3<FCL_REAL> > Matrix3f;
+# if FCL_USE_NATIVE_EIGEN
+ typedef Matrix3fX<FCL_REAL> Matrix3f;
+# else
+ typedef Matrix3fX<details::eigen_m3<FCL_REAL> > Matrix3f;
+# endif
#elif FCL_HAVE_SSE
typedef Matrix3fX<details::sse_meta_f12> Matrix3f;
#else
diff --git a/include/hpp/fcl/math/matrix_3fx.h b/include/hpp/fcl/math/matrix_3fx.h
new file mode 100644
index 0000000000000000000000000000000000000000..18584b0652ee56f850567b76672eff362301f54e
--- /dev/null
+++ b/include/hpp/fcl/math/matrix_3fx.h
@@ -0,0 +1,470 @@
+/*
+ * 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 Jia Pan */
+
+#ifndef FCL_MATRIX_3FX_H
+#define FCL_MATRIX_3FX_H
+
+#include <hpp/fcl/math/vec_3f.h>
+
+namespace fcl
+{
+
+/// @brief Matrix2 class wrapper. the core data is in the template parameter class
+template<typename T>
+class Matrix3fX
+{
+public:
+ typedef typename T::meta_type U;
+ typedef typename T::vector_type S;
+ T data;
+
+ Matrix3fX() {}
+ Matrix3fX(U xx, U xy, U xz,
+ U yx, U yy, U yz,
+ U zx, U zy, U zz) : data(xx, xy, xz, yx, yy, yz, zx, zy, zz)
+ {}
+
+ Matrix3fX(const Vec3fX<S>& v1, const Vec3fX<S>& v2, const Vec3fX<S>& v3)
+ : data(v1.data, v2.data, v3.data) {}
+
+ Matrix3fX(const Matrix3fX<T>& other) : data(other.data) {}
+
+ Matrix3fX(const T& data_) : data(data_) {}
+
+ inline Vec3fX<S> getColumn(size_t i) const
+ {
+ return Vec3fX<S>(data.getColumn(i));
+ }
+
+ inline Vec3fX<S> getRow(size_t i) const
+ {
+ return Vec3fX<S>(data.getRow(i));
+ }
+
+ inline U operator () (size_t i, size_t j) const
+ {
+ return data(i, j);
+ }
+
+ inline U& operator () (size_t i, size_t j)
+ {
+ return data(i, j);
+ }
+
+ inline Vec3fX<S> operator * (const Vec3fX<S>& v) const
+ {
+ return Vec3fX<S>(data * v.data);
+ }
+
+ inline Matrix3fX<T> operator * (const Matrix3fX<T>& m) const
+ {
+ return Matrix3fX<T>(data * m.data);
+ }
+
+ inline Matrix3fX<T> operator + (const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data + other.data);
+ }
+
+ inline Matrix3fX<T> operator - (const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data - other.data);
+ }
+
+ inline Matrix3fX<T> operator + (U c) const
+ {
+ return Matrix3fX<T>(data + c);
+ }
+
+ inline Matrix3fX<T> operator - (U c) const
+ {
+ return Matrix3fX<T>(data - c);
+ }
+
+ inline Matrix3fX<T> operator * (U c) const
+ {
+ return Matrix3fX<T>(data * c);
+ }
+
+ inline Matrix3fX<T> operator / (U c) const
+ {
+ return Matrix3fX<T>(data / c);
+ }
+
+ inline Matrix3fX<T>& operator *= (const Matrix3fX<T>& other)
+ {
+ data *= other.data;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator += (const Matrix3fX<T>& other)
+ {
+ data += other.data;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator -= (const Matrix3fX<T>& other)
+ {
+ data -= other.data;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator += (U c)
+ {
+ data += c;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator -= (U c)
+ {
+ data -= c;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator *= (U c)
+ {
+ data *= c;
+ return *this;
+ }
+
+ inline Matrix3fX<T>& operator /= (U c)
+ {
+ data /= c;
+ return *this;
+ }
+
+ inline void setIdentity()
+ {
+ data.setIdentity();
+ }
+
+ inline bool isIdentity () const
+ {
+ return
+ data (0,0) == 1 && data (0,1) == 0 && data (0,2) == 0 &&
+ data (1,0) == 0 && data (1,1) == 1 && data (1,2) == 0 &&
+ data (2,0) == 0 && data (2,1) == 0 && data (2,2) == 1;
+ }
+
+ inline void setZero()
+ {
+ data.setZero();
+ }
+
+ /// @brief Set the matrix from euler angles YPR around ZYX axes
+ /// @param eulerX Roll about X axis
+ /// @param eulerY Pitch around Y axis
+ /// @param eulerZ Yaw aboud Z axis
+ ///
+ /// These angles are used to produce a rotation matrix. The euler
+ /// angles are applied in ZYX order. I.e a vector is first rotated
+ /// about X then Y and then Z
+ inline void setEulerZYX(FCL_REAL eulerX, FCL_REAL eulerY, FCL_REAL eulerZ)
+ {
+ FCL_REAL ci(cos(eulerX));
+ FCL_REAL cj(cos(eulerY));
+ FCL_REAL ch(cos(eulerZ));
+ FCL_REAL si(sin(eulerX));
+ FCL_REAL sj(sin(eulerY));
+ FCL_REAL sh(sin(eulerZ));
+ FCL_REAL cc = ci * ch;
+ FCL_REAL cs = ci * sh;
+ FCL_REAL sc = si * ch;
+ FCL_REAL ss = si * sh;
+
+ setValue(cj * ch, sj * sc - cs, sj * cc + ss,
+ cj * sh, sj * ss + cc, sj * cs - sc,
+ -sj, cj * si, cj * ci);
+
+ }
+
+ /// @brief Set the matrix from euler angles using YPR around YXZ respectively
+ /// @param yaw Yaw about Y axis
+ /// @param pitch Pitch about X axis
+ /// @param roll Roll about Z axis
+ void setEulerYPR(FCL_REAL yaw, FCL_REAL pitch, FCL_REAL roll)
+ {
+ setEulerZYX(roll, pitch, yaw);
+ }
+
+ inline U determinant() const
+ {
+ return data.determinant();
+ }
+
+ Matrix3fX<T>& transpose()
+ {
+ data.transpose();
+ return *this;
+ }
+
+ Matrix3fX<T>& inverse()
+ {
+ data.inverse();
+ return *this;
+ }
+
+ Matrix3fX<T>& abs()
+ {
+ data.abs();
+ return *this;
+ }
+
+ static const Matrix3fX<T>& getIdentity()
+ {
+ static const Matrix3fX<T> I((U)1, (U)0, (U)0,
+ (U)0, (U)1, (U)0,
+ (U)0, (U)0, (U)1);
+ return I;
+ }
+
+ Matrix3fX<T> transposeTimes(const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data.transposeTimes(other.data));
+ }
+
+ Matrix3fX<T> timesTranspose(const Matrix3fX<T>& other) const
+ {
+ return Matrix3fX<T>(data.timesTranspose(other.data));
+ }
+
+ Vec3fX<S> transposeTimes(const Vec3fX<S>& v) const
+ {
+ return Vec3fX<S>(data.transposeTimes(v.data));
+ }
+
+ Matrix3fX<T> tensorTransform(const Matrix3fX<T>& m) const
+ {
+ Matrix3fX<T> res(*this);
+ res *= m;
+ return res.timesTranspose(*this);
+ }
+
+ // (1 0 0)^T (*this)^T v
+ inline U transposeDotX(const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(0, v.data);
+ }
+
+ // (0 1 0)^T (*this)^T v
+ inline U transposeDotY(const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(1, v.data);
+ }
+
+ // (0 0 1)^T (*this)^T v
+ inline U transposeDotZ(const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(2, v.data);
+ }
+
+ // (\delta_{i3})^T (*this)^T v
+ inline U transposeDot(size_t i, const Vec3fX<S>& v) const
+ {
+ return data.transposeDot(i, v.data);
+ }
+
+ // (1 0 0)^T (*this) v
+ inline U dotX(const Vec3fX<S>& v) const
+ {
+ return data.dot(0, v.data);
+ }
+
+ // (0 1 0)^T (*this) v
+ inline U dotY(const Vec3fX<S>& v) const
+ {
+ return data.dot(1, v.data);
+ }
+
+ // (0 0 1)^T (*this) v
+ inline U dotZ(const Vec3fX<S>& v) const
+ {
+ return data.dot(2, v.data);
+ }
+
+ // (\delta_{i3})^T (*this) v
+ inline U dot(size_t i, const Vec3fX<S>& v) const
+ {
+ return data.dot(i, v.data);
+ }
+
+ inline void setValue(U xx, U xy, U xz,
+ U yx, U yy, U yz,
+ U zx, U zy, U zz)
+ {
+ data.setValue(xx, xy, xz,
+ yx, yy, yz,
+ zx, zy, zz);
+ }
+
+ inline void setValue(U x)
+ {
+ data.setValue(x);
+ }
+};
+
+template<typename T>
+void hat(Matrix3fX<T>& mat, const Vec3fX<typename T::vector_type>& vec)
+{
+ mat.setValue(0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0);
+}
+
+template<typename T>
+void relativeTransform(const Matrix3fX<T>& R1, const Vec3fX<typename T::vector_type>& t1,
+ const Matrix3fX<T>& R2, const Vec3fX<typename T::vector_type>& t2,
+ Matrix3fX<T>& R, Vec3fX<typename T::vector_type>& t)
+{
+ R = R1.transposeTimes(R2);
+ t = R1.transposeTimes(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 T>
+void eigen(const Matrix3fX<T>& m, typename T::meta_type dout[3], Vec3fX<typename T::vector_type> vout[3])
+{
+ Matrix3fX<T> R(m);
+ int n = 3;
+ int j, iq, ip, i;
+ typename T::meta_type tresh, theta, tau, t, sm, s, h, g, c;
+ int nrot;
+ typename T::meta_type b[3];
+ typename T::meta_type z[3];
+ typename T::meta_type v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+ typename T::meta_type 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].setValue(v[0][0], v[0][1], v[0][2]);
+ vout[1].setValue(v[1][0], v[1][1], v[1][2]);
+ vout[2].setValue(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 T>
+Matrix3fX<T> abs(const Matrix3fX<T>& R)
+{
+ return Matrix3fX<T>(abs(R.data));
+}
+
+template<typename T>
+Matrix3fX<T> transpose(const Matrix3fX<T>& R)
+{
+ return Matrix3fX<T>(transpose(R.data));
+}
+
+template<typename T>
+Matrix3fX<T> inverse(const Matrix3fX<T>& R)
+{
+ return Matrix3fX<T>(inverse(R.data));
+}
+
+template<typename T>
+typename T::meta_type quadraticForm(const Matrix3fX<T>& R, const Vec3fX<typename T::vector_type>& v)
+{
+ return v.dot(R * v);
+}
+
+}
+
+#endif
diff --git a/include/hpp/fcl/math/vec_3f.h b/include/hpp/fcl/math/vec_3f.h
index 5a6f481acf48b935801d10c1b56bbe501a41c7b8..f7db1ba3ff9c7aea873fab9e1cf554f0f4ddc5e5 100644
--- a/include/hpp/fcl/math/vec_3f.h
+++ b/include/hpp/fcl/math/vec_3f.h
@@ -40,12 +40,21 @@
#include <hpp/fcl/config-fcl.hh>
#include <hpp/fcl/data_types.h>
-#include <hpp/fcl/math/math_details.h>
+
+#if FCL_USE_NATIVE_EIGEN
+# include <hpp/fcl/eigen/vec_3fx.h>
+#else
+# include <hpp/fcl/math/vec_3fx.h>
+#endif
#if FCL_HAVE_EIGEN
-#include <hpp/fcl/eigen/math_details.h>
+# if ! FCL_USE_NATIVE_EIGEN
+# include <hpp/fcl/eigen/math_details.h>
+# endif
#elif FCL_HAVE_SSE
-#include <hpp/fcl/simd/math_simd_details.h>
+# include <hpp/fcl/simd/math_simd_details.h>
+#else
+# include <hpp/fcl/math/math_details.h>
#endif
#include <cmath>
@@ -56,187 +65,12 @@
namespace fcl
{
-/// @brief Vector3 class wrapper. The core data is in the template parameter class.
-template <typename T>
-class Vec3fX
-{
-public:
- typedef typename T::meta_type U;
-
- /// @brief interval vector3 data
- T data;
-
- Vec3fX() {}
- Vec3fX(const Vec3fX& other) : data(other.data) {}
-
- /// @brief create Vector (x, y, z)
- Vec3fX(U x, U y, U z) : data(x, y, z) {}
-
- /// @brief create vector (x, x, x)
- Vec3fX(U x) : data(x) {}
-
- /// @brief create vector using the internal data type
- Vec3fX(const T& data_) : data(data_) {}
-
- inline U operator [] (size_t i) const { return data[i]; }
- inline U& operator [] (size_t i) { return data[i]; }
-
- inline Vec3fX operator + (const Vec3fX& other) const { return Vec3fX(data + other.data); }
- inline Vec3fX operator - (const Vec3fX& other) const { return Vec3fX(data - other.data); }
- inline Vec3fX operator * (const Vec3fX& other) const { return Vec3fX(data * other.data); }
- inline Vec3fX operator / (const Vec3fX& other) const { return Vec3fX(data / other.data); }
- inline Vec3fX& operator += (const Vec3fX& other) { data += other.data; return *this; }
- inline Vec3fX& operator -= (const Vec3fX& other) { data -= other.data; return *this; }
- inline Vec3fX& operator *= (const Vec3fX& other) { data *= other.data; return *this; }
- inline Vec3fX& operator /= (const Vec3fX& other) { data /= other.data; return *this; }
- inline Vec3fX operator + (U t) const { return Vec3fX(data + t); }
- inline Vec3fX operator - (U t) const { return Vec3fX(data - t); }
- inline Vec3fX operator * (U t) const { return Vec3fX(data * t); }
- inline Vec3fX operator / (U t) const { return Vec3fX(data / t); }
- inline Vec3fX& operator += (U t) { data += t; return *this; }
- inline Vec3fX& operator -= (U t) { data -= t; return *this; }
- inline Vec3fX& operator *= (U t) { data *= t; return *this; }
- inline Vec3fX& operator /= (U t) { data /= t; return *this; }
- inline Vec3fX operator - () const { return Vec3fX(-data); }
- inline Vec3fX cross(const Vec3fX& other) const { return Vec3fX(details::cross_prod(data, other.data)); }
- inline U dot(const Vec3fX& other) const { return details::dot_prod3(data, other.data); }
- inline Vec3fX& normalize()
- {
- U sqr_length = details::dot_prod3(data, data);
- if(sqr_length > 0)
- *this /= (U)sqrt(sqr_length);
- return *this;
- }
-
- inline Vec3fX& normalize(bool* signal)
- {
- U sqr_length = details::dot_prod3(data, data);
- if(sqr_length > 0)
- {
- *this /= (U)sqrt(sqr_length);
- *signal = true;
- }
- else
- *signal = false;
- return *this;
- }
-
- inline Vec3fX& abs()
- {
- data = abs(data);
- return *this;
- }
-
- inline U length() const { return sqrt(details::dot_prod3(data, data)); }
- inline U norm() const { return sqrt(details::dot_prod3(data, data)); }
- inline U sqrLength() const { return details::dot_prod3(data, data); }
- inline U squaredNorm() const { return details::dot_prod3(data, data); }
- inline void setValue(U x, U y, U z) { data.setValue(x, y, z); }
- 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
- {
- return equal(other, 0);
- }
-
- bool operator != (const Vec3fX& other) const
- {
- return !(*this == other);
- }
-
-
- inline Vec3fX<T>& ubound(const Vec3fX<T>& u)
- {
- data.ubound(u.data);
- return *this;
- }
-
- inline Vec3fX<T>& lbound(const Vec3fX<T>& l)
- {
- data.lbound(l.data);
- return *this;
- }
-
- bool isZero() const
- {
- return (data[0] == 0) && (data[1] == 0) && (data[2] == 0);
- }
-
-};
-
-template<typename T>
-static inline Vec3fX<T> normalize(const Vec3fX<T>& v)
-{
- typename T::meta_type sqr_length = details::dot_prod3(v.data, v.data);
- if(sqr_length > 0)
- return v / (typename T::meta_type)sqrt(sqr_length);
- else
- return v;
-}
-
-template <typename T>
-static inline typename T::meta_type triple(const Vec3fX<T>& x, const Vec3fX<T>& y, const Vec3fX<T>& z)
-{
- return x.dot(y.cross(z));
-}
-
-template <typename T>
-std::ostream& operator << (std::ostream& out, const Vec3fX<T>& x)
-{
- out << x[0] << " " << x[1] << " " << x[2];
- return out;
-}
-
-template <typename T>
-static inline Vec3fX<T> min(const Vec3fX<T>& x, const Vec3fX<T>& y)
-{
- return Vec3fX<T>(details::min(x.data, y.data));
-}
-
-template <typename T>
-static inline Vec3fX<T> max(const Vec3fX<T>& x, const Vec3fX<T>& y)
-{
- return Vec3fX<T>(details::max(x.data, y.data));
-}
-
-template <typename T>
-static inline Vec3fX<T> abs(const Vec3fX<T>& x)
-{
- return Vec3fX<T>(details::abs(x.data));
-}
-
-template <typename T>
-void generateCoordinateSystem(const Vec3fX<T>& w, Vec3fX<T>& u, Vec3fX<T>& v)
-{
- typedef typename T::meta_type U;
- U inv_length;
- if(std::abs(w[0]) >= std::abs(w[1]))
- {
- inv_length = (U)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
- u[0] = -w[2] * inv_length;
- u[1] = (U)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 = (U)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
- u[0] = (U)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];
- }
-}
-
#if FCL_HAVE_EIGEN
+# if FCL_USE_NATIVE_EIGEN
+ typedef Vec3fX<FCL_REAL> Vec3f;
+# else
typedef Vec3fX<details::eigen_v3<FCL_REAL> > Vec3f;
+# endif
#elif FCL_HAVE_SSE
typedef Vec3fX<details::sse_meta_f4> Vec3f;
#else
@@ -249,14 +83,6 @@ static inline std::ostream& operator << (std::ostream& o, const Vec3f& v)
return o;
}
- template <typename T>
- inline Vec3fX <T> operator * (const typename Vec3fX <T>::U& t,
- const Vec3fX <T>& v)
- {
- return Vec3fX <T> (v.data * t);
- }
-
-
}
diff --git a/include/hpp/fcl/math/vec_3fx.h b/include/hpp/fcl/math/vec_3fx.h
new file mode 100644
index 0000000000000000000000000000000000000000..bfa228b0eef85a9358ff60600a145b105c48445c
--- /dev/null
+++ b/include/hpp/fcl/math/vec_3fx.h
@@ -0,0 +1,243 @@
+/*
+ * 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 Jia Pan */
+
+#ifndef FCL_VEC_3FX_H
+#define FCL_VEC_3FX_H
+
+#include <hpp/fcl/config-fcl.hh>
+#include <hpp/fcl/data_types.h>
+
+#include <hpp/fcl/math/math_details.h>
+
+#include <cmath>
+#include <iostream>
+#include <limits>
+
+
+namespace fcl
+{
+
+/// @brief Vector3 class wrapper. The core data is in the template parameter class.
+template <typename T>
+class Vec3fX
+{
+public:
+ typedef typename T::meta_type U;
+
+ /// @brief interval vector3 data
+ T data;
+
+ Vec3fX() {}
+ Vec3fX(const Vec3fX& other) : data(other.data) {}
+
+ /// @brief create Vector (x, y, z)
+ Vec3fX(U x, U y, U z) : data(x, y, z) {}
+
+ /// @brief create vector (x, x, x)
+ Vec3fX(U x) : data(x) {}
+
+ /// @brief create vector using the internal data type
+ Vec3fX(const T& data_) : data(data_) {}
+
+ inline U operator [] (size_t i) const { return data[i]; }
+ inline U& operator [] (size_t i) { return data[i]; }
+
+ inline Vec3fX operator + (const Vec3fX& other) const { return Vec3fX(data + other.data); }
+ inline Vec3fX operator - (const Vec3fX& other) const { return Vec3fX(data - other.data); }
+ inline Vec3fX operator * (const Vec3fX& other) const { return Vec3fX(data * other.data); }
+ inline Vec3fX operator / (const Vec3fX& other) const { return Vec3fX(data / other.data); }
+ inline Vec3fX& operator += (const Vec3fX& other) { data += other.data; return *this; }
+ inline Vec3fX& operator -= (const Vec3fX& other) { data -= other.data; return *this; }
+ inline Vec3fX& operator *= (const Vec3fX& other) { data *= other.data; return *this; }
+ inline Vec3fX& operator /= (const Vec3fX& other) { data /= other.data; return *this; }
+ inline Vec3fX operator + (U t) const { return Vec3fX(data + t); }
+ inline Vec3fX operator - (U t) const { return Vec3fX(data - t); }
+ inline Vec3fX operator * (U t) const { return Vec3fX(data * t); }
+ inline Vec3fX operator / (U t) const { return Vec3fX(data / t); }
+ inline Vec3fX& operator += (U t) { data += t; return *this; }
+ inline Vec3fX& operator -= (U t) { data -= t; return *this; }
+ inline Vec3fX& operator *= (U t) { data *= t; return *this; }
+ inline Vec3fX& operator /= (U t) { data /= t; return *this; }
+ inline Vec3fX operator - () const { return Vec3fX(-data); }
+ inline Vec3fX cross(const Vec3fX& other) const { return Vec3fX(details::cross_prod(data, other.data)); }
+ inline U dot(const Vec3fX& other) const { return details::dot_prod3(data, other.data); }
+ inline Vec3fX& normalize()
+ {
+ U sqr_length = details::dot_prod3(data, data);
+ if(sqr_length > 0)
+ *this /= (U)sqrt(sqr_length);
+ return *this;
+ }
+
+ inline Vec3fX& normalize(bool* signal)
+ {
+ U sqr_length = details::dot_prod3(data, data);
+ if(sqr_length > 0)
+ {
+ *this /= (U)sqrt(sqr_length);
+ *signal = true;
+ }
+ else
+ *signal = false;
+ return *this;
+ }
+
+ inline Vec3fX& abs()
+ {
+ data = abs(data);
+ return *this;
+ }
+
+ inline U length() const { return sqrt(details::dot_prod3(data, data)); }
+ inline U norm() const { return sqrt(details::dot_prod3(data, data)); }
+ inline U sqrLength() const { return details::dot_prod3(data, data); }
+ inline U squaredNorm() const { return details::dot_prod3(data, data); }
+ inline void setValue(U x, U y, U z) { data.setValue(x, y, z); }
+ 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
+ {
+ return equal(other, 0);
+ }
+
+ bool operator != (const Vec3fX& other) const
+ {
+ return !(*this == other);
+ }
+
+
+ inline Vec3fX<T>& ubound(const Vec3fX<T>& u)
+ {
+ data.ubound(u.data);
+ return *this;
+ }
+
+ inline Vec3fX<T>& lbound(const Vec3fX<T>& l)
+ {
+ data.lbound(l.data);
+ return *this;
+ }
+
+ bool isZero() const
+ {
+ return (data[0] == 0) && (data[1] == 0) && (data[2] == 0);
+ }
+
+};
+
+template<typename T>
+static inline Vec3fX<T> normalize(const Vec3fX<T>& v)
+{
+ typename T::meta_type sqr_length = details::dot_prod3(v.data, v.data);
+ if(sqr_length > 0)
+ return v / (typename T::meta_type)sqrt(sqr_length);
+ else
+ return v;
+}
+
+template <typename T>
+static inline typename T::meta_type triple(const Vec3fX<T>& x, const Vec3fX<T>& y, const Vec3fX<T>& z)
+{
+ return x.dot(y.cross(z));
+}
+
+template <typename T>
+std::ostream& operator << (std::ostream& out, const Vec3fX<T>& x)
+{
+ out << x[0] << " " << x[1] << " " << x[2];
+ return out;
+}
+
+template <typename T>
+static inline Vec3fX<T> min(const Vec3fX<T>& x, const Vec3fX<T>& y)
+{
+ return Vec3fX<T>(details::min(x.data, y.data));
+}
+
+template <typename T>
+static inline Vec3fX<T> max(const Vec3fX<T>& x, const Vec3fX<T>& y)
+{
+ return Vec3fX<T>(details::max(x.data, y.data));
+}
+
+template <typename T>
+static inline Vec3fX<T> abs(const Vec3fX<T>& x)
+{
+ return Vec3fX<T>(details::abs(x.data));
+}
+
+template <typename T>
+void generateCoordinateSystem(const Vec3fX<T>& w, Vec3fX<T>& u, Vec3fX<T>& v)
+{
+ typedef typename T::meta_type U;
+ U inv_length;
+ if(std::abs(w[0]) >= std::abs(w[1]))
+ {
+ inv_length = (U)1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
+ u[0] = -w[2] * inv_length;
+ u[1] = (U)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 = (U)1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
+ u[0] = (U)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];
+ }
+}
+
+ template <typename T>
+ inline Vec3fX <T> operator * (const typename Vec3fX <T>::U& t,
+ const Vec3fX <T>& v)
+ {
+ return Vec3fX <T> (v.data * t);
+ }
+
+}
+
+
+#endif