diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index 67bff808e267fe1f64abe479b699d30fd16ea7bc..d948e8b182b3182ed6e46e01680afae6f2b35804 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -32,7 +32,7 @@ set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
add_definitions(-DUSE_PQP=0)
add_definitions(-DUSE_SVMLIGHT=0)
-rosbuild_add_library(${PROJECT_NAME} src/AABB.cpp src/OBB.cpp src/RSS.cpp src/vec_3f.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/geometric_shapes_intersect.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp)
+rosbuild_add_library(${PROJECT_NAME} src/AABB.cpp src/OBB.cpp src/RSS.cpp src/vec_3f.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/geometric_shapes_intersect.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp)
rosbuild_add_gtest(test_core_collision test/test_core_collision.cpp)
target_link_libraries(test_core_collision fcl)
diff --git a/trunk/fcl/include/fcl/matrix_3f.h b/trunk/fcl/include/fcl/matrix_3f.h
new file mode 100644
index 0000000000000000000000000000000000000000..fc38585a0471179d723104d7cb7b64a3c58e93c6
--- /dev/null
+++ b/trunk/fcl/include/fcl/matrix_3f.h
@@ -0,0 +1,166 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Willow Garage, Inc. 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_3F_H
+#define FCL_MATRIX_3F_H
+
+#include "fcl/vec_3f.h"
+
+namespace fcl
+{
+ class Matrix3f
+ {
+ public:
+ Vec3f v_[3];
+
+ /** \brief All zero matrix */
+ Matrix3f() {}
+
+ Matrix3f(BVH_REAL xx, BVH_REAL xy, BVH_REAL xz,
+ BVH_REAL yx, BVH_REAL yy, BVH_REAL yz,
+ BVH_REAL zx, BVH_REAL zy, BVH_REAL zz)
+ {
+ setValue(xx, xy, xz,
+ yz, yy, yz,
+ zx, zy, zz);
+ }
+
+ Matrix3f(const Matrix3f& other)
+ {
+ v_[0] = other.v_[0];
+ v_[1] = other.v_[1];
+ v_[2] = other.v_[2];
+ }
+
+ Matrix3f& operator = (const Matrix3f& other)
+ {
+ v_[0] = other.v_[0];
+ v_[1] = other.v_[1];
+ v_[2] = other.v_[2];
+ return *this;
+ }
+
+ inline Vec3f getColumn(size_t i) const
+ {
+ return Vec3f(v_[0][i], v_[1][i], v_[2][i]);
+ }
+
+ inline const Vec3f& getRow(size_t i) const
+ {
+ return v_[i];
+ }
+
+ inline Vec3f& operator [](size_t i)
+ {
+ return v_[i];
+ }
+
+ inline const Vec3f& operator [](size_t i) const
+ {
+ return v_[i];
+ }
+
+ Matrix3f& operator *= (const Matrix3f& other);
+
+ void setIdentity()
+ {
+ setValue((BVH_REAL)1.0, (BVH_REAL)0.0, (BVH_REAL)0.0,
+ (BVH_REAL)0.0, (BVH_REAL)1.0, (BVH_REAL)0.0,
+ (BVH_REAL)0.0, (BVH_REAL)0.0, (BVH_REAL)1.0);
+ }
+
+ static const Matrix3f& getIdentity()
+ {
+ static const Matrix3f I((BVH_REAL)1.0, (BVH_REAL)0.0, (BVH_REAL)0.0,
+ (BVH_REAL)0.0, (BVH_REAL)1.0, (BVH_REAL)0.0,
+ (BVH_REAL)0.0, (BVH_REAL)0.0, (BVH_REAL)1.0);
+ return I;
+ }
+
+ BVH_REAL determinant() const;
+ Matrix3f transpose() const;
+ Matrix3f inverse() const;
+
+ Matrix3f transposeTimes(const Matrix3f& m) const;
+ Matrix3f timesTranspose(const Matrix3f& m) const;
+ Matrix3f operator * (const Matrix3f& m) const;
+
+ Vec3f operator * (const Vec3f& v) const;
+ Vec3f transposeTimes(const Vec3f& v) const;
+
+ inline BVH_REAL quadraticForm(const Vec3f& v) const
+ {
+ return v[0] * v[0] * v_[0][0] + v[0] * v[1] * v_[0][1] + v[0] * v[2] * v_[0][2] +
+ v[1] * v[0] * v_[1][0] + v[1] * v[1] * v_[1][1] + v[1] * v[2] * v_[1][2] +
+ v[2] * v[0] * v_[2][0] + v[2] * v[1] * v_[2][1] + v[2] * v[2] * v_[2][2];
+ }
+
+ /** S * M * S' */
+ Matrix3f tensorTransform(const Matrix3f& m) const;
+
+ inline BVH_REAL transposeDotX(const Vec3f& v) const
+ {
+ return v_[0][0] * v[0] + v_[1][0] * v[1] + v_[2][0] * v[2];
+ }
+
+ inline BVH_REAL transposeDotY(const Vec3f& v) const
+ {
+ return v_[0][1] * v[0] + v_[1][1] * v[1] + v_[2][1] * v[2];
+ }
+
+ inline BVH_REAL transposeDotZ(const Vec3f& v) const
+ {
+ return v_[0][2] * v[0] + v_[1][2] * v[1] + v_[2][2] * v[2];
+ }
+
+ inline Matrix3f& setValue(BVH_REAL xx, BVH_REAL xy, BVH_REAL xz,
+ BVH_REAL yx, BVH_REAL yy, BVH_REAL yz,
+ BVH_REAL zx, BVH_REAL zy, BVH_REAL zz)
+ {
+ v_[0].setValue(xx, xy, xz);
+ v_[1].setValue(yx, yy, yz);
+ v_[2].setValue(zx, zy, zz);
+
+ return *this;
+ }
+ };
+
+ void relativeTransform(const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, Matrix3f& R, Vec3f& T);
+
+ void matEigen(const Matrix3f& R, BVH_REAL dout[3], Vec3f vout[3]);
+}
+
+#endif
diff --git a/trunk/fcl/include/fcl/vec_3f.h b/trunk/fcl/include/fcl/vec_3f.h
index 67d39d8ecee9470f3b59886401be3e8e4b3fe616..ad1ed408472d20942b78a9033dca93413dd0f5ca 100644
--- a/trunk/fcl/include/fcl/vec_3f.h
+++ b/trunk/fcl/include/fcl/vec_3f.h
@@ -247,6 +247,13 @@ namespace fcl
(v_[2] - other.v_[2] > -EPSILON));
}
+ inline BVH_REAL triple(const Vec3f& v1, const Vec3f& v2) const
+ {
+ return v_[0] * (v1.v_[1] * v2.v_[2] - v1.v_[2] * v2.v_[1]) +
+ v_[1] * (v1.v_[2] * v2.v_[0] - v1.v_[0] * v2.v_[2]) +
+ v_[2] * (v1.v_[0] * v2.v_[1] - v1.v_[1] * v2.v_[0]);
+ }
+
private:
/** \brief Tolerance for comparision */
static const float EPSILON;
@@ -284,13 +291,6 @@ namespace fcl
Vec3f() { v_[0] = 0; v_[1] = 0; v_[2] = 0; }
- Vec3f(const Vec3f& other)
- {
- v_[0] = other.v_[0];
- v_[1] = other.v_[1];
- v_[2] = other.v_[2];
- }
-
Vec3f(const BVH_REAL* v)
{
v_[0] = v[0];
@@ -346,11 +346,12 @@ namespace fcl
}
/** \brief Negate the vector */
- inline void negate()
+ inline Vec3f& negate()
{
v_[0] = - v_[0];
v_[1] = - v_[1];
v_[2] = - v_[2];
+ return *this;
}
/** \brief Return a negated vector */
@@ -436,6 +437,13 @@ namespace fcl
(v_[2] - other.v_[2] > -EPSILON));
}
+ inline BVH_REAL triple(const Vec3f& v1, const Vec3f& v2) const
+ {
+ return v_[0] * (v1.v_[1] * v2.v_[2] - v1.v_[2] * v2.v_[1]) +
+ v_[1] * (v1.v_[2] * v2.v_[0] - v1.v_[0] * v2.v_[2]) +
+ v_[2] * (v1.v_[0] * v2.v_[1] - v1.v_[1] * v2.v_[0]);
+ }
+
private:
/** \brief Tolerance for comparision */
static const BVH_REAL EPSILON;
@@ -464,8 +472,14 @@ namespace fcl
return Vec3f(x, y, z);
}
+
#endif
+ inline BVH_REAL triple(const Vec3f& a, const Vec3f& b, const Vec3f& c)
+ {
+ return a.triple(b, c);
+ }
+
/** \brief M * v */
Vec3f matMulVec(const Vec3f M[3], const Vec3f& v);
diff --git a/trunk/fcl/src/matrix_3f.cpp b/trunk/fcl/src/matrix_3f.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ab39f3855a0e9b60931f2dc80dec97ad60edd76d
--- /dev/null
+++ b/trunk/fcl/src/matrix_3f.cpp
@@ -0,0 +1,223 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * 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 Willow Garage, Inc. 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 */
+
+
+#include "fcl/matrix_3f.h"
+#include <iostream>
+
+namespace fcl
+{
+
+Matrix3f& Matrix3f::operator *= (const Matrix3f& other)
+{
+ setValue(other.transposeDotX(v_[0]), other.transposeDotY(v_[0]), other.transposeDotZ(v_[0]),
+ other.transposeDotX(v_[1]), other.transposeDotY(v_[1]), other.transposeDotZ(v_[1]),
+ other.transposeDotX(v_[2]), other.transposeDotY(v_[2]), other.transposeDotZ(v_[2]));
+
+ return *this;
+}
+
+BVH_REAL Matrix3f::determinant() const
+{
+ return triple(v_[0], v_[1], v_[2]);
+}
+
+Matrix3f Matrix3f::transpose() const
+{
+ return Matrix3f(v_[0][0], v_[1][0], v_[2][0],
+ v_[0][1], v_[1][1], v_[2][1],
+ v_[0][2], v_[1][2], v_[2][2]);
+}
+
+Matrix3f Matrix3f::inverse() const
+{
+ BVH_REAL det = determinant();
+ BVH_REAL inv_det = 1.0 / det;
+
+ return Matrix3f((v_[1][1] * v_[2][2] - v_[1][2] * v_[2][1]) * inv_det,
+ (v_[0][2] * v_[2][1] - v_[0][1] * v_[2][2]) * inv_det,
+ (v_[0][1] * v_[1][2] - v_[0][2] * v_[1][1]) * inv_det,
+ (v_[1][2] * v_[2][0] - v_[1][0] * v_[2][2]) * inv_det,
+ (v_[0][0] * v_[2][2] - v_[0][2] * v_[2][0]) * inv_det,
+ (v_[0][2] * v_[1][0] - v_[0][0] * v_[1][2]) * inv_det,
+ (v_[1][0] * v_[2][1] - v_[1][1] * v_[2][0]) * inv_det,
+ (v_[0][1] * v_[2][0] - v_[0][0] * v_[2][1]) * inv_det,
+ (v_[0][0] * v_[1][1] - v_[0][1] * v_[1][0]) * inv_det);
+}
+
+
+Matrix3f Matrix3f::transposeTimes(const Matrix3f& m) const
+{
+ return Matrix3f(
+ v_[0][0] * m[0][0] + v_[1][0] * m[1][0] + v_[2][0] * m[2][0],
+ v_[0][0] * m[0][1] + v_[1][0] * m[1][1] + v_[2][0] * m[2][1],
+ v_[0][0] * m[0][2] + v_[1][0] * m[1][2] + v_[2][0] * m[2][2],
+ v_[0][1] * m[0][0] + v_[1][1] * m[1][0] + v_[2][1] * m[2][0],
+ v_[0][1] * m[0][1] + v_[1][1] * m[1][1] + v_[2][1] * m[2][1],
+ v_[0][1] * m[0][2] + v_[1][1] * m[1][2] + v_[2][1] * m[2][2],
+ v_[0][2] * m[0][0] + v_[1][2] * m[1][0] + v_[2][2] * m[2][0],
+ v_[0][2] * m[0][1] + v_[1][2] * m[1][1] + v_[2][2] * m[2][1],
+ v_[0][2] * m[0][2] + v_[1][2] * m[1][2] + v_[2][2] * m[2][2]);
+}
+
+Matrix3f Matrix3f::timesTranspose(const Matrix3f& m) const
+{
+ return Matrix3f(v_[0].dot(m[0]), v_[0].dot(m[1]), v_[0].dot(m[2]),
+ v_[1].dot(m[0]), v_[1].dot(m[1]), v_[1].dot(m[2]),
+ v_[2].dot(m[0]), v_[2].dot(m[1]), v_[2].dot(m[2]));
+}
+
+Vec3f Matrix3f::operator * (const Vec3f& v) const
+{
+ return Vec3f(v_[0].dot(v), v_[1].dot(v), v_[2].dot(v));
+}
+
+Vec3f Matrix3f::transposeTimes(const Vec3f& v) const
+{
+ return Vec3f(transposeDotX(v), transposeDotY(v), transposeDotZ(v));
+}
+
+Matrix3f Matrix3f::tensorTransform(const Matrix3f& m) const
+{
+ Matrix3f res = *this;
+ res *= m;
+ return res.timesTranspose(*this);
+}
+
+Matrix3f Matrix3f::operator * (const Matrix3f& m) const
+{
+ Matrix3f res = *this;
+ return res *= m;
+}
+
+void relativeTransform(const Matrix3f& R1, const Vec3f& T1, const Matrix3f& R2, const Vec3f& T2, Matrix3f& R, Vec3f& T)
+{
+ R = R1.transposeTimes(R2);
+ Vec3f temp = T2 - T1;
+ T = R1.transposeTimes(temp);
+}
+
+void matEigen(const Matrix3f& m, BVH_REAL dout[3], Vec3f vout[3])
+{
+ Matrix3f R(m);
+ int n = 3;
+ int j, iq, ip, i;
+ BVH_REAL tresh, theta, tau, t, sm, s, h, g, c;
+ int nrot;
+ BVH_REAL b[3];
+ BVH_REAL z[3];
+ BVH_REAL v[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+ BVH_REAL d[3];
+
+ for(ip = 0; ip < n; ++ip)
+ {
+ b[ip] = R[ip][ip];
+ d[ip] = R[ip][ip];
+ z[ip] = 0.0;
+ }
+
+ nrot = 0;
+
+ for(i = 0; i < 50; ++i)
+ {
+ sm = 0.0;
+ for(ip = 0; ip < n; ++ip)
+ for(iq = ip + 1; iq < n; ++iq)
+ sm += fabs(R[ip][iq]);
+ if(sm == 0.0)
+ {
+ vout[0] = Vec3f(v[0][0], v[0][1], v[0][2]);
+ vout[1] = Vec3f(v[1][0], v[1][1], v[1][2]);
+ vout[2] = Vec3f(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 * fabs(R[ip][iq]);
+ if(i > 3 &&
+ fabs(d[ip]) + g == fabs(d[ip]) &&
+ fabs(d[iq]) + g == fabs(d[iq]))
+ R[ip][iq] = 0.0;
+ else if(fabs(R[ip][iq]) > tresh)
+ {
+ h = d[iq] - d[ip];
+ if(fabs(h) + g == fabs(h)) t = (R[ip][iq]) / h;
+ else
+ {
+ theta = 0.5 * h / (R[ip][iq]);
+ t = 1.0 /(fabs(theta) + sqrt(1.0 + theta * theta));
+ if(theta < 0.0) t = -t;
+ }
+ c = 1.0 / 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;
+
+}
+
+
+}