Commit f65f5fcf authored by jpan's avatar jpan
Browse files

add kIOS which can handle collision and distance simultaneously. and refactor...

add kIOS which can handle collision and distance simultaneously. and refactor the collision_func_matrix.cpp


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@82 253336fb-580f-4252-a368-f3cef5a2a82b
parent cd38ef9b
......@@ -37,7 +37,7 @@ link_directories(${CCD_LIBRARY_DIRS})
add_definitions(-DUSE_SVMLIGHT=0)
add_library(${PROJECT_NAME} SHARED 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 src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp)
add_library(${PROJECT_NAME} SHARED src/AABB.cpp src/OBB.cpp src/RSS.cpp src/kIOS.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 src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp)
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES})
......
......@@ -284,6 +284,9 @@ NODE_TYPE BVHModel<OBB>::getNodeType() const;
template<>
NODE_TYPE BVHModel<RSS>::getNodeType() const;
template<>
NODE_TYPE BVHModel<kIOS>::getNodeType() const;
template<>
NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const;
......
......@@ -111,6 +111,15 @@ void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, Vec
*/
void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent);
/** \brief Compute the center and radius for a triangle's circumcircle */
void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, BVH_REAL& radius);
/** \brief Compute the maximum distance from a given center point to a point cloud */
BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, const Vec3f& query);
BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query);
}
#endif
......@@ -43,6 +43,7 @@
#include "fcl/vec_3f.h"
#include "fcl/OBB.h"
#include "fcl/RSS.h"
#include "fcl/kIOS.h"
#include <iostream>
/** \brief Main namespace */
......@@ -65,6 +66,9 @@ void fit<OBB>(Vec3f* ps, int n, OBB& bv);
template<>
void fit<RSS>(Vec3f* ps, int n, RSS& bv);
template<>
void fit<kIOS>(Vec3f* ps, int n, kIOS& bv);
template<typename BV>
class BVFitterBase
......@@ -254,6 +258,52 @@ private:
BVHModelType type;
};
/** \brief Specification of BVFitter for kIOS bounding volume */
template<>
class BVFitter<kIOS> : public BVFitterBase<kIOS>
{
public:
/** \brief Prepare the geometry primitive data for fitting */
void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
prev_vertices = NULL;
tri_indices = tri_indices_;
type = type_;
}
/** \brief Prepare the geometry primitive data for fitting */
void set(Vec3f* vertices_, Vec3f* prev_vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
prev_vertices = prev_vertices_;
tri_indices = tri_indices_;
type = type_;
}
/** \brief Compute a bounding volume that fits a set of primitives (points or triangles).
* The primitive data was set by set function and primitive_indices is the primitive index relative to the data.
*/
kIOS fit(unsigned int* primitive_indices, int num_primitives);
/** \brief Clear the geometry primitive data */
void clear()
{
vertices = NULL;
prev_vertices = NULL;
tri_indices = NULL;
type = BVH_MODEL_UNKNOWN;
}
private:
Vec3f* vertices;
Vec3f* prev_vertices;
Triangle* tri_indices;
BVHModelType type;
};
}
#endif
......@@ -42,6 +42,8 @@
#include "fcl/primitive.h"
#include "fcl/vec_3f.h"
#include "fcl/OBB.h"
#include "fcl/RSS.h"
#include "fcl/kIOS.h"
#include <vector>
#include <iostream>
......@@ -302,6 +304,153 @@ private:
SplitMethodType split_method;
};
/** \brief BVHSplitRule specialization for RSS */
template<>
class BVSplitter<RSS> : public BVSplitterBase<RSS>
{
public:
BVSplitter(SplitMethodType method)
{
split_method = method;
}
/** \brief Set the geometry data needed by the split rule */
void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
tri_indices = tri_indices_;
type = type_;
}
/** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
void computeRule(const RSS& bv, unsigned int* primitive_indices, int num_primitives)
{
switch(split_method)
{
case SPLIT_METHOD_MEAN:
computeRule_mean(bv, primitive_indices, num_primitives);
break;
case SPLIT_METHOD_MEDIAN:
computeRule_median(bv, primitive_indices, num_primitives);
break;
case SPLIT_METHOD_BV_CENTER:
computeRule_bvcenter(bv, primitive_indices, num_primitives);
break;
default:
std::cerr << "Split method not supported" << std::endl;
}
}
/** \brief Apply the split rule on a given point */
bool apply(const Vec3f& q) const
{
return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
}
/** \brief Clear the geometry data set before */
void clear()
{
vertices = NULL;
tri_indices = NULL;
type = BVH_MODEL_UNKNOWN;
}
private:
/** \brief Split the node from center */
void computeRule_bvcenter(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
/** \brief Split the node according to the mean of the data contained */
void computeRule_mean(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
/** \brief Split the node according to the median of the data contained */
void computeRule_median(const RSS& bv, unsigned int* primitive_indices, int num_primitives);
BVH_REAL split_value;
Vec3f split_vector;
Vec3f* vertices;
Triangle* tri_indices;
BVHModelType type;
SplitMethodType split_method;
};
/** \brief BVHSplitRule specialization for RSS */
template<>
class BVSplitter<kIOS> : public BVSplitterBase<kIOS>
{
public:
BVSplitter(SplitMethodType method)
{
split_method = method;
}
/** \brief Set the geometry data needed by the split rule */
void set(Vec3f* vertices_, Triangle* tri_indices_, BVHModelType type_)
{
vertices = vertices_;
tri_indices = tri_indices_;
type = type_;
}
/** \brief Compute the split rule according to a subset of geometry and the corresponding BV node */
void computeRule(const kIOS& bv, unsigned int* primitive_indices, int num_primitives)
{
switch(split_method)
{
case SPLIT_METHOD_MEAN:
computeRule_mean(bv, primitive_indices, num_primitives);
break;
case SPLIT_METHOD_MEDIAN:
computeRule_median(bv, primitive_indices, num_primitives);
break;
case SPLIT_METHOD_BV_CENTER:
computeRule_bvcenter(bv, primitive_indices, num_primitives);
break;
default:
std::cerr << "Split method not supported" << std::endl;
}
}
/** \brief Apply the split rule on a given point */
bool apply(const Vec3f& q) const
{
return split_vector.dot(Vec3f(q[0], q[1], q[2])) > split_value;
}
/** \brief Clear the geometry data set before */
void clear()
{
vertices = NULL;
tri_indices = NULL;
type = BVH_MODEL_UNKNOWN;
}
private:
/** \brief Split the node from center */
void computeRule_bvcenter(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
/** \brief Split the node according to the mean of the data contained */
void computeRule_mean(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
/** \brief Split the node according to the median of the data contained */
void computeRule_median(const kIOS& bv, unsigned int* primitive_indices, int num_primitives);
BVH_REAL split_value;
Vec3f split_vector;
Vec3f* vertices;
Triangle* tri_indices;
BVHModelType type;
SplitMethodType split_method;
};
}
#endif
......@@ -118,7 +118,7 @@ public:
}
/** \brief Center of the OBB */
inline Vec3f center() const
inline const Vec3f& center() const
{
return To;
}
......
......@@ -121,7 +121,7 @@ public:
}
/** \brief The RSS center */
inline Vec3f center() const
inline const Vec3f& center() const
{
return Tr;
}
......
......@@ -57,6 +57,8 @@ void distance(DistanceTraversalNodeBase* node, BVHFrontList* front_list = NULL,
void distance(MeshDistanceTraversalNodeRSS* node, BVHFrontList* front_list = NULL, int qsize = 2);
void distance(MeshDistanceTraversalNodekIOS* node, BVHFrontList* front_list = NULL, int qsize = 2);
}
#endif
......@@ -47,7 +47,7 @@ namespace fcl
enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM};
enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_KIOS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE};
class CollisionGeometry
......
/*
* 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_KIOS_H
#define FCL_KIOS_H
#include "fcl/BVH_internal.h"
#include "fcl/vec_3f.h"
#include "fcl/matrix_3f.h"
/** \brief Main namespace */
namespace fcl
{
/** \brief kIOS class */
class kIOS
{
struct kIOS_Sphere
{
Vec3f o;
BVH_REAL r;
};
static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1)
{
Vec3f d = s1.o - s0.o;
BVH_REAL dist2 = d.sqrLength();
BVH_REAL diff_r = s1.r - s0.r;
/** The sphere with the larger radius encloses the other */
if(diff_r * diff_r >= dist2)
{
if(s1.r > s0.r) return s1;
else return s0;
}
else /** spheres partially overlapping or disjoint */
{
float dist = sqrt(dist2);
kIOS_Sphere s;
s.r = dist + s0.r + s1.r;
s.o = s0.o;
if(dist > 0)
s.o += d * ((s.r - s0.r) / dist);
return s;
}
}
public:
/** \brief The (at most) three spheres for intersection */
kIOS_Sphere spheres[5];
unsigned int num_spheres; // num_spheres <= 5
kIOS() {}
/** \brief Check collision between two kIOS */
bool overlap(const kIOS& other) const;
/** \brief Check collision between two kIOS and return the overlap part.
* For kIOS, we return nothing, as the overlappart of two kIOS usually is not an kIOS
*/
bool overlap(const kIOS& other, kIOS& overlap_part) const
{
return overlap(other);
}
/** \brief Check whether the kIOS contains a point */
inline bool contain(const Vec3f& p) const;
/** \brief A simple way to merge the kIOS and a point */
kIOS& operator += (const Vec3f& p);
/** \brief Merge the kIOS and another kIOS */
kIOS& operator += (const kIOS& other)
{
*this = *this + other;
return *this;
}
/** \brief Return the merged kIOS of current kIOS and the other one */
kIOS operator + (const kIOS& other) const;
/** \brief Center of the kIOS */
const Vec3f& center() const
{
return spheres[0].o;
}
/** \brief width of the kIOS */
BVH_REAL width() const;
/** \brief height of the kIOS */
BVH_REAL height() const;
/** \brief depth of the kIOS */
BVH_REAL depth() const;
/** \brief volume of the kIOS */
BVH_REAL volume() const;
/** \brief size of the kIOS, for split order */
BVH_REAL size() const;
/** \brief The distance between two kIOS */
BVH_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
private:
};
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
BVH_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
}
#endif
......@@ -279,6 +279,10 @@ bool initialize(MeshCollisionTraversalNodeRSS& node,
const BVHModel<RSS>& model2, const SimpleTransform& tf2,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
bool initialize(MeshCollisionTraversalNodekIOS& node,
const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
const BVHModel<kIOS>& model2, const SimpleTransform& tf2,
int num_max_contacts = 1, bool exhaustive = false, bool enable_contact = false);
#if USE_SVMLIGHT
......@@ -544,6 +548,11 @@ bool initialize(MeshDistanceTraversalNodeRSS& node,
const BVHModel<RSS>& model2, const SimpleTransform& tf2);
bool initialize(MeshDistanceTraversalNodekIOS& node,
const BVHModel<kIOS>& model1, const SimpleTransform& tf1,
const BVHModel<kIOS>& model2, const SimpleTransform& tf2);
/** \brief Initialize traversal node for continuous collision detection between two meshes */
template<typename BV>
......
......@@ -301,7 +301,18 @@ public:
Vec3f T;
};
class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode<kIOS>
{
public:
MeshCollisionTraversalNodekIOS();
bool BVTesting(int b1, int b2) const;
void leafTesting(int b1, int b2) const;
Matrix3f R;
Vec3f T;
};
#if USE_SVMLIGHT
......@@ -1048,6 +1059,19 @@ public:
};
class MeshDistanceTraversalNodekIOS : public MeshDistanceTraversalNode<kIOS>
{
public:
MeshDistanceTraversalNodekIOS();
BVH_REAL BVTesting(int b1, int b2) const;
void leafTesting(int b1, int b2) const;
Matrix3f R;
Vec3f T;
};
struct ConservativeAdvancementStackData
{
......
......@@ -891,6 +891,12 @@ NODE_TYPE BVHModel<RSS>::getNodeType() const
return BV_RSS;
}
template<>
NODE_TYPE BVHModel<kIOS>::getNodeType() const
{
return BV_KIOS;
}
template<>
NODE_TYPE BVHModel<KDOP<16> >::getNodeType() const
{
......@@ -917,5 +923,5 @@ template class BVHModel<KDOP<24> >;
template class BVHModel<OBB>;
template class BVHModel<AABB>;
template class BVHModel<RSS>;
template class BVHModel<kIOS>;
}
......@@ -1026,6 +1026,81 @@ void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indic
}
void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, BVH_REAL& radius)
{
Vec3f e1 = a - c;
Vec3f e2 = b - c;
BVH_REAL e1_len2 = e1.sqrLength();
BVH_REAL e2_len2 = e2.sqrLength();
Vec3f e3 = e1.cross(e2);
BVH_REAL e3_len2 = e3.sqrLength();
radius = e1_len2 * e2_len2 * (e1 - e2).sqrLength() / e3_len2;
radius = sqrt(radius) / 2.0;
center = (e2 * e1_len2 - e1 * e2_len2).cross(e3) * (0.5 * 1 / e3_len2) + c;
}
BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query)
{
bool indirect_index = true;
if(!indices) indirect_index = false;
BVH_REAL maxD = 0;
for(int i = 0; i < n; ++i)
{
unsigned int index = indirect_index ? indices[i] : i;
const Triangle& t = ts[index];
for(int j = 0; j < 3; ++j)
{
int point_id = t[j];
const Vec3f& p = ps[point_id];
BVH_REAL d = (p - query).sqrLength();
if(d > maxD) maxD = d;
}
if(ps2)
{
for(int j = 0; j < 3; ++j)
{
int point_id = t[j];
const Vec3f& p = ps2[point_id];
BVH_REAL d = (p - query).sqrLength();
if(d > maxD) maxD = d;
}
}
}
return sqrt(maxD);
}
BVH_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, unsigned int* indices, int n, const Vec3f& query)
{
bool indirect_index = true;
if(!indices) indirect_index = false;
BVH_REAL maxD = 0;
for(unsigned int i = 0; i < n; ++i)
{
int index = indirect_index ? indices[i] : i;
const Vec3f& p = ps[index];