summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
diff options
context:
space:
mode:
authorRémi Verschelde <rverschelde@gmail.com>2018-01-13 14:01:53 +0100
committerRémi Verschelde <rverschelde@gmail.com>2018-01-13 14:08:45 +0100
commite12c89e8c9896b2e5cdd70dbd2d2acb449ff4b94 (patch)
treeaf68e434545e20c538f896e28b73f2db7d626edd /thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
parent53c65ae7619ac9e80c89a321c70de64f3745e2aa (diff)
bullet: Streamline bundling, remove extraneous src/ folder
Document version and how to extract sources in thirdparty/README.md. Drop unnecessary CMake and Premake files. Simplify SCsub, drop unused one.
Diffstat (limited to 'thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp')
-rw-r--r--thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp528
1 files changed, 528 insertions, 0 deletions
diff --git a/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
new file mode 100644
index 0000000000..4528758c37
--- /dev/null
+++ b/thirdparty/bullet/BulletCollision/Gimpact/btGImpactQuantizedBvh.cpp
@@ -0,0 +1,528 @@
+/*! \file gim_box_set.h
+\author Francisco Leon Najera
+*/
+/*
+This source file is part of GIMPACT Library.
+
+For the latest info, see http://gimpact.sourceforge.net/
+
+Copyright (c) 2007 Francisco Leon Najera. C.C. 80087371.
+email: projectileman@yahoo.com
+
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#include "btGImpactQuantizedBvh.h"
+#include "LinearMath/btQuickprof.h"
+
+#ifdef TRI_COLLISION_PROFILING
+btClock g_q_tree_clock;
+
+
+float g_q_accum_tree_collision_time = 0;
+int g_q_count_traversing = 0;
+
+
+void bt_begin_gim02_q_tree_time()
+{
+ g_q_tree_clock.reset();
+}
+
+void bt_end_gim02_q_tree_time()
+{
+ g_q_accum_tree_collision_time += g_q_tree_clock.getTimeMicroseconds();
+ g_q_count_traversing++;
+}
+
+
+//! Gets the average time in miliseconds of tree collisions
+float btGImpactQuantizedBvh::getAverageTreeCollisionTime()
+{
+ if(g_q_count_traversing == 0) return 0;
+
+ float avgtime = g_q_accum_tree_collision_time;
+ avgtime /= (float)g_q_count_traversing;
+
+ g_q_accum_tree_collision_time = 0;
+ g_q_count_traversing = 0;
+ return avgtime;
+
+// float avgtime = g_q_count_traversing;
+// g_q_count_traversing = 0;
+// return avgtime;
+
+}
+
+#endif //TRI_COLLISION_PROFILING
+
+/////////////////////// btQuantizedBvhTree /////////////////////////////////
+
+void btQuantizedBvhTree::calc_quantization(
+ GIM_BVH_DATA_ARRAY & primitive_boxes, btScalar boundMargin)
+{
+ //calc globa box
+ btAABB global_bound;
+ global_bound.invalidate();
+
+ for (int i=0;i<primitive_boxes.size() ;i++ )
+ {
+ global_bound.merge(primitive_boxes[i].m_bound);
+ }
+
+ bt_calc_quantization_parameters(
+ m_global_bound.m_min,m_global_bound.m_max,m_bvhQuantization,global_bound.m_min,global_bound.m_max,boundMargin);
+
+}
+
+
+
+int btQuantizedBvhTree::_calc_splitting_axis(
+ GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex)
+{
+
+ int i;
+
+ btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+ btVector3 variance(btScalar(0.),btScalar(0.),btScalar(0.));
+ int numIndices = endIndex-startIndex;
+
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+ primitive_boxes[i].m_bound.m_min);
+ means+=center;
+ }
+ means *= (btScalar(1.)/(btScalar)numIndices);
+
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+ primitive_boxes[i].m_bound.m_min);
+ btVector3 diff2 = center-means;
+ diff2 = diff2 * diff2;
+ variance += diff2;
+ }
+ variance *= (btScalar(1.)/ ((btScalar)numIndices-1) );
+
+ return variance.maxAxis();
+}
+
+
+int btQuantizedBvhTree::_sort_and_calc_splitting_index(
+ GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex,
+ int endIndex, int splitAxis)
+{
+ int i;
+ int splitIndex =startIndex;
+ int numIndices = endIndex - startIndex;
+
+ // average of centers
+ btScalar splitValue = 0.0f;
+
+ btVector3 means(btScalar(0.),btScalar(0.),btScalar(0.));
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+ primitive_boxes[i].m_bound.m_min);
+ means+=center;
+ }
+ means *= (btScalar(1.)/(btScalar)numIndices);
+
+ splitValue = means[splitAxis];
+
+
+ //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+ for (i=startIndex;i<endIndex;i++)
+ {
+ btVector3 center = btScalar(0.5)*(primitive_boxes[i].m_bound.m_max +
+ primitive_boxes[i].m_bound.m_min);
+ if (center[splitAxis] > splitValue)
+ {
+ //swap
+ primitive_boxes.swap(i,splitIndex);
+ //swapLeafNodes(i,splitIndex);
+ splitIndex++;
+ }
+ }
+
+ //if the splitIndex causes unbalanced trees, fix this by using the center in between startIndex and endIndex
+ //otherwise the tree-building might fail due to stack-overflows in certain cases.
+ //unbalanced1 is unsafe: it can cause stack overflows
+ //bool unbalanced1 = ((splitIndex==startIndex) || (splitIndex == (endIndex-1)));
+
+ //unbalanced2 should work too: always use center (perfect balanced trees)
+ //bool unbalanced2 = true;
+
+ //this should be safe too:
+ int rangeBalancedIndices = numIndices/3;
+ bool unbalanced = ((splitIndex<=(startIndex+rangeBalancedIndices)) || (splitIndex >=(endIndex-1-rangeBalancedIndices)));
+
+ if (unbalanced)
+ {
+ splitIndex = startIndex+ (numIndices>>1);
+ }
+
+ btAssert(!((splitIndex==startIndex) || (splitIndex == (endIndex))));
+
+ return splitIndex;
+
+}
+
+
+void btQuantizedBvhTree::_build_sub_tree(GIM_BVH_DATA_ARRAY & primitive_boxes, int startIndex, int endIndex)
+{
+ int curIndex = m_num_nodes;
+ m_num_nodes++;
+
+ btAssert((endIndex-startIndex)>0);
+
+ if ((endIndex-startIndex)==1)
+ {
+ //We have a leaf node
+ setNodeBound(curIndex,primitive_boxes[startIndex].m_bound);
+ m_node_array[curIndex].setDataIndex(primitive_boxes[startIndex].m_data);
+
+ return;
+ }
+ //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+
+ //split axis
+ int splitIndex = _calc_splitting_axis(primitive_boxes,startIndex,endIndex);
+
+ splitIndex = _sort_and_calc_splitting_index(
+ primitive_boxes,startIndex,endIndex,
+ splitIndex//split axis
+ );
+
+
+ //calc this node bounding box
+
+ btAABB node_bound;
+ node_bound.invalidate();
+
+ for (int i=startIndex;i<endIndex;i++)
+ {
+ node_bound.merge(primitive_boxes[i].m_bound);
+ }
+
+ setNodeBound(curIndex,node_bound);
+
+
+ //build left branch
+ _build_sub_tree(primitive_boxes, startIndex, splitIndex );
+
+
+ //build right branch
+ _build_sub_tree(primitive_boxes, splitIndex ,endIndex);
+
+ m_node_array[curIndex].setEscapeIndex(m_num_nodes - curIndex);
+
+
+}
+
+//! stackless build tree
+void btQuantizedBvhTree::build_tree(
+ GIM_BVH_DATA_ARRAY & primitive_boxes)
+{
+ calc_quantization(primitive_boxes);
+ // initialize node count to 0
+ m_num_nodes = 0;
+ // allocate nodes
+ m_node_array.resize(primitive_boxes.size()*2);
+
+ _build_sub_tree(primitive_boxes, 0, primitive_boxes.size());
+}
+
+////////////////////////////////////class btGImpactQuantizedBvh
+
+void btGImpactQuantizedBvh::refit()
+{
+ int nodecount = getNodeCount();
+ while(nodecount--)
+ {
+ if(isLeafNode(nodecount))
+ {
+ btAABB leafbox;
+ m_primitive_manager->get_primitive_box(getNodeData(nodecount),leafbox);
+ setNodeBound(nodecount,leafbox);
+ }
+ else
+ {
+ //const GIM_BVH_TREE_NODE * nodepointer = get_node_pointer(nodecount);
+ //get left bound
+ btAABB bound;
+ bound.invalidate();
+
+ btAABB temp_box;
+
+ int child_node = getLeftNode(nodecount);
+ if(child_node)
+ {
+ getNodeBound(child_node,temp_box);
+ bound.merge(temp_box);
+ }
+
+ child_node = getRightNode(nodecount);
+ if(child_node)
+ {
+ getNodeBound(child_node,temp_box);
+ bound.merge(temp_box);
+ }
+
+ setNodeBound(nodecount,bound);
+ }
+ }
+}
+
+//! this rebuild the entire set
+void btGImpactQuantizedBvh::buildSet()
+{
+ //obtain primitive boxes
+ GIM_BVH_DATA_ARRAY primitive_boxes;
+ primitive_boxes.resize(m_primitive_manager->get_primitive_count());
+
+ for (int i = 0;i<primitive_boxes.size() ;i++ )
+ {
+ m_primitive_manager->get_primitive_box(i,primitive_boxes[i].m_bound);
+ primitive_boxes[i].m_data = i;
+ }
+
+ m_box_tree.build_tree(primitive_boxes);
+}
+
+//! returns the indices of the primitives in the m_primitive_manager
+bool btGImpactQuantizedBvh::boxQuery(const btAABB & box, btAlignedObjectArray<int> & collided_results) const
+{
+ int curIndex = 0;
+ int numNodes = getNodeCount();
+
+ //quantize box
+
+ unsigned short quantizedMin[3];
+ unsigned short quantizedMax[3];
+
+ m_box_tree.quantizePoint(quantizedMin,box.m_min);
+ m_box_tree.quantizePoint(quantizedMax,box.m_max);
+
+
+ while (curIndex < numNodes)
+ {
+
+ //catch bugs in tree data
+
+ bool aabbOverlap = m_box_tree.testQuantizedBoxOverlapp(curIndex, quantizedMin,quantizedMax);
+ bool isleafnode = isLeafNode(curIndex);
+
+ if (isleafnode && aabbOverlap)
+ {
+ collided_results.push_back(getNodeData(curIndex));
+ }
+
+ if (aabbOverlap || isleafnode)
+ {
+ //next subnode
+ curIndex++;
+ }
+ else
+ {
+ //skip node
+ curIndex+= getEscapeNodeIndex(curIndex);
+ }
+ }
+ if(collided_results.size()>0) return true;
+ return false;
+}
+
+
+
+//! returns the indices of the primitives in the m_primitive_manager
+bool btGImpactQuantizedBvh::rayQuery(
+ const btVector3 & ray_dir,const btVector3 & ray_origin ,
+ btAlignedObjectArray<int> & collided_results) const
+{
+ int curIndex = 0;
+ int numNodes = getNodeCount();
+
+ while (curIndex < numNodes)
+ {
+ btAABB bound;
+ getNodeBound(curIndex,bound);
+
+ //catch bugs in tree data
+
+ bool aabbOverlap = bound.collide_ray(ray_origin,ray_dir);
+ bool isleafnode = isLeafNode(curIndex);
+
+ if (isleafnode && aabbOverlap)
+ {
+ collided_results.push_back(getNodeData( curIndex));
+ }
+
+ if (aabbOverlap || isleafnode)
+ {
+ //next subnode
+ curIndex++;
+ }
+ else
+ {
+ //skip node
+ curIndex+= getEscapeNodeIndex(curIndex);
+ }
+ }
+ if(collided_results.size()>0) return true;
+ return false;
+}
+
+
+SIMD_FORCE_INLINE bool _quantized_node_collision(
+ const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1,
+ const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+ int node0 ,int node1, bool complete_primitive_tests)
+{
+ btAABB box0;
+ boxset0->getNodeBound(node0,box0);
+ btAABB box1;
+ boxset1->getNodeBound(node1,box1);
+
+ return box0.overlapping_trans_cache(box1,trans_cache_1to0,complete_primitive_tests );
+// box1.appy_transform_trans_cache(trans_cache_1to0);
+// return box0.has_collision(box1);
+
+}
+
+
+//stackless recursive collision routine
+static void _find_quantized_collision_pairs_recursive(
+ const btGImpactQuantizedBvh * boxset0, const btGImpactQuantizedBvh * boxset1,
+ btPairSet * collision_pairs,
+ const BT_BOX_BOX_TRANSFORM_CACHE & trans_cache_1to0,
+ int node0, int node1, bool complete_primitive_tests)
+{
+
+
+
+ if( _quantized_node_collision(
+ boxset0,boxset1,trans_cache_1to0,
+ node0,node1,complete_primitive_tests) ==false) return;//avoid colliding internal nodes
+
+ if(boxset0->isLeafNode(node0))
+ {
+ if(boxset1->isLeafNode(node1))
+ {
+ // collision result
+ collision_pairs->push_pair(
+ boxset0->getNodeData(node0),boxset1->getNodeData(node1));
+ return;
+ }
+ else
+ {
+
+ //collide left recursive
+
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ node0,boxset1->getLeftNode(node1),false);
+
+ //collide right recursive
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ node0,boxset1->getRightNode(node1),false);
+
+
+ }
+ }
+ else
+ {
+ if(boxset1->isLeafNode(node1))
+ {
+
+ //collide left recursive
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getLeftNode(node0),node1,false);
+
+
+ //collide right recursive
+
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getRightNode(node0),node1,false);
+
+
+ }
+ else
+ {
+ //collide left0 left1
+
+
+
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getLeftNode(node0),boxset1->getLeftNode(node1),false);
+
+ //collide left0 right1
+
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getLeftNode(node0),boxset1->getRightNode(node1),false);
+
+
+ //collide right0 left1
+
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getRightNode(node0),boxset1->getLeftNode(node1),false);
+
+ //collide right0 right1
+
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ collision_pairs,trans_cache_1to0,
+ boxset0->getRightNode(node0),boxset1->getRightNode(node1),false);
+
+ }// else if node1 is not a leaf
+ }// else if node0 is not a leaf
+}
+
+
+void btGImpactQuantizedBvh::find_collision(const btGImpactQuantizedBvh * boxset0, const btTransform & trans0,
+ const btGImpactQuantizedBvh * boxset1, const btTransform & trans1,
+ btPairSet & collision_pairs)
+{
+
+ if(boxset0->getNodeCount()==0 || boxset1->getNodeCount()==0 ) return;
+
+ BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0;
+
+ trans_cache_1to0.calc_from_homogenic(trans0,trans1);
+
+#ifdef TRI_COLLISION_PROFILING
+ bt_begin_gim02_q_tree_time();
+#endif //TRI_COLLISION_PROFILING
+
+ _find_quantized_collision_pairs_recursive(
+ boxset0,boxset1,
+ &collision_pairs,trans_cache_1to0,0,0,true);
+#ifdef TRI_COLLISION_PROFILING
+ bt_end_gim02_q_tree_time();
+#endif //TRI_COLLISION_PROFILING
+
+}
+
+