Fixed several bugs: python refcounting related and Bullet related (basic add/remove...
[blender.git] / extern / bullet / Bullet / CollisionShapes / OptimizedBvh.cpp
1 /*
2  * Copyright (c) 2006 Erwin Coumans http://continuousphysics.com/Bullet/
3  *
4  * Permission to use, copy, modify, distribute and sell this software
5  * and its documentation for any purpose is hereby granted without fee,
6  * provided that the above copyright notice appear in all copies.
7  * Erwin Coumans makes no representations about the suitability 
8  * of this software for any purpose.  
9  * It is provided "as is" without express or implied warranty.
10 */
11
12 #include "OptimizedBvh.h"
13 #include "StridingMeshInterface.h"
14 #include "AabbUtil2.h"
15
16
17
18 void OptimizedBvh::Build(StridingMeshInterface* triangles)
19 {
20         int countTriangles = 0;
21
22         
23
24         // NodeArray    triangleNodes;
25
26         struct  NodeTriangleCallback : public InternalTriangleIndexCallback
27         {
28                 NodeArray&      m_triangleNodes;
29
30                 NodeTriangleCallback(NodeArray& triangleNodes)
31                         :m_triangleNodes(triangleNodes)
32                 {
33
34                 }
35
36                 virtual void InternalProcessTriangleIndex(SimdVector3* triangle,int partId,int  triangleIndex)
37                 {
38
39                         OptimizedBvhNode node;
40                         node.m_aabbMin = SimdVector3(1e30f,1e30f,1e30f); 
41                         node.m_aabbMax = SimdVector3(-1e30f,-1e30f,-1e30f); 
42                         node.m_aabbMin.setMin(triangle[0]);
43                         node.m_aabbMax.setMax(triangle[0]);
44                         node.m_aabbMin.setMin(triangle[1]);
45                         node.m_aabbMax.setMax(triangle[1]);
46                         node.m_aabbMin.setMin(triangle[2]);
47                         node.m_aabbMax.setMax(triangle[2]);
48
49                         node.m_escapeIndex = -1;
50                         node.m_leftChild = 0;
51                         node.m_rightChild = 0;
52
53
54                         //for child nodes
55                         node.m_subPart = partId;
56                         node.m_triangleIndex = triangleIndex;
57
58                         
59                         m_triangleNodes.push_back(node);
60                 }
61         };
62
63         
64
65         NodeTriangleCallback    callback(m_leafNodes);
66
67         SimdVector3 aabbMin(-1e30f,-1e30f,-1e30f);
68         SimdVector3 aabbMax(1e30f,1e30f,1e30f);
69
70         triangles->InternalProcessAllTriangles(&callback,aabbMin,aabbMax);
71
72         //now we have an array of leafnodes in m_leafNodes
73
74         m_contiguousNodes = new OptimizedBvhNode[2*m_leafNodes.size()];
75         m_curNodeIndex = 0;
76
77         m_rootNode1 = BuildTree(m_leafNodes,0,m_leafNodes.size());
78
79
80         ///create the leafnodes first
81 //      OptimizedBvhNode* leafNodes = new OptimizedBvhNode;
82 }
83
84
85 OptimizedBvhNode*       OptimizedBvh::BuildTree (NodeArray&     leafNodes,int startIndex,int endIndex)
86 {
87
88         int numIndices =endIndex-startIndex;
89         assert(numIndices>0);
90
91         int curIndex = m_curNodeIndex;
92
93         if (numIndices==1)
94         {
95                 return new (&m_contiguousNodes[m_curNodeIndex++]) OptimizedBvhNode(leafNodes[startIndex]);
96         }
97         //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
98         
99         int splitAxis = CalcSplittingAxis(leafNodes,startIndex,endIndex);
100
101         int splitIndex = SortAndCalcSplittingIndex(leafNodes,startIndex,endIndex,splitAxis);
102
103         OptimizedBvhNode* internalNode = &m_contiguousNodes[m_curNodeIndex++];
104         
105         internalNode->m_aabbMax.setValue(-1e30f,-1e30f,-1e30f);
106         internalNode->m_aabbMin.setValue(1e30f,1e30f,1e30f);
107         
108         for (int i=startIndex;i<endIndex;i++)
109         {
110                 internalNode->m_aabbMax.setMax(leafNodes[i].m_aabbMax);
111                 internalNode->m_aabbMin.setMin(leafNodes[i].m_aabbMin);
112         }
113
114         
115
116         //internalNode->m_escapeIndex;
117         internalNode->m_leftChild = BuildTree(leafNodes,startIndex,splitIndex);
118         internalNode->m_rightChild = BuildTree(leafNodes,splitIndex,endIndex);
119
120         internalNode->m_escapeIndex  = m_curNodeIndex - curIndex;
121         return internalNode;
122 }
123
124 int     OptimizedBvh::SortAndCalcSplittingIndex(NodeArray&      leafNodes,int startIndex,int endIndex,int splitAxis)
125 {
126         int splitIndex =startIndex;
127         int numIndices = endIndex - startIndex;
128
129         SimdVector3 means(0.f,0.f,0.f);
130         for (int i=startIndex;i<endIndex;i++)
131         {
132                 SimdVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
133                 means+=center;
134         }
135         means *= (1.f/(float)numIndices);
136         
137         float splitValue = means[splitAxis];
138         
139         //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
140         for (int i=startIndex;i<endIndex;i++)
141         {
142                 SimdVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
143                 if (center[splitAxis] > splitValue)
144                 {
145                         //swap
146                         OptimizedBvhNode tmp = leafNodes[i];
147                         leafNodes[i] = leafNodes[splitIndex];
148                         leafNodes[splitIndex] = tmp;
149                         splitIndex++;
150                 }
151         }
152         if ((splitIndex==startIndex) || (splitIndex == (endIndex-1)))
153         {
154                 splitIndex = startIndex+ (numIndices>>1);
155         }
156         return splitIndex;
157 }
158
159
160 int     OptimizedBvh::CalcSplittingAxis(NodeArray&      leafNodes,int startIndex,int endIndex)
161 {
162         SimdVector3 means(0.f,0.f,0.f);
163         int numIndices = endIndex-startIndex;
164
165         for (int i=startIndex;i<endIndex;i++)
166         {
167                 SimdVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
168                 means+=center;
169         }
170         means *= (1.f/(float)numIndices);
171                 
172         SimdVector3 variance(0.f,0.f,0.f);
173
174         for (int i=startIndex;i<endIndex;i++)
175         {
176                 SimdVector3 center = 0.5f*(leafNodes[i].m_aabbMax+leafNodes[i].m_aabbMin);
177                 SimdVector3 diff2 = center-means;
178                 diff2 = diff2 * diff2;
179                 variance += diff2;
180         }
181         variance *= (1.f/       ((float)numIndices-1)   );
182         
183         int biggestAxis = variance.maxAxis();
184         return biggestAxis;
185
186 }
187
188
189         
190 void    OptimizedBvh::ReportAabbOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
191 {
192         if (aabbMin.length() > 1000.f)
193         {
194                 for (int i=0;i<m_leafNodes.size();i++)
195                 {
196                         const OptimizedBvhNode& node = m_leafNodes[i];
197                         nodeCallback->ProcessNode(&node);
198                 }
199         } else
200         {
201                 //WalkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
202                 WalkStacklessTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
203         }
204 }
205
206 void    OptimizedBvh::WalkTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
207 {
208         bool aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax);
209         if (aabbOverlap)
210         {
211                 bool isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild);
212                 if (isLeafNode)
213                 {
214                         nodeCallback->ProcessNode(rootNode);
215                 } else
216                 {
217                         WalkTree(rootNode->m_leftChild,nodeCallback,aabbMin,aabbMax);
218                         WalkTree(rootNode->m_rightChild,nodeCallback,aabbMin,aabbMax);
219                 }
220         }
221
222 }
223
224 int maxIterations = 0;
225
226 void    OptimizedBvh::WalkStacklessTree(OptimizedBvhNode* rootNode,NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
227 {
228         int curIndex = 0;
229         int walkIterations = 0;
230
231         while (curIndex < m_curNodeIndex)
232         {
233                 //catch bugs in tree data
234                 assert (walkIterations < m_curNodeIndex);
235
236                 walkIterations++;
237                 bool aabbOverlap = TestAabbAgainstAabb2(aabbMin,aabbMax,rootNode->m_aabbMin,rootNode->m_aabbMax);
238                 bool isLeafNode = (!rootNode->m_leftChild && !rootNode->m_rightChild);
239                 
240                 if (isLeafNode && aabbOverlap)
241                 {
242                         nodeCallback->ProcessNode(rootNode);
243                 } 
244                 
245                 if (aabbOverlap || isLeafNode)
246                 {
247                         rootNode++;
248                         curIndex++;
249                 } else
250                 {
251                         int escapeIndex = rootNode->m_escapeIndex;
252                         rootNode += escapeIndex;
253                         curIndex += escapeIndex;
254                 }
255                 
256         }
257
258         if (maxIterations < walkIterations)
259                 maxIterations = walkIterations;
260
261 }
262
263
264 void    OptimizedBvh::ReportSphereOverlappingNodex(NodeOverlapCallback* nodeCallback,const SimdVector3& aabbMin,const SimdVector3& aabbMax) const
265 {
266
267 }
268