Commit a1d8ec4b authored by jpan's avatar jpan
Browse files

sphere-sphere, box-box special implementation. seems the precision is lower...

sphere-sphere, box-box special implementation. seems the precision is lower than standard GJK. So now is still not used.


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@106 253336fb-580f-4252-a368-f3cef5a2a82b
parent b36e78ec
......@@ -37,7 +37,7 @@ link_directories(${CCD_LIBRARY_DIRS})
add_definitions(-DUSE_SVMLIGHT=0)
add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.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/gjk_libccd.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 src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/gjk.cpp)
add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/BV/OBBRSS.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/gjk_libccd.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 src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/gjk.cpp src/narrowphase.cpp)
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES})
......
......@@ -137,7 +137,6 @@ namespace fcl
BVH_REAL determinant() const;
Matrix3f transpose() const;
Matrix3f inverse() const;
Matrix3f abs() const;
Matrix3f transposeTimes(const Matrix3f& m) const;
Matrix3f timesTranspose(const Matrix3f& m) const;
......@@ -171,6 +170,11 @@ namespace fcl
return v_[0][2] * v[0] + v_[1][2] * v[1] + v_[2][2] * v[2];
}
inline BVH_REAL transposeDot(size_t i, const Vec3f& v) const
{
return v_[0][i] * v[0] + v_[1][i] * v[1] + v_[2][i] * v[2];
}
inline void 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)
......@@ -179,7 +183,7 @@ namespace fcl
v_[1].setValue(yx, yy, yz);
v_[2].setValue(zx, zy, zz);
}
inline void setValue(BVH_REAL x)
{
v_[0].setValue(x);
......@@ -191,6 +195,8 @@ namespace fcl
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]);
Matrix3f abs(const Matrix3f& R);
}
#endif
......@@ -41,6 +41,7 @@
#include "fcl/narrowphase/gjk_libccd.h"
namespace fcl
{
......@@ -192,6 +193,30 @@ struct GJKSolver_libccd
template<>
bool GJKSolver_libccd::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
const Sphere& s2, const SimpleTransform& tf2,
Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
/*
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeTriangleIntersect(const Sphere& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T, Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
template<>
bool GJKSolver_libccd::shapeIntersect<Box, Box>(const Box& s1, const SimpleTransform& tf1,
const Box& s2, const SimpleTransform& tf2,
Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
*/
template<>
bool GJKSolver_libccd::shapeDistance<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
const Sphere& s2, const SimpleTransform& tf2,
BVH_REAL* dist) const;
struct GJKSolver_indep
{
......@@ -453,8 +478,22 @@ struct GJKSolver_indep
BVH_REAL gjk_max_iterations;
};
template<>
bool GJKSolver_indep::shapeIntersect<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
const Sphere& s2, const SimpleTransform& tf2,
Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
/*
template<>
bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const SimpleTransform& tf1,
const Box& s2, const SimpleTransform& tf2,
Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal) const;
*/
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>(const Sphere& s1, const SimpleTransform& tf1,
const Sphere& s2, const SimpleTransform& tf2,
BVH_REAL* dist) const;
}
......
......@@ -113,7 +113,7 @@ bool OBB::obbDisjoint(const Matrix3f& B, const Vec3f& T, const Vec3f& a, const V
register BVH_REAL t, s;
const BVH_REAL reps = 1e-6;
Matrix3f Bf = B.abs();
Matrix3f Bf = abs(B);
Bf += reps;
// if any of these tests are one-sided, then the polyhedra are disjoint
......
......@@ -71,13 +71,6 @@ Matrix3f Matrix3f::transpose() const
v_[0][2], v_[1][2], v_[2][2]);
}
Matrix3f Matrix3f::abs() const
{
return Matrix3f(fabs(v_[0][0]), fabs(v_[0][1]), fabs(v_[0][2]),
fabs(v_[1][0]), fabs(v_[1][1]), fabs(v_[1][2]),
fabs(v_[2][0]), fabs(v_[2][1]), fabs(v_[2][2]));
}
Matrix3f Matrix3f::inverse() const
{
BVH_REAL det = determinant();
......@@ -235,5 +228,13 @@ void matEigen(const Matrix3f& m, BVH_REAL dout[3], Vec3f vout[3])
}
Matrix3f abs(const Matrix3f& R)
{
return Matrix3f(fabs(R.v_[0][0]), fabs(R.v_[0][1]), fabs(R.v_[0][2]),
fabs(R.v_[1][0]), fabs(R.v_[1][1]), fabs(R.v_[1][2]),
fabs(R.v_[2][0]), fabs(R.v_[2][1]), fabs(R.v_[2][2]));
}
}
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