diff options
Diffstat (limited to 'extern/solid/src/complex/DT_BBoxTree.h')
| -rw-r--r-- | extern/solid/src/complex/DT_BBoxTree.h | 540 |
1 files changed, 0 insertions, 540 deletions
diff --git a/extern/solid/src/complex/DT_BBoxTree.h b/extern/solid/src/complex/DT_BBoxTree.h deleted file mode 100644 index 3d9da6e34ba..00000000000 --- a/extern/solid/src/complex/DT_BBoxTree.h +++ /dev/null @@ -1,540 +0,0 @@ -/* - * SOLID - Software Library for Interference Detection - * - * Copyright (C) 2001-2003 Dtecta. All rights reserved. - * - * This library may be distributed under the terms of the Q Public License - * (QPL) as defined by Trolltech AS of Norway and appearing in the file - * LICENSE.QPL included in the packaging of this file. - * - * This library may be distributed and/or modified under the terms of the - * GNU General Public License (GPL) version 2 as published by the Free Software - * Foundation and appearing in the file LICENSE.GPL included in the - * packaging of this file. - * - * This library is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE - * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. - * - * Commercial use or any other use of this library not covered by either - * the QPL or the GPL requires an additional license from Dtecta. - * Please contact info@dtecta.com for enquiries about the terms of commercial - * use of this library. - */ - -#ifndef DT_BBOXTREE_H -#define DT_BBOXTREE_H - -#include <new> -#include <algorithm> - -#include "DT_Convex.h" -#include "DT_CBox.h" - - -class DT_BBoxTree { -public: - enum NodeType { INTERNAL = 0, LEAF = 1 }; - - DT_BBoxTree() {} - DT_BBoxTree(const DT_CBox& cbox, DT_Index index, NodeType type) - : m_cbox(cbox), - m_index(index), - m_type(type) - {} - - DT_CBox m_cbox; - DT_Index m_index; - NodeType m_type; -}; - - - -class DT_BBoxNode { -public: - DT_BBoxNode() {} - DT_BBoxNode(int first, int last, int& node, DT_BBoxNode *free_nodes, const DT_CBox *boxes, DT_Index *indices, const DT_CBox& bbox); - - void makeChildren(DT_BBoxTree& ltree, DT_BBoxTree& rtree) const; - void makeChildren(const DT_CBox& added, DT_BBoxTree& ltree, DT_BBoxTree& rtree) const; - - DT_CBox hull() const { return m_lbox.hull(m_rbox); } - - enum FlagType { LLEAF = 0x80, RLEAF = 0x40 }; - - DT_CBox m_lbox; - DT_CBox m_rbox; - DT_Index m_lchild; - DT_Index m_rchild; - unsigned char m_flags; -}; - -inline void DT_BBoxNode::makeChildren(DT_BBoxTree& ltree, DT_BBoxTree& rtree) const -{ - new (<ree) DT_BBoxTree(m_lbox, m_lchild, (m_flags & LLEAF) ? DT_BBoxTree::LEAF : DT_BBoxTree::INTERNAL); - new (&rtree) DT_BBoxTree(m_rbox, m_rchild, (m_flags & RLEAF) ? DT_BBoxTree::LEAF : DT_BBoxTree::INTERNAL); - -} - -inline void DT_BBoxNode::makeChildren(const DT_CBox& added, DT_BBoxTree& ltree, DT_BBoxTree& rtree) const -{ - new (<ree) DT_BBoxTree(m_lbox + added, m_lchild, (m_flags & LLEAF) ? DT_BBoxTree::LEAF : DT_BBoxTree::INTERNAL); - new (&rtree) DT_BBoxTree(m_rbox + added, m_rchild, (m_flags & RLEAF) ? DT_BBoxTree::LEAF : DT_BBoxTree::INTERNAL); -} - - -template <typename Shape> -class DT_RootData { -public: - DT_RootData(const DT_BBoxNode *nodes, - const Shape *leaves) - : m_nodes(nodes), - m_leaves(leaves) - {} - - const DT_BBoxNode *m_nodes; - const Shape *m_leaves; -}; - -template <typename Shape1, typename Shape2> -class DT_ObjectData : public DT_RootData<Shape1> { -public: - DT_ObjectData(const DT_BBoxNode *nodes, - const Shape1 *leaves, - const MT_Transform& xform, - Shape2 plus) - : DT_RootData<Shape1>(nodes, leaves), - m_xform(xform), - m_inv_xform(xform.inverse()), - m_plus(plus), - m_added(computeCBox(plus, m_inv_xform)) - {} - - const MT_Transform& m_xform; - MT_Transform m_inv_xform; - Shape2 m_plus; - DT_CBox m_added; -}; - -template <typename Shape1, typename Shape2> -class DT_Pack { -public: - DT_Pack(const DT_ObjectData<Shape1, Shape2>& a, const DT_Convex& b) - : m_a(a), - m_b(b), - m_b_cbox(b.bbox(m_a.m_inv_xform)) - {} - - DT_ObjectData<Shape1, Shape2> m_a; - const DT_Convex& m_b; - DT_CBox m_b_cbox; -}; - -template <typename Shape1, typename Shape2> -class DT_HybridPack : public DT_Pack<Shape1, Shape2> { -public: - DT_HybridPack(const DT_ObjectData<Shape1, Shape2>& a, const DT_Convex& b, MT_Scalar margin) - : DT_Pack<Shape1, Shape2>(a, b), - m_margin(margin) - { - this->m_b_cbox += computeCBox(margin, this->m_a.m_inv_xform); - } - - MT_Scalar m_margin; -}; - -template <typename Shape1, typename Shape2> -class DT_DuoPack { -public: - DT_DuoPack(const DT_ObjectData<Shape1, Shape2>& a, const DT_ObjectData<Shape1, Shape2>& b) - : m_a(a), - m_b(b) - { - m_b2a = a.m_inv_xform * b.m_xform; - m_a2b = b.m_inv_xform * a.m_xform; - m_abs_b2a = m_b2a.getBasis().absolute(); - m_abs_a2b = m_a2b.getBasis().absolute(); - } - - DT_ObjectData<Shape1, Shape2> m_a, m_b; - MT_Transform m_b2a, m_a2b; - MT_Matrix3x3 m_abs_b2a, m_abs_a2b; -}; - - -template <typename Shape> -inline void refit(DT_BBoxNode& node, const DT_RootData<Shape>& rd) -{ - node.m_lbox = (node.m_flags & DT_BBoxNode::LLEAF) ? - computeCBox(rd.m_leaves[node.m_lchild]) : - rd.m_nodes[node.m_lchild].hull(); - node.m_rbox = (node.m_flags & DT_BBoxNode::RLEAF) ? - computeCBox(rd.m_leaves[node.m_rchild]) : - rd.m_nodes[node.m_rchild].hull(); -} - - -template <typename Shape> -bool ray_cast(const DT_BBoxTree& a, const DT_RootData<Shape>& rd, - const MT_Point3& source, const MT_Point3& target, - MT_Scalar& lambda, MT_Vector3& normal) -{ - if (!a.m_cbox.overlapsLineSegment(source, source.lerp(target, lambda))) - { - return false; - } - - if (a.m_type == DT_BBoxTree::LEAF) - { - return ray_cast(rd, a.m_index, source, target, lambda, normal); - } - else - { - DT_BBoxTree ltree, rtree; - rd.m_nodes[a.m_index].makeChildren(ltree, rtree); - - bool lresult = ray_cast(ltree, rd, source, target, lambda, normal); - bool rresult = ray_cast(rtree, rd, source, target, lambda, normal); - return lresult || rresult; - } -} - - -#ifdef STATISTICS -int num_box_tests = 0; -#endif - -template <typename Shape1, typename Shape2> -inline bool intersect(const DT_CBox& a, const DT_CBox& b, const DT_DuoPack<Shape1, Shape2>& pack) -{ -#ifdef STATISTICS - ++num_box_tests; -#endif - - - MT_Vector3 abs_pos_b2a = (pack.m_b2a(b.getCenter()) - a.getCenter()).absolute(); - MT_Vector3 abs_pos_a2b = (pack.m_a2b(a.getCenter()) - b.getCenter()).absolute(); - return (a.getExtent()[0] + pack.m_abs_b2a[0].dot(b.getExtent()) >= abs_pos_b2a[0]) && - (a.getExtent()[1] + pack.m_abs_b2a[1].dot(b.getExtent()) >= abs_pos_b2a[1]) && - (a.getExtent()[2] + pack.m_abs_b2a[2].dot(b.getExtent()) >= abs_pos_b2a[2]) && - (b.getExtent()[0] + pack.m_abs_a2b[0].dot(a.getExtent()) >= abs_pos_a2b[0]) && - (b.getExtent()[1] + pack.m_abs_a2b[1].dot(a.getExtent()) >= abs_pos_a2b[1]) && - (b.getExtent()[2] + pack.m_abs_a2b[2].dot(a.getExtent()) >= abs_pos_a2b[2]); -} - - - - -template <typename Shape1, typename Shape2> -bool intersect(const DT_BBoxTree& a, const DT_Pack<Shape1, Shape2>& pack, MT_Vector3& v) -{ - if (!a.m_cbox.overlaps(pack.m_b_cbox)) - { - return false; - } - - if (a.m_type == DT_BBoxTree::LEAF) - { - return intersect(pack, a.m_index, v); - } - else - { - DT_BBoxTree a_ltree, a_rtree; - pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); - return intersect(a_ltree, pack, v) || intersect(a_rtree, pack, v); - } -} - -template <typename Shape1, typename Shape2> -bool intersect(const DT_BBoxTree& a, const DT_BBoxTree& b, const DT_DuoPack<Shape1, Shape2>& pack, MT_Vector3& v) -{ - if (!intersect(a.m_cbox, b.m_cbox, pack)) - { - return false; - } - - if (a.m_type == DT_BBoxTree::LEAF && b.m_type == DT_BBoxTree::LEAF) - { - return intersect(pack, a.m_index, b.m_index, v); - } - else if (a.m_type == DT_BBoxTree::LEAF || - (b.m_type != DT_BBoxTree::LEAF && a.m_cbox.size() < b.m_cbox.size())) - { - DT_BBoxTree b_ltree, b_rtree; - pack.m_b.m_nodes[b.m_index].makeChildren(pack.m_b.m_added, b_ltree, b_rtree); - - return intersect(a, b_ltree, pack, v) || intersect(a, b_rtree, pack, v); - } - else - { - DT_BBoxTree a_ltree, a_rtree; - pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); - return intersect(a_ltree, b, pack, v) || intersect(a_rtree, b, pack, v); - } -} - -template <typename Shape1, typename Shape2> -bool common_point(const DT_BBoxTree& a, const DT_Pack<Shape1, Shape2>& pack, - MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) -{ - if (!a.m_cbox.overlaps(pack.m_b_cbox)) - { - return false; - } - - if (a.m_type == DT_BBoxTree::LEAF) - { - return common_point(pack, a.m_index, v, pa, pb); - } - else - { - DT_BBoxTree a_ltree, a_rtree; - pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); - return common_point(a_ltree, pack, v, pa, pb) || - common_point(a_rtree, pack, v, pa ,pb); - } -} - -template <typename Shape1, typename Shape2> -bool common_point(const DT_BBoxTree& a, const DT_BBoxTree& b, const DT_DuoPack<Shape1, Shape2>& pack, - MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) -{ - if (!intersect(a.m_cbox, b.m_cbox, pack)) - { - return false; - } - - if (a.m_type == DT_BBoxTree::LEAF && b.m_type == DT_BBoxTree::LEAF) - { - return common_point(pack, a.m_index, b.m_index, v, pa, pb); - } - else if (a.m_type == DT_BBoxTree::LEAF || - (b.m_type != DT_BBoxTree::LEAF && a.m_cbox.size() < b.m_cbox.size())) - { - DT_BBoxTree b_ltree, b_rtree; - pack.m_b.m_nodes[b.m_index].makeChildren(pack.m_b.m_added, b_ltree, b_rtree); - return common_point(a, b_ltree, pack, v, pa, pb) || - common_point(a, b_rtree, pack, v, pa, pb); - } - else - { - DT_BBoxTree a_ltree, a_rtree; - pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); - return common_point(a_ltree, b, pack, v, pa, pb) || - common_point(a_rtree, b, pack, v, pa ,pb); - } -} - - -template <typename Shape1, typename Shape2> -bool penetration_depth(const DT_BBoxTree& a, const DT_HybridPack<Shape1, Shape2>& pack, - MT_Vector3& v, MT_Point3& pa, MT_Point3& pb, MT_Scalar& max_pen_len) -{ - if (!a.m_cbox.overlaps(pack.m_b_cbox)) - { - return false; - } - - if (a.m_type == DT_BBoxTree::LEAF) - { - if (penetration_depth(pack, a.m_index, v, pa, pb)) - { - max_pen_len = pa.distance2(pb); - return true; - } - else - { - return false; - } - } - else - { - DT_BBoxTree a_ltree, a_rtree; - pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); - if (penetration_depth(a_ltree, pack, v, pa, pb, max_pen_len)) - { - MT_Vector3 rv; - MT_Point3 rpa, rpb; - MT_Scalar rmax_pen_len; - if (penetration_depth(a_rtree, pack, rv, rpa, rpb, rmax_pen_len) && - (max_pen_len < rmax_pen_len)) - { - max_pen_len = rmax_pen_len; - v = rv; - pa = rpa; - pb = rpb; - } - return true; - } - else - { - return penetration_depth(a_rtree, pack, v, pa, pb, max_pen_len); - } - } -} - -template <typename Shape1, typename Shape2> -bool penetration_depth(const DT_BBoxTree& a, const DT_BBoxTree& b, const DT_DuoPack<Shape1, Shape2>& pack, - MT_Vector3& v, MT_Point3& pa, MT_Point3& pb, MT_Scalar& max_pen_len) -{ - if (!intersect(a.m_cbox, b.m_cbox, pack)) - { - return false; - } - - if (a.m_type == DT_BBoxTree::LEAF && b.m_type == DT_BBoxTree::LEAF) - { - if (penetration_depth(pack, a.m_index, b.m_index, v, pa, pb)) - { - max_pen_len = pa.distance2(pb); - return true; - } - else - { - return false; - } - } - else if (a.m_type == DT_BBoxTree::LEAF || - (b.m_type != DT_BBoxTree::LEAF && a.m_cbox.size() < b.m_cbox.size())) - { - DT_BBoxTree b_ltree, b_rtree; - pack.m_b.m_nodes[b.m_index].makeChildren(pack.m_b.m_added, b_ltree, b_rtree); - if (penetration_depth(a, b_ltree, pack, v, pa, pb, max_pen_len)) - { - MT_Point3 rpa, rpb; - MT_Scalar rmax_pen_len; - if (penetration_depth(a, b_rtree, pack, v, rpa, rpb, rmax_pen_len) && - (max_pen_len < rmax_pen_len)) - { - max_pen_len = rmax_pen_len; - pa = rpa; - pb = rpb; - } - return true; - } - else - { - return penetration_depth(a, b_rtree, pack, v, pa, pb, max_pen_len); - } - } - else - { - DT_BBoxTree a_ltree, a_rtree; - pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); - if (penetration_depth(a_ltree, b, pack, v, pa, pb, max_pen_len)) - { - MT_Point3 rpa, rpb; - MT_Scalar rmax_pen_len; - if (penetration_depth(a_rtree, b, pack, v, rpa, rpb, rmax_pen_len) && - (max_pen_len < rmax_pen_len)) - { - max_pen_len = rmax_pen_len; - pa = rpa; - pb = rpb; - } - return true; - } - else - { - return penetration_depth(a_rtree, b, pack, v, pa, pb, max_pen_len); - } - } -} - - -// Returns a lower bound for the distance for quick rejection in closest_points -inline MT_Scalar distance2(const DT_CBox& a, const MT_Transform& a2w, - const DT_CBox& b, const MT_Transform& b2w) -{ - MT_Vector3 v = b2w(b.getCenter()) - a2w(a.getCenter()); - MT_Scalar dist2 = v.length2(); - if (dist2 > MT_Scalar(0.0)) - { - MT_Vector3 w = b2w(b.support(-v * b2w.getBasis())) - a2w(a.support(v * a2w.getBasis())); - MT_Scalar delta = v.dot(w); - return delta > MT_Scalar(0.0) ? delta * delta / dist2 : MT_Scalar(0.0); - } - return MT_Scalar(0.0); -} - - -template <typename Shape1, typename Shape2> -MT_Scalar closest_points(const DT_BBoxTree& a, const DT_Pack<Shape1, Shape2>& pack, - MT_Scalar max_dist2, MT_Point3& pa, MT_Point3& pb) -{ - if (a.m_type == DT_BBoxTree::LEAF) - { - return closest_points(pack, a.m_index, max_dist2, pa, pb); - } - else - { - DT_BBoxTree a_ltree, a_rtree; - pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); - MT_Scalar ldist2 = distance2(a_ltree.m_cbox, pack.m_a.m_xform, pack.m_b_cbox, pack.m_a.m_xform); - MT_Scalar rdist2 = distance2(a_rtree.m_cbox, pack.m_a.m_xform, pack.m_b_cbox, pack.m_a.m_xform); - if (ldist2 < rdist2) - { - MT_Scalar dist2 = ldist2 < max_dist2 ? closest_points(a_ltree, pack, max_dist2, pa, pb) : MT_INFINITY; - GEN_set_min(max_dist2, dist2); - return rdist2 < max_dist2 ? GEN_min(dist2, closest_points(a_rtree, pack, max_dist2, pa, pb)) : dist2; - } - else - { - MT_Scalar dist2 = rdist2 < max_dist2 ? closest_points(a_rtree, pack, max_dist2, pa, pb) : MT_INFINITY; - GEN_set_min(max_dist2, dist2); - return ldist2 < max_dist2 ? GEN_min(dist2, closest_points(a_ltree, pack, max_dist2, pa, pb)) : dist2; - } - } -} - - -template <typename Shape1, typename Shape2> -MT_Scalar closest_points(const DT_BBoxTree& a, const DT_BBoxTree& b, const DT_DuoPack<Shape1, Shape2>& pack, - MT_Scalar max_dist2, MT_Point3& pa, MT_Point3& pb) -{ - if (a.m_type == DT_BBoxTree::LEAF && b.m_type == DT_BBoxTree::LEAF) - { - return closest_points(pack, a.m_index, b.m_index, max_dist2, pa, pb); - } - else if (a.m_type == DT_BBoxTree::LEAF || - (b.m_type != DT_BBoxTree::LEAF && a.m_cbox.size() < b.m_cbox.size())) - { - DT_BBoxTree b_ltree, b_rtree; - pack.m_b.m_nodes[b.m_index].makeChildren(pack.m_b.m_added, b_ltree, b_rtree); - MT_Scalar ldist2 = distance2(a.m_cbox, pack.m_a.m_xform, b_ltree.m_cbox, pack.m_b.m_xform); - MT_Scalar rdist2 = distance2(a.m_cbox, pack.m_a.m_xform, b_rtree.m_cbox, pack.m_b.m_xform); - if (ldist2 < rdist2) - { - MT_Scalar dist2 = ldist2 < max_dist2 ? closest_points(a, b_ltree, pack, max_dist2, pa, pb): MT_INFINITY;; - GEN_set_min(max_dist2, dist2); - return rdist2 < max_dist2 ? GEN_min(dist2, closest_points(a, b_rtree, pack, max_dist2, pa, pb)) : dist2; - } - else - { - MT_Scalar dist2 = rdist2 < max_dist2 ? closest_points(a, b_rtree, pack, max_dist2, pa, pb) : MT_INFINITY;; - GEN_set_min(max_dist2, dist2); - return ldist2 < max_dist2 ? GEN_min(dist2, closest_points(a, b_ltree, pack, max_dist2, pa, pb)) : dist2; - } - } - else - { - DT_BBoxTree a_ltree, a_rtree; - pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); - MT_Scalar ldist2 = distance2(a_ltree.m_cbox, pack.m_a.m_xform, b.m_cbox, pack.m_b.m_xform); - MT_Scalar rdist2 = distance2(a_rtree.m_cbox, pack.m_a.m_xform, b.m_cbox, pack.m_b.m_xform); - if (ldist2 < rdist2) - { - MT_Scalar dist2 = ldist2 < max_dist2 ? closest_points(a_ltree, b, pack, max_dist2, pa, pb) : MT_INFINITY;; - GEN_set_min(max_dist2, dist2); - return rdist2 < max_dist2 ? GEN_min(dist2,closest_points(a_rtree, b, pack, max_dist2, pa, pb)) : dist2; - } - else - { - MT_Scalar dist2 = rdist2 < max_dist2 ? closest_points(a_rtree, b, pack, max_dist2, pa, pb) : MT_INFINITY; - GEN_set_min(max_dist2, dist2); - return ldist2 < max_dist2 ? GEN_min(dist2, closest_points(a_ltree, b, pack, max_dist2, pa, pb)) : dist2; - } - } -} - -#endif - |