Commit 97f6b180 authored by jpan's avatar jpan
Browse files

matrix


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@47 253336fb-580f-4252-a368-f3cef5a2a82b
parent 40a150e7
......@@ -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)
......
/*
* 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
......@@ -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);
......
/*
* 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;
}
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment