diff --git a/include/fcl/broadphase/broadphase_SSaP-inl.h b/include/fcl/broadphase/broadphase_SSaP-inl.h index 526b620c0..4a3b67568 100644 --- a/include/fcl/broadphase/broadphase_SSaP-inl.h +++ b/include/fcl/broadphase/broadphase_SSaP-inl.h @@ -31,7 +31,7 @@ * 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 */ diff --git a/include/fcl/broadphase/broadphase_SSaP.h b/include/fcl/broadphase/broadphase_SSaP.h index 1d0225e76..185b9a0c2 100644 --- a/include/fcl/broadphase/broadphase_SSaP.h +++ b/include/fcl/broadphase/broadphase_SSaP.h @@ -31,7 +31,7 @@ * 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 */ @@ -44,7 +44,7 @@ namespace fcl { -/// @brief Simple SAP collision manager +/// @brief Simple SAP collision manager template class SSaPCollisionManager : public BroadPhaseCollisionManager { @@ -89,7 +89,7 @@ class SSaPCollisionManager : public BroadPhaseCollisionManager /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; @@ -97,13 +97,13 @@ class SSaPCollisionManager : public BroadPhaseCollisionManager /// @brief check collision between one object and a list of objects, return value is whether stop is possible bool checkColl(typename std::vector*>::const_iterator pos_start, typename std::vector*>::const_iterator pos_end, CollisionObject* obj, void* cdata, CollisionCallBack callback) const; - + /// @brief check distance between one object and a list of objects, return value is whether stop is possible bool checkDis(typename std::vector*>::const_iterator pos_start, typename std::vector*>::const_iterator pos_end, CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const; bool collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const; - + bool distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, S& min_dist) const; static size_t selectOptimalAxis( diff --git a/include/fcl/broadphase/broadphase_SaP-inl.h b/include/fcl/broadphase/broadphase_SaP-inl.h index a4ea8cac4..3bfaa9588 100644 --- a/include/fcl/broadphase/broadphase_SaP-inl.h +++ b/include/fcl/broadphase/broadphase_SaP-inl.h @@ -31,7 +31,7 @@ * 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 */ diff --git a/include/fcl/broadphase/broadphase_SaP.h b/include/fcl/broadphase/broadphase_SaP.h index e5b210c27..869e6a26b 100644 --- a/include/fcl/broadphase/broadphase_SaP.h +++ b/include/fcl/broadphase/broadphase_SaP.h @@ -31,7 +31,7 @@ * 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 */ @@ -103,7 +103,7 @@ class SaPCollisionManager : public BroadPhaseCollisionManager /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; @@ -130,7 +130,7 @@ class SaPCollisionManager : public BroadPhaseCollisionManager /// @brief End point list for x, y, z coordinates EndPoint* elist[3]; - + /// @brief vector version of elist, for acceleration std::vector velist[3]; diff --git a/include/fcl/broadphase/broadphase_bruteforce-inl.h b/include/fcl/broadphase/broadphase_bruteforce-inl.h index 1958e9748..89aa74816 100644 --- a/include/fcl/broadphase/broadphase_bruteforce-inl.h +++ b/include/fcl/broadphase/broadphase_bruteforce-inl.h @@ -31,7 +31,7 @@ * 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 */ diff --git a/include/fcl/broadphase/broadphase_bruteforce.h b/include/fcl/broadphase/broadphase_bruteforce.h index 322576b02..2f9179dea 100644 --- a/include/fcl/broadphase/broadphase_bruteforce.h +++ b/include/fcl/broadphase/broadphase_bruteforce.h @@ -31,7 +31,7 @@ * 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 */ @@ -92,7 +92,7 @@ class NaiveCollisionManager : public BroadPhaseCollisionManager /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; diff --git a/include/fcl/broadphase/broadphase_collision_manager.h b/include/fcl/broadphase/broadphase_collision_manager.h index 6c4306a69..c60c21569 100644 --- a/include/fcl/broadphase/broadphase_collision_manager.h +++ b/include/fcl/broadphase/broadphase_collision_manager.h @@ -31,7 +31,7 @@ * 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 */ @@ -117,7 +117,7 @@ class BroadPhaseCollisionManager /// @brief whether the manager is empty virtual bool empty() const = 0; - + /// @brief the number of objects managed by the manager virtual size_t size() const = 0; diff --git a/include/fcl/broadphase/broadphase_continuous_collision_manager.h b/include/fcl/broadphase/broadphase_continuous_collision_manager.h index f9950eb46..7cfa2156c 100644 --- a/include/fcl/broadphase/broadphase_continuous_collision_manager.h +++ b/include/fcl/broadphase/broadphase_continuous_collision_manager.h @@ -31,7 +31,7 @@ * 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 */ @@ -117,7 +117,7 @@ class BroadPhaseContinuousCollisionManager /// @brief whether the manager is empty virtual bool empty() const = 0; - + /// @brief the number of objects managed by the manager virtual size_t size() const = 0; }; diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h index f792f563b..7f3defa89 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h @@ -31,7 +31,7 @@ * 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 */ diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h index e6cfc8902..bfe0ef6db 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree.h @@ -31,7 +31,7 @@ * 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 */ @@ -71,7 +71,7 @@ class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager /// @brief add objects to the manager void registerObjects(const std::vector*>& other_objs); - + /// @brief add one object to the manager void registerObject(CollisionObject* obj); @@ -113,10 +113,10 @@ class DynamicAABBTreeCollisionManager : public BroadPhaseCollisionManager /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const; - + /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h index 1080a42cc..845579835 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array-inl.h @@ -31,7 +31,7 @@ * 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 */ diff --git a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h index 58b1432b9..df4421edb 100644 --- a/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h +++ b/include/fcl/broadphase/broadphase_dynamic_AABB_tree_array.h @@ -31,7 +31,7 @@ * 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 */ @@ -67,12 +67,12 @@ class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager< bool octree_as_geometry_collide; bool octree_as_geometry_distance; - + DynamicAABBTreeCollisionManager_Array(); /// @brief add objects to the manager void registerObjects(const std::vector*>& other_objs); - + /// @brief add one object to the manager void registerObject(CollisionObject* obj); @@ -114,10 +114,10 @@ class DynamicAABBTreeCollisionManager_Array : public BroadPhaseCollisionManager< /// @brief perform distance test with objects belonging to another manager void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const; - + /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; diff --git a/include/fcl/broadphase/broadphase_interval_tree-inl.h b/include/fcl/broadphase/broadphase_interval_tree-inl.h index f7f6f95e9..f959a1982 100644 --- a/include/fcl/broadphase/broadphase_interval_tree-inl.h +++ b/include/fcl/broadphase/broadphase_interval_tree-inl.h @@ -31,7 +31,7 @@ * 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 */ diff --git a/include/fcl/broadphase/broadphase_interval_tree.h b/include/fcl/broadphase/broadphase_interval_tree.h index b7b174d0c..8d28f2143 100644 --- a/include/fcl/broadphase/broadphase_interval_tree.h +++ b/include/fcl/broadphase/broadphase_interval_tree.h @@ -31,7 +31,7 @@ * 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 */ @@ -99,7 +99,7 @@ class IntervalTreeCollisionManager : public BroadPhaseCollisionManager /// @brief whether the manager is empty bool empty() const; - + /// @brief the number of objects managed by the manager size_t size() const; diff --git a/include/fcl/broadphase/broadphase_spatialhash-inl.h b/include/fcl/broadphase/broadphase_spatialhash-inl.h index 85c3a6a0f..3bb86f02a 100644 --- a/include/fcl/broadphase/broadphase_spatialhash-inl.h +++ b/include/fcl/broadphase/broadphase_spatialhash-inl.h @@ -31,7 +31,7 @@ * 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 */ diff --git a/include/fcl/broadphase/broadphase_spatialhash.h b/include/fcl/broadphase/broadphase_spatialhash.h index 0cedef753..96529b2f4 100644 --- a/include/fcl/broadphase/broadphase_spatialhash.h +++ b/include/fcl/broadphase/broadphase_spatialhash.h @@ -31,7 +31,7 @@ * 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 */ diff --git a/include/fcl/broadphase/detail/hierarchy_tree.h b/include/fcl/broadphase/detail/hierarchy_tree.h index bdabe905b..bdea6c940 100644 --- a/include/fcl/broadphase/detail/hierarchy_tree.h +++ b/include/fcl/broadphase/detail/hierarchy_tree.h @@ -70,7 +70,7 @@ class HierarchyTree HierarchyTree(int bu_threshold_ = 16, int topdown_level_ = 0); ~HierarchyTree(); - + /// @brief Initialize the tree by a set of leaves using algorithm with a given level. void init(std::vector& leaves, int level = 0); @@ -80,10 +80,10 @@ class HierarchyTree /// @brief Remove a leaf node void remove(NodeType* leaf); - /// @brief Clear the tree + /// @brief Clear the tree void clear(); - /// @brief Whether the tree is empty + /// @brief Whether the tree is empty bool empty() const; /// @brief Updates a `leaf` node. A use case is when the bounding volume @@ -102,10 +102,10 @@ class HierarchyTree /// @brief update the tree when the bounding volume of a given leaf has changed bool update(NodeType* leaf, const BV& bv); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(NodeType* leaf, const BV& bv, const Vector3& vel, S margin); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(NodeType* leaf, const BV& bv, const Vector3& vel); /// @brief get the max height of the tree @@ -114,19 +114,19 @@ class HierarchyTree /// @brief get the max depth of the tree size_t getMaxDepth() const; - /// @brief balance the tree from bottom + /// @brief balance the tree from bottom void balanceBottomup(); - /// @brief balance the tree from top + /// @brief balance the tree from top void balanceTopdown(); - - /// @brief balance the tree in an incremental way + + /// @brief balance the tree in an incremental way void balanceIncremental(int iterations); - + /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a bottom-up manner void refit(); - /// @brief extract all the leaves of the tree + /// @brief extract all the leaves of the tree void extractLeaves(const NodeType* root, std::vector& leaves) const; /// @brief number of leaves in the tree @@ -153,10 +153,10 @@ class HierarchyTree } }; - /// @brief construct a tree for a set of leaves from bottom -- very heavy way + /// @brief construct a tree for a set of leaves from bottom -- very heavy way void bottomup(const NodeVecIterator lbeg, const NodeVecIterator lend); - /// @brief construct a tree for a set of leaves from top + /// @brief construct a tree for a set of leaves from top NodeType* topdown(const NodeVecIterator lbeg, const NodeVecIterator lend); /// @brief compute the maximum height of a subtree rooted from a given node @@ -184,24 +184,24 @@ class HierarchyTree void init_1(std::vector& leaves); /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code, - /// we split the leaves into two parts with the same size simply using the node index. + /// we split the leaves into two parts with the same size simply using the node index. void init_2(std::vector& leaves); /// @brief init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the leaves into parts with the same size simply using the node index. void init_3(std::vector& leaves); - + NodeType* mortonRecurse_0(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32& split, int bits); NodeType* mortonRecurse_1(const NodeVecIterator lbeg, const NodeVecIterator lend, const uint32& split, int bits); NodeType* mortonRecurse_2(const NodeVecIterator lbeg, const NodeVecIterator lend); - /// @brief update one leaf node's bounding volume + /// @brief update one leaf node's bounding volume void update_(NodeType* leaf, const BV& bv); - /// @brief sort node n and its parent according to their memory position + /// @brief sort node n and its parent according to their memory position NodeType* sort(NodeType* n, NodeType*& r); - + /// @brief Insert a leaf node and also update its ancestors. Maintain the /// tree as a full binary tree (every interior node has exactly two children). /// Furthermore, adjust the BV of interior nodes so that each parent's BV @@ -219,13 +219,13 @@ class HierarchyTree // adjusted. NodeType* removeLeaf(NodeType* const leaf); - /// @brief Delete all internal nodes and return all leaves nodes with given depth from root + /// @brief Delete all internal nodes and return all leaves nodes with given depth from root void fetchLeaves(NodeType* root, std::vector& leaves, int depth = -1); static size_t indexOf(NodeType* node); - /// @brief create one node (leaf or internal) - NodeType* createNode(NodeType* parent, + /// @brief create one node (leaf or internal) + NodeType* createNode(NodeType* parent, const BV& bv, void* data); @@ -233,7 +233,7 @@ class HierarchyTree const BV& bv1, const BV& bv2, void* data); - + NodeType* createNode(NodeType* parent, void* data); @@ -250,11 +250,11 @@ class HierarchyTree unsigned int opath; - /// This is a one NodeType cache, the reason is that we need to remove a node and then add it again frequently. - NodeType* free_node; + /// This is a one NodeType cache, the reason is that we need to remove a node and then add it again frequently. + NodeType* free_node; int max_lookahead_level; - + public: /// @brief decide which topdown algorithm to use int topdown_level; diff --git a/include/fcl/broadphase/detail/hierarchy_tree_array.h b/include/fcl/broadphase/detail/hierarchy_tree_array.h index bd5e40328..8145a04be 100644 --- a/include/fcl/broadphase/detail/hierarchy_tree_array.h +++ b/include/fcl/broadphase/detail/hierarchy_tree_array.h @@ -62,7 +62,7 @@ class HierarchyTree { using S = typename BV::S; typedef NodeBase NodeType; - + struct SortByMorton { SortByMorton(NodeType* nodes_in) : nodes(nodes_in) {} @@ -102,22 +102,22 @@ class HierarchyTree /// @brief Remove a leaf node void remove(size_t leaf); - /// @brief Clear the tree + /// @brief Clear the tree void clear(); - /// @brief Whether the tree is empty + /// @brief Whether the tree is empty bool empty() const; - - /// @brief update one leaf node + + /// @brief update one leaf node void update(size_t leaf, int lookahead_level = -1); /// @brief update the tree when the bounding volume of a given leaf has changed bool update(size_t leaf, const BV& bv); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(size_t leaf, const BV& bv, const Vector3& vel, S margin); - /// @brief update one leaf's bounding volume, with prediction + /// @brief update one leaf's bounding volume, with prediction bool update(size_t leaf, const BV& bv, const Vector3& vel); /// @brief get the max height of the tree @@ -126,19 +126,19 @@ class HierarchyTree /// @brief get the max depth of the tree size_t getMaxDepth() const; - /// @brief balance the tree from bottom + /// @brief balance the tree from bottom void balanceBottomup(); - /// @brief balance the tree from top + /// @brief balance the tree from top void balanceTopdown(); - /// @brief balance the tree in an incremental way + /// @brief balance the tree in an incremental way void balanceIncremental(int iterations); /// @brief refit the tree, i.e., when the leaf nodes' bounding volumes change, update the entire tree in a bottom-up manner void refit(); - /// @brief extract all the leaves of the tree + /// @brief extract all the leaves of the tree void extractLeaves(size_t root, NodeType*& leaves) const; /// @brief number of leaves in the tree @@ -155,10 +155,10 @@ class HierarchyTree private: - /// @brief construct a tree for a set of leaves from bottom -- very heavy way + /// @brief construct a tree for a set of leaves from bottom -- very heavy way void bottomup(size_t* lbeg, size_t* lend); - - /// @brief construct a tree for a set of leaves from top + + /// @brief construct a tree for a set of leaves from top size_t topdown(size_t* lbeg, size_t* lend); /// @brief compute the maximum height of a subtree rooted from a given node @@ -186,7 +186,7 @@ class HierarchyTree void init_1(NodeType* leaves, int n_leaves_); /// @brief init tree from leaves using morton code. It uses morton_0, i.e., for nodes which is of depth more than the maximum bits of the morton code, - /// we split the leaves into two parts with the same size simply using the node index. + /// we split the leaves into two parts with the same size simply using the node index. void init_2(NodeType* leaves, int n_leaves_); /// @brief init tree from leaves using morton code. It uses morton_2, i.e., for all nodes, we simply divide the leaves into parts with the same size simply using the node index. @@ -198,17 +198,17 @@ class HierarchyTree size_t mortonRecurse_2(size_t* lbeg, size_t* lend); - /// @brief update one leaf node's bounding volume + /// @brief update one leaf node's bounding volume void update_(size_t leaf, const BV& bv); - /// @brief Insert a leaf node and also update its ancestors + /// @brief Insert a leaf node and also update its ancestors void insertLeaf(size_t root, size_t leaf); /// @brief Remove a leaf. The leaf node itself is not deleted yet, but all the unnecessary internal nodes are deleted. - /// return the node with the smallest depth and is influenced by the remove operation + /// return the node with the smallest depth and is influenced by the remove operation size_t removeLeaf(size_t leaf); - /// @brief Delete all internal nodes and return all leaves nodes with given depth from root + /// @brief Delete all internal nodes and return all leaves nodes with given depth from root void fetchLeaves(size_t root, NodeType*& leaves, int depth = -1); size_t indexOf(size_t node); @@ -216,13 +216,13 @@ class HierarchyTree size_t allocateNode(); /// @brief create one node (leaf or internal) - size_t createNode(size_t parent, + size_t createNode(size_t parent, const BV& bv1, const BV& bv2, void* data); size_t createNode(size_t parent, - const BV& bv, + const BV& bv, void* data); size_t createNode(size_t parent, @@ -237,7 +237,7 @@ class HierarchyTree NodeType* nodes; size_t n_nodes; size_t n_nodes_alloc; - + size_t n_leaves; size_t freelist; unsigned int opath; diff --git a/include/fcl/broadphase/detail/interval_tree.h b/include/fcl/broadphase/detail/interval_tree.h index a2a2f317f..714dac7a8 100644 --- a/include/fcl/broadphase/detail/interval_tree.h +++ b/include/fcl/broadphase/detail/interval_tree.h @@ -113,7 +113,7 @@ class IntervalTree /// @brief Inserts node into the tree as if it were a regular binary tree void recursiveInsert(IntervalTreeNode* node); - /// @brief recursively print a subtree + /// @brief recursively print a subtree void recursivePrint(IntervalTreeNode* node) const; /// @brief recursively find the node corresponding to the interval diff --git a/include/fcl/broadphase/detail/interval_tree_node.h b/include/fcl/broadphase/detail/interval_tree_node.h index 9d60e6e8d..8e243b3e3 100644 --- a/include/fcl/broadphase/detail/interval_tree_node.h +++ b/include/fcl/broadphase/detail/interval_tree_node.h @@ -60,7 +60,7 @@ class IntervalTreeNode friend class IntervalTree; friend class IntervalTree; - + /// @brief Create an empty node IntervalTreeNode(); @@ -83,7 +83,7 @@ class IntervalTreeNode S max_high; /// @brief red or black node: if red = false then the node is black - bool red; + bool red; IntervalTreeNode* left; diff --git a/include/fcl/broadphase/detail/simple_interval.h b/include/fcl/broadphase/detail/simple_interval.h index 0c5e8e253..1b8516381 100644 --- a/include/fcl/broadphase/detail/simple_interval.h +++ b/include/fcl/broadphase/detail/simple_interval.h @@ -53,7 +53,7 @@ struct SimpleInterval { public: virtual ~SimpleInterval(); - + virtual void print(); /// @brief interval is defined as [low, high] diff --git a/include/fcl/broadphase/detail/sparse_hash_table.h b/include/fcl/broadphase/detail/sparse_hash_table.h index 7d1f07aef..f2d4f0f4f 100644 --- a/include/fcl/broadphase/detail/sparse_hash_table.h +++ b/include/fcl/broadphase/detail/sparse_hash_table.h @@ -62,14 +62,14 @@ class SparseHashTable HashFnc h_; typedef std::list Bin; typedef TableT Table; - + Table table_; public: SparseHashTable(const HashFnc& h); /// @brief Init the hash table. The bucket size is dynamically decided. void init(size_t); - + /// @brief insert one key-value pair into the hash table void insert(Key key, Data value); diff --git a/include/fcl/broadphase/detail/spatial_hash-inl.h b/include/fcl/broadphase/detail/spatial_hash-inl.h index 9eba06d63..b97354f4b 100644 --- a/include/fcl/broadphase/detail/spatial_hash-inl.h +++ b/include/fcl/broadphase/detail/spatial_hash-inl.h @@ -31,7 +31,7 @@ * 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 */ diff --git a/include/fcl/broadphase/detail/spatial_hash.h b/include/fcl/broadphase/detail/spatial_hash.h index f3fdf7e15..ebb25a57b 100644 --- a/include/fcl/broadphase/detail/spatial_hash.h +++ b/include/fcl/broadphase/detail/spatial_hash.h @@ -31,7 +31,7 @@ * 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 */ @@ -53,7 +53,7 @@ struct SpatialHash using S = S_; SpatialHash(const AABB& scene_limit_, S cell_size_); - + std::vector operator() (const AABB& aabb) const; private: diff --git a/include/fcl/geometry/bvh/BVH_model.h b/include/fcl/geometry/bvh/BVH_model.h index 15849ee24..bab7304f5 100644 --- a/include/fcl/geometry/bvh/BVH_model.h +++ b/include/fcl/geometry/bvh/BVH_model.h @@ -74,7 +74,7 @@ class BVHModel : public CollisionGeometry /// @brief We provide getBV() and getNumBVs() because BVH may be compressed /// (in future), so we must provide some flexibility here - + /// @brief Access the bv giving the its index const BVNode& getBV(int id) const; @@ -147,7 +147,7 @@ class BVHModel : public CollisionGeometry /// @brief Check the number of memory used int memUsage(int msg) const; - /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent + /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent /// BV node. When traversing the BVH, this can save one matrix transformation. void makeParentRelative(); @@ -211,7 +211,7 @@ class BVHModel : public CollisionGeometry /// @brief Recursive kernel for hierarchy construction int recursiveBuildTree(int bv_id, int first_primitive, int num_primitives); - /// @brief Recursive kernel for bottomup refitting + /// @brief Recursive kernel for bottomup refitting int recursiveRefitTree_bottomup(int bv_id); /// @recursively compute each bv's transform related to its parent. For diff --git a/include/fcl/geometry/bvh/BV_node_base.h b/include/fcl/geometry/bvh/BV_node_base.h index 2b7f175a0..63d9e4be9 100644 --- a/include/fcl/geometry/bvh/BV_node_base.h +++ b/include/fcl/geometry/bvh/BV_node_base.h @@ -59,7 +59,7 @@ struct FCL_API BVNodeBase /// we can obtain the primitive's index in original data indirectly. int first_primitive; - /// @brief The number of primitives belonging to the current node + /// @brief The number of primitives belonging to the current node int num_primitives; /// @brief Whether current node is a leaf node (i.e. contains a primitive index diff --git a/include/fcl/geometry/octree/octree.h b/include/fcl/geometry/octree/octree.h index b23b09dd8..d0a6b1c22 100644 --- a/include/fcl/geometry/octree/octree.h +++ b/include/fcl/geometry/octree/octree.h @@ -125,7 +125,7 @@ class OcTree : public CollisionGeometry /// @return const ptr to child number childIdx of node const OcTreeNode* getNodeChild(const OcTreeNode* node, unsigned int childIdx) const; - + /// @brief return true if the child at childIdx exists bool nodeChildExists(const OcTreeNode* node, unsigned int childIdx) const; diff --git a/include/fcl/geometry/shape/capsule.h b/include/fcl/geometry/shape/capsule.h index 8c21a96fe..831deed8c 100644 --- a/include/fcl/geometry/shape/capsule.h +++ b/include/fcl/geometry/shape/capsule.h @@ -45,7 +45,7 @@ namespace fcl { -/// @brief Center at zero point capsule +/// @brief Center at zero point capsule template class Capsule : public ShapeBase { @@ -56,16 +56,16 @@ class Capsule : public ShapeBase /// @brief Constructor Capsule(S radius, S lz); - /// @brief Radius of capsule + /// @brief Radius of capsule S radius; - /// @brief Length along z axis + /// @brief Length along z axis S lz; /// @brief Compute AABB void computeLocalAABB() override; - /// @brief Get node type: a capsule + /// @brief Get node type: a capsule NODE_TYPE getNodeType() const override; // Documentation inherited diff --git a/include/fcl/geometry/shape/cone.h b/include/fcl/geometry/shape/cone.h index 30fff4305..528122069 100644 --- a/include/fcl/geometry/shape/cone.h +++ b/include/fcl/geometry/shape/cone.h @@ -45,7 +45,7 @@ namespace fcl { -/// @brief Center at zero cone +/// @brief Center at zero cone template class Cone : public ShapeBase { @@ -55,16 +55,16 @@ class Cone : public ShapeBase Cone(S radius, S lz); - /// @brief Radius of the cone + /// @brief Radius of the cone S radius; - /// @brief Length along z axis + /// @brief Length along z axis S lz; /// @brief Compute AABB void computeLocalAABB() override; - /// @brief Get node type: a cone + /// @brief Get node type: a cone NODE_TYPE getNodeType() const override; // Documentation inherited diff --git a/include/fcl/geometry/shape/convex.h b/include/fcl/geometry/shape/convex.h index e05f50aea..e911613c8 100644 --- a/include/fcl/geometry/shape/convex.h +++ b/include/fcl/geometry/shape/convex.h @@ -110,7 +110,7 @@ class Convex : public ShapeBase int num_faces, const std::shared_ptr>& faces, bool throw_if_invalid = false); - /// @brief Copy constructor + /// @brief Copy constructor Convex(const Convex& other); ~Convex() = default; diff --git a/include/fcl/geometry/shape/cylinder.h b/include/fcl/geometry/shape/cylinder.h index 6e7280d1e..5e33aecfd 100644 --- a/include/fcl/geometry/shape/cylinder.h +++ b/include/fcl/geometry/shape/cylinder.h @@ -45,7 +45,7 @@ namespace fcl { -/// @brief Center at zero cylinder +/// @brief Center at zero cylinder template class Cylinder : public ShapeBase { @@ -55,17 +55,17 @@ class Cylinder : public ShapeBase /// @brief Constructor Cylinder(S radius, S lz); - - /// @brief Radius of the cylinder + + /// @brief Radius of the cylinder S radius; - /// @brief Length along z axis + /// @brief Length along z axis S lz; - /// @brief Compute AABB + /// @brief Compute AABB void computeLocalAABB() override; - /// @brief Get node type: a cylinder + /// @brief Get node type: a cylinder NODE_TYPE getNodeType() const override; // Documentation inherited diff --git a/include/fcl/geometry/shape/halfspace.h b/include/fcl/geometry/shape/halfspace.h index 7d14c14c4..641241cf0 100644 --- a/include/fcl/geometry/shape/halfspace.h +++ b/include/fcl/geometry/shape/halfspace.h @@ -79,10 +79,10 @@ class Halfspace : public ShapeBase /// @brief Get node type: a half space NODE_TYPE getNodeType() const override; - + /// @brief Planed normal Vector3 n; - + /// @brief Planed offset S d; diff --git a/include/fcl/geometry/shape/plane.h b/include/fcl/geometry/shape/plane.h index 77a8ce2f7..6dfb07458 100644 --- a/include/fcl/geometry/shape/plane.h +++ b/include/fcl/geometry/shape/plane.h @@ -45,7 +45,7 @@ namespace fcl { -/// @brief Infinite plane +/// @brief Infinite plane template class Plane : public ShapeBase { @@ -53,10 +53,10 @@ class Plane : public ShapeBase using S = S_; - /// @brief Construct a plane with normal direction and offset + /// @brief Construct a plane with normal direction and offset Plane(const Vector3& n, S d); - - /// @brief Construct a plane with normal direction and offset + + /// @brief Construct a plane with normal direction and offset Plane(S a, S b, S c, S d); Plane(); @@ -68,13 +68,13 @@ class Plane : public ShapeBase /// @brief Compute AABB void computeLocalAABB() override; - /// @brief Get node type: a plane + /// @brief Get node type: a plane NODE_TYPE getNodeType() const override; - /// @brief Plane normal + /// @brief Plane normal Vector3 n; - /// @brief Plane offset + /// @brief Plane offset S d; friend @@ -84,8 +84,8 @@ class Plane : public ShapeBase } protected: - - /// @brief Turn non-unit normal into unit + + /// @brief Turn non-unit normal into unit void unitNormalTest(); }; diff --git a/include/fcl/geometry/shape/triangle_p.h b/include/fcl/geometry/shape/triangle_p.h index da3c92a53..28e911ac0 100644 --- a/include/fcl/geometry/shape/triangle_p.h +++ b/include/fcl/geometry/shape/triangle_p.h @@ -59,7 +59,7 @@ class TriangleP : public ShapeBase /// @brief virtual function of compute AABB in local coordinate void computeLocalAABB() override; - + // Documentation inherited NODE_TYPE getNodeType() const override; diff --git a/include/fcl/math/bv/OBB.h b/include/fcl/math/bv/OBB.h index 1f3f0c4ef..c611070ed 100644 --- a/include/fcl/math/bv/OBB.h +++ b/include/fcl/math/bv/OBB.h @@ -75,10 +75,10 @@ class OBB const Vector3& center, const Vector3& extent); - /// @brief Check collision between two OBB, return true if collision happens. + /// @brief Check collision between two OBB, return true if collision happens. bool overlap(const OBB& other) const; - - /// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb. + + /// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb. bool overlap(const OBB& other, OBB& overlap_part) const; /// @brief Check whether the OBB contains a point. diff --git a/include/fcl/math/bv/kDOP.h b/include/fcl/math/bv/kDOP.h index 31dc4ba74..8b312162c 100644 --- a/include/fcl/math/bv/kDOP.h +++ b/include/fcl/math/bv/kDOP.h @@ -47,7 +47,7 @@ namespace fcl { /// @brief KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24 -/// The KDOP structure is defined by some pairs of parallel planes defined by some axes. +/// The KDOP structure is defined by some pairs of parallel planes defined by some axes. /// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges: /// (-1,0,0) and (1,0,0) -> indices 0 and 8 /// (0,-1,0) and (0,1,0) -> indices 1 and 9 @@ -95,7 +95,7 @@ class KDOP /// @brief Creating kDOP containing two points KDOP(const Vector3& a, const Vector3& b); - + /// @brief Check whether two KDOPs are overlapped bool overlap(const KDOP& other) const; diff --git a/include/fcl/math/bv/kIOS.h b/include/fcl/math/bv/kIOS.h index 0ce7ba496..3cf941778 100644 --- a/include/fcl/math/bv/kIOS.h +++ b/include/fcl/math/bv/kIOS.h @@ -42,7 +42,7 @@ namespace fcl { - + /// @brief A class describing the kIOS collision structure, which is a set of spheres. template class kIOS diff --git a/include/fcl/math/detail/polysolver-inl.h b/include/fcl/math/detail/polysolver-inl.h index 24370c528..405fea6e3 100644 --- a/include/fcl/math/detail/polysolver-inl.h +++ b/include/fcl/math/detail/polysolver-inl.h @@ -1,201 +1,201 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_POLYSOLVER_INL_H -#define FCL_NARROWPHASE_DETAIL_POLYSOLVER_INL_H - -#include "fcl/math/detail/polysolver.h" - -#include -#include "fcl/common/types.h" - -namespace fcl -{ - -namespace detail { - -//============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API PolySolver; - -//============================================================================== -template -int PolySolver::solveLinear(S c[2], S s[1]) -{ - if(isZero(c[1])) - return 0; - s[0] = - c[0] / c[1]; - return 1; -} - -//============================================================================== -template -int PolySolver::solveQuadric(S c[3], S s[2]) -{ - S p, q, D; - - // make sure we have a d2 equation - - if(isZero(c[2])) - return solveLinear(c, s); - - // normal for: x^2 + px + q - p = c[1] / (2.0 * c[2]); - q = c[0] / c[2]; - D = p * p - q; - - if(isZero(D)) - { - // one S root - s[0] = s[1] = -p; - return 1; - } - - if(D < 0.0) - // no real root - return 0; - else - { - // two real roots - S sqrt_D = sqrt(D); - s[0] = sqrt_D - p; - s[1] = -sqrt_D - p; - return 2; - } -} - -//============================================================================== -template -int PolySolver::solveCubic(S c[4], S s[3]) -{ - int i, num; - S sub, A, B, C, sq_A, p, q, cb_p, D; - const S ONE_OVER_THREE = 1 / 3.0; - const S PI = 3.14159265358979323846; - - // make sure we have a d2 equation - if(isZero(c[3])) - return solveQuadric(c, s); - - // normalize the equation:x ^ 3 + Ax ^ 2 + Bx + C = 0 - A = c[2] / c[3]; - B = c[1] / c[3]; - C = c[0] / c[3]; - - // substitute x = y - A / 3 to eliminate the quadratic term: x^3 + px + q = 0 - sq_A = A * A; - p = (-ONE_OVER_THREE * sq_A + B) * ONE_OVER_THREE; - q = 0.5 * (2.0 / 27.0 * A * sq_A - ONE_OVER_THREE * A * B + C); - - // use Cardano's formula - cb_p = p * p * p; - D = q * q + cb_p; - - if(isZero(D)) - { - if(isZero(q)) - { - // one triple solution - s[0] = 0.0; - num = 1; - } - else - { - // one single and one S solution - S u = cbrt(-q); - s[0] = 2.0 * u; - s[1] = -u; - num = 2; - } - } - else - { - if(D < 0.0) - { - // three real solutions - S phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p)); - S t = 2.0 * sqrt(-p); - s[0] = t * cos(phi); - s[1] = -t * cos(phi + PI / 3.0); - s[2] = -t * cos(phi - PI / 3.0); - num = 3; - } - else - { - // one real solution - S sqrt_D = sqrt(D); - S u = cbrt(sqrt_D + fabs(q)); - if(q > 0.0) - s[0] = - u + p / u ; - else - s[0] = u - p / u; - num = 1; - } - } - - // re-substitute - sub = ONE_OVER_THREE * A; - for(i = 0; i < num; i++) - s[i] -= sub; - return num; -} - -//============================================================================== -template -bool PolySolver::isZero(S v) -{ - return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold()); -} - -//============================================================================== -template -bool PolySolver::cbrt(S v) -{ - return std::pow(v, 1.0 / 3.0); -} - -//============================================================================== -template -constexpr S PolySolver::getNearZeroThreshold() -{ - return 1e-9; -} - -} // namespace detail -} // namespace fcl - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_POLYSOLVER_INL_H +#define FCL_NARROWPHASE_DETAIL_POLYSOLVER_INL_H + +#include "fcl/math/detail/polysolver.h" + +#include +#include "fcl/common/types.h" + +namespace fcl +{ + +namespace detail { + +//============================================================================== +extern template +class FCL_EXTERN_TEMPLATE_API PolySolver; + +//============================================================================== +template +int PolySolver::solveLinear(S c[2], S s[1]) +{ + if(isZero(c[1])) + return 0; + s[0] = - c[0] / c[1]; + return 1; +} + +//============================================================================== +template +int PolySolver::solveQuadric(S c[3], S s[2]) +{ + S p, q, D; + + // make sure we have a d2 equation + + if(isZero(c[2])) + return solveLinear(c, s); + + // normal for: x^2 + px + q + p = c[1] / (2.0 * c[2]); + q = c[0] / c[2]; + D = p * p - q; + + if(isZero(D)) + { + // one S root + s[0] = s[1] = -p; + return 1; + } + + if(D < 0.0) + // no real root + return 0; + else + { + // two real roots + S sqrt_D = sqrt(D); + s[0] = sqrt_D - p; + s[1] = -sqrt_D - p; + return 2; + } +} + +//============================================================================== +template +int PolySolver::solveCubic(S c[4], S s[3]) +{ + int i, num; + S sub, A, B, C, sq_A, p, q, cb_p, D; + const S ONE_OVER_THREE = 1 / 3.0; + const S PI = 3.14159265358979323846; + + // make sure we have a d2 equation + if(isZero(c[3])) + return solveQuadric(c, s); + + // normalize the equation:x ^ 3 + Ax ^ 2 + Bx + C = 0 + A = c[2] / c[3]; + B = c[1] / c[3]; + C = c[0] / c[3]; + + // substitute x = y - A / 3 to eliminate the quadratic term: x^3 + px + q = 0 + sq_A = A * A; + p = (-ONE_OVER_THREE * sq_A + B) * ONE_OVER_THREE; + q = 0.5 * (2.0 / 27.0 * A * sq_A - ONE_OVER_THREE * A * B + C); + + // use Cardano's formula + cb_p = p * p * p; + D = q * q + cb_p; + + if(isZero(D)) + { + if(isZero(q)) + { + // one triple solution + s[0] = 0.0; + num = 1; + } + else + { + // one single and one S solution + S u = cbrt(-q); + s[0] = 2.0 * u; + s[1] = -u; + num = 2; + } + } + else + { + if(D < 0.0) + { + // three real solutions + S phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p)); + S t = 2.0 * sqrt(-p); + s[0] = t * cos(phi); + s[1] = -t * cos(phi + PI / 3.0); + s[2] = -t * cos(phi - PI / 3.0); + num = 3; + } + else + { + // one real solution + S sqrt_D = sqrt(D); + S u = cbrt(sqrt_D + fabs(q)); + if(q > 0.0) + s[0] = - u + p / u ; + else + s[0] = u - p / u; + num = 1; + } + } + + // re-substitute + sub = ONE_OVER_THREE * A; + for(i = 0; i < num; i++) + s[i] -= sub; + return num; +} + +//============================================================================== +template +bool PolySolver::isZero(S v) +{ + return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold()); +} + +//============================================================================== +template +bool PolySolver::cbrt(S v) +{ + return std::pow(v, 1.0 / 3.0); +} + +//============================================================================== +template +constexpr S PolySolver::getNearZeroThreshold() +{ + return 1e-9; +} + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/math/detail/polysolver.h b/include/fcl/math/detail/polysolver.h index d79217f2d..bcad09f86 100644 --- a/include/fcl/math/detail/polysolver.h +++ b/include/fcl/math/detail/polysolver.h @@ -1,80 +1,80 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_POLYSOLVER_H -#define FCL_NARROWPHASE_DETAIL_POLYSOLVER_H - -#include "fcl/export.h" - -namespace fcl -{ - -namespace detail { - -/// @brief A class solves polynomial degree (1,2,3) equations -template -class PolySolver -{ -public: - /// @brief Solve a linear equation with coefficients c, return roots s and number of roots - static int solveLinear(S c[2], S s[1]); - - /// @brief Solve a quadratic function with coefficients c, return roots s and number of roots - static int solveQuadric(S c[3], S s[2]); - - /// @brief Solve a cubic function with coefficients c, return roots s and number of roots - static int solveCubic(S c[4], S s[3]); - -private: - /// @brief Check whether v is zero - static bool isZero(S v); - - /// @brief Compute v^{1/3} - static bool cbrt(S v); - - static constexpr S getNearZeroThreshold(); -}; - -using PolySolverf = PolySolver; -using PolySolverd = PolySolver; - -} // namespace detail -} // namespace fcl - -#include "fcl/math/detail/polysolver-inl.h" - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_POLYSOLVER_H +#define FCL_NARROWPHASE_DETAIL_POLYSOLVER_H + +#include "fcl/export.h" + +namespace fcl +{ + +namespace detail { + +/// @brief A class solves polynomial degree (1,2,3) equations +template +class PolySolver +{ +public: + /// @brief Solve a linear equation with coefficients c, return roots s and number of roots + static int solveLinear(S c[2], S s[1]); + + /// @brief Solve a quadratic function with coefficients c, return roots s and number of roots + static int solveQuadric(S c[3], S s[2]); + + /// @brief Solve a cubic function with coefficients c, return roots s and number of roots + static int solveCubic(S c[4], S s[3]); + +private: + /// @brief Check whether v is zero + static bool isZero(S v); + + /// @brief Compute v^{1/3} + static bool cbrt(S v); + + static constexpr S getNearZeroThreshold(); +}; + +using PolySolverf = PolySolver; +using PolySolverd = PolySolver; + +} // namespace detail +} // namespace fcl + +#include "fcl/math/detail/polysolver-inl.h" + +#endif diff --git a/include/fcl/math/detail/project-inl.h b/include/fcl/math/detail/project-inl.h index 9cccf5e51..1b29dffc2 100644 --- a/include/fcl/math/detail/project-inl.h +++ b/include/fcl/math/detail/project-inl.h @@ -1,320 +1,320 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_PROJECT_INL_H -#define FCL_NARROWPHASE_DETAIL_PROJECT_INL_H - -#include "fcl/math/detail/project.h" - -namespace fcl -{ - -namespace detail -{ - -//============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API Project; - -//============================================================================== -template -typename Project::ProjectResult Project::projectLine(const Vector3& a, const Vector3& b, const Vector3& p) -{ - ProjectResult res; - - const Vector3 d = b - a; - const S l = d.squaredNorm(); - - if(l > 0) - { - const S t = (p - a).dot(d); - res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); - res.parameterization[0] = 1 - res.parameterization[1]; - if(t >= l) { res.sqr_distance = (p - b).squaredNorm(); res.encode = 2; /* 0x10 */ } - else if(t <= 0) { res.sqr_distance = (p - a).squaredNorm(); res.encode = 1; /* 0x01 */ } - else { res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); res.encode = 3; /* 0x00 */ } - } - - return res; -} - -//============================================================================== -template -typename Project::ProjectResult Project::projectTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p) -{ - ProjectResult res; - - static const size_t nexti[3] = {1, 2, 0}; - const Vector3* vt[] = {&a, &b, &c}; - const Vector3 dl[] = {a - b, b - c, c - a}; - const Vector3& n = dl[0].cross(dl[1]); - const S l = n.squaredNorm(); - - if(l > 0) - { - S mindist = -1; - for(size_t i = 0; i < 3; ++i) - { - if((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge - { - size_t j = nexti[i]; - ProjectResult res_line = projectLine(*vt[i], *vt[j], p); - - if(mindist < 0 || res_line.sqr_distance < mindist) - { - mindist = res_line.sqr_distance; - res.encode = static_cast(((res_line.encode&1)?1< p_to_project = n * (d / l); - mindist = p_to_project.squaredNorm(); - res.encode = 7; // m = 0x111 - res.parameterization[0] = dl[1].cross(b - p -p_to_project).norm() / s; - res.parameterization[1] = dl[2].cross(c - p -p_to_project).norm() / s; - res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; - } - - res.sqr_distance = mindist; - } - - return res; - -} - -//============================================================================== -template -typename Project::ProjectResult Project::projectTetrahedra(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& p) -{ - ProjectResult res; - - static const size_t nexti[] = {1, 2, 0}; - const Vector3* vt[] = {&a, &b, &c, &d}; - const Vector3 dl[3] = {a-d, b-d, c-d}; - S vl = triple(dl[0], dl[1], dl[2]); - bool ng = (vl * (a-p).dot((b-c).cross(a-b))) <= 0; - if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) - { - S mindist = -1; - - for(size_t i = 0; i < 3; ++i) - { - size_t j = nexti[i]; - S s = vl * (d-p).dot(dl[i].cross(dl[j])); - if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face - { - ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p); - if(mindist < 0 || res_triangle.sqr_distance < mindist) - { - mindist = res_triangle.sqr_distance; - res.encode = static_cast( (res_triangle.encode&1?1< -typename Project::ProjectResult Project::projectLineOrigin(const Vector3& a, const Vector3& b) -{ - ProjectResult res; - - const Vector3 d = b - a; - const S l = d.squaredNorm(); - - if(l > 0) - { - const S t = - a.dot(d); - res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); - res.parameterization[0] = 1 - res.parameterization[1]; - if(t >= l) { res.sqr_distance = b.squaredNorm(); res.encode = 2; /* 0x10 */ } - else if(t <= 0) { res.sqr_distance = a.squaredNorm(); res.encode = 1; /* 0x01 */ } - else { res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); res.encode = 3; /* 0x00 */ } - } - - return res; -} - -//============================================================================== -template -typename Project::ProjectResult Project::projectTriangleOrigin(const Vector3& a, const Vector3& b, const Vector3& c) -{ - ProjectResult res; - - static const size_t nexti[3] = {1, 2, 0}; - const Vector3* vt[] = {&a, &b, &c}; - const Vector3 dl[] = {a - b, b - c, c - a}; - const Vector3& n = dl[0].cross(dl[1]); - const S l = n.squaredNorm(); - - if(l > 0) - { - S mindist = -1; - for(size_t i = 0; i < 3; ++i) - { - if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge - { - size_t j = nexti[i]; - ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]); - - if(mindist < 0 || res_line.sqr_distance < mindist) - { - mindist = res_line.sqr_distance; - res.encode = static_cast(((res_line.encode&1)?1< o_to_project = n * (d / l); - mindist = o_to_project.squaredNorm(); - res.encode = 7; // m = 0x111 - res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s; - res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s; - res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; - } - - res.sqr_distance = mindist; - } - - return res; - -} - -//============================================================================== -template -typename Project::ProjectResult Project::projectTetrahedraOrigin(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) -{ - ProjectResult res; - - static const size_t nexti[] = {1, 2, 0}; - const Vector3* vt[] = {&a, &b, &c, &d}; - const Vector3 dl[3] = {a-d, b-d, c-d}; - S vl = triple(dl[0], dl[1], dl[2]); - bool ng = (vl * a.dot((b-c).cross(a-b))) <= 0; - if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) - { - S mindist = -1; - - for(size_t i = 0; i < 3; ++i) - { - size_t j = nexti[i]; - S s = vl * d.dot(dl[i].cross(dl[j])); - if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face - { - ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d); - if(mindist < 0 || res_triangle.sqr_distance < mindist) - { - mindist = res_triangle.sqr_distance; - res.encode = static_cast( (res_triangle.encode&1?1< -Project::ProjectResult::ProjectResult() - : parameterization{0.0, 0.0, 0.0, 0.0}, sqr_distance(-1), encode(0) -{ - // Do nothing -} - -} // namespace detail -} // namespace fcl - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_PROJECT_INL_H +#define FCL_NARROWPHASE_DETAIL_PROJECT_INL_H + +#include "fcl/math/detail/project.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +extern template +class FCL_EXTERN_TEMPLATE_API Project; + +//============================================================================== +template +typename Project::ProjectResult Project::projectLine(const Vector3& a, const Vector3& b, const Vector3& p) +{ + ProjectResult res; + + const Vector3 d = b - a; + const S l = d.squaredNorm(); + + if(l > 0) + { + const S t = (p - a).dot(d); + res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); + res.parameterization[0] = 1 - res.parameterization[1]; + if(t >= l) { res.sqr_distance = (p - b).squaredNorm(); res.encode = 2; /* 0x10 */ } + else if(t <= 0) { res.sqr_distance = (p - a).squaredNorm(); res.encode = 1; /* 0x01 */ } + else { res.sqr_distance = (a + d * res.parameterization[1] - p).squaredNorm(); res.encode = 3; /* 0x00 */ } + } + + return res; +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p) +{ + ProjectResult res; + + static const size_t nexti[3] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c}; + const Vector3 dl[] = {a - b, b - c, c - a}; + const Vector3& n = dl[0].cross(dl[1]); + const S l = n.squaredNorm(); + + if(l > 0) + { + S mindist = -1; + for(size_t i = 0; i < 3; ++i) + { + if((*vt[i] - p).dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge + { + size_t j = nexti[i]; + ProjectResult res_line = projectLine(*vt[i], *vt[j], p); + + if(mindist < 0 || res_line.sqr_distance < mindist) + { + mindist = res_line.sqr_distance; + res.encode = static_cast(((res_line.encode&1)?1< p_to_project = n * (d / l); + mindist = p_to_project.squaredNorm(); + res.encode = 7; // m = 0x111 + res.parameterization[0] = dl[1].cross(b - p -p_to_project).norm() / s; + res.parameterization[1] = dl[2].cross(c - p -p_to_project).norm() / s; + res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; + } + + res.sqr_distance = mindist; + } + + return res; + +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTetrahedra(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& p) +{ + ProjectResult res; + + static const size_t nexti[] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c, &d}; + const Vector3 dl[3] = {a-d, b-d, c-d}; + S vl = triple(dl[0], dl[1], dl[2]); + bool ng = (vl * (a-p).dot((b-c).cross(a-b))) <= 0; + if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) + { + S mindist = -1; + + for(size_t i = 0; i < 3; ++i) + { + size_t j = nexti[i]; + S s = vl * (d-p).dot(dl[i].cross(dl[j])); + if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face + { + ProjectResult res_triangle = projectTriangle(*vt[i], *vt[j], d, p); + if(mindist < 0 || res_triangle.sqr_distance < mindist) + { + mindist = res_triangle.sqr_distance; + res.encode = static_cast( (res_triangle.encode&1?1< +typename Project::ProjectResult Project::projectLineOrigin(const Vector3& a, const Vector3& b) +{ + ProjectResult res; + + const Vector3 d = b - a; + const S l = d.squaredNorm(); + + if(l > 0) + { + const S t = - a.dot(d); + res.parameterization[1] = (t >= l) ? 1 : ((t <= 0) ? 0 : (t / l)); + res.parameterization[0] = 1 - res.parameterization[1]; + if(t >= l) { res.sqr_distance = b.squaredNorm(); res.encode = 2; /* 0x10 */ } + else if(t <= 0) { res.sqr_distance = a.squaredNorm(); res.encode = 1; /* 0x01 */ } + else { res.sqr_distance = (a + d * res.parameterization[1]).squaredNorm(); res.encode = 3; /* 0x00 */ } + } + + return res; +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTriangleOrigin(const Vector3& a, const Vector3& b, const Vector3& c) +{ + ProjectResult res; + + static const size_t nexti[3] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c}; + const Vector3 dl[] = {a - b, b - c, c - a}; + const Vector3& n = dl[0].cross(dl[1]); + const S l = n.squaredNorm(); + + if(l > 0) + { + S mindist = -1; + for(size_t i = 0; i < 3; ++i) + { + if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge + { + size_t j = nexti[i]; + ProjectResult res_line = projectLineOrigin(*vt[i], *vt[j]); + + if(mindist < 0 || res_line.sqr_distance < mindist) + { + mindist = res_line.sqr_distance; + res.encode = static_cast(((res_line.encode&1)?1< o_to_project = n * (d / l); + mindist = o_to_project.squaredNorm(); + res.encode = 7; // m = 0x111 + res.parameterization[0] = dl[1].cross(b - o_to_project).norm() / s; + res.parameterization[1] = dl[2].cross(c - o_to_project).norm() / s; + res.parameterization[2] = 1 - res.parameterization[0] - res.parameterization[1]; + } + + res.sqr_distance = mindist; + } + + return res; + +} + +//============================================================================== +template +typename Project::ProjectResult Project::projectTetrahedraOrigin(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) +{ + ProjectResult res; + + static const size_t nexti[] = {1, 2, 0}; + const Vector3* vt[] = {&a, &b, &c, &d}; + const Vector3 dl[3] = {a-d, b-d, c-d}; + S vl = triple(dl[0], dl[1], dl[2]); + bool ng = (vl * a.dot((b-c).cross(a-b))) <= 0; + if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face) + { + S mindist = -1; + + for(size_t i = 0; i < 3; ++i) + { + size_t j = nexti[i]; + S s = vl * d.dot(dl[i].cross(dl[j])); + if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face + { + ProjectResult res_triangle = projectTriangleOrigin(*vt[i], *vt[j], d); + if(mindist < 0 || res_triangle.sqr_distance < mindist) + { + mindist = res_triangle.sqr_distance; + res.encode = static_cast( (res_triangle.encode&1?1< +Project::ProjectResult::ProjectResult() + : parameterization{0.0, 0.0, 0.0, 0.0}, sqr_distance(-1), encode(0) +{ + // Do nothing +} + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/math/detail/project.h b/include/fcl/math/detail/project.h index c08f065da..4b19b0dbe 100644 --- a/include/fcl/math/detail/project.h +++ b/include/fcl/math/detail/project.h @@ -1,96 +1,96 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_PROJECT_H -#define FCL_NARROWPHASE_DETAIL_PROJECT_H - -#include "fcl/common/types.h" -#include "fcl/math/geometry.h" - -namespace fcl -{ - -namespace detail -{ - -/// @brief Project functions -template -class Project -{ -public: - struct ProjectResult - { - /// @brief Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the array) - S parameterization[4]; - - /// @brief square distance from the query point to the projected simplex - S sqr_distance; - - /// @brief the code of the projection type - unsigned int encode; - - ProjectResult(); - }; - - /// @brief Project point p onto line a-b - static ProjectResult projectLine(const Vector3& a, const Vector3& b, const Vector3& p); - - /// @brief Project point p onto triangle a-b-c - static ProjectResult projectTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p); - - /// @brief Project point p onto tetrahedra a-b-c-d - static ProjectResult projectTetrahedra(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& p); - - /// @brief Project origin (0) onto line a-b - static ProjectResult projectLineOrigin(const Vector3& a, const Vector3& b); - - /// @brief Project origin (0) onto triangle a-b-c - static ProjectResult projectTriangleOrigin(const Vector3& a, const Vector3& b, const Vector3& c); - - /// @brief Project origin (0) onto tetrahedran a-b-c-d - static ProjectResult projectTetrahedraOrigin(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d); -}; - -using Projectf = Project; -using Projectd = Project; - -} // namespace detail -} // namespace fcl - -#include "fcl/math/detail/project-inl.h" - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_PROJECT_H +#define FCL_NARROWPHASE_DETAIL_PROJECT_H + +#include "fcl/common/types.h" +#include "fcl/math/geometry.h" + +namespace fcl +{ + +namespace detail +{ + +/// @brief Project functions +template +class Project +{ +public: + struct ProjectResult + { + /// @brief Parameterization of the projected point (based on the simplex to be projected, use 2 or 3 or 4 of the array) + S parameterization[4]; + + /// @brief square distance from the query point to the projected simplex + S sqr_distance; + + /// @brief the code of the projection type + unsigned int encode; + + ProjectResult(); + }; + + /// @brief Project point p onto line a-b + static ProjectResult projectLine(const Vector3& a, const Vector3& b, const Vector3& p); + + /// @brief Project point p onto triangle a-b-c + static ProjectResult projectTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& p); + + /// @brief Project point p onto tetrahedra a-b-c-d + static ProjectResult projectTetrahedra(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Vector3& p); + + /// @brief Project origin (0) onto line a-b + static ProjectResult projectLineOrigin(const Vector3& a, const Vector3& b); + + /// @brief Project origin (0) onto triangle a-b-c + static ProjectResult projectTriangleOrigin(const Vector3& a, const Vector3& b, const Vector3& c); + + /// @brief Project origin (0) onto tetrahedran a-b-c-d + static ProjectResult projectTetrahedraOrigin(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d); +}; + +using Projectf = Project; +using Projectd = Project; + +} // namespace detail +} // namespace fcl + +#include "fcl/math/detail/project-inl.h" + +#endif diff --git a/include/fcl/math/motion/interp_motion.h b/include/fcl/math/motion/interp_motion.h index 55ec717c9..69cbd8efb 100644 --- a/include/fcl/math/motion/interp_motion.h +++ b/include/fcl/math/motion/interp_motion.h @@ -81,7 +81,7 @@ class InterpMotion : public MotionBase /// @brief Compute the motion bound for a bounding volume along a given direction n, which is defined in the visitor S computeMotionBound(const BVMotionBoundVisitor& mb_visitor) const; - /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor + /// @brief Compute the motion bound for a triangle along a given direction n, which is defined in the visitor S computeMotionBound(const TriangleMotionBoundVisitor& mb_visitor) const; /// @brief Get the rotation and translation in current step @@ -94,9 +94,9 @@ class InterpMotion : public MotionBase void computeVelocity(); Quaternion deltaRotation(S dt) const; - + Quaternion absoluteRotation(S dt) const; - + /// @brief The transformation at time 0 Transform3 tf1; diff --git a/include/fcl/math/motion/spline_motion.h b/include/fcl/math/motion/spline_motion.h index 7267f4d5f..a6082ac04 100644 --- a/include/fcl/math/motion/spline_motion.h +++ b/include/fcl/math/motion/spline_motion.h @@ -63,7 +63,7 @@ class SplineMotion : public MotionBase SplineMotion(const Transform3& tf1, const Transform3& tf2); - + /// @brief Integrate the motion from 0 to dt /// We compute the current transformation from zero point instead of from last integrate time, for precision. bool integrate(S dt) const override; @@ -86,7 +86,7 @@ class SplineMotion : public MotionBase S getWeight1(S t) const; S getWeight2(S t) const; S getWeight3(S t) const; - + Vector3 Td[4]; Vector3 Rd[4]; @@ -102,7 +102,7 @@ class SplineMotion : public MotionBase public: S computeTBound(const Vector3& n) const; - + S computeDWMax() const; S getCurrentTime() const; diff --git a/include/fcl/math/motion/taylor_model/interval_vector.h b/include/fcl/math/motion/taylor_model/interval_vector.h index d4d3473f9..c59e81af2 100644 --- a/include/fcl/math/motion/taylor_model/interval_vector.h +++ b/include/fcl/math/motion/taylor_model/interval_vector.h @@ -72,7 +72,7 @@ struct IVector3 void setValue(const Vector3& v); void setValue(S v[3]); - + IVector3 operator + (const IVector3& other) const; IVector3& operator += (const IVector3& other); @@ -90,7 +90,7 @@ struct IVector3 Interval& operator [] (size_t i); Vector3 getLow() const; - + Vector3 getHigh() const; void print() const; diff --git a/include/fcl/math/motion/taylor_model/taylor_matrix.h b/include/fcl/math/motion/taylor_model/taylor_matrix.h index eb1180582..2c4a11126 100644 --- a/include/fcl/math/motion/taylor_model/taylor_matrix.h +++ b/include/fcl/math/motion/taylor_model/taylor_matrix.h @@ -48,7 +48,7 @@ template class TMatrix3 { TVector3 v_[3]; - + public: TMatrix3(); TMatrix3(const std::shared_ptr>& time_interval); diff --git a/include/fcl/math/motion/taylor_model/taylor_model.h b/include/fcl/math/motion/taylor_model/taylor_model.h index 8b5120318..e80561fd2 100644 --- a/include/fcl/math/motion/taylor_model/taylor_model.h +++ b/include/fcl/math/motion/taylor_model/taylor_model.h @@ -69,7 +69,7 @@ class TaylorModel public: void setTimeInterval(S l, S r); - + void setTimeInterval(const std::shared_ptr>& time_interval); const std::shared_ptr>& getTimeInterval() const; @@ -78,7 +78,7 @@ class TaylorModel S& coeff(std::size_t i); const Interval& remainder() const; Interval& remainder(); - + TaylorModel(); TaylorModel(const std::shared_ptr>& time_interval); TaylorModel(S coeff, const std::shared_ptr>& time_interval); diff --git a/include/fcl/math/motion/taylor_model/taylor_vector.h b/include/fcl/math/motion/taylor_model/taylor_vector.h index 72a18172b..884a3caab 100644 --- a/include/fcl/math/motion/taylor_model/taylor_vector.h +++ b/include/fcl/math/motion/taylor_model/taylor_vector.h @@ -50,13 +50,13 @@ class TVector3 TaylorModel i_[3]; public: - + TVector3(); TVector3(const std::shared_ptr>& time_interval); TVector3(TaylorModel v[3]); TVector3(const TaylorModel& v0, const TaylorModel& v1, const TaylorModel& v2); TVector3(const Vector3& v, const std::shared_ptr>& time_interval); - + TVector3 operator + (const TVector3& other) const; TVector3& operator += (const TVector3& other); diff --git a/include/fcl/math/sampler/sampler_se3_euler_ball.h b/include/fcl/math/sampler/sampler_se3_euler_ball.h index 610908a42..137d76606 100644 --- a/include/fcl/math/sampler/sampler_se3_euler_ball.h +++ b/include/fcl/math/sampler/sampler_se3_euler_ball.h @@ -53,7 +53,7 @@ class SamplerSE3Euler_ball : public SamplerBase SamplerSE3Euler_ball(S r_); void setBound(const S& r_); - + void getBound(S& r_) const; Vector6 sample() const; diff --git a/include/fcl/narrowphase/collision_request.h b/include/fcl/narrowphase/collision_request.h index fa85ac153..b1604f46c 100644 --- a/include/fcl/narrowphase/collision_request.h +++ b/include/fcl/narrowphase/collision_request.h @@ -81,7 +81,7 @@ struct CollisionRequest // single std::optional>. /// @brief If true, uses the provided initial guess for the GJK algorithm. bool enable_cached_gjk_guess; - + /// @brief The initial guess to use in the GJK algorithm. Vector3 cached_gjk_guess; diff --git a/include/fcl/narrowphase/collision_result.h b/include/fcl/narrowphase/collision_result.h index b7f36312c..8ce60cf9b 100644 --- a/include/fcl/narrowphase/collision_result.h +++ b/include/fcl/narrowphase/collision_result.h @@ -85,7 +85,7 @@ struct CollisionResult /// @brief get all the contacts void getContacts(std::vector>& contacts_); - /// @brief get all the cost sources + /// @brief get all the cost sources void getCostSources(std::vector>& cost_sources_); /// @brief clear the results obtained diff --git a/include/fcl/narrowphase/contact.h b/include/fcl/narrowphase/contact.h index ee3207916..686086c61 100644 --- a/include/fcl/narrowphase/contact.h +++ b/include/fcl/narrowphase/contact.h @@ -66,7 +66,7 @@ struct Contact /// if object 2 is octree, it is the query cell id (see /// OcTree::getNodeByQueryCellId) intptr_t b2; - + /// @brief contact normal, pointing from o1 to o2 Vector3 normal; @@ -76,7 +76,7 @@ struct Contact /// @brief penetration depth S penetration_depth; - + /// @brief invalid contact primitive information static const int NONE = -1; diff --git a/include/fcl/narrowphase/continuous_collision_request.h b/include/fcl/narrowphase/continuous_collision_request.h index f1a0f67ad..518b09598 100644 --- a/include/fcl/narrowphase/continuous_collision_request.h +++ b/include/fcl/narrowphase/continuous_collision_request.h @@ -65,13 +65,13 @@ struct ContinuousCollisionRequest /// @brief ccd solver type CCDSolverType ccd_solver_type; - + ContinuousCollisionRequest(std::size_t num_max_iterations_ = 10, S toc_err_ = 0.0001, CCDMotionType ccd_motion_type_ = CCDM_TRANS, GJKSolverType gjk_solver_type_ = GST_LIBCCD, CCDSolverType ccd_solver_type_ = CCDC_NAIVE); - + }; using ContinuousCollisionRequestf = ContinuousCollisionRequest; diff --git a/include/fcl/narrowphase/continuous_collision_result.h b/include/fcl/narrowphase/continuous_collision_result.h index 473a856ec..8e5f8334a 100644 --- a/include/fcl/narrowphase/continuous_collision_result.h +++ b/include/fcl/narrowphase/continuous_collision_result.h @@ -49,14 +49,14 @@ struct ContinuousCollisionResult { /// @brief collision or not bool is_collide; - + /// @brief time of contact in [0, 1] S time_of_contact; Transform3 contact_tf1; Transform3 contact_tf2; - + ContinuousCollisionResult(); EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/epa.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/epa.h index 8d81137f8..131dc7a7f 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/epa.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/epa.h @@ -101,7 +101,7 @@ struct EPA public: enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed}; - + Status status; typename GJK::Simplex result; Vector3 normal; @@ -130,8 +130,8 @@ struct EPA Status evaluate(GJK& gjk, const Vector3& guess); - /// @brief the goal is to add a face connecting vertex w and face edge f[e] - bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon); + /// @brief the goal is to add a face connecting vertex w and face edge f[e] + bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon); }; using EPAf = EPA; diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk.h index 5fb481ffc..7949bba28 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk.h @@ -63,7 +63,7 @@ struct GJK { /// @brief simplex vertex SimplexV* c[4]; - /// @brief weight + /// @brief weight S p[4]; /// @brief size of simplex (number of vertices) size_t rank; @@ -79,7 +79,7 @@ struct GJK Simplex simplices[2]; GJK(unsigned int max_iterations_, S tolerance_); - + void initialize(); /// @brief GJK algorithm, given the initial value guess diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff.h index d3f967d07..6e073accf 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/minkowski_diff.h @@ -63,14 +63,14 @@ struct MinkowskiDiff /// @brief rotation from shape0 to shape1 Matrix3 toshape1; - /// @brief transform from shape1 to shape0 + /// @brief transform from shape1 to shape0 Transform3 toshape0; MinkowskiDiff(); /// @brief support function for shape0 Vector3 support0(const Vector3& d) const; - + /// @brief support function for shape1 Vector3 support1(const Vector3& d) const; diff --git a/include/fcl/narrowphase/detail/failed_at_this_configuration.h b/include/fcl/narrowphase/detail/failed_at_this_configuration.h index bef56934a..1856fa2c3 100644 --- a/include/fcl/narrowphase/detail/failed_at_this_configuration.h +++ b/include/fcl/narrowphase/detail/failed_at_this_configuration.h @@ -87,7 +87,7 @@ FCL_API void ThrowFailedAtThisConfiguration( /** Helper class for propagating a low-level exception upwards but with configuration-specific details appended. The parameters - + @param s1 The first shape in the query. @param X_FS1 The pose of the first shape in frame F. @param s2 The second shape in the query. diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h index 9beef7625..ec58038b3 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h @@ -1,467 +1,467 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_INL_H -#define FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_INL_H - -#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h" - -namespace fcl -{ - -namespace detail -{ - -//============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API TriangleDistance; - -//============================================================================== -template -void TriangleDistance::segPoints(const Vector3& P, const Vector3& A, const Vector3& Q, const Vector3& B, - Vector3& VEC, Vector3& X, Vector3& Y) -{ - Vector3 T; - S A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; - Vector3 TMP; - - T = Q - P; - A_dot_A = A.dot(A); - B_dot_B = B.dot(B); - A_dot_B = A.dot(B); - A_dot_T = A.dot(T); - B_dot_T = B.dot(T); - - // t parameterizes ray P,A - // u parameterizes ray Q,B - - S t, u; - - // compute t for the closest point on ray P,A to - // ray Q,B - - S denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B; - - t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom; - - // clamp result so t is on the segment P,A - - if((t < 0) || std::isnan(t)) t = 0; else if(t > 1) t = 1; - - // find u for point on ray Q,B closest to point at t - - u = (t*A_dot_B - B_dot_T) / B_dot_B; - - // if u is on segment Q,B, t and u correspond to - // closest points, otherwise, clamp u, recompute and - // clamp t - - if((u <= 0) || std::isnan(u)) - { - Y = Q; - - t = A_dot_T / A_dot_A; - - if((t <= 0) || std::isnan(t)) - { - X = P; - VEC = Q - P; - } - else if(t >= 1) - { - X = P + A; - VEC = Q - X; - } - else - { - X = P + A * t; - TMP = T.cross(A); - VEC = A.cross(TMP); - } - } - else if (u >= 1) - { - Y = Q + B; - - t = (A_dot_B + A_dot_T) / A_dot_A; - - if((t <= 0) || std::isnan(t)) - { - X = P; - VEC = Y - P; - } - else if(t >= 1) - { - X = P + A; - VEC = Y - X; - } - else - { - X = P + A * t; - T = Y - P; - TMP = T.cross(A); - VEC= A.cross(TMP); - } - } - else - { - Y = Q + B * u; - - if((t <= 0) || std::isnan(t)) - { - X = P; - TMP = T.cross(B); - VEC = B.cross(TMP); - } - else if(t >= 1) - { - X = P + A; - T = Q - X; - TMP = T.cross(B); - VEC = B.cross(TMP); - } - else - { - X = P + A * t; - VEC = A.cross(B); - if(VEC.dot(T) < 0) - { - VEC = VEC * (-1); - } - } - } -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], Vector3& P, Vector3& Q) -{ - // Compute vectors along the 6 sides - - Vector3 Sv[3]; - Vector3 Tv[3]; - Vector3 VEC; - - Sv[0] = T1[1] - T1[0]; - Sv[1] = T1[2] - T1[1]; - Sv[2] = T1[0] - T1[2]; - - Tv[0] = T2[1] - T2[0]; - Tv[1] = T2[2] - T2[1]; - Tv[2] = T2[0] - T2[2]; - - // For each edge pair, the vector connecting the closest points - // of the edges defines a slab (parallel planes at head and tail - // enclose the slab). If we can show that the off-edge vertex of - // each triangle is outside of the slab, then the closest points - // of the edges are the closest points for the triangles. - // Even if these tests fail, it may be helpful to know the closest - // points found, and whether the triangles were shown disjoint - - Vector3 V; - Vector3 Z; - Vector3 minP = Vector3::Zero(); - Vector3 minQ = Vector3::Zero(); - S mindd; - int shown_disjoint = 0; - - mindd = (T1[0] - T2[0]).squaredNorm() + 1; // Set first minimum safely high - - for(int i = 0; i < 3; ++i) - { - for(int j = 0; j < 3; ++j) - { - // Find closest points on edges i & j, plus the - // vector (and distance squared) between these points - segPoints(T1[i], Sv[i], T2[j], Tv[j], VEC, P, Q); - - V = Q - P; - S dd = V.dot(V); - - // Verify this closest point pair only if the distance - // squared is less than the minimum found thus far. - - if(dd <= mindd) - { - minP = P; - minQ = Q; - mindd = dd; - - Z = T1[(i+2)%3] - P; - S a = Z.dot(VEC); - Z = T2[(j+2)%3] - Q; - S b = Z.dot(VEC); - - if((a <= 0) && (b >= 0)) return sqrt(dd); - - S p = V.dot(VEC); - - if(a < 0) a = 0; - if(b > 0) b = 0; - if((p - a + b) > 0) shown_disjoint = 1; - } - } - } - - // No edge pairs contained the closest points. - // either: - // 1. one of the closest points is a vertex, and the - // other point is interior to a face. - // 2. the triangles are overlapping. - // 3. an edge of one triangle is parallel to the other's face. If - // cases 1 and 2 are not true, then the closest points from the 9 - // edge pairs checks above can be taken as closest points for the - // triangles. - // 4. possibly, the triangles were degenerate. When the - // triangle points are nearly colinear or coincident, one - // of above tests might fail even though the edges tested - // contain the closest points. - - // First check for case 1 - - Vector3 Sn; - S Snl; - - Sn = Sv[0].cross(Sv[1]); // Compute normal to T1 triangle - Snl = Sn.dot(Sn); // Compute square of length of normal - - // If cross product is long enough, - - if(Snl > 1e-15) - { - // Get projection lengths of T2 points - - Vector3 Tp; - - V = T1[0] - T2[0]; - Tp[0] = V.dot(Sn); - - V = T1[0] - T2[1]; - Tp[1] = V.dot(Sn); - - V = T1[0] - T2[2]; - Tp[2] = V.dot(Sn); - - // If Sn is a separating direction, - // find point with smallest projection - - int point = -1; - if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) - { - if(Tp[0] < Tp[1]) point = 0; else point = 1; - if(Tp[2] < Tp[point]) point = 2; - } - else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) - { - if(Tp[0] > Tp[1]) point = 0; else point = 1; - if(Tp[2] > Tp[point]) point = 2; - } - - // If Sn is a separating direction, - - if(point >= 0) - { - shown_disjoint = 1; - - // Test whether the point found, when projected onto the - // other triangle, lies within the face. - - V = T2[point] - T1[0]; - Z = Sn.cross(Sv[0]); - if(V.dot(Z) > 0) - { - V = T2[point] - T1[1]; - Z = Sn.cross(Sv[1]); - if(V.dot(Z) > 0) - { - V = T2[point] - T1[2]; - Z = Sn.cross(Sv[2]); - if(V.dot(Z) > 0) - { - // T[point] passed the test - it's a closest point for - // the T2 triangle; the other point is on the face of T1 - P = T2[point] + Sn * (Tp[point] / Snl); - Q = T2[point]; - return (P - Q).norm(); - } - } - } - } - } - - Vector3 Tn; - S Tnl; - - Tn = Tv[0].cross(Tv[1]); - Tnl = Tn.dot(Tn); - - if(Tnl > 1e-15) - { - Vector3 Sp; - - V = T2[0] - T1[0]; - Sp[0] = V.dot(Tn); - - V = T2[0] - T1[1]; - Sp[1] = V.dot(Tn); - - V = T2[0] - T1[2]; - Sp[2] = V.dot(Tn); - - int point = -1; - if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) - { - if(Sp[0] < Sp[1]) point = 0; else point = 1; - if(Sp[2] < Sp[point]) point = 2; - } - else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) - { - if(Sp[0] > Sp[1]) point = 0; else point = 1; - if(Sp[2] > Sp[point]) point = 2; - } - - if(point >= 0) - { - shown_disjoint = 1; - - V = T1[point] - T2[0]; - Z = Tn.cross(Tv[0]); - if(V.dot(Z) > 0) - { - V = T1[point] - T2[1]; - Z = Tn.cross(Tv[1]); - if(V.dot(Z) > 0) - { - V = T1[point] - T2[2]; - Z = Tn.cross(Tv[2]); - if(V.dot(Z) > 0) - { - P = T1[point]; - Q = T1[point] + Tn * (Sp[point] / Tnl); - return (P - Q).norm(); - } - } - } - } - } - - // Case 1 can't be shown. - // If one of these tests showed the triangles disjoint, - // we assume case 3 or 4, otherwise we conclude case 2, - // that the triangles overlap. - - if(shown_disjoint) - { - P = minP; - Q = minQ; - return sqrt(mindd); - } - else return 0; -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - Vector3& P, Vector3& Q) -{ - Vector3 U[3]; - Vector3 T[3]; - U[0] = S1; U[1] = S2; U[2] = S3; - T[0] = T1; T[1] = T2; T[2] = T3; - - return triDistance(U, T, P, Q); -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], - const Matrix3& R, const Vector3& Tl, - Vector3& P, Vector3& Q) -{ - Vector3 T_transformed[3]; - T_transformed[0] = R * T2[0] + Tl; - T_transformed[1] = R * T2[1] + Tl; - T_transformed[2] = R * T2[2] + Tl; - - return triDistance(T1, T_transformed, P, Q); -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], - const Transform3& tf, - Vector3& P, Vector3& Q) -{ - Vector3 T_transformed[3]; - T_transformed[0] = tf * T2[0]; - T_transformed[1] = tf * T2[1]; - T_transformed[2] = tf * T2[2]; - - return triDistance(T1, T_transformed, P, Q); -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - const Matrix3& R, const Vector3& Tl, - Vector3& P, Vector3& Q) -{ - Vector3 T1_transformed = R * T1 + Tl; - Vector3 T2_transformed = R * T2 + Tl; - Vector3 T3_transformed = R * T3 + Tl; - return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); -} - -//============================================================================== -template -S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - const Transform3& tf, - Vector3& P, Vector3& Q) -{ - Vector3 T1_transformed = tf * T1; - Vector3 T2_transformed = tf * T2; - Vector3 T3_transformed = tf * T3; - return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); -} - -} // namespace detail -} // namespace fcl - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_INL_H +#define FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_INL_H + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +extern template +class FCL_EXTERN_TEMPLATE_API TriangleDistance; + +//============================================================================== +template +void TriangleDistance::segPoints(const Vector3& P, const Vector3& A, const Vector3& Q, const Vector3& B, + Vector3& VEC, Vector3& X, Vector3& Y) +{ + Vector3 T; + S A_dot_A, B_dot_B, A_dot_B, A_dot_T, B_dot_T; + Vector3 TMP; + + T = Q - P; + A_dot_A = A.dot(A); + B_dot_B = B.dot(B); + A_dot_B = A.dot(B); + A_dot_T = A.dot(T); + B_dot_T = B.dot(T); + + // t parameterizes ray P,A + // u parameterizes ray Q,B + + S t, u; + + // compute t for the closest point on ray P,A to + // ray Q,B + + S denom = A_dot_A*B_dot_B - A_dot_B*A_dot_B; + + t = (A_dot_T*B_dot_B - B_dot_T*A_dot_B) / denom; + + // clamp result so t is on the segment P,A + + if((t < 0) || std::isnan(t)) t = 0; else if(t > 1) t = 1; + + // find u for point on ray Q,B closest to point at t + + u = (t*A_dot_B - B_dot_T) / B_dot_B; + + // if u is on segment Q,B, t and u correspond to + // closest points, otherwise, clamp u, recompute and + // clamp t + + if((u <= 0) || std::isnan(u)) + { + Y = Q; + + t = A_dot_T / A_dot_A; + + if((t <= 0) || std::isnan(t)) + { + X = P; + VEC = Q - P; + } + else if(t >= 1) + { + X = P + A; + VEC = Q - X; + } + else + { + X = P + A * t; + TMP = T.cross(A); + VEC = A.cross(TMP); + } + } + else if (u >= 1) + { + Y = Q + B; + + t = (A_dot_B + A_dot_T) / A_dot_A; + + if((t <= 0) || std::isnan(t)) + { + X = P; + VEC = Y - P; + } + else if(t >= 1) + { + X = P + A; + VEC = Y - X; + } + else + { + X = P + A * t; + T = Y - P; + TMP = T.cross(A); + VEC= A.cross(TMP); + } + } + else + { + Y = Q + B * u; + + if((t <= 0) || std::isnan(t)) + { + X = P; + TMP = T.cross(B); + VEC = B.cross(TMP); + } + else if(t >= 1) + { + X = P + A; + T = Q - X; + TMP = T.cross(B); + VEC = B.cross(TMP); + } + else + { + X = P + A * t; + VEC = A.cross(B); + if(VEC.dot(T) < 0) + { + VEC = VEC * (-1); + } + } + } +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], Vector3& P, Vector3& Q) +{ + // Compute vectors along the 6 sides + + Vector3 Sv[3]; + Vector3 Tv[3]; + Vector3 VEC; + + Sv[0] = T1[1] - T1[0]; + Sv[1] = T1[2] - T1[1]; + Sv[2] = T1[0] - T1[2]; + + Tv[0] = T2[1] - T2[0]; + Tv[1] = T2[2] - T2[1]; + Tv[2] = T2[0] - T2[2]; + + // For each edge pair, the vector connecting the closest points + // of the edges defines a slab (parallel planes at head and tail + // enclose the slab). If we can show that the off-edge vertex of + // each triangle is outside of the slab, then the closest points + // of the edges are the closest points for the triangles. + // Even if these tests fail, it may be helpful to know the closest + // points found, and whether the triangles were shown disjoint + + Vector3 V; + Vector3 Z; + Vector3 minP = Vector3::Zero(); + Vector3 minQ = Vector3::Zero(); + S mindd; + int shown_disjoint = 0; + + mindd = (T1[0] - T2[0]).squaredNorm() + 1; // Set first minimum safely high + + for(int i = 0; i < 3; ++i) + { + for(int j = 0; j < 3; ++j) + { + // Find closest points on edges i & j, plus the + // vector (and distance squared) between these points + segPoints(T1[i], Sv[i], T2[j], Tv[j], VEC, P, Q); + + V = Q - P; + S dd = V.dot(V); + + // Verify this closest point pair only if the distance + // squared is less than the minimum found thus far. + + if(dd <= mindd) + { + minP = P; + minQ = Q; + mindd = dd; + + Z = T1[(i+2)%3] - P; + S a = Z.dot(VEC); + Z = T2[(j+2)%3] - Q; + S b = Z.dot(VEC); + + if((a <= 0) && (b >= 0)) return sqrt(dd); + + S p = V.dot(VEC); + + if(a < 0) a = 0; + if(b > 0) b = 0; + if((p - a + b) > 0) shown_disjoint = 1; + } + } + } + + // No edge pairs contained the closest points. + // either: + // 1. one of the closest points is a vertex, and the + // other point is interior to a face. + // 2. the triangles are overlapping. + // 3. an edge of one triangle is parallel to the other's face. If + // cases 1 and 2 are not true, then the closest points from the 9 + // edge pairs checks above can be taken as closest points for the + // triangles. + // 4. possibly, the triangles were degenerate. When the + // triangle points are nearly colinear or coincident, one + // of above tests might fail even though the edges tested + // contain the closest points. + + // First check for case 1 + + Vector3 Sn; + S Snl; + + Sn = Sv[0].cross(Sv[1]); // Compute normal to T1 triangle + Snl = Sn.dot(Sn); // Compute square of length of normal + + // If cross product is long enough, + + if(Snl > 1e-15) + { + // Get projection lengths of T2 points + + Vector3 Tp; + + V = T1[0] - T2[0]; + Tp[0] = V.dot(Sn); + + V = T1[0] - T2[1]; + Tp[1] = V.dot(Sn); + + V = T1[0] - T2[2]; + Tp[2] = V.dot(Sn); + + // If Sn is a separating direction, + // find point with smallest projection + + int point = -1; + if((Tp[0] > 0) && (Tp[1] > 0) && (Tp[2] > 0)) + { + if(Tp[0] < Tp[1]) point = 0; else point = 1; + if(Tp[2] < Tp[point]) point = 2; + } + else if((Tp[0] < 0) && (Tp[1] < 0) && (Tp[2] < 0)) + { + if(Tp[0] > Tp[1]) point = 0; else point = 1; + if(Tp[2] > Tp[point]) point = 2; + } + + // If Sn is a separating direction, + + if(point >= 0) + { + shown_disjoint = 1; + + // Test whether the point found, when projected onto the + // other triangle, lies within the face. + + V = T2[point] - T1[0]; + Z = Sn.cross(Sv[0]); + if(V.dot(Z) > 0) + { + V = T2[point] - T1[1]; + Z = Sn.cross(Sv[1]); + if(V.dot(Z) > 0) + { + V = T2[point] - T1[2]; + Z = Sn.cross(Sv[2]); + if(V.dot(Z) > 0) + { + // T[point] passed the test - it's a closest point for + // the T2 triangle; the other point is on the face of T1 + P = T2[point] + Sn * (Tp[point] / Snl); + Q = T2[point]; + return (P - Q).norm(); + } + } + } + } + } + + Vector3 Tn; + S Tnl; + + Tn = Tv[0].cross(Tv[1]); + Tnl = Tn.dot(Tn); + + if(Tnl > 1e-15) + { + Vector3 Sp; + + V = T2[0] - T1[0]; + Sp[0] = V.dot(Tn); + + V = T2[0] - T1[1]; + Sp[1] = V.dot(Tn); + + V = T2[0] - T1[2]; + Sp[2] = V.dot(Tn); + + int point = -1; + if((Sp[0] > 0) && (Sp[1] > 0) && (Sp[2] > 0)) + { + if(Sp[0] < Sp[1]) point = 0; else point = 1; + if(Sp[2] < Sp[point]) point = 2; + } + else if((Sp[0] < 0) && (Sp[1] < 0) && (Sp[2] < 0)) + { + if(Sp[0] > Sp[1]) point = 0; else point = 1; + if(Sp[2] > Sp[point]) point = 2; + } + + if(point >= 0) + { + shown_disjoint = 1; + + V = T1[point] - T2[0]; + Z = Tn.cross(Tv[0]); + if(V.dot(Z) > 0) + { + V = T1[point] - T2[1]; + Z = Tn.cross(Tv[1]); + if(V.dot(Z) > 0) + { + V = T1[point] - T2[2]; + Z = Tn.cross(Tv[2]); + if(V.dot(Z) > 0) + { + P = T1[point]; + Q = T1[point] + Tn * (Sp[point] / Tnl); + return (P - Q).norm(); + } + } + } + } + } + + // Case 1 can't be shown. + // If one of these tests showed the triangles disjoint, + // we assume case 3 or 4, otherwise we conclude case 2, + // that the triangles overlap. + + if(shown_disjoint) + { + P = minP; + Q = minQ; + return sqrt(mindd); + } + else return 0; +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + Vector3& P, Vector3& Q) +{ + Vector3 U[3]; + Vector3 T[3]; + U[0] = S1; U[1] = S2; U[2] = S3; + T[0] = T1; T[1] = T2; T[2] = T3; + + return triDistance(U, T, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q) +{ + Vector3 T_transformed[3]; + T_transformed[0] = R * T2[0] + Tl; + T_transformed[1] = R * T2[1] + Tl; + T_transformed[2] = R * T2[2] + Tl; + + return triDistance(T1, T_transformed, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Transform3& tf, + Vector3& P, Vector3& Q) +{ + Vector3 T_transformed[3]; + T_transformed[0] = tf * T2[0]; + T_transformed[1] = tf * T2[1]; + T_transformed[2] = tf * T2[2]; + + return triDistance(T1, T_transformed, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q) +{ + Vector3 T1_transformed = R * T1 + Tl; + Vector3 T2_transformed = R * T2 + Tl; + Vector3 T3_transformed = R * T3 + Tl; + return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); +} + +//============================================================================== +template +S TriangleDistance::triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + const Transform3& tf, + Vector3& P, Vector3& Q) +{ + Vector3 T1_transformed = tf * T1; + Vector3 T2_transformed = tf * T2; + Vector3 T3_transformed = tf * T3; + return triDistance(S1, S2, S3, T1_transformed, T2_transformed, T3_transformed, P, Q); +} + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h b/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h index b32408dc5..cc800f0f1 100644 --- a/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h +++ b/include/fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance.h @@ -1,114 +1,114 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_H -#define FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_H - -#include "fcl/common/types.h" - -namespace fcl -{ - -namespace detail -{ - -/// @brief Triangle distance functions -template -class TriangleDistance -{ -public: - - /// @brief Returns closest points between an segment pair. - /// The first segment is P + t * A - /// The second segment is Q + t * B - /// X, Y are the closest points on the two segments - /// VEC is the vector between X and Y - static void segPoints(const Vector3& P, const Vector3& A, const Vector3& Q, const Vector3& B, - Vector3& VEC, Vector3& X, Vector3& Y); - - /// @brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them - /// T1 and T2 are two triangles - /// If the triangles are disjoint, P and Q give the closet points of T1 and T2 respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not - /// coincident points on the intersection of the triangles, as might be expected. - static S triDistance(const Vector3 T1[3], const Vector3 T2[3], Vector3& P, Vector3& Q); - - static S triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - Vector3& P, Vector3& Q); - - /// @brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them - /// T1 and T2 are two triangles - /// If the triangles are disjoint, P and Q give the closet points of T1 and T2 respectively. However, - /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not - /// coincident points on the intersection of the triangles, as might be expected. - /// The returned P and Q are both in the coordinate of the first triangle's coordinate - static S triDistance(const Vector3 T1[3], const Vector3 T2[3], - const Matrix3& R, const Vector3& Tl, - Vector3& P, Vector3& Q); - - static S triDistance(const Vector3 T1[3], const Vector3 T2[3], - const Transform3& tf, - Vector3& P, Vector3& Q); - - static S triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, - const Vector3& T1, const Vector3& T2, const Vector3& T3, - const Matrix3& R, const Vector3& Tl, - Vector3& P, Vector3& Q); - - static S triDistance( - const Vector3& S1, - const Vector3& S2, - const Vector3& S3, - const Vector3& T1, - const Vector3& T2, - const Vector3& T3, - const Transform3& tf, - Vector3& P, - Vector3& Q); - -}; - -using TriangleDistancef = TriangleDistance; -using TriangleDistanced = TriangleDistance; - -} // namespace detail -} // namespace fcl - -#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h" - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_H +#define FCL_NARROWPHASE_DETAIL_TRIANGLEDISTANCE_H + +#include "fcl/common/types.h" + +namespace fcl +{ + +namespace detail +{ + +/// @brief Triangle distance functions +template +class TriangleDistance +{ +public: + + /// @brief Returns closest points between an segment pair. + /// The first segment is P + t * A + /// The second segment is Q + t * B + /// X, Y are the closest points on the two segments + /// VEC is the vector between X and Y + static void segPoints(const Vector3& P, const Vector3& A, const Vector3& Q, const Vector3& B, + Vector3& VEC, Vector3& X, Vector3& Y); + + /// @brief Compute the closest points on two triangles given their absolute coordinate, and returns the distance between them + /// T1 and T2 are two triangles + /// If the triangles are disjoint, P and Q give the closet points of T1 and T2 respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not + /// coincident points on the intersection of the triangles, as might be expected. + static S triDistance(const Vector3 T1[3], const Vector3 T2[3], Vector3& P, Vector3& Q); + + static S triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + Vector3& P, Vector3& Q); + + /// @brief Compute the closest points on two triangles given the relative transform between them, and returns the distance between them + /// T1 and T2 are two triangles + /// If the triangles are disjoint, P and Q give the closet points of T1 and T2 respectively. However, + /// if the triangles overlap, P and Q are basically a random pair of points from the triangles, not + /// coincident points on the intersection of the triangles, as might be expected. + /// The returned P and Q are both in the coordinate of the first triangle's coordinate + static S triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q); + + static S triDistance(const Vector3 T1[3], const Vector3 T2[3], + const Transform3& tf, + Vector3& P, Vector3& Q); + + static S triDistance(const Vector3& S1, const Vector3& S2, const Vector3& S3, + const Vector3& T1, const Vector3& T2, const Vector3& T3, + const Matrix3& R, const Vector3& Tl, + Vector3& P, Vector3& Q); + + static S triDistance( + const Vector3& S1, + const Vector3& S2, + const Vector3& S3, + const Vector3& T1, + const Vector3& T2, + const Vector3& T3, + const Transform3& tf, + Vector3& P, + Vector3& Q); + +}; + +using TriangleDistancef = TriangleDistance; +using TriangleDistanced = TriangleDistance; + +} // namespace detail +} // namespace fcl + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h" + +#endif diff --git a/include/fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h index d1473867e..44cfe31f4 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/collision/bvh_collision_traversal_node.h @@ -81,7 +81,7 @@ class BVHCollisionTraversalNode /// @brief BV culling test in one BVTT node bool BVTesting(int b1, int b2) const; - + /// @brief The first BVH model const BVHModel* model1; diff --git a/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base.h b/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base.h index 959d532df..58ea12561 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base.h +++ b/include/fcl/narrowphase/detail/traversal/collision/collision_traversal_node_base.h @@ -74,7 +74,7 @@ class CollisionTraversalNodeBase : public TraversalNodeBase /// @brief collision result kept during the traversal iteration CollisionResult* result; - /// @brief Whether stores statistics + /// @brief Whether stores statistics bool enable_statistics; }; diff --git a/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h b/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h index 7b7f02fda..027c791a9 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h +++ b/include/fcl/narrowphase/detail/traversal/collision/intersect-inl.h @@ -1,1146 +1,1146 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_INTERSECT_INL_H -#define FCL_NARROWPHASE_DETAIL_INTERSECT_INL_H - -#include "fcl/narrowphase/detail/traversal/collision/intersect.h" - -namespace fcl -{ - -namespace detail -{ - -//============================================================================== -extern template -class FCL_EXTERN_TEMPLATE_API Intersect; - -//============================================================================== -template -bool Intersect::isZero(S v) -{ - return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold()); -} - -//============================================================================== -/// @brief data: only used for EE, return the intersect point -template -bool Intersect::solveCubicWithIntervalNewton(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S& l, S& r, bool bVF, S coeffs[], Vector3* data) -{ - S v2[2]= {l*l,r*r}; - S v[2]= {l,r}; - S r_backup; - - unsigned char min3, min2, min1, max3, max2, max1; - - min3= *((unsigned char*)&coeffs[3]+7)>>7; max3=min3^1; - min2= *((unsigned char*)&coeffs[2]+7)>>7; max2=min2^1; - min1= *((unsigned char*)&coeffs[1]+7)>>7; max1=min1^1; - - // bound the cubic - - S minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0]; - S major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0]; - - if(major<0) return false; - if(minor>0) return false; - - // starting here, the bounds have opposite values - S m = 0.5 * (r + l); - - // bound the derivative - S dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1]; - S dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1]; - - if((dminor > 0)||(dmajor < 0)) // we can use Newton - { - S m2 = m*m; - S fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0]; - S nl = m; - S nu = m; - if(fm>0) - { - nl-=(fm/dminor); - nu-=(fm/dmajor); - } - else - { - nu-=(fm/dminor); - nl-=(fm/dmajor); - } - - //intersect with [l,r] - - if(nl>r) return false; - if(nul) - { - if(nu -bool Intersect::insideTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3&p) -{ - Vector3 ab = b - a; - Vector3 ac = c - a; - Vector3 n = ab.cross(ac); - - Vector3 pa = a - p; - Vector3 pb = b - p; - Vector3 pc = c - p; - - if((pb.cross(pc)).dot(n) < -getEpsilon()) return false; - if((pc.cross(pa)).dot(n) < -getEpsilon()) return false; - if((pa.cross(pb)).dot(n) < -getEpsilon()) return false; - - return true; -} - -//============================================================================== -template -bool Intersect::insideLineSegment(const Vector3& a, const Vector3& b, const Vector3& p) -{ - return (p - a).dot(p - b) <= 0; -} - -//============================================================================== -/// @brief Calculate the line segment papb that is the shortest route between -/// two lines p1p2 and p3p4. Calculate also the values of mua and mub where -/// pa = p1 + mua (p2 - p1) -/// pb = p3 + mub (p4 - p3) -/// Return FALSE if no solution exists. -template -bool Intersect::linelineIntersect(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, - Vector3* pa, Vector3* pb, S* mua, S* mub) -{ - Vector3 p31 = p1 - p3; - Vector3 p34 = p4 - p3; - if(fabs(p34[0]) < getEpsilon() && fabs(p34[1]) < getEpsilon() && fabs(p34[2]) < getEpsilon()) - return false; - - Vector3 p12 = p2 - p1; - if(fabs(p12[0]) < getEpsilon() && fabs(p12[1]) < getEpsilon() && fabs(p12[2]) < getEpsilon()) - return false; - - S d3134 = p31.dot(p34); - S d3412 = p34.dot(p12); - S d3112 = p31.dot(p12); - S d3434 = p34.dot(p34); - S d1212 = p12.dot(p12); - - S denom = d1212 * d3434 - d3412 * d3412; - if(fabs(denom) < getEpsilon()) - return false; - S numer = d3134 * d3412 - d3112 * d3434; - - *mua = numer / denom; - if(*mua < 0 || *mua > 1) - return false; - - *mub = (d3134 + d3412 * (*mua)) / d3434; - if(*mub < 0 || *mub > 1) - return false; - - *pa = p1 + p12 * (*mua); - *pb = p3 + p34 * (*mub); - return true; -} - -//============================================================================== -template -bool Intersect::checkRootValidity_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, - S t) -{ - return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t); -} - -//============================================================================== -template -bool Intersect::checkRootValidity_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S t, Vector3* q_i) -{ - Vector3 a = a0 + va * t; - Vector3 b = b0 + vb * t; - Vector3 c = c0 + vc * t; - Vector3 d = d0 + vd * t; - Vector3 p1, p2; - S t_ab, t_cd; - if(linelineIntersect(a, b, c, d, &p1, &p2, &t_ab, &t_cd)) - { - if(q_i) *q_i = p1; - return true; - } - - return false; -} - -//============================================================================== -template -bool Intersect::checkRootValidity_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp, - S t) -{ - return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t); -} - -//============================================================================== -template -bool Intersect::solveSquare(S a, S b, S c, - const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - bool bVF, - S* ret) -{ - S discriminant = b * b - 4 * a * c; - if(discriminant < 0) - return false; - - S sqrt_dis = sqrt(discriminant); - S r1 = (-b + sqrt_dis) / (2 * a); - bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r1) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r1)) : false; - - S r2 = (-b - sqrt_dis) / (2 * a); - bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r2) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r2)) : false; - - if(v1 && v2) - { - *ret = (r1 > r2) ? r2 : r1; - return true; - } - if(v1) - { - *ret = r1; - return true; - } - if(v2) - { - *ret = r2; - return true; - } - - return false; -} - -//============================================================================== -template -bool Intersect::solveSquare(S a, S b, S c, - const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp) -{ - if(isZero(a)) - { - S t = -c/b; - return (t >= 0 && t <= 1) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, t) : false; - } - - S discriminant = b*b-4*a*c; - if(discriminant < 0) - return false; - - S sqrt_dis = sqrt(discriminant); - - S r1 = (-b+sqrt_dis) / (2 * a); - bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r1) : false; - if(v1) return true; - - S r2 = (-b-sqrt_dis) / (2 * a); - bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r2) : false; - return v2; -} - -//============================================================================== -/// @brief Compute the cubic coefficients for VF case -/// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. -template -void Intersect::computeCubicCoeff_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, - S* a, S* b, S* c, S* d) -{ - Vector3 vavb = vb - va; - Vector3 vavc = vc - va; - Vector3 vavp = vp - va; - Vector3 a0b0 = b0 - a0; - Vector3 a0c0 = c0 - a0; - Vector3 a0p0 = p0 - a0; - - Vector3 vavb_cross_vavc = vavb.cross(vavc); - Vector3 vavb_cross_a0c0 = vavb.cross(a0c0); - Vector3 a0b0_cross_vavc = a0b0.cross(vavc); - Vector3 a0b0_cross_a0c0 = a0b0.cross(a0c0); - - *a = vavp.dot(vavb_cross_vavc); - *b = a0p0.dot(vavb_cross_vavc) + vavp.dot(vavb_cross_a0c0 + a0b0_cross_vavc); - *c = vavp.dot(a0b0_cross_a0c0) + a0p0.dot(vavb_cross_a0c0 + a0b0_cross_vavc); - *d = a0p0.dot(a0b0_cross_a0c0); -} - -//============================================================================== -template -void Intersect::computeCubicCoeff_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S* a, S* b, S* c, S* d) -{ - Vector3 vavb = vb - va; - Vector3 vcvd = vd - vc; - Vector3 vavc = vc - va; - Vector3 c0d0 = d0 - c0; - Vector3 a0b0 = b0 - a0; - Vector3 a0c0 = c0 - a0; - Vector3 vavb_cross_vcvd = vavb.cross(vcvd); - Vector3 vavb_cross_c0d0 = vavb.cross(c0d0); - Vector3 a0b0_cross_vcvd = a0b0.cross(vcvd); - Vector3 a0b0_cross_c0d0 = a0b0.cross(c0d0); - - *a = vavc.dot(vavb_cross_vcvd); - *b = a0c0.dot(vavb_cross_vcvd) + vavc.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); - *c = vavc.dot(a0b0_cross_c0d0) + a0c0.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); - *d = a0c0.dot(a0b0_cross_c0d0); -} - -//============================================================================== -template -void Intersect::computeCubicCoeff_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp, - const Vector3& L, - S* a, S* b, S* c) -{ - Vector3 vbva = va - vb; - Vector3 vbvp = vp - vb; - Vector3 b0a0 = a0 - b0; - Vector3 b0p0 = p0 - b0; - - Vector3 L_cross_vbvp = L.cross(vbvp); - Vector3 L_cross_b0p0 = L.cross(b0p0); - - *a = L_cross_vbvp.dot(vbva); - *b = L_cross_vbvp.dot(b0a0) + L_cross_b0p0.dot(vbva); - *c = L_cross_b0p0.dot(b0a0); -} - -//============================================================================== -template -bool Intersect::intersect_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, - S* collision_time, Vector3* p_i, bool useNewton) -{ - *collision_time = 2.0; - - Vector3 vp, va, vb, vc; - vp = p1 - p0; - va = a1 - a0; - vb = b1 - b0; - vc = c1 - c0; - - S a, b, c, d; - computeCubicCoeff_VF(a0, b0, c0, p0, va, vb, vc, vp, &a, &b, &c, &d); - - if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) - { - return false; - } - - - /// if(isZero(a)) - /// { - /// return solveSquare(b, c, d, a0, b0, c0, p0, va, vb, vc, vp, true, collision_time); - /// } - - S coeffs[4]; - coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; - - if(useNewton) - { - S l = 0; - S r = 1; - - if(solveCubicWithIntervalNewton(a0, b0, c0, p0, va, vb, vc, vp, l, r, true, coeffs)) - { - *collision_time = 0.5 * (l + r); - } - } - else - { - S roots[3]; - int num = PolySolver::solveCubic(coeffs, roots); - for(int i = 0; i < num; ++i) - { - S r = roots[i]; - if(r < 0 || r > 1) continue; - if(checkRootValidity_VF(a0, b0, c0, p0, va, vb, vc, vp, r)) - { - *collision_time = r; - break; - } - } - } - - if(*collision_time > 1) - { - return false; - } - - *p_i = vp * (*collision_time) + p0; - return true; -} - -//============================================================================== -template -bool Intersect::intersect_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, - S* collision_time, Vector3* p_i, bool useNewton) -{ - *collision_time = 2.0; - - Vector3 va, vb, vc, vd; - va = a1 - a0; - vb = b1 - b0; - vc = c1 - c0; - vd = d1 - d0; - - S a, b, c, d; - computeCubicCoeff_EE(a0, b0, c0, d0, va, vb, vc, vd, &a, &b, &c, &d); - - if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) - { - return false; - } - - /// if(isZero(a)) - /// { - /// return solveSquare(b, c, d, a0, b0, c0, d0, va, vb, vc, vd, collision_time, false); - /// } - - - S coeffs[4]; - coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; - - if(useNewton) - { - S l = 0; - S r = 1; - - if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, false, coeffs, p_i)) - { - *collision_time = (l + r) * 0.5; - } - } - else - { - S roots[3]; - int num = PolySolver::solveCubic(coeffs, roots); - for(int i = 0; i < num; ++i) - { - S r = roots[i]; - if(r < 0 || r > 1) continue; - - if(checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, p_i)) - { - *collision_time = r; - break; - } - } - } - - if(*collision_time > 1) - { - return false; - } - - return true; -} - -//============================================================================== -template -bool Intersect::intersect_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& p1, - const Vector3& L) -{ - Vector3 va, vb, vp; - va = a1 - a0; - vb = b1 - b0; - vp = p1 - p0; - - S a, b, c; - computeCubicCoeff_VE(a0, b0, p0, va, vb, vp, L, &a, &b, &c); - - if(isZero(a) && isZero(b) && isZero(c)) - return true; - - return solveSquare(a, b, c, a0, b0, p0, va, vb, vp); - -} - -//============================================================================== -/// @brief Prefilter for intersection, works for both VF and EE -template -bool Intersect::intersectPreFiltering(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1) -{ - Vector3 n0 = (b0 - a0).cross(c0 - a0); - Vector3 n1 = (b1 - a1).cross(c1 - a1); - Vector3 a0a1 = a1 - a0; - Vector3 b0b1 = b1 - b0; - Vector3 c0c1 = c1 - c0; - Vector3 delta = (b0b1 - a0a1).cross(c0c1 - a0a1); - Vector3 nx = (n0 + n1 - delta) * 0.5; - - Vector3 a0d0 = d0 - a0; - Vector3 a1d1 = d1 - a1; - - S A = n0.dot(a0d0); - S B = n1.dot(a1d1); - S C = nx.dot(a0d0); - S D = nx.dot(a1d1); - S E = n1.dot(a0d0); - S F = n0.dot(a1d1); - - if(A > 0 && B > 0 && (2*C +F) > 0 && (2*D+E) > 0) - return false; - if(A < 0 && B < 0 && (2*C +F) < 0 && (2*D+E) < 0) - return false; - - return true; -} - -//============================================================================== -template -bool Intersect::intersect_VF_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, - S* collision_time, Vector3* p_i, bool useNewton) -{ - if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1)) - { - return intersect_VF(a0, b0, c0, p0, a1, b1, c1, p1, collision_time, p_i, useNewton); - } - else - return false; -} - -//============================================================================== -template -bool Intersect::intersect_EE_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, - S* collision_time, Vector3* p_i, bool useNewton) -{ - if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1)) - { - return intersect_EE(a0, b0, c0, d0, a1, b1, c1, d1, collision_time, p_i, useNewton); - } - else - return false; -} - -//============================================================================== -template -bool Intersect::intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - const Matrix3& R, - const Vector3& T, - Vector3* contact_points, - unsigned int* num_contact_points, - S* penetration_depth, - Vector3* normal) -{ - Vector3 Q1_ = R * Q1 + T; - Vector3 Q2_ = R * Q2 + T; - Vector3 Q3_ = R * Q3 + T; - - return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); -} - -//============================================================================== -template -bool Intersect::intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - const Transform3& tf, - Vector3* contact_points, - unsigned int* num_contact_points, - S* penetration_depth, - Vector3* normal) -{ - Vector3 Q1_ = tf * Q1; - Vector3 Q2_ = tf * Q2; - Vector3 Q3_ = tf * Q3; - - return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); -} - -//============================================================================== -template -bool Intersect::intersect_Triangle_ODE_style( - const Vector3& P1, const Vector3& P2, const Vector3& P3, - const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, - Vector3* contact_points, - unsigned int* num_contact_points, - S* penetration_depth, - Vector3* normal) -{ - Vector3 n1; - S t1; - bool b1 = buildTrianglePlane(P1, P2, P3, &n1, &t1); - if(!b1) return false; - - Vector3 n2; - S t2; - bool b2 = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); - if(!b2) return false; - - if(sameSideOfPlane(P1, P2, P3, n2, t2)) - return false; - - if(sameSideOfPlane(Q1, Q2, Q3, n1, t1)) - return false; - - Vector3 clipped_points1[getMaxTriangleClips()]; - unsigned int num_clipped_points1 = 0; - Vector3 clipped_points2[getMaxTriangleClips()]; - unsigned int num_clipped_points2 = 0; - - Vector3 deepest_points1[getMaxTriangleClips()]; - unsigned int num_deepest_points1 = 0; - Vector3 deepest_points2[getMaxTriangleClips()]; - unsigned int num_deepest_points2 = 0; - S penetration_depth1 = -1, penetration_depth2 = -1; - - clipTriangleByTriangleAndEdgePlanes(Q1, Q2, Q3, P1, P2, P3, n1, t1, clipped_points2, &num_clipped_points2); - - if(num_clipped_points2 == 0) - return false; - - computeDeepestPoints(clipped_points2, num_clipped_points2, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); - if(num_deepest_points2 == 0) - return false; - - clipTriangleByTriangleAndEdgePlanes(P1, P2, P3, Q1, Q2, Q3, n2, t2, clipped_points1, &num_clipped_points1); - if(num_clipped_points1 == 0) - return false; - - computeDeepestPoints(clipped_points1, num_clipped_points1, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); - if(num_deepest_points1 == 0) - return false; - - - /// Return contact information - if(contact_points && num_contact_points && penetration_depth && normal) - { - if(penetration_depth1 > penetration_depth2) - { - *num_contact_points = num_deepest_points2; - for(unsigned int i = 0; i < num_deepest_points2; ++i) - { - contact_points[i] = deepest_points2[i]; - } - - *normal = n1; - *penetration_depth = penetration_depth2; - } - else - { - *num_contact_points = num_deepest_points1; - for(unsigned int i = 0; i < num_deepest_points1; ++i) - { - contact_points[i] = deepest_points1[i]; - } - - *normal = -n2; - *penetration_depth = penetration_depth1; - } - } - - return true; -} - -//============================================================================== -template -bool Intersect::intersect_Triangle( - const Vector3& P1, const Vector3& P2, const Vector3& P3, - const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, - Vector3* contact_points, - unsigned int* num_contact_points, - S* penetration_depth, - Vector3* normal) -{ - Vector3 p1 = P1 - P1; - Vector3 p2 = P2 - P1; - Vector3 p3 = P3 - P1; - Vector3 q1 = Q1 - P1; - Vector3 q2 = Q2 - P1; - Vector3 q3 = Q3 - P1; - - Vector3 e1 = p2 - p1; - Vector3 e2 = p3 - p2; - Vector3 n1 = e1.cross(e2); - if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 f1 = q2 - q1; - Vector3 f2 = q3 - q2; - Vector3 m1 = f1.cross(f2); - if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef11 = e1.cross(f1); - if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef12 = e1.cross(f2); - if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 f3 = q1 - q3; - Vector3 ef13 = e1.cross(f3); - if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef21 = e2.cross(f1); - if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef22 = e2.cross(f2); - if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef23 = e2.cross(f3); - if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 e3 = p1 - p3; - Vector3 ef31 = e3.cross(f1); - if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef32 = e3.cross(f2); - if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 ef33 = e3.cross(f3); - if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 g1 = e1.cross(n1); - if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 g2 = e2.cross(n1); - if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 g3 = e3.cross(n1); - if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 h1 = f1.cross(m1); - if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 h2 = f2.cross(m1); - if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false; - - Vector3 h3 = f3.cross(m1); - if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false; - - if(contact_points && num_contact_points && penetration_depth && normal) - { - Vector3 n1, n2; - S t1, t2; - buildTrianglePlane(P1, P2, P3, &n1, &t1); - buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); - - Vector3 deepest_points1[3]; - unsigned int num_deepest_points1 = 0; - Vector3 deepest_points2[3]; - unsigned int num_deepest_points2 = 0; - S penetration_depth1, penetration_depth2; - - Vector3 P[3] = {P1, P2, P3}; - Vector3 Q[3] = {Q1, Q2, Q3}; - - computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); - computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); - - - if(penetration_depth1 > penetration_depth2) - { - *num_contact_points = std::min(num_deepest_points2, (unsigned int)2); - for(unsigned int i = 0; i < *num_contact_points; ++i) - { - contact_points[i] = deepest_points2[i]; - } - - *normal = n1; - *penetration_depth = penetration_depth2; - } - else - { - *num_contact_points = std::min(num_deepest_points1, (unsigned int)2); - for(unsigned int i = 0; i < *num_contact_points; ++i) - { - contact_points[i] = deepest_points1[i]; - } - - *normal = -n2; - *penetration_depth = penetration_depth1; - } - } - - return true; -} - -//============================================================================== -template -void Intersect::computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, S t, S* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points) -{ - *num_deepest_points = 0; - S max_depth = -std::numeric_limits::max(); - unsigned int num_deepest_points_ = 0; - unsigned int num_neg = 0; - unsigned int num_pos = 0; - unsigned int num_zero = 0; - - for(unsigned int i = 0; i < num_clipped_points; ++i) - { - S dist = -distanceToPlane(n, t, clipped_points[i]); - if(dist > getEpsilon()) num_pos++; - else if(dist < -getEpsilon()) num_neg++; - else num_zero++; - if(dist > max_depth) - { - max_depth = dist; - num_deepest_points_ = 1; - deepest_points[num_deepest_points_ - 1] = clipped_points[i]; - } - else if(dist + 1e-6 >= max_depth) - { - num_deepest_points_++; - deepest_points[num_deepest_points_ - 1] = clipped_points[i]; - } - } - - if(max_depth < -getEpsilon()) - num_deepest_points_ = 0; - - if(num_zero == 0 && ((num_neg == 0) || (num_pos == 0))) - num_deepest_points_ = 0; - - *penetration_depth = max_depth; - *num_deepest_points = num_deepest_points_; -} - -//============================================================================== -template -void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vector3& v1, const Vector3& v2, const Vector3& v3, - const Vector3& t1, const Vector3& t2, const Vector3& t3, - const Vector3& tn, S to, - Vector3 clipped_points[], unsigned int* num_clipped_points, - bool clip_triangle) -{ - *num_clipped_points = 0; - Vector3 temp_clip[getMaxTriangleClips()]; - Vector3 temp_clip2[getMaxTriangleClips()]; - unsigned int num_temp_clip = 0; - unsigned int num_temp_clip2 = 0; - Vector3 v[3] = {v1, v2, v3}; - - Vector3 plane_n; - S plane_dist; - - if(buildEdgePlane(t1, t2, tn, &plane_n, &plane_dist)) - { - clipPolygonByPlane(v, 3, plane_n, plane_dist, temp_clip, &num_temp_clip); - if(num_temp_clip > 0) - { - if(buildEdgePlane(t2, t3, tn, &plane_n, &plane_dist)) - { - clipPolygonByPlane(temp_clip, num_temp_clip, plane_n, plane_dist, temp_clip2, &num_temp_clip2); - if(num_temp_clip2 > 0) - { - if(buildEdgePlane(t3, t1, tn, &plane_n, &plane_dist)) - { - if(clip_triangle) - { - num_temp_clip = 0; - clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, temp_clip, &num_temp_clip); - if(num_temp_clip > 0) - { - clipPolygonByPlane(temp_clip, num_temp_clip, tn, to, clipped_points, num_clipped_points); - } - } - else - { - clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, clipped_points, num_clipped_points); - } - } - } - } - } - } -} - -//============================================================================== -template -void Intersect::clipPolygonByPlane(Vector3* polygon_points, unsigned int num_polygon_points, const Vector3& n, S t, Vector3 clipped_points[], unsigned int* num_clipped_points) -{ - *num_clipped_points = 0; - - unsigned int num_clipped_points_ = 0; - unsigned int vi; - unsigned int prev_classify = 2; - unsigned int classify; - for(unsigned int i = 0; i <= num_polygon_points; ++i) - { - vi = (i % num_polygon_points); - S d = distanceToPlane(n, t, polygon_points[i]); - classify = ((d > getEpsilon()) ? 1 : 0); - if(classify == 0) - { - if(prev_classify == 1) - { - if(num_clipped_points_ < getMaxTriangleClips()) - { - Vector3 tmp; - clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); - if(num_clipped_points_ > 0) - { - if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon()) - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - else - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - } - - if(num_clipped_points_ < getMaxTriangleClips() && i < num_polygon_points) - { - clipped_points[num_clipped_points_] = polygon_points[vi]; - num_clipped_points_++; - } - } - else - { - if(prev_classify == 0) - { - if(num_clipped_points_ < getMaxTriangleClips()) - { - Vector3 tmp; - clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); - if(num_clipped_points_ > 0) - { - if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon()) - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - else - { - clipped_points[num_clipped_points_] = tmp; - num_clipped_points_++; - } - } - } - } - - prev_classify = classify; - } - - if(num_clipped_points_ > 2) - { - if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).squaredNorm() < getEpsilon()) - { - num_clipped_points_--; - } - } - - *num_clipped_points = num_clipped_points_; -} - -//============================================================================== -template -void Intersect::clipSegmentByPlane(const Vector3& v1, const Vector3& v2, const Vector3& n, S t, Vector3* clipped_point) -{ - S dist1 = distanceToPlane(n, t, v1); - Vector3 tmp = v2 - v1; - S dist2 = tmp.dot(n); - *clipped_point = tmp * (-dist1 / dist2) + v1; -} - -//============================================================================== -template -S Intersect::distanceToPlane(const Vector3& n, S t, const Vector3& v) -{ - return n.dot(v) - t; -} - -//============================================================================== -template -bool Intersect::buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, S* t) -{ - Vector3 n_ = (v2 - v1).cross(v3 - v1); - bool can_normalize = false; - normalize(n_, &can_normalize); - if(can_normalize) - { - *n = n_; - *t = n_.dot(v1); - return true; - } - - return false; -} - -//============================================================================== -template -bool Intersect::buildEdgePlane(const Vector3& v1, const Vector3& v2, const Vector3& tn, Vector3* n, S* t) -{ - Vector3 n_ = (v2 - v1).cross(tn); - bool can_normalize = false; - normalize(n_, &can_normalize); - if(can_normalize) - { - *n = n_; - *t = n_.dot(v1); - return true; - } - - return false; -} - -//============================================================================== -template -bool Intersect::sameSideOfPlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, const Vector3& n, S t) -{ - S dist1 = distanceToPlane(n, t, v1); - S dist2 = dist1 * distanceToPlane(n, t, v2); - S dist3 = dist1 * distanceToPlane(n, t, v3); - if((dist2 > 0) && (dist3 > 0)) - return true; - return false; -} - -//============================================================================== -template -int Intersect::project6(const Vector3& ax, - const Vector3& p1, const Vector3& p2, const Vector3& p3, - const Vector3& q1, const Vector3& q2, const Vector3& q3) -{ - S P1 = ax.dot(p1); - S P2 = ax.dot(p2); - S P3 = ax.dot(p3); - S Q1 = ax.dot(q1); - S Q2 = ax.dot(q2); - S Q3 = ax.dot(q3); - - S mn1 = std::min(P1, std::min(P2, P3)); - S mx2 = std::max(Q1, std::max(Q2, Q3)); - if(mn1 > mx2) return 0; - - S mx1 = std::max(P1, std::max(P2, P3)); - S mn2 = std::min(Q1, std::min(Q2, Q3)); - - if(mn2 > mx1) return 0; - return 1; -} - -//============================================================================== -template -S Intersect::gaussianCDF(S x) -{ - return 0.5 * std::erfc(-x / sqrt(2.0)); -} - -//============================================================================== -template -constexpr S Intersect::getEpsilon() -{ - return 1e-5; -} - -//============================================================================== -template -constexpr S Intersect::getNearZeroThreshold() -{ - return 1e-7; -} - -//============================================================================== -template -constexpr S Intersect::getCcdResolution() -{ - return 1e-7; -} - -//============================================================================== -template -constexpr unsigned int Intersect::getMaxTriangleClips() -{ - return 8; -} - -} // namespace detail -} // namespace fcl - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_INTERSECT_INL_H +#define FCL_NARROWPHASE_DETAIL_INTERSECT_INL_H + +#include "fcl/narrowphase/detail/traversal/collision/intersect.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +extern template +class FCL_EXTERN_TEMPLATE_API Intersect; + +//============================================================================== +template +bool Intersect::isZero(S v) +{ + return (v < getNearZeroThreshold()) && (v > -getNearZeroThreshold()); +} + +//============================================================================== +/// @brief data: only used for EE, return the intersect point +template +bool Intersect::solveCubicWithIntervalNewton(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S& l, S& r, bool bVF, S coeffs[], Vector3* data) +{ + S v2[2]= {l*l,r*r}; + S v[2]= {l,r}; + S r_backup; + + unsigned char min3, min2, min1, max3, max2, max1; + + min3= *((unsigned char*)&coeffs[3]+7)>>7; max3=min3^1; + min2= *((unsigned char*)&coeffs[2]+7)>>7; max2=min2^1; + min1= *((unsigned char*)&coeffs[1]+7)>>7; max1=min1^1; + + // bound the cubic + + S minor = coeffs[3]*v2[min3]*v[min3]+coeffs[2]*v2[min2]+coeffs[1]*v[min1]+coeffs[0]; + S major = coeffs[3]*v2[max3]*v[max3]+coeffs[2]*v2[max2]+coeffs[1]*v[max1]+coeffs[0]; + + if(major<0) return false; + if(minor>0) return false; + + // starting here, the bounds have opposite values + S m = 0.5 * (r + l); + + // bound the derivative + S dminor = 3.0*coeffs[3]*v2[min3]+2.0*coeffs[2]*v[min2]+coeffs[1]; + S dmajor = 3.0*coeffs[3]*v2[max3]+2.0*coeffs[2]*v[max2]+coeffs[1]; + + if((dminor > 0)||(dmajor < 0)) // we can use Newton + { + S m2 = m*m; + S fm = coeffs[3]*m2*m+coeffs[2]*m2+coeffs[1]*m+coeffs[0]; + S nl = m; + S nu = m; + if(fm>0) + { + nl-=(fm/dminor); + nu-=(fm/dmajor); + } + else + { + nu-=(fm/dminor); + nl-=(fm/dmajor); + } + + //intersect with [l,r] + + if(nl>r) return false; + if(nul) + { + if(nu +bool Intersect::insideTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3&p) +{ + Vector3 ab = b - a; + Vector3 ac = c - a; + Vector3 n = ab.cross(ac); + + Vector3 pa = a - p; + Vector3 pb = b - p; + Vector3 pc = c - p; + + if((pb.cross(pc)).dot(n) < -getEpsilon()) return false; + if((pc.cross(pa)).dot(n) < -getEpsilon()) return false; + if((pa.cross(pb)).dot(n) < -getEpsilon()) return false; + + return true; +} + +//============================================================================== +template +bool Intersect::insideLineSegment(const Vector3& a, const Vector3& b, const Vector3& p) +{ + return (p - a).dot(p - b) <= 0; +} + +//============================================================================== +/// @brief Calculate the line segment papb that is the shortest route between +/// two lines p1p2 and p3p4. Calculate also the values of mua and mub where +/// pa = p1 + mua (p2 - p1) +/// pb = p3 + mub (p4 - p3) +/// Return FALSE if no solution exists. +template +bool Intersect::linelineIntersect(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, + Vector3* pa, Vector3* pb, S* mua, S* mub) +{ + Vector3 p31 = p1 - p3; + Vector3 p34 = p4 - p3; + if(fabs(p34[0]) < getEpsilon() && fabs(p34[1]) < getEpsilon() && fabs(p34[2]) < getEpsilon()) + return false; + + Vector3 p12 = p2 - p1; + if(fabs(p12[0]) < getEpsilon() && fabs(p12[1]) < getEpsilon() && fabs(p12[2]) < getEpsilon()) + return false; + + S d3134 = p31.dot(p34); + S d3412 = p34.dot(p12); + S d3112 = p31.dot(p12); + S d3434 = p34.dot(p34); + S d1212 = p12.dot(p12); + + S denom = d1212 * d3434 - d3412 * d3412; + if(fabs(denom) < getEpsilon()) + return false; + S numer = d3134 * d3412 - d3112 * d3434; + + *mua = numer / denom; + if(*mua < 0 || *mua > 1) + return false; + + *mub = (d3134 + d3412 * (*mua)) / d3434; + if(*mub < 0 || *mub > 1) + return false; + + *pa = p1 + p12 * (*mua); + *pb = p3 + p34 * (*mub); + return true; +} + +//============================================================================== +template +bool Intersect::checkRootValidity_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S t) +{ + return insideTriangle(a0 + va * t, b0 + vb * t, c0 + vc * t, p0 + vp * t); +} + +//============================================================================== +template +bool Intersect::checkRootValidity_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S t, Vector3* q_i) +{ + Vector3 a = a0 + va * t; + Vector3 b = b0 + vb * t; + Vector3 c = c0 + vc * t; + Vector3 d = d0 + vd * t; + Vector3 p1, p2; + S t_ab, t_cd; + if(linelineIntersect(a, b, c, d, &p1, &p2, &t_ab, &t_cd)) + { + if(q_i) *q_i = p1; + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::checkRootValidity_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + S t) +{ + return insideLineSegment(a0 + va * t, b0 + vb * t, p0 + vp * t); +} + +//============================================================================== +template +bool Intersect::solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + bool bVF, + S* ret) +{ + S discriminant = b * b - 4 * a * c; + if(discriminant < 0) + return false; + + S sqrt_dis = sqrt(discriminant); + S r1 = (-b + sqrt_dis) / (2 * a); + bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r1) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r1)) : false; + + S r2 = (-b - sqrt_dis) / (2 * a); + bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? ((bVF) ? checkRootValidity_VF(a0, b0, c0, d0, va, vb, vc, vd, r2) : checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r2)) : false; + + if(v1 && v2) + { + *ret = (r1 > r2) ? r2 : r1; + return true; + } + if(v1) + { + *ret = r1; + return true; + } + if(v2) + { + *ret = r2; + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp) +{ + if(isZero(a)) + { + S t = -c/b; + return (t >= 0 && t <= 1) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, t) : false; + } + + S discriminant = b*b-4*a*c; + if(discriminant < 0) + return false; + + S sqrt_dis = sqrt(discriminant); + + S r1 = (-b+sqrt_dis) / (2 * a); + bool v1 = (r1 >= 0.0 && r1 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r1) : false; + if(v1) return true; + + S r2 = (-b-sqrt_dis) / (2 * a); + bool v2 = (r2 >= 0.0 && r2 <= 1.0) ? checkRootValidity_VE(a0, b0, p0, va, vb, vp, r2) : false; + return v2; +} + +//============================================================================== +/// @brief Compute the cubic coefficients for VF case +/// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. +template +void Intersect::computeCubicCoeff_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S* a, S* b, S* c, S* d) +{ + Vector3 vavb = vb - va; + Vector3 vavc = vc - va; + Vector3 vavp = vp - va; + Vector3 a0b0 = b0 - a0; + Vector3 a0c0 = c0 - a0; + Vector3 a0p0 = p0 - a0; + + Vector3 vavb_cross_vavc = vavb.cross(vavc); + Vector3 vavb_cross_a0c0 = vavb.cross(a0c0); + Vector3 a0b0_cross_vavc = a0b0.cross(vavc); + Vector3 a0b0_cross_a0c0 = a0b0.cross(a0c0); + + *a = vavp.dot(vavb_cross_vavc); + *b = a0p0.dot(vavb_cross_vavc) + vavp.dot(vavb_cross_a0c0 + a0b0_cross_vavc); + *c = vavp.dot(a0b0_cross_a0c0) + a0p0.dot(vavb_cross_a0c0 + a0b0_cross_vavc); + *d = a0p0.dot(a0b0_cross_a0c0); +} + +//============================================================================== +template +void Intersect::computeCubicCoeff_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S* a, S* b, S* c, S* d) +{ + Vector3 vavb = vb - va; + Vector3 vcvd = vd - vc; + Vector3 vavc = vc - va; + Vector3 c0d0 = d0 - c0; + Vector3 a0b0 = b0 - a0; + Vector3 a0c0 = c0 - a0; + Vector3 vavb_cross_vcvd = vavb.cross(vcvd); + Vector3 vavb_cross_c0d0 = vavb.cross(c0d0); + Vector3 a0b0_cross_vcvd = a0b0.cross(vcvd); + Vector3 a0b0_cross_c0d0 = a0b0.cross(c0d0); + + *a = vavc.dot(vavb_cross_vcvd); + *b = a0c0.dot(vavb_cross_vcvd) + vavc.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); + *c = vavc.dot(a0b0_cross_c0d0) + a0c0.dot(vavb_cross_c0d0 + a0b0_cross_vcvd); + *d = a0c0.dot(a0b0_cross_c0d0); +} + +//============================================================================== +template +void Intersect::computeCubicCoeff_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + const Vector3& L, + S* a, S* b, S* c) +{ + Vector3 vbva = va - vb; + Vector3 vbvp = vp - vb; + Vector3 b0a0 = a0 - b0; + Vector3 b0p0 = p0 - b0; + + Vector3 L_cross_vbvp = L.cross(vbvp); + Vector3 L_cross_b0p0 = L.cross(b0p0); + + *a = L_cross_vbvp.dot(vbva); + *b = L_cross_vbvp.dot(b0a0) + L_cross_b0p0.dot(vbva); + *c = L_cross_b0p0.dot(b0a0); +} + +//============================================================================== +template +bool Intersect::intersect_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + *collision_time = 2.0; + + Vector3 vp, va, vb, vc; + vp = p1 - p0; + va = a1 - a0; + vb = b1 - b0; + vc = c1 - c0; + + S a, b, c, d; + computeCubicCoeff_VF(a0, b0, c0, p0, va, vb, vc, vp, &a, &b, &c, &d); + + if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) + { + return false; + } + + + /// if(isZero(a)) + /// { + /// return solveSquare(b, c, d, a0, b0, c0, p0, va, vb, vc, vp, true, collision_time); + /// } + + S coeffs[4]; + coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; + + if(useNewton) + { + S l = 0; + S r = 1; + + if(solveCubicWithIntervalNewton(a0, b0, c0, p0, va, vb, vc, vp, l, r, true, coeffs)) + { + *collision_time = 0.5 * (l + r); + } + } + else + { + S roots[3]; + int num = PolySolver::solveCubic(coeffs, roots); + for(int i = 0; i < num; ++i) + { + S r = roots[i]; + if(r < 0 || r > 1) continue; + if(checkRootValidity_VF(a0, b0, c0, p0, va, vb, vc, vp, r)) + { + *collision_time = r; + break; + } + } + } + + if(*collision_time > 1) + { + return false; + } + + *p_i = vp * (*collision_time) + p0; + return true; +} + +//============================================================================== +template +bool Intersect::intersect_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + *collision_time = 2.0; + + Vector3 va, vb, vc, vd; + va = a1 - a0; + vb = b1 - b0; + vc = c1 - c0; + vd = d1 - d0; + + S a, b, c, d; + computeCubicCoeff_EE(a0, b0, c0, d0, va, vb, vc, vd, &a, &b, &c, &d); + + if(isZero(a) && isZero(b) && isZero(c) && isZero(d)) + { + return false; + } + + /// if(isZero(a)) + /// { + /// return solveSquare(b, c, d, a0, b0, c0, d0, va, vb, vc, vd, collision_time, false); + /// } + + + S coeffs[4]; + coeffs[3] = a, coeffs[2] = b, coeffs[1] = c, coeffs[0] = d; + + if(useNewton) + { + S l = 0; + S r = 1; + + if(solveCubicWithIntervalNewton(a0, b0, c0, d0, va, vb, vc, vd, l, r, false, coeffs, p_i)) + { + *collision_time = (l + r) * 0.5; + } + } + else + { + S roots[3]; + int num = PolySolver::solveCubic(coeffs, roots); + for(int i = 0; i < num; ++i) + { + S r = roots[i]; + if(r < 0 || r > 1) continue; + + if(checkRootValidity_EE(a0, b0, c0, d0, va, vb, vc, vd, r, p_i)) + { + *collision_time = r; + break; + } + } + } + + if(*collision_time > 1) + { + return false; + } + + return true; +} + +//============================================================================== +template +bool Intersect::intersect_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& p1, + const Vector3& L) +{ + Vector3 va, vb, vp; + va = a1 - a0; + vb = b1 - b0; + vp = p1 - p0; + + S a, b, c; + computeCubicCoeff_VE(a0, b0, p0, va, vb, vp, L, &a, &b, &c); + + if(isZero(a) && isZero(b) && isZero(c)) + return true; + + return solveSquare(a, b, c, a0, b0, p0, va, vb, vp); + +} + +//============================================================================== +/// @brief Prefilter for intersection, works for both VF and EE +template +bool Intersect::intersectPreFiltering(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1) +{ + Vector3 n0 = (b0 - a0).cross(c0 - a0); + Vector3 n1 = (b1 - a1).cross(c1 - a1); + Vector3 a0a1 = a1 - a0; + Vector3 b0b1 = b1 - b0; + Vector3 c0c1 = c1 - c0; + Vector3 delta = (b0b1 - a0a1).cross(c0c1 - a0a1); + Vector3 nx = (n0 + n1 - delta) * 0.5; + + Vector3 a0d0 = d0 - a0; + Vector3 a1d1 = d1 - a1; + + S A = n0.dot(a0d0); + S B = n1.dot(a1d1); + S C = nx.dot(a0d0); + S D = nx.dot(a1d1); + S E = n1.dot(a0d0); + S F = n0.dot(a1d1); + + if(A > 0 && B > 0 && (2*C +F) > 0 && (2*D+E) > 0) + return false; + if(A < 0 && B < 0 && (2*C +F) < 0 && (2*D+E) < 0) + return false; + + return true; +} + +//============================================================================== +template +bool Intersect::intersect_VF_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + if(intersectPreFiltering(a0, b0, c0, p0, a1, b1, c1, p1)) + { + return intersect_VF(a0, b0, c0, p0, a1, b1, c1, p1, collision_time, p_i, useNewton); + } + else + return false; +} + +//============================================================================== +template +bool Intersect::intersect_EE_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton) +{ + if(intersectPreFiltering(a0, b0, c0, d0, a1, b1, c1, d1)) + { + return intersect_EE(a0, b0, c0, d0, a1, b1, c1, d1, collision_time, p_i, useNewton); + } + else + return false; +} + +//============================================================================== +template +bool Intersect::intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Matrix3& R, + const Vector3& T, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 Q1_ = R * Q1 + T; + Vector3 Q2_ = R * Q2 + T; + Vector3 Q3_ = R * Q3 + T; + + return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); +} + +//============================================================================== +template +bool Intersect::intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Transform3& tf, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 Q1_ = tf * Q1; + Vector3 Q2_ = tf * Q2; + Vector3 Q3_ = tf * Q3; + + return intersect_Triangle(P1, P2, P3, Q1_, Q2_, Q3_, contact_points, num_contact_points, penetration_depth, normal); +} + +//============================================================================== +template +bool Intersect::intersect_Triangle_ODE_style( + const Vector3& P1, const Vector3& P2, const Vector3& P3, + const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 n1; + S t1; + bool b1 = buildTrianglePlane(P1, P2, P3, &n1, &t1); + if(!b1) return false; + + Vector3 n2; + S t2; + bool b2 = buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); + if(!b2) return false; + + if(sameSideOfPlane(P1, P2, P3, n2, t2)) + return false; + + if(sameSideOfPlane(Q1, Q2, Q3, n1, t1)) + return false; + + Vector3 clipped_points1[getMaxTriangleClips()]; + unsigned int num_clipped_points1 = 0; + Vector3 clipped_points2[getMaxTriangleClips()]; + unsigned int num_clipped_points2 = 0; + + Vector3 deepest_points1[getMaxTriangleClips()]; + unsigned int num_deepest_points1 = 0; + Vector3 deepest_points2[getMaxTriangleClips()]; + unsigned int num_deepest_points2 = 0; + S penetration_depth1 = -1, penetration_depth2 = -1; + + clipTriangleByTriangleAndEdgePlanes(Q1, Q2, Q3, P1, P2, P3, n1, t1, clipped_points2, &num_clipped_points2); + + if(num_clipped_points2 == 0) + return false; + + computeDeepestPoints(clipped_points2, num_clipped_points2, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); + if(num_deepest_points2 == 0) + return false; + + clipTriangleByTriangleAndEdgePlanes(P1, P2, P3, Q1, Q2, Q3, n2, t2, clipped_points1, &num_clipped_points1); + if(num_clipped_points1 == 0) + return false; + + computeDeepestPoints(clipped_points1, num_clipped_points1, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); + if(num_deepest_points1 == 0) + return false; + + + /// Return contact information + if(contact_points && num_contact_points && penetration_depth && normal) + { + if(penetration_depth1 > penetration_depth2) + { + *num_contact_points = num_deepest_points2; + for(unsigned int i = 0; i < num_deepest_points2; ++i) + { + contact_points[i] = deepest_points2[i]; + } + + *normal = n1; + *penetration_depth = penetration_depth2; + } + else + { + *num_contact_points = num_deepest_points1; + for(unsigned int i = 0; i < num_deepest_points1; ++i) + { + contact_points[i] = deepest_points1[i]; + } + + *normal = -n2; + *penetration_depth = penetration_depth1; + } + } + + return true; +} + +//============================================================================== +template +bool Intersect::intersect_Triangle( + const Vector3& P1, const Vector3& P2, const Vector3& P3, + const Vector3& Q1, const Vector3& Q2, const Vector3& Q3, + Vector3* contact_points, + unsigned int* num_contact_points, + S* penetration_depth, + Vector3* normal) +{ + Vector3 p1 = P1 - P1; + Vector3 p2 = P2 - P1; + Vector3 p3 = P3 - P1; + Vector3 q1 = Q1 - P1; + Vector3 q2 = Q2 - P1; + Vector3 q3 = Q3 - P1; + + Vector3 e1 = p2 - p1; + Vector3 e2 = p3 - p2; + Vector3 n1 = e1.cross(e2); + if (!project6(n1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 f1 = q2 - q1; + Vector3 f2 = q3 - q2; + Vector3 m1 = f1.cross(f2); + if (!project6(m1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef11 = e1.cross(f1); + if (!project6(ef11, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef12 = e1.cross(f2); + if (!project6(ef12, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 f3 = q1 - q3; + Vector3 ef13 = e1.cross(f3); + if (!project6(ef13, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef21 = e2.cross(f1); + if (!project6(ef21, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef22 = e2.cross(f2); + if (!project6(ef22, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef23 = e2.cross(f3); + if (!project6(ef23, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 e3 = p1 - p3; + Vector3 ef31 = e3.cross(f1); + if (!project6(ef31, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef32 = e3.cross(f2); + if (!project6(ef32, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 ef33 = e3.cross(f3); + if (!project6(ef33, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 g1 = e1.cross(n1); + if (!project6(g1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 g2 = e2.cross(n1); + if (!project6(g2, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 g3 = e3.cross(n1); + if (!project6(g3, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 h1 = f1.cross(m1); + if (!project6(h1, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 h2 = f2.cross(m1); + if (!project6(h2, p1, p2, p3, q1, q2, q3)) return false; + + Vector3 h3 = f3.cross(m1); + if (!project6(h3, p1, p2, p3, q1, q2, q3)) return false; + + if(contact_points && num_contact_points && penetration_depth && normal) + { + Vector3 n1, n2; + S t1, t2; + buildTrianglePlane(P1, P2, P3, &n1, &t1); + buildTrianglePlane(Q1, Q2, Q3, &n2, &t2); + + Vector3 deepest_points1[3]; + unsigned int num_deepest_points1 = 0; + Vector3 deepest_points2[3]; + unsigned int num_deepest_points2 = 0; + S penetration_depth1, penetration_depth2; + + Vector3 P[3] = {P1, P2, P3}; + Vector3 Q[3] = {Q1, Q2, Q3}; + + computeDeepestPoints(Q, 3, n1, t1, &penetration_depth2, deepest_points2, &num_deepest_points2); + computeDeepestPoints(P, 3, n2, t2, &penetration_depth1, deepest_points1, &num_deepest_points1); + + + if(penetration_depth1 > penetration_depth2) + { + *num_contact_points = std::min(num_deepest_points2, (unsigned int)2); + for(unsigned int i = 0; i < *num_contact_points; ++i) + { + contact_points[i] = deepest_points2[i]; + } + + *normal = n1; + *penetration_depth = penetration_depth2; + } + else + { + *num_contact_points = std::min(num_deepest_points1, (unsigned int)2); + for(unsigned int i = 0; i < *num_contact_points; ++i) + { + contact_points[i] = deepest_points1[i]; + } + + *normal = -n2; + *penetration_depth = penetration_depth1; + } + } + + return true; +} + +//============================================================================== +template +void Intersect::computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, S t, S* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points) +{ + *num_deepest_points = 0; + S max_depth = -std::numeric_limits::max(); + unsigned int num_deepest_points_ = 0; + unsigned int num_neg = 0; + unsigned int num_pos = 0; + unsigned int num_zero = 0; + + for(unsigned int i = 0; i < num_clipped_points; ++i) + { + S dist = -distanceToPlane(n, t, clipped_points[i]); + if(dist > getEpsilon()) num_pos++; + else if(dist < -getEpsilon()) num_neg++; + else num_zero++; + if(dist > max_depth) + { + max_depth = dist; + num_deepest_points_ = 1; + deepest_points[num_deepest_points_ - 1] = clipped_points[i]; + } + else if(dist + 1e-6 >= max_depth) + { + num_deepest_points_++; + deepest_points[num_deepest_points_ - 1] = clipped_points[i]; + } + } + + if(max_depth < -getEpsilon()) + num_deepest_points_ = 0; + + if(num_zero == 0 && ((num_neg == 0) || (num_pos == 0))) + num_deepest_points_ = 0; + + *penetration_depth = max_depth; + *num_deepest_points = num_deepest_points_; +} + +//============================================================================== +template +void Intersect::clipTriangleByTriangleAndEdgePlanes(const Vector3& v1, const Vector3& v2, const Vector3& v3, + const Vector3& t1, const Vector3& t2, const Vector3& t3, + const Vector3& tn, S to, + Vector3 clipped_points[], unsigned int* num_clipped_points, + bool clip_triangle) +{ + *num_clipped_points = 0; + Vector3 temp_clip[getMaxTriangleClips()]; + Vector3 temp_clip2[getMaxTriangleClips()]; + unsigned int num_temp_clip = 0; + unsigned int num_temp_clip2 = 0; + Vector3 v[3] = {v1, v2, v3}; + + Vector3 plane_n; + S plane_dist; + + if(buildEdgePlane(t1, t2, tn, &plane_n, &plane_dist)) + { + clipPolygonByPlane(v, 3, plane_n, plane_dist, temp_clip, &num_temp_clip); + if(num_temp_clip > 0) + { + if(buildEdgePlane(t2, t3, tn, &plane_n, &plane_dist)) + { + clipPolygonByPlane(temp_clip, num_temp_clip, plane_n, plane_dist, temp_clip2, &num_temp_clip2); + if(num_temp_clip2 > 0) + { + if(buildEdgePlane(t3, t1, tn, &plane_n, &plane_dist)) + { + if(clip_triangle) + { + num_temp_clip = 0; + clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, temp_clip, &num_temp_clip); + if(num_temp_clip > 0) + { + clipPolygonByPlane(temp_clip, num_temp_clip, tn, to, clipped_points, num_clipped_points); + } + } + else + { + clipPolygonByPlane(temp_clip2, num_temp_clip2, plane_n, plane_dist, clipped_points, num_clipped_points); + } + } + } + } + } + } +} + +//============================================================================== +template +void Intersect::clipPolygonByPlane(Vector3* polygon_points, unsigned int num_polygon_points, const Vector3& n, S t, Vector3 clipped_points[], unsigned int* num_clipped_points) +{ + *num_clipped_points = 0; + + unsigned int num_clipped_points_ = 0; + unsigned int vi; + unsigned int prev_classify = 2; + unsigned int classify; + for(unsigned int i = 0; i <= num_polygon_points; ++i) + { + vi = (i % num_polygon_points); + S d = distanceToPlane(n, t, polygon_points[i]); + classify = ((d > getEpsilon()) ? 1 : 0); + if(classify == 0) + { + if(prev_classify == 1) + { + if(num_clipped_points_ < getMaxTriangleClips()) + { + Vector3 tmp; + clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); + if(num_clipped_points_ > 0) + { + if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon()) + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + else + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + } + + if(num_clipped_points_ < getMaxTriangleClips() && i < num_polygon_points) + { + clipped_points[num_clipped_points_] = polygon_points[vi]; + num_clipped_points_++; + } + } + else + { + if(prev_classify == 0) + { + if(num_clipped_points_ < getMaxTriangleClips()) + { + Vector3 tmp; + clipSegmentByPlane(polygon_points[i - 1], polygon_points[vi], n, t, &tmp); + if(num_clipped_points_ > 0) + { + if((tmp - clipped_points[num_clipped_points_ - 1]).squaredNorm() > getEpsilon()) + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + else + { + clipped_points[num_clipped_points_] = tmp; + num_clipped_points_++; + } + } + } + } + + prev_classify = classify; + } + + if(num_clipped_points_ > 2) + { + if((clipped_points[0] - clipped_points[num_clipped_points_ - 1]).squaredNorm() < getEpsilon()) + { + num_clipped_points_--; + } + } + + *num_clipped_points = num_clipped_points_; +} + +//============================================================================== +template +void Intersect::clipSegmentByPlane(const Vector3& v1, const Vector3& v2, const Vector3& n, S t, Vector3* clipped_point) +{ + S dist1 = distanceToPlane(n, t, v1); + Vector3 tmp = v2 - v1; + S dist2 = tmp.dot(n); + *clipped_point = tmp * (-dist1 / dist2) + v1; +} + +//============================================================================== +template +S Intersect::distanceToPlane(const Vector3& n, S t, const Vector3& v) +{ + return n.dot(v) - t; +} + +//============================================================================== +template +bool Intersect::buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, S* t) +{ + Vector3 n_ = (v2 - v1).cross(v3 - v1); + bool can_normalize = false; + normalize(n_, &can_normalize); + if(can_normalize) + { + *n = n_; + *t = n_.dot(v1); + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::buildEdgePlane(const Vector3& v1, const Vector3& v2, const Vector3& tn, Vector3* n, S* t) +{ + Vector3 n_ = (v2 - v1).cross(tn); + bool can_normalize = false; + normalize(n_, &can_normalize); + if(can_normalize) + { + *n = n_; + *t = n_.dot(v1); + return true; + } + + return false; +} + +//============================================================================== +template +bool Intersect::sameSideOfPlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, const Vector3& n, S t) +{ + S dist1 = distanceToPlane(n, t, v1); + S dist2 = dist1 * distanceToPlane(n, t, v2); + S dist3 = dist1 * distanceToPlane(n, t, v3); + if((dist2 > 0) && (dist3 > 0)) + return true; + return false; +} + +//============================================================================== +template +int Intersect::project6(const Vector3& ax, + const Vector3& p1, const Vector3& p2, const Vector3& p3, + const Vector3& q1, const Vector3& q2, const Vector3& q3) +{ + S P1 = ax.dot(p1); + S P2 = ax.dot(p2); + S P3 = ax.dot(p3); + S Q1 = ax.dot(q1); + S Q2 = ax.dot(q2); + S Q3 = ax.dot(q3); + + S mn1 = std::min(P1, std::min(P2, P3)); + S mx2 = std::max(Q1, std::max(Q2, Q3)); + if(mn1 > mx2) return 0; + + S mx1 = std::max(P1, std::max(P2, P3)); + S mn2 = std::min(Q1, std::min(Q2, Q3)); + + if(mn2 > mx1) return 0; + return 1; +} + +//============================================================================== +template +S Intersect::gaussianCDF(S x) +{ + return 0.5 * std::erfc(-x / sqrt(2.0)); +} + +//============================================================================== +template +constexpr S Intersect::getEpsilon() +{ + return 1e-5; +} + +//============================================================================== +template +constexpr S Intersect::getNearZeroThreshold() +{ + return 1e-7; +} + +//============================================================================== +template +constexpr S Intersect::getCcdResolution() +{ + return 1e-7; +} + +//============================================================================== +template +constexpr unsigned int Intersect::getMaxTriangleClips() +{ + return 8; +} + +} // namespace detail +} // namespace fcl + +#endif diff --git a/include/fcl/narrowphase/detail/traversal/collision/intersect.h b/include/fcl/narrowphase/detail/traversal/collision/intersect.h index df33a8cdc..355876eb6 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/intersect.h +++ b/include/fcl/narrowphase/detail/traversal/collision/intersect.h @@ -1,265 +1,265 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_INTERSECT_H -#define FCL_NARROWPHASE_DETAIL_INTERSECT_H - -#include -#include "fcl/common/types.h" -#include "fcl/math/geometry.h" -#include "fcl/math/detail/polysolver.h" - -namespace fcl -{ - -namespace detail -{ - -/// @brief CCD intersect kernel among primitives -template -class Intersect -{ - -public: - - /// @brief CCD intersect between one vertex and one face - /// [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1 - /// p0 and p1 are points for vertex in time t0 and t1 - /// p_i returns the coordinate of the collision point - static bool intersect_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, - S* collision_time, Vector3* p_i, bool useNewton = true); - - /// @brief CCD intersect between two edges - /// [a0, b0] and [a1, b1] are points for one edge in time t0 and t1 - /// [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1 - /// p_i returns the coordinate of the collision point - static bool intersect_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, - S* collision_time, Vector3* p_i, bool useNewton = true); - - /// @brief CCD intersect between one vertex and one face, using additional filter - static bool intersect_VF_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, - S* collision_time, Vector3* p_i, bool useNewton = true); - - /// @brief CCD intersect between two edges, using additional filter - static bool intersect_EE_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, - S* collision_time, Vector3* p_i, bool useNewton = true); - - /// @brief CCD intersect between one vertex and and one edge - static bool intersect_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& a1, const Vector3& b1, const Vector3& p1, - const Vector3& L); - - /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] - static bool intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - Vector3* contact_points = nullptr, - unsigned int* num_contact_points = nullptr, - S* penetration_depth = nullptr, - Vector3* normal = nullptr); - - /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] - static bool intersect_Triangle_ODE_style( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - Vector3* contact_points = nullptr, - unsigned int* num_contact_points = nullptr, - S* penetration_depth = nullptr, - Vector3* normal = nullptr); - - static bool intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - const Matrix3& R, - const Vector3& T, - Vector3* contact_points = nullptr, - unsigned int* num_contact_points = nullptr, - S* penetration_depth = nullptr, - Vector3* normal = nullptr); - - static bool intersect_Triangle( - const Vector3& P1, - const Vector3& P2, - const Vector3& P3, - const Vector3& Q1, - const Vector3& Q2, - const Vector3& Q3, - const Transform3& tf, - Vector3* contact_points = nullptr, - unsigned int* num_contact_points = nullptr, - S* penetration_depth = nullptr, - Vector3* normal = nullptr); - -private: - - /// @brief Project function used in intersect_Triangle() - static int project6(const Vector3& ax, - const Vector3& p1, const Vector3& p2, const Vector3& p3, - const Vector3& q1, const Vector3& q2, const Vector3& q3); - - /// @brief Check whether one value is zero - static bool isZero(S v); - - /// @brief Solve the cubic function using Newton method, also satisfies the interval restriction - static bool solveCubicWithIntervalNewton(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S& l, S& r, bool bVF, S coeffs[], Vector3* data = nullptr); - - /// @brief Check whether one point p is within triangle [a, b, c] - static bool insideTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3&p); - - /// @brief Check whether one point p is within a line segment [a, b] - static bool insideLineSegment(const Vector3& a, const Vector3& b, const Vector3& p); - - /// @brief Calculate the line segment papb that is the shortest route between - /// two lines p1p2 and p3p4. Calculate also the values of mua and mub where - /// pa = p1 + mua (p2 - p1) - /// pb = p3 + mub (p4 - p3) - /// return FALSE if no solution exists. - static bool linelineIntersect(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, - Vector3* pa, Vector3* pb, S* mua, S* mub); - - /// @brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t - static bool checkRootValidity_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, - S t); - - /// @brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time - static bool checkRootValidity_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S t, Vector3* q_i = nullptr); - - /// @brief Check whether a root for VE intersection is valid - static bool checkRootValidity_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp, - S t); - - /// @brief Solve a square function for EE intersection (with interval restriction) - static bool solveSquare(S a, S b, S c, - const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - bool bVF, - S* ret); - - /// @brief Solve a square function for VE intersection (with interval restriction) - static bool solveSquare(S a, S b, S c, - const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp); - - /// @brief Compute the cubic coefficients for VF intersection - /// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. - - static void computeCubicCoeff_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, - S* a, S* b, S* c, S* d); - - /// @brief Compute the cubic coefficients for EE intersection - static void computeCubicCoeff_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, - S* a, S* b, S* c, S* d); - - /// @brief Compute the cubic coefficients for VE intersection - static void computeCubicCoeff_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, - const Vector3& va, const Vector3& vb, const Vector3& vp, - const Vector3& L, - S* a, S* b, S* c); - - /// @brief filter for intersection, works for both VF and EE - static bool intersectPreFiltering(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, - const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1); - - /// @brief distance of point v to a plane n * x - t = 0 - static S distanceToPlane(const Vector3& n, S t, const Vector3& v); - - /// @brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0 - static bool sameSideOfPlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, const Vector3& n, S t); - - /// @brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to - static void clipTriangleByTriangleAndEdgePlanes(const Vector3& v1, const Vector3& v2, const Vector3& v3, - const Vector3& t1, const Vector3& t2, const Vector3& t3, - const Vector3& tn, S to, - Vector3 clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false); - - /// @brief build a plane passed through triangle v1 v2 v3 - static bool buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, S* t); - - /// @brief build a plane pass through edge v1 and v2, normal is tn - static bool buildEdgePlane(const Vector3& v1, const Vector3& v2, const Vector3& tn, Vector3* n, S* t); - - /// @brief compute the points which has deepest penetration depth - static void computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, S t, S* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points); - - /// @brief clip polygon by plane - static void clipPolygonByPlane(Vector3* polygon_points, unsigned int num_polygon_points, const Vector3& n, S t, Vector3 clipped_points[], unsigned int* num_clipped_points); - - /// @brief clip a line segment by plane - static void clipSegmentByPlane(const Vector3& v1, const Vector3& v2, const Vector3& n, S t, Vector3* clipped_point); - - /// @brief compute the cdf(x) - static S gaussianCDF(S x); - - static constexpr S getEpsilon(); - static constexpr S getNearZeroThreshold(); - static constexpr S getCcdResolution(); - static constexpr unsigned int getMaxTriangleClips(); -}; - -using Intersectf = Intersect; -using Intersectd = Intersect; - -} // namespace detail -} // namespace fcl - -#include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h" - -#endif +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, 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_NARROWPHASE_DETAIL_INTERSECT_H +#define FCL_NARROWPHASE_DETAIL_INTERSECT_H + +#include +#include "fcl/common/types.h" +#include "fcl/math/geometry.h" +#include "fcl/math/detail/polysolver.h" + +namespace fcl +{ + +namespace detail +{ + +/// @brief CCD intersect kernel among primitives +template +class Intersect +{ + +public: + + /// @brief CCD intersect between one vertex and one face + /// [a0, b0, c0] and [a1, b1, c1] are points for the triangle face in time t0 and t1 + /// p0 and p1 are points for vertex in time t0 and t1 + /// p_i returns the coordinate of the collision point + static bool intersect_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton = true); + + /// @brief CCD intersect between two edges + /// [a0, b0] and [a1, b1] are points for one edge in time t0 and t1 + /// [c0, d0] and [c1, d1] are points for the other edge in time t0 and t1 + /// p_i returns the coordinate of the collision point + static bool intersect_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton = true); + + /// @brief CCD intersect between one vertex and one face, using additional filter + static bool intersect_VF_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& p1, + S* collision_time, Vector3* p_i, bool useNewton = true); + + /// @brief CCD intersect between two edges, using additional filter + static bool intersect_EE_filtered(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1, + S* collision_time, Vector3* p_i, bool useNewton = true); + + /// @brief CCD intersect between one vertex and and one edge + static bool intersect_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& a1, const Vector3& b1, const Vector3& p1, + const Vector3& L); + + /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] + static bool intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); + + /// @brief CD intersect between two triangles [P1, P2, P3] and [Q1, Q2, Q3] + static bool intersect_Triangle_ODE_style( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); + + static bool intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Matrix3& R, + const Vector3& T, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); + + static bool intersect_Triangle( + const Vector3& P1, + const Vector3& P2, + const Vector3& P3, + const Vector3& Q1, + const Vector3& Q2, + const Vector3& Q3, + const Transform3& tf, + Vector3* contact_points = nullptr, + unsigned int* num_contact_points = nullptr, + S* penetration_depth = nullptr, + Vector3* normal = nullptr); + +private: + + /// @brief Project function used in intersect_Triangle() + static int project6(const Vector3& ax, + const Vector3& p1, const Vector3& p2, const Vector3& p3, + const Vector3& q1, const Vector3& q2, const Vector3& q3); + + /// @brief Check whether one value is zero + static bool isZero(S v); + + /// @brief Solve the cubic function using Newton method, also satisfies the interval restriction + static bool solveCubicWithIntervalNewton(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S& l, S& r, bool bVF, S coeffs[], Vector3* data = nullptr); + + /// @brief Check whether one point p is within triangle [a, b, c] + static bool insideTriangle(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3&p); + + /// @brief Check whether one point p is within a line segment [a, b] + static bool insideLineSegment(const Vector3& a, const Vector3& b, const Vector3& p); + + /// @brief Calculate the line segment papb that is the shortest route between + /// two lines p1p2 and p3p4. Calculate also the values of mua and mub where + /// pa = p1 + mua (p2 - p1) + /// pb = p3 + mub (p4 - p3) + /// return FALSE if no solution exists. + static bool linelineIntersect(const Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4, + Vector3* pa, Vector3* pb, S* mua, S* mub); + + /// @brief Check whether a root for VF intersection is valid (i.e. within the triangle at intersection t + static bool checkRootValidity_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S t); + + /// @brief Check whether a root for EE intersection is valid (i.e. within the two edges intersected at the given time + static bool checkRootValidity_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S t, Vector3* q_i = nullptr); + + /// @brief Check whether a root for VE intersection is valid + static bool checkRootValidity_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + S t); + + /// @brief Solve a square function for EE intersection (with interval restriction) + static bool solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + bool bVF, + S* ret); + + /// @brief Solve a square function for VE intersection (with interval restriction) + static bool solveSquare(S a, S b, S c, + const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp); + + /// @brief Compute the cubic coefficients for VF intersection + /// See Paper "Interactive Continuous Collision Detection between Deformable Models using Connectivity-Based Culling", Equation 1. + + static void computeCubicCoeff_VF(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vp, + S* a, S* b, S* c, S* d); + + /// @brief Compute the cubic coefficients for EE intersection + static void computeCubicCoeff_EE(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& va, const Vector3& vb, const Vector3& vc, const Vector3& vd, + S* a, S* b, S* c, S* d); + + /// @brief Compute the cubic coefficients for VE intersection + static void computeCubicCoeff_VE(const Vector3& a0, const Vector3& b0, const Vector3& p0, + const Vector3& va, const Vector3& vb, const Vector3& vp, + const Vector3& L, + S* a, S* b, S* c); + + /// @brief filter for intersection, works for both VF and EE + static bool intersectPreFiltering(const Vector3& a0, const Vector3& b0, const Vector3& c0, const Vector3& d0, + const Vector3& a1, const Vector3& b1, const Vector3& c1, const Vector3& d1); + + /// @brief distance of point v to a plane n * x - t = 0 + static S distanceToPlane(const Vector3& n, S t, const Vector3& v); + + /// @brief check wether points v1, v2, v2 are on the same side of plane n * x - t = 0 + static bool sameSideOfPlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, const Vector3& n, S t); + + /// @brief clip triangle v1, v2, v3 by the prism made by t1, t2 and t3. The normal of the prism is tn and is cutted up by to + static void clipTriangleByTriangleAndEdgePlanes(const Vector3& v1, const Vector3& v2, const Vector3& v3, + const Vector3& t1, const Vector3& t2, const Vector3& t3, + const Vector3& tn, S to, + Vector3 clipped_points[], unsigned int* num_clipped_points, bool clip_triangle = false); + + /// @brief build a plane passed through triangle v1 v2 v3 + static bool buildTrianglePlane(const Vector3& v1, const Vector3& v2, const Vector3& v3, Vector3* n, S* t); + + /// @brief build a plane pass through edge v1 and v2, normal is tn + static bool buildEdgePlane(const Vector3& v1, const Vector3& v2, const Vector3& tn, Vector3* n, S* t); + + /// @brief compute the points which has deepest penetration depth + static void computeDeepestPoints(Vector3* clipped_points, unsigned int num_clipped_points, const Vector3& n, S t, S* penetration_depth, Vector3* deepest_points, unsigned int* num_deepest_points); + + /// @brief clip polygon by plane + static void clipPolygonByPlane(Vector3* polygon_points, unsigned int num_polygon_points, const Vector3& n, S t, Vector3 clipped_points[], unsigned int* num_clipped_points); + + /// @brief clip a line segment by plane + static void clipSegmentByPlane(const Vector3& v1, const Vector3& v2, const Vector3& n, S t, Vector3* clipped_point); + + /// @brief compute the cdf(x) + static S gaussianCDF(S x); + + static constexpr S getEpsilon(); + static constexpr S getNearZeroThreshold(); + static constexpr S getCcdResolution(); + static constexpr unsigned int getMaxTriangleClips(); +}; + +using Intersectf = Intersect; +using Intersectd = Intersect; + +} // namespace detail +} // namespace fcl + +#include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h" + +#endif diff --git a/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.h b/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.h index 6d0ec97f5..b3f8d7567 100644 --- a/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/collision/mesh_collision_traversal_node.h @@ -179,7 +179,7 @@ class MeshCollisionTraversalNodekIOS : public MeshCollisionTraversalNode { public: MeshCollisionTraversalNodekIOS(); - + bool BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; @@ -210,7 +210,7 @@ class MeshCollisionTraversalNodeOBBRSS : public MeshCollisionTraversalNode* vertices; Triangle* tri_indices; - + S cost_density; const NarrowPhaseSolver* nsolver; diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h index a1cbfcd68..3f8035a6a 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_conservative_advancement_traversal_node.h @@ -69,9 +69,9 @@ class MeshConservativeAdvancementTraversalNode bool canStop(S c) const; mutable S min_distance; - + mutable Vector3 closest_p1, closest_p2; - + mutable int last_tri_id1, last_tri_id2; /// @brief CA controlling variable: early stop for the early iterations of CA diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.h index acd6a2a7e..894d95683 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_distance_traversal_node.h @@ -139,7 +139,7 @@ class MeshDistanceTraversalNodekIOS MeshDistanceTraversalNodekIOS(); void preprocess(); - + void postprocess(); S BVTesting(int b1, int b2) const diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h index 20b7e232b..68b97ec7b 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_conservative_advancement_traversal_node.h @@ -72,7 +72,7 @@ class MeshShapeConservativeAdvancementTraversalNode mutable Vector3 closest_p1, closest_p2; mutable int last_tri_id; - + /// @brief CA controlling variable: early stop for the early iterations of CA S w; diff --git a/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h index 79c5fde91..058a5c8a3 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/mesh_shape_distance_traversal_node.h @@ -51,7 +51,7 @@ namespace detail template class MeshShapeDistanceTraversalNode : public BVHShapeDistanceTraversalNode -{ +{ public: using S = typename BV::S; @@ -69,7 +69,7 @@ class MeshShapeDistanceTraversalNode S rel_err; S abs_err; - + const NarrowPhaseSolver* nsolver; }; @@ -192,7 +192,7 @@ class MeshShapeDistanceTraversalNodeOBBRSS S BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; - + }; /// @brief Initialize traversal node for distance computation between one mesh and one shape, specialized for OBBRSS type diff --git a/include/fcl/narrowphase/detail/traversal/distance/shape_bvh_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/shape_bvh_distance_traversal_node.h index 05ae2c10b..3d176a089 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/shape_bvh_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/shape_bvh_distance_traversal_node.h @@ -74,7 +74,7 @@ class ShapeBVHDistanceTraversalNode const Shape* model1; const BVHModel* model2; BV model1_bv; - + mutable int num_bv_tests; mutable int num_leaf_tests; mutable S query_time_seconds; diff --git a/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h index 01b599527..069491bb7 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_conservative_advancement_traversal_node.h @@ -72,7 +72,7 @@ class ShapeMeshConservativeAdvancementTraversalNode mutable Vector3 closest_p1, closest_p2; mutable int last_tri_id; - + /// @brief CA controlling variable: early stop for the early iterations of CA S w; diff --git a/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node.h b/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node.h index a90c0077f..1777b8a8b 100644 --- a/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node.h +++ b/include/fcl/narrowphase/detail/traversal/distance/shape_mesh_distance_traversal_node.h @@ -51,7 +51,7 @@ namespace detail template class ShapeMeshDistanceTraversalNode : public ShapeBVHDistanceTraversalNode -{ +{ public: using S = typename BV::S; @@ -69,7 +69,7 @@ class ShapeMeshDistanceTraversalNode S rel_err; S abs_err; - + const NarrowPhaseSolver* nsolver; }; @@ -140,7 +140,7 @@ class ShapeMeshDistanceTraversalNodekIOS S BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; - + }; /// @brief Initialize traversal node for distance computation between one shape @@ -174,7 +174,7 @@ class ShapeMeshDistanceTraversalNodeOBBRSS S BVTesting(int b1, int b2) const; void leafTesting(int b1, int b2) const; - + }; /// @brief Initialize traversal node for distance computation between one shape diff --git a/include/fcl/narrowphase/detail/traversal/traversal_node_base.h b/include/fcl/narrowphase/detail/traversal/traversal_node_base.h index 93cd7f096..abcc4ebdf 100644 --- a/include/fcl/narrowphase/detail/traversal/traversal_node_base.h +++ b/include/fcl/narrowphase/detail/traversal/traversal_node_base.h @@ -54,10 +54,10 @@ class TraversalNodeBase virtual ~TraversalNodeBase(); virtual void preprocess(); - + virtual void postprocess(); - /// @brief Whether b is a leaf node in the first BVH tree + /// @brief Whether b is a leaf node in the first BVH tree virtual bool isFirstNodeLeaf(int b) const; /// @brief Whether b is a leaf node in the second BVH tree diff --git a/include/fcl/narrowphase/distance.h b/include/fcl/narrowphase/distance.h index 9d77e52d0..cda024fe5 100644 --- a/include/fcl/narrowphase/distance.h +++ b/include/fcl/narrowphase/distance.h @@ -46,7 +46,7 @@ namespace fcl { -/// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them. +/// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them. /// Return value is the minimum distance generated between the two objects. template S distance( diff --git a/include/fcl/narrowphase/distance_result.h b/include/fcl/narrowphase/distance_result.h index 4430889b2..66ec7c26d 100644 --- a/include/fcl/narrowphase/distance_result.h +++ b/include/fcl/narrowphase/distance_result.h @@ -90,7 +90,7 @@ struct DistanceResult /// @brief invalid contact primitive information static const int NONE = -1; - + DistanceResult(S min_distance_ = std::numeric_limits::max()); /// @brief add distance information into the result diff --git a/src/broadphase/broadphase_SSaP.cpp b/src/broadphase/broadphase_SSaP.cpp index a80060abf..81df6ad42 100644 --- a/src/broadphase/broadphase_SSaP.cpp +++ b/src/broadphase/broadphase_SSaP.cpp @@ -31,7 +31,7 @@ * 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 */ diff --git a/src/broadphase/broadphase_SaP.cpp b/src/broadphase/broadphase_SaP.cpp index 60fe06e06..d1975d16e 100644 --- a/src/broadphase/broadphase_SaP.cpp +++ b/src/broadphase/broadphase_SaP.cpp @@ -31,7 +31,7 @@ * 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 */ diff --git a/src/broadphase/broadphase_bruteforce.cpp b/src/broadphase/broadphase_bruteforce.cpp index 3ff81662d..c86bed19b 100644 --- a/src/broadphase/broadphase_bruteforce.cpp +++ b/src/broadphase/broadphase_bruteforce.cpp @@ -31,7 +31,7 @@ * 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 */ diff --git a/src/broadphase/broadphase_collision_manager.cpp b/src/broadphase/broadphase_collision_manager.cpp index 8f3b24335..1afb67e69 100644 --- a/src/broadphase/broadphase_collision_manager.cpp +++ b/src/broadphase/broadphase_collision_manager.cpp @@ -31,7 +31,7 @@ * 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 */ diff --git a/src/broadphase/broadphase_continuous_collision_manager.cpp b/src/broadphase/broadphase_continuous_collision_manager.cpp index 7084d769e..e30121133 100644 --- a/src/broadphase/broadphase_continuous_collision_manager.cpp +++ b/src/broadphase/broadphase_continuous_collision_manager.cpp @@ -31,7 +31,7 @@ * 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 */ diff --git a/src/broadphase/broadphase_dynamic_AABB_tree.cpp b/src/broadphase/broadphase_dynamic_AABB_tree.cpp index e43bcdb9d..2d51012c0 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree.cpp @@ -31,7 +31,7 @@ * 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 */ diff --git a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp index 375c78b1d..8b9b065f0 100644 --- a/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp +++ b/src/broadphase/broadphase_dynamic_AABB_tree_array.cpp @@ -31,7 +31,7 @@ * 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 */ diff --git a/src/broadphase/broadphase_interval_tree.cpp b/src/broadphase/broadphase_interval_tree.cpp index 10c0cf368..70e527b94 100644 --- a/src/broadphase/broadphase_interval_tree.cpp +++ b/src/broadphase/broadphase_interval_tree.cpp @@ -31,7 +31,7 @@ * 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 */ diff --git a/src/broadphase/broadphase_spatialhash.cpp b/src/broadphase/broadphase_spatialhash.cpp index 0592102b4..d1fa84f09 100644 --- a/src/broadphase/broadphase_spatialhash.cpp +++ b/src/broadphase/broadphase_spatialhash.cpp @@ -31,7 +31,7 @@ * 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 */ diff --git a/src/broadphase/detail/spatial_hash.cpp b/src/broadphase/detail/spatial_hash.cpp index 19450b399..11a67b3f4 100644 --- a/src/broadphase/detail/spatial_hash.cpp +++ b/src/broadphase/detail/spatial_hash.cpp @@ -31,7 +31,7 @@ * 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 */ diff --git a/src/geometry/octree/octree.cpp b/src/geometry/octree/octree.cpp index f94622fae..2e27cbb5e 100644 --- a/src/geometry/octree/octree.cpp +++ b/src/geometry/octree/octree.cpp @@ -55,4 +55,4 @@ void computeChildBV(const AABB& root_bv, unsigned int i, AABB& c } // namespace fcl -#endif +#endif \ No newline at end of file diff --git a/src/math/bv/kIOS.cpp b/src/math/bv/kIOS.cpp index 8c62ab7c3..b43a3e5e4 100644 --- a/src/math/bv/kIOS.cpp +++ b/src/math/bv/kIOS.cpp @@ -39,7 +39,7 @@ namespace fcl { - + template class FCL_INSTANTIATION_DEF_API kIOS; diff --git a/src/math/detail/polysolver.cpp b/src/math/detail/polysolver.cpp index 222063407..ed2f1ce78 100644 --- a/src/math/detail/polysolver.cpp +++ b/src/math/detail/polysolver.cpp @@ -1,49 +1,49 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, 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 */ - -#include "fcl/math/detail/polysolver-inl.h" - -namespace fcl -{ - -namespace detail { - -template -class FCL_INSTANTIATION_DEF_API PolySolver; - -} // namespace detail -} // namespace fcl +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, 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 */ + +#include "fcl/math/detail/polysolver-inl.h" + +namespace fcl +{ + +namespace detail { + +template +class FCL_INSTANTIATION_DEF_API PolySolver; + +} // namespace detail +} // namespace fcl diff --git a/src/math/detail/project.cpp b/src/math/detail/project.cpp index b26fecddf..9024d2efb 100644 --- a/src/math/detail/project.cpp +++ b/src/math/detail/project.cpp @@ -1,50 +1,50 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, 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 */ - -#include "fcl/math/detail/project-inl.h" - -namespace fcl -{ - -namespace detail -{ - -template -class FCL_INSTANTIATION_DEF_API Project; - -} // namespace detail -} // namespace fcl +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, 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 */ + +#include "fcl/math/detail/project-inl.h" + +namespace fcl +{ + +namespace detail +{ + +template +class FCL_INSTANTIATION_DEF_API Project; + +} // namespace detail +} // namespace fcl diff --git a/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp b/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp index c0ef3efda..094def0e0 100644 --- a/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/intersect.cpp @@ -1,50 +1,50 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, 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 */ - -#include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h" - -namespace fcl -{ - -namespace detail -{ - -template -class FCL_INSTANTIATION_DEF_API Intersect; - -} // namespace detail -} // namespace fcl +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, 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 */ + +#include "fcl/narrowphase/detail/traversal/collision/intersect-inl.h" + +namespace fcl +{ + +namespace detail +{ + +template +class FCL_INSTANTIATION_DEF_API Intersect; + +} // namespace detail +} // namespace fcl diff --git a/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp b/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp index 62c14a1dc..abaeb602a 100644 --- a/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp +++ b/src/narrowphase/detail/primitive_shape_algorithm/triangle_distance.cpp @@ -1,51 +1,51 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011-2014, Willow Garage, Inc. - * Copyright (c) 2014-2016, 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 */ - -#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h" - -namespace fcl -{ - -namespace detail -{ - -//============================================================================== -template -class FCL_INSTANTIATION_DEF_API TriangleDistance; - -} // namespace detail -} // namespace fcl +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2016, 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 */ + +#include "fcl/narrowphase/detail/primitive_shape_algorithm/triangle_distance-inl.h" + +namespace fcl +{ + +namespace detail +{ + +//============================================================================== +template +class FCL_INSTANTIATION_DEF_API TriangleDistance; + +} // namespace detail +} // namespace fcl