[dali_2.3.21] Merge branch 'devel/master'
[platform/core/uifw/dali-toolkit.git] / dali-physics / third-party / bullet3 / src / BulletCollision / Gimpact / gim_box_set.h
1 #ifndef GIM_BOX_SET_H_INCLUDED
2 #define GIM_BOX_SET_H_INCLUDED
3
4 /*! \file gim_box_set.h
5 \author Francisco Leon Najera
6 */
7 /*
8 -----------------------------------------------------------------------------
9 This source file is part of GIMPACT Library.
10
11 For the latest info, see http://gimpact.sourceforge.net/
12
13 Copyright (c) 2006 Francisco Leon Najera. C.C. 80087371.
14 email: projectileman@yahoo.com
15
16  This library is free software; you can redistribute it and/or
17  modify it under the terms of EITHER:
18    (1) The GNU Lesser General Public License as published by the Free
19        Software Foundation; either version 2.1 of the License, or (at
20        your option) any later version. The text of the GNU Lesser
21        General Public License is included with this library in the
22        file GIMPACT-LICENSE-LGPL.TXT.
23    (2) The BSD-style license that is included with this library in
24        the file GIMPACT-LICENSE-BSD.TXT.
25    (3) The zlib/libpng license that is included with this library in
26        the file GIMPACT-LICENSE-ZLIB.TXT.
27
28  This library is distributed in the hope that it will be useful,
29  but WITHOUT ANY WARRANTY; without even the implied warranty of
30  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files
31  GIMPACT-LICENSE-LGPL.TXT, GIMPACT-LICENSE-ZLIB.TXT and GIMPACT-LICENSE-BSD.TXT for more details.
32
33 -----------------------------------------------------------------------------
34 */
35
36 #include "gim_array.h"
37 #include "gim_radixsort.h"
38 #include "gim_box_collision.h"
39 #include "gim_tri_collision.h"
40 #include "gim_pair.h"
41
42 //! A pairset array
43 class gim_pair_set : public gim_array<GIM_PAIR>
44 {
45 public:
46         gim_pair_set() : gim_array<GIM_PAIR>(32)
47         {
48         }
49         inline void push_pair(GUINT index1, GUINT index2)
50         {
51                 push_back(GIM_PAIR(index1, index2));
52         }
53
54         inline void push_pair_inv(GUINT index1, GUINT index2)
55         {
56                 push_back(GIM_PAIR(index2, index1));
57         }
58 };
59
60 //! Prototype Base class for primitive classification
61 /*!
62 This class is a wrapper for primitive collections.
63 This tells relevant info for the Bounding Box set classes, which take care of space classification.
64 This class can manage Compound shapes and trimeshes, and if it is managing trimesh then the  Hierarchy Bounding Box classes will take advantage of primitive Vs Box overlapping tests for getting optimal results and less Per Box compairisons.
65 */
66 class GIM_PRIMITIVE_MANAGER_PROTOTYPE
67 {
68 public:
69         virtual ~GIM_PRIMITIVE_MANAGER_PROTOTYPE() {}
70         //! determines if this manager consist on only triangles, which special case will be optimized
71         virtual bool is_trimesh() = 0;
72         virtual GUINT get_primitive_count() = 0;
73         virtual void get_primitive_box(GUINT prim_index, GIM_AABB& primbox) = 0;
74         virtual void get_primitive_triangle(GUINT prim_index, GIM_TRIANGLE& triangle) = 0;
75 };
76
77 struct GIM_AABB_DATA
78 {
79         GIM_AABB m_bound;
80         GUINT m_data;
81 };
82
83 //! Node Structure for trees
84 struct GIM_BOX_TREE_NODE
85 {
86         GIM_AABB m_bound;
87         GUINT m_left;         //!< Left subtree
88         GUINT m_right;        //!< Right subtree
89         GUINT m_escapeIndex;  //!< Scape index for traversing
90         GUINT m_data;         //!< primitive index if apply
91
92         GIM_BOX_TREE_NODE()
93         {
94                 m_left = 0;
95                 m_right = 0;
96                 m_escapeIndex = 0;
97                 m_data = 0;
98         }
99
100         SIMD_FORCE_INLINE bool is_leaf_node() const
101         {
102                 return (!m_left && !m_right);
103         }
104 };
105
106 //! Basic Box tree structure
107 class GIM_BOX_TREE
108 {
109 protected:
110         GUINT m_num_nodes;
111         gim_array<GIM_BOX_TREE_NODE> m_node_array;
112
113 protected:
114         GUINT _sort_and_calc_splitting_index(
115                 gim_array<GIM_AABB_DATA>& primitive_boxes,
116                 GUINT startIndex, GUINT endIndex, GUINT splitAxis);
117
118         GUINT _calc_splitting_axis(gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex);
119
120         void _build_sub_tree(gim_array<GIM_AABB_DATA>& primitive_boxes, GUINT startIndex, GUINT endIndex);
121
122 public:
123         GIM_BOX_TREE()
124         {
125                 m_num_nodes = 0;
126         }
127
128         //! prototype functions for box tree management
129         //!@{
130         void build_tree(gim_array<GIM_AABB_DATA>& primitive_boxes);
131
132         SIMD_FORCE_INLINE void clearNodes()
133         {
134                 m_node_array.clear();
135                 m_num_nodes = 0;
136         }
137
138         //! node count
139         SIMD_FORCE_INLINE GUINT getNodeCount() const
140         {
141                 return m_num_nodes;
142         }
143
144         //! tells if the node is a leaf
145         SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const
146         {
147                 return m_node_array[nodeindex].is_leaf_node();
148         }
149
150         SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const
151         {
152                 return m_node_array[nodeindex].m_data;
153         }
154
155         SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB& bound) const
156         {
157                 bound = m_node_array[nodeindex].m_bound;
158         }
159
160         SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB& bound)
161         {
162                 m_node_array[nodeindex].m_bound = bound;
163         }
164
165         SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const
166         {
167                 return m_node_array[nodeindex].m_left;
168         }
169
170         SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const
171         {
172                 return m_node_array[nodeindex].m_right;
173         }
174
175         SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const
176         {
177                 return m_node_array[nodeindex].m_escapeIndex;
178         }
179
180         //!@}
181 };
182
183 //! Generic Box Tree Template
184 /*!
185 This class offers an structure for managing a box tree of primitives.
186 Requires a Primitive prototype (like GIM_PRIMITIVE_MANAGER_PROTOTYPE ) and
187 a Box tree structure ( like GIM_BOX_TREE).
188 */
189 template <typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE, typename _GIM_BOX_TREE_PROTOTYPE>
190 class GIM_BOX_TREE_TEMPLATE_SET
191 {
192 protected:
193         _GIM_PRIMITIVE_MANAGER_PROTOTYPE m_primitive_manager;
194         _GIM_BOX_TREE_PROTOTYPE m_box_tree;
195
196 protected:
197         //stackless refit
198         SIMD_FORCE_INLINE void refit()
199         {
200                 GUINT nodecount = getNodeCount();
201                 while (nodecount--)
202                 {
203                         if (isLeafNode(nodecount))
204                         {
205                                 GIM_AABB leafbox;
206                                 m_primitive_manager.get_primitive_box(getNodeData(nodecount), leafbox);
207                                 setNodeBound(nodecount, leafbox);
208                         }
209                         else
210                         {
211                                 //get left bound
212                                 GUINT childindex = getLeftNodeIndex(nodecount);
213                                 GIM_AABB bound;
214                                 getNodeBound(childindex, bound);
215                                 //get right bound
216                                 childindex = getRightNodeIndex(nodecount);
217                                 GIM_AABB bound2;
218                                 getNodeBound(childindex, bound2);
219                                 bound.merge(bound2);
220
221                                 setNodeBound(nodecount, bound);
222                         }
223                 }
224         }
225
226 public:
227         GIM_BOX_TREE_TEMPLATE_SET()
228         {
229         }
230
231         SIMD_FORCE_INLINE GIM_AABB getGlobalBox() const
232         {
233                 GIM_AABB totalbox;
234                 getNodeBound(0, totalbox);
235                 return totalbox;
236         }
237
238         SIMD_FORCE_INLINE void setPrimitiveManager(const _GIM_PRIMITIVE_MANAGER_PROTOTYPE& primitive_manager)
239         {
240                 m_primitive_manager = primitive_manager;
241         }
242
243         const _GIM_PRIMITIVE_MANAGER_PROTOTYPE& getPrimitiveManager() const
244         {
245                 return m_primitive_manager;
246         }
247
248         _GIM_PRIMITIVE_MANAGER_PROTOTYPE& getPrimitiveManager()
249         {
250                 return m_primitive_manager;
251         }
252
253         //! node manager prototype functions
254         ///@{
255
256         //! this attemps to refit the box set.
257         SIMD_FORCE_INLINE void update()
258         {
259                 refit();
260         }
261
262         //! this rebuild the entire set
263         SIMD_FORCE_INLINE void buildSet()
264         {
265                 //obtain primitive boxes
266                 gim_array<GIM_AABB_DATA> primitive_boxes;
267                 primitive_boxes.resize(m_primitive_manager.get_primitive_count(), false);
268
269                 for (GUINT i = 0; i < primitive_boxes.size(); i++)
270                 {
271                         m_primitive_manager.get_primitive_box(i, primitive_boxes[i].m_bound);
272                         primitive_boxes[i].m_data = i;
273                 }
274
275                 m_box_tree.build_tree(primitive_boxes);
276         }
277
278         //! returns the indices of the primitives in the m_primitive_manager
279         SIMD_FORCE_INLINE bool boxQuery(const GIM_AABB& box, gim_array<GUINT>& collided_results) const
280         {
281                 GUINT curIndex = 0;
282                 GUINT numNodes = getNodeCount();
283
284                 while (curIndex < numNodes)
285                 {
286                         GIM_AABB bound;
287                         getNodeBound(curIndex, bound);
288
289                         //catch bugs in tree data
290
291                         bool aabbOverlap = bound.has_collision(box);
292                         bool isleafnode = isLeafNode(curIndex);
293
294                         if (isleafnode && aabbOverlap)
295                         {
296                                 collided_results.push_back(getNodeData(curIndex));
297                         }
298
299                         if (aabbOverlap || isleafnode)
300                         {
301                                 //next subnode
302                                 curIndex++;
303                         }
304                         else
305                         {
306                                 //skip node
307                                 curIndex += getScapeNodeIndex(curIndex);
308                         }
309                 }
310                 if (collided_results.size() > 0) return true;
311                 return false;
312         }
313
314         //! returns the indices of the primitives in the m_primitive_manager
315         SIMD_FORCE_INLINE bool boxQueryTrans(const GIM_AABB& box,
316                                                                                  const btTransform& transform, gim_array<GUINT>& collided_results) const
317         {
318                 GIM_AABB transbox = box;
319                 transbox.appy_transform(transform);
320                 return boxQuery(transbox, collided_results);
321         }
322
323         //! returns the indices of the primitives in the m_primitive_manager
324         SIMD_FORCE_INLINE bool rayQuery(
325                 const btVector3& ray_dir, const btVector3& ray_origin,
326                 gim_array<GUINT>& collided_results) const
327         {
328                 GUINT curIndex = 0;
329                 GUINT numNodes = getNodeCount();
330
331                 while (curIndex < numNodes)
332                 {
333                         GIM_AABB bound;
334                         getNodeBound(curIndex, bound);
335
336                         //catch bugs in tree data
337
338                         bool aabbOverlap = bound.collide_ray(ray_origin, ray_dir);
339                         bool isleafnode = isLeafNode(curIndex);
340
341                         if (isleafnode && aabbOverlap)
342                         {
343                                 collided_results.push_back(getNodeData(curIndex));
344                         }
345
346                         if (aabbOverlap || isleafnode)
347                         {
348                                 //next subnode
349                                 curIndex++;
350                         }
351                         else
352                         {
353                                 //skip node
354                                 curIndex += getScapeNodeIndex(curIndex);
355                         }
356                 }
357                 if (collided_results.size() > 0) return true;
358                 return false;
359         }
360
361         //! tells if this set has hierarcht
362         SIMD_FORCE_INLINE bool hasHierarchy() const
363         {
364                 return true;
365         }
366
367         //! tells if this set is a trimesh
368         SIMD_FORCE_INLINE bool isTrimesh() const
369         {
370                 return m_primitive_manager.is_trimesh();
371         }
372
373         //! node count
374         SIMD_FORCE_INLINE GUINT getNodeCount() const
375         {
376                 return m_box_tree.getNodeCount();
377         }
378
379         //! tells if the node is a leaf
380         SIMD_FORCE_INLINE bool isLeafNode(GUINT nodeindex) const
381         {
382                 return m_box_tree.isLeafNode(nodeindex);
383         }
384
385         SIMD_FORCE_INLINE GUINT getNodeData(GUINT nodeindex) const
386         {
387                 return m_box_tree.getNodeData(nodeindex);
388         }
389
390         SIMD_FORCE_INLINE void getNodeBound(GUINT nodeindex, GIM_AABB& bound) const
391         {
392                 m_box_tree.getNodeBound(nodeindex, bound);
393         }
394
395         SIMD_FORCE_INLINE void setNodeBound(GUINT nodeindex, const GIM_AABB& bound)
396         {
397                 m_box_tree.setNodeBound(nodeindex, bound);
398         }
399
400         SIMD_FORCE_INLINE GUINT getLeftNodeIndex(GUINT nodeindex) const
401         {
402                 return m_box_tree.getLeftNodeIndex(nodeindex);
403         }
404
405         SIMD_FORCE_INLINE GUINT getRightNodeIndex(GUINT nodeindex) const
406         {
407                 return m_box_tree.getRightNodeIndex(nodeindex);
408         }
409
410         SIMD_FORCE_INLINE GUINT getScapeNodeIndex(GUINT nodeindex) const
411         {
412                 return m_box_tree.getScapeNodeIndex(nodeindex);
413         }
414
415         SIMD_FORCE_INLINE void getNodeTriangle(GUINT nodeindex, GIM_TRIANGLE& triangle) const
416         {
417                 m_primitive_manager.get_primitive_triangle(getNodeData(nodeindex), triangle);
418         }
419 };
420
421 //! Class for Box Tree Sets
422 /*!
423 this has the GIM_BOX_TREE implementation for bounding boxes.
424 */
425 template <typename _GIM_PRIMITIVE_MANAGER_PROTOTYPE>
426 class GIM_BOX_TREE_SET : public GIM_BOX_TREE_TEMPLATE_SET<_GIM_PRIMITIVE_MANAGER_PROTOTYPE, GIM_BOX_TREE>
427 {
428 public:
429 };
430
431 /// GIM_BOX_SET collision methods
432 template <typename BOX_SET_CLASS0, typename BOX_SET_CLASS1>
433 class GIM_TREE_TREE_COLLIDER
434 {
435 public:
436         gim_pair_set* m_collision_pairs;
437         BOX_SET_CLASS0* m_boxset0;
438         BOX_SET_CLASS1* m_boxset1;
439         GUINT current_node0;
440         GUINT current_node1;
441         bool node0_is_leaf;
442         bool node1_is_leaf;
443         bool t0_is_trimesh;
444         bool t1_is_trimesh;
445         bool node0_has_triangle;
446         bool node1_has_triangle;
447         GIM_AABB m_box0;
448         GIM_AABB m_box1;
449         GIM_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
450         btTransform trans_cache_0to1;
451         GIM_TRIANGLE m_tri0;
452         btVector4 m_tri0_plane;
453         GIM_TRIANGLE m_tri1;
454         btVector4 m_tri1_plane;
455
456 public:
457         GIM_TREE_TREE_COLLIDER()
458         {
459                 current_node0 = G_UINT_INFINITY;
460                 current_node1 = G_UINT_INFINITY;
461         }
462
463 protected:
464         SIMD_FORCE_INLINE void retrieve_node0_triangle(GUINT node0)
465         {
466                 if (node0_has_triangle) return;
467                 m_boxset0->getNodeTriangle(node0, m_tri0);
468                 //transform triangle
469                 m_tri0.m_vertices[0] = trans_cache_0to1(m_tri0.m_vertices[0]);
470                 m_tri0.m_vertices[1] = trans_cache_0to1(m_tri0.m_vertices[1]);
471                 m_tri0.m_vertices[2] = trans_cache_0to1(m_tri0.m_vertices[2]);
472                 m_tri0.get_plane(m_tri0_plane);
473
474                 node0_has_triangle = true;
475         }
476
477         SIMD_FORCE_INLINE void retrieve_node1_triangle(GUINT node1)
478         {
479                 if (node1_has_triangle) return;
480                 m_boxset1->getNodeTriangle(node1, m_tri1);
481                 //transform triangle
482                 m_tri1.m_vertices[0] = trans_cache_1to0.transform(m_tri1.m_vertices[0]);
483                 m_tri1.m_vertices[1] = trans_cache_1to0.transform(m_tri1.m_vertices[1]);
484                 m_tri1.m_vertices[2] = trans_cache_1to0.transform(m_tri1.m_vertices[2]);
485                 m_tri1.get_plane(m_tri1_plane);
486
487                 node1_has_triangle = true;
488         }
489
490         SIMD_FORCE_INLINE void retrieve_node0_info(GUINT node0)
491         {
492                 if (node0 == current_node0) return;
493                 m_boxset0->getNodeBound(node0, m_box0);
494                 node0_is_leaf = m_boxset0->isLeafNode(node0);
495                 node0_has_triangle = false;
496                 current_node0 = node0;
497         }
498
499         SIMD_FORCE_INLINE void retrieve_node1_info(GUINT node1)
500         {
501                 if (node1 == current_node1) return;
502                 m_boxset1->getNodeBound(node1, m_box1);
503                 node1_is_leaf = m_boxset1->isLeafNode(node1);
504                 node1_has_triangle = false;
505                 current_node1 = node1;
506         }
507
508         SIMD_FORCE_INLINE bool node_collision(GUINT node0, GUINT node1)
509         {
510                 retrieve_node0_info(node0);
511                 retrieve_node1_info(node1);
512                 bool result = m_box0.overlapping_trans_cache(m_box1, trans_cache_1to0, true);
513                 if (!result) return false;
514
515                 if (t0_is_trimesh && node0_is_leaf)
516                 {
517                         //perform primitive vs box collision
518                         retrieve_node0_triangle(node0);
519                         //do triangle vs box collision
520                         m_box1.increment_margin(m_tri0.m_margin);
521
522                         result = m_box1.collide_triangle_exact(
523                                 m_tri0.m_vertices[0], m_tri0.m_vertices[1], m_tri0.m_vertices[2], m_tri0_plane);
524
525                         m_box1.increment_margin(-m_tri0.m_margin);
526
527                         if (!result) return false;
528                         return true;
529                 }
530                 else if (t1_is_trimesh && node1_is_leaf)
531                 {
532                         //perform primitive vs box collision
533                         retrieve_node1_triangle(node1);
534                         //do triangle vs box collision
535                         m_box0.increment_margin(m_tri1.m_margin);
536
537                         result = m_box0.collide_triangle_exact(
538                                 m_tri1.m_vertices[0], m_tri1.m_vertices[1], m_tri1.m_vertices[2], m_tri1_plane);
539
540                         m_box0.increment_margin(-m_tri1.m_margin);
541
542                         if (!result) return false;
543                         return true;
544                 }
545                 return true;
546         }
547
548         //stackless collision routine
549         void find_collision_pairs()
550         {
551                 gim_pair_set stack_collisions;
552                 stack_collisions.reserve(32);
553
554                 //add the first pair
555                 stack_collisions.push_pair(0, 0);
556
557                 while (stack_collisions.size())
558                 {
559                         //retrieve the last pair and pop
560                         GUINT node0 = stack_collisions.back().m_index1;
561                         GUINT node1 = stack_collisions.back().m_index2;
562                         stack_collisions.pop_back();
563                         if (node_collision(node0, node1))  // a collision is found
564                         {
565                                 if (node0_is_leaf)
566                                 {
567                                         if (node1_is_leaf)
568                                         {
569                                                 m_collision_pairs->push_pair(m_boxset0->getNodeData(node0), m_boxset1->getNodeData(node1));
570                                         }
571                                         else
572                                         {
573                                                 //collide left
574                                                 stack_collisions.push_pair(node0, m_boxset1->getLeftNodeIndex(node1));
575
576                                                 //collide right
577                                                 stack_collisions.push_pair(node0, m_boxset1->getRightNodeIndex(node1));
578                                         }
579                                 }
580                                 else
581                                 {
582                                         if (node1_is_leaf)
583                                         {
584                                                 //collide left
585                                                 stack_collisions.push_pair(m_boxset0->getLeftNodeIndex(node0), node1);
586                                                 //collide right
587                                                 stack_collisions.push_pair(m_boxset0->getRightNodeIndex(node0), node1);
588                                         }
589                                         else
590                                         {
591                                                 GUINT left0 = m_boxset0->getLeftNodeIndex(node0);
592                                                 GUINT right0 = m_boxset0->getRightNodeIndex(node0);
593                                                 GUINT left1 = m_boxset1->getLeftNodeIndex(node1);
594                                                 GUINT right1 = m_boxset1->getRightNodeIndex(node1);
595                                                 //collide left
596                                                 stack_collisions.push_pair(left0, left1);
597                                                 //collide right
598                                                 stack_collisions.push_pair(left0, right1);
599                                                 //collide left
600                                                 stack_collisions.push_pair(right0, left1);
601                                                 //collide right
602                                                 stack_collisions.push_pair(right0, right1);
603
604                                         }  // else if node1 is not a leaf
605                                 }      // else if node0 is not a leaf
606
607                         }  // if(node_collision(node0,node1))
608                 }      //while(stack_collisions.size())
609         }
610
611 public:
612         void find_collision(BOX_SET_CLASS0* boxset1, const btTransform& trans1,
613                                                 BOX_SET_CLASS1* boxset2, const btTransform& trans2,
614                                                 gim_pair_set& collision_pairs, bool complete_primitive_tests = true)
615         {
616                 m_collision_pairs = &collision_pairs;
617                 m_boxset0 = boxset1;
618                 m_boxset1 = boxset2;
619
620                 trans_cache_1to0.calc_from_homogenic(trans1, trans2);
621
622                 trans_cache_0to1 = trans2.inverse();
623                 trans_cache_0to1 *= trans1;
624
625                 if (complete_primitive_tests)
626                 {
627                         t0_is_trimesh = boxset1->getPrimitiveManager().is_trimesh();
628                         t1_is_trimesh = boxset2->getPrimitiveManager().is_trimesh();
629                 }
630                 else
631                 {
632                         t0_is_trimesh = false;
633                         t1_is_trimesh = false;
634                 }
635
636                 find_collision_pairs();
637         }
638 };
639
640 #endif  // GIM_BOXPRUNING_H_INCLUDED