Panda3D
physxMeshHash.cxx
1 // Filename: physxMeshHash.cxx
2 // Created by: enn0x (13Sep10)
3 //
4 ////////////////////////////////////////////////////////////////////
5 //
6 // PANDA 3D SOFTWARE
7 // Copyright (c) Carnegie Mellon University. All rights reserved.
8 //
9 // All use of this software is subject to the terms of the revised BSD
10 // license. You should have received a copy of this license along
11 // with this source code in a file named "LICENSE."
12 //
13 ////////////////////////////////////////////////////////////////////
14 
15 #include "physxMeshHash.h"
16 
17 ////////////////////////////////////////////////////////////////////
18 // Function: PhysxMeshHash::quick_sort
19 // Access: Private
20 // Description:
21 ////////////////////////////////////////////////////////////////////
22 void PhysxMeshHash::
23 quick_sort(pvector<int> &itemIndices, int l, int r) {
24 
25  int i, j, mi;
26  int k, m;
27 
28  i = l; j = r; mi = (l + r)/2;
29  m = itemIndices[mi];
30 
31  while (i <= j) {
32  while(itemIndices[i] < m) i++;
33  while(m < itemIndices[j]) j--;
34 
35  if (i <= j) {
36  k = itemIndices[i]; itemIndices[i] = itemIndices[j]; itemIndices[j] = k;
37  i++; j--;
38  }
39  }
40 
41  if (l < j) quick_sort(itemIndices, l, j);
42  if (i < r) quick_sort(itemIndices, i, r);
43 }
44 
45 ////////////////////////////////////////////////////////////////////
46 // Function: PhysxMeshHash::compress_indices
47 // Access: Private
48 // Description:
49 ////////////////////////////////////////////////////////////////////
50 void PhysxMeshHash::
51 compress_indices(pvector<int> &itemIndices) {
52 
53  if (itemIndices.size() == 0) return;
54 
55  // Sort results
56  quick_sort(itemIndices, 0, itemIndices.size() - 1);
57 
58  // Mark duplicates
59  int i = 0;
60  while (i < (int)itemIndices.size()) {
61  int j = i+1;
62  while (j < (int)itemIndices.size() && itemIndices[i] == itemIndices[j]) {
63  itemIndices[j] = -1; j++;
64  }
65  i = j;
66  }
67 
68  // Remove duplicates
69  i = 0;
70  while (i < (int)itemIndices.size()) {
71  if (itemIndices[i] < 0) {
72  itemIndices[i] = itemIndices[itemIndices.size()-1];
73  itemIndices.pop_back();
74  }
75  else i++;
76  }
77 }
78 
79 ////////////////////////////////////////////////////////////////////
80 // Function: PhysxMeshHash::set_grid_spacing
81 // Access: Public
82 // Description:
83 ////////////////////////////////////////////////////////////////////
84 void PhysxMeshHash::
85 set_grid_spacing(float spacing) {
86 
87  _spacing = spacing;
88  _invSpacing = 1.0f / spacing;
89 
90  reset();
91 }
92 
93 ////////////////////////////////////////////////////////////////////
94 // Function: PhysxMeshHash::reset
95 // Access: Public
96 // Description:
97 ////////////////////////////////////////////////////////////////////
98 void PhysxMeshHash::
99 reset() {
100 
101  _time++;
102  _entries.clear();
103 }
104 
105 ////////////////////////////////////////////////////////////////////
106 // Function: PhysxMeshHash::add
107 // Access: Public
108 // Description:
109 ////////////////////////////////////////////////////////////////////
110 void PhysxMeshHash::
111 add(const NxBounds3 &bounds, int itemIndex) {
112 
113  int x1,y1,z1;
114  int x2,y2,z2;
115  int x,y,z;
116 
117  cell_coord_of(bounds.min, x1, y1, z1);
118  cell_coord_of(bounds.max, x2, y2, z2);
119 
120  MeshHashEntry entry;
121  entry.itemIndex = itemIndex;
122 
123  for (x = x1; x <= x2; x++) {
124  for (y = y1; y <= y2; y++) {
125  for (z = z1; z <= z2; z++) {
126 
127  int h = hash_function(x, y, z);
128  MeshHashRoot &r = _hashIndex[h];
129  int n = _entries.size();
130 
131  if (r.timeStamp != _time || r.first < 0)
132  entry.next = -1;
133  else
134  entry.next = r.first;
135 
136  r.first = n;
137  r.timeStamp = _time;
138 
139  _entries.push_back(entry);
140  }
141  }
142  }
143 }
144 
145 ////////////////////////////////////////////////////////////////////
146 // Function: PhysxMeshHash::add
147 // Access: Public
148 // Description:
149 ////////////////////////////////////////////////////////////////////
150 void PhysxMeshHash::
151 add(const NxVec3 &pos, int itemIndex) {
152 
153  int x, y, z;
154 
155  cell_coord_of(pos, x, y, z);
156 
157  MeshHashEntry entry;
158  entry.itemIndex = itemIndex;
159 
160  int h = hash_function(x, y, z);
161  MeshHashRoot &r = _hashIndex[h];
162  int n = _entries.size();
163 
164  if (r.timeStamp != _time || r.first < 0)
165  entry.next = -1;
166  else
167  entry.next = r.first;
168 
169  r.first = n;
170  r.timeStamp = _time;
171 
172  _entries.push_back(entry);
173 }
174 
175 ////////////////////////////////////////////////////////////////////
176 // Function: PhysxMeshHash::query
177 // Access: Public
178 // Description:
179 ////////////////////////////////////////////////////////////////////
180 void PhysxMeshHash::
181 query(const NxBounds3 &bounds, pvector<int> &itemIndices, int maxIndices) {
182 
183  int x1, y1, z1;
184  int x2, y2, z2;
185  int x, y, z;
186 
187  cell_coord_of(bounds.min, x1, y1, z1);
188  cell_coord_of(bounds.max, x2, y2, z2);
189 
190  itemIndices.clear();
191 
192  for (x=x1; x<=x2; x++) {
193  for (y=y1; y<=y2; y++) {
194  for (z=z1; z<=z2; z++) {
195 
196  int h = hash_function(x, y, z);
197 
198  MeshHashRoot &r = _hashIndex[h];
199  if (r.timeStamp != _time) continue;
200  int i = r.first;
201 
202  while (i >= 0) {
203  MeshHashEntry &entry = _entries[i];
204  itemIndices.push_back(entry.itemIndex);
205  if (maxIndices >= 0 && (int)itemIndices.size() >= maxIndices) return;
206  i = entry.next;
207  }
208  }
209  }
210  }
211 }
212 
213 ////////////////////////////////////////////////////////////////////
214 // Function: PhysxMeshHash::query_unique
215 // Access: Public
216 // Description:
217 ////////////////////////////////////////////////////////////////////
218 void PhysxMeshHash::
219 query_unique(const NxBounds3 &bounds, pvector<int> &itemIndices, int maxIndices) {
220 
221  query(bounds, itemIndices, maxIndices);
222  compress_indices(itemIndices);
223 }
224 
225 ////////////////////////////////////////////////////////////////////
226 // Function: PhysxMeshHash::query
227 // Access: Public
228 // Description:
229 ////////////////////////////////////////////////////////////////////
230 void PhysxMeshHash::
231 query(const NxVec3 &pos, pvector<int> &itemIndices, int maxIndices) {
232 
233  int x, y, z;
234 
235  cell_coord_of(pos, x, y, z);
236 
237  itemIndices.clear();
238 
239  int h = hash_function(x, y, z);
240  MeshHashRoot &r = _hashIndex[h];
241  if (r.timeStamp != _time) return;
242  int i = r.first;
243 
244  while (i >= 0) {
245  MeshHashEntry &entry = _entries[i];
246  itemIndices.push_back(entry.itemIndex);
247  if (maxIndices >= 0 && (int)itemIndices.size() >= maxIndices) return;
248  i = entry.next;
249  }
250 }
251 
252 ////////////////////////////////////////////////////////////////////
253 // Function: PhysxMeshHash::query_unique
254 // Access: Public
255 // Description:
256 ////////////////////////////////////////////////////////////////////
257 void PhysxMeshHash::
258 query_unique(const NxVec3 &pos, pvector<int> &itemIndices, int maxIndices) {
259 
260  query(pos, itemIndices, maxIndices);
261  compress_indices(itemIndices);
262 }
263