1 |
using System; |
2 |
using System.Collections.Generic; |
3 |
|
4 |
namespace Oni.Akira |
5 |
{ |
6 |
internal class OctreeNode |
7 |
{ |
8 |
public const int FaceCount = 6; |
9 |
public const int ChildCount = 8; |
10 |
|
11 |
private const float MinNodeSize = 16.0f; |
12 |
private const int MaxQuadsPerLeaf = 4096; |
13 |
private const int MaxRoomsPerLeaf = 255; |
14 |
|
15 |
#region Private data |
16 |
private int index; |
17 |
private BoundingBox bbox; |
18 |
private Polygon[] polygons; |
19 |
private OctreeNode[] children; |
20 |
private OctreeNode[] adjacency = new OctreeNode[FaceCount]; |
21 |
private Room[] rooms; |
22 |
#endregion |
23 |
|
24 |
#region public enum Axis |
25 |
|
26 |
public enum Axis |
27 |
{ |
28 |
Z, |
29 |
Y, |
30 |
X |
31 |
} |
32 |
|
33 |
#endregion |
34 |
#region public enum Direction |
35 |
|
36 |
public enum Direction |
37 |
{ |
38 |
Negative, |
39 |
Positive |
40 |
} |
41 |
|
42 |
#endregion |
43 |
#region public struct Face |
44 |
|
45 |
public struct Face |
46 |
{ |
47 |
private readonly int index; |
48 |
|
49 |
public Face(int index) |
50 |
{ |
51 |
this.index = index; |
52 |
} |
53 |
|
54 |
public int Index => index; |
55 |
public Axis Axis => (Axis)(2 - ((index & 6) >> 1)); |
56 |
public Direction Direction => (Direction)(index & 1); |
57 |
|
58 |
public static IEnumerable<Face> All |
59 |
{ |
60 |
get |
61 |
{ |
62 |
for (int i = 0; i < FaceCount; i++) |
63 |
yield return new Face(i); |
64 |
} |
65 |
} |
66 |
} |
67 |
|
68 |
#endregion |
69 |
#region public struct ChildPosition |
70 |
|
71 |
public struct ChildPosition |
72 |
{ |
73 |
private int index; |
74 |
|
75 |
public ChildPosition(int index) |
76 |
{ |
77 |
this.index = index; |
78 |
} |
79 |
|
80 |
public int Index => index; |
81 |
|
82 |
public int X => this[Axis.X]; |
83 |
public int Y => this[Axis.Y]; |
84 |
public int Z => this[Axis.Z]; |
85 |
|
86 |
public int this[Axis axis] |
87 |
{ |
88 |
get |
89 |
{ |
90 |
return ((index >> (int)axis) & 1); |
91 |
} |
92 |
set |
93 |
{ |
94 |
int mask = (1 << (int)axis); |
95 |
|
96 |
if (value == 0) |
97 |
index &= ~mask; |
98 |
else |
99 |
index |= mask; |
100 |
} |
101 |
} |
102 |
|
103 |
public static IEnumerable<ChildPosition> All |
104 |
{ |
105 |
get |
106 |
{ |
107 |
for (int i = 0; i < 8; i++) |
108 |
yield return new ChildPosition(i); |
109 |
} |
110 |
} |
111 |
} |
112 |
|
113 |
#endregion |
114 |
|
115 |
public OctreeNode(BoundingBox bbox, IEnumerable<Polygon> polygons, IEnumerable<Room> rooms) |
116 |
{ |
117 |
this.bbox = bbox; |
118 |
this.polygons = polygons.ToArray(); |
119 |
this.rooms = rooms.ToArray(); |
120 |
} |
121 |
|
122 |
private OctreeNode(BoundingBox bbox, Polygon[] polygons, Room[] rooms) |
123 |
{ |
124 |
this.bbox = bbox; |
125 |
this.polygons = polygons; |
126 |
this.rooms = rooms; |
127 |
} |
128 |
|
129 |
public int Index |
130 |
{ |
131 |
get { return index; } |
132 |
set { index = value; } |
133 |
} |
134 |
|
135 |
public BoundingBox BoundingBox => bbox; |
136 |
public OctreeNode[] Children => children; |
137 |
public OctreeNode[] Adjacency => adjacency; |
138 |
public bool IsLeaf => polygons != null; |
139 |
public ICollection<Polygon> Polygons => polygons; |
140 |
public ICollection<Room> Rooms => rooms; |
141 |
private Vector3 Center => (bbox.Min + bbox.Max) * 0.5f; |
142 |
private float Size => bbox.Max.X - bbox.Min.X; |
143 |
|
144 |
public void Build() |
145 |
{ |
146 |
BuildRecursive(); |
147 |
|
148 |
// |
149 |
// Force a split of the root node because the root cannot be a leaf. |
150 |
// |
151 |
|
152 |
if (children == null) |
153 |
Split(); |
154 |
} |
155 |
|
156 |
private void BuildRecursive() |
157 |
{ |
158 |
if ((polygons == null || polygons.Length <= 19) && (rooms == null || rooms.Length < 16)) |
159 |
{ |
160 |
return; |
161 |
} |
162 |
|
163 |
if (Size <= MinNodeSize) |
164 |
{ |
165 |
if (polygons.Length > MaxQuadsPerLeaf) |
166 |
throw new NotSupportedException(string.Format("Octtree: Quad density too big: current {0} max 4096 bbox {1}", polygons.Length, BoundingBox)); |
167 |
|
168 |
if (rooms.Length > MaxRoomsPerLeaf) |
169 |
throw new NotSupportedException(string.Format("Octtree: Room density too big: current {0} max 255 bbox {1}", rooms.Length, BoundingBox)); |
170 |
|
171 |
return; |
172 |
} |
173 |
|
174 |
Split(); |
175 |
} |
176 |
|
177 |
private void Split() |
178 |
{ |
179 |
children = SplitCore(); |
180 |
polygons = null; |
181 |
rooms = null; |
182 |
|
183 |
BuildSimpleAdjaceny(); |
184 |
|
185 |
foreach (var child in children) |
186 |
child.BuildRecursive(); |
187 |
} |
188 |
|
189 |
private OctreeNode[] SplitCore() |
190 |
{ |
191 |
var children = new OctreeNode[ChildCount]; |
192 |
var center = Center; |
193 |
var childPolygons = new List<Polygon>(polygons.Length); |
194 |
var childRooms = new List<Room>(rooms.Length); |
195 |
|
196 |
foreach (var position in ChildPosition.All) |
197 |
{ |
198 |
var childNodeBBox = new BoundingBox(center, center); |
199 |
|
200 |
if (position.X == 0) |
201 |
childNodeBBox.Min.X = bbox.Min.X; |
202 |
else |
203 |
childNodeBBox.Max.X = bbox.Max.X; |
204 |
|
205 |
if (position.Y == 0) |
206 |
childNodeBBox.Min.Y = bbox.Min.Y; |
207 |
else |
208 |
childNodeBBox.Max.Y = bbox.Max.Y; |
209 |
|
210 |
if (position.Z == 0) |
211 |
childNodeBBox.Min.Z = bbox.Min.Z; |
212 |
else |
213 |
childNodeBBox.Max.Z = bbox.Max.Z; |
214 |
|
215 |
childPolygons.Clear(); |
216 |
childRooms.Clear(); |
217 |
|
218 |
var boxIntersector = new PolygonBoxIntersector(ref childNodeBBox); |
219 |
|
220 |
foreach (var polygon in polygons) |
221 |
{ |
222 |
if (boxIntersector.Intersects(polygon)) |
223 |
childPolygons.Add(polygon); |
224 |
} |
225 |
|
226 |
foreach (var room in rooms) |
227 |
{ |
228 |
if (room.Intersect(childNodeBBox)) |
229 |
childRooms.Add(room); |
230 |
} |
231 |
|
232 |
children[position.Index] = new OctreeNode(childNodeBBox, childPolygons.ToArray(), childRooms.ToArray()); |
233 |
} |
234 |
|
235 |
return children; |
236 |
} |
237 |
|
238 |
private void BuildSimpleAdjaceny() |
239 |
{ |
240 |
foreach (ChildPosition position in ChildPosition.All) |
241 |
{ |
242 |
var child = children[position.Index]; |
243 |
|
244 |
foreach (var face in Face.All) |
245 |
child.Adjacency[face.Index] = GetChildAdjacency(position, face); |
246 |
} |
247 |
} |
248 |
|
249 |
private OctreeNode GetChildAdjacency(ChildPosition position, Face face) |
250 |
{ |
251 |
if (face.Direction == Direction.Positive) |
252 |
{ |
253 |
if (position[face.Axis] == 0) |
254 |
{ |
255 |
position[face.Axis] = 1; |
256 |
return children[position.Index]; |
257 |
} |
258 |
} |
259 |
else |
260 |
{ |
261 |
if (position[face.Axis] == 1) |
262 |
{ |
263 |
position[face.Axis] = 0; |
264 |
return children[position.Index]; |
265 |
} |
266 |
} |
267 |
|
268 |
return adjacency[face.Index]; |
269 |
} |
270 |
|
271 |
public void RefineAdjacency() |
272 |
{ |
273 |
Vector3 center = Center; |
274 |
float size = Size; |
275 |
|
276 |
foreach (var face in Face.All) |
277 |
{ |
278 |
var node = adjacency[face.Index]; |
279 |
|
280 |
if (node != null && !node.IsLeaf && node.Size > Size) |
281 |
{ |
282 |
Vector3 adjacentCenter = MovePoint(center, face, size); |
283 |
adjacency[face.Index] = node.FindLargestOrEqual(adjacentCenter, size); |
284 |
} |
285 |
} |
286 |
} |
287 |
|
288 |
public QuadtreeNode BuildFaceQuadTree(Face face) |
289 |
{ |
290 |
Vector3 faceCenter = MovePoint(Center, face, Size * 0.5f); |
291 |
var quadTreeNode = new QuadtreeNode(faceCenter, Size, face); |
292 |
quadTreeNode.Build(adjacency[face.Index]); |
293 |
return quadTreeNode; |
294 |
} |
295 |
|
296 |
public void DfsTraversal(Action<OctreeNode> action) |
297 |
{ |
298 |
action(this); |
299 |
|
300 |
if (!IsLeaf) |
301 |
{ |
302 |
foreach (var child in children) |
303 |
child.DfsTraversal(action); |
304 |
} |
305 |
} |
306 |
|
307 |
public static Vector3 MovePoint(Vector3 point, Face face, float delta) |
308 |
{ |
309 |
if (face.Direction == Direction.Negative) |
310 |
delta = -delta; |
311 |
|
312 |
if (face.Axis == Axis.X) |
313 |
point.X += delta; |
314 |
else if (face.Axis == Axis.Y) |
315 |
point.Y += delta; |
316 |
else |
317 |
point.Z += delta; |
318 |
|
319 |
return point; |
320 |
} |
321 |
|
322 |
private struct TriangleBoxIntersector |
323 |
{ |
324 |
private Vector3 center; |
325 |
private Vector3 size; |
326 |
private Vector3[] triangle; |
327 |
private Vector3 edge; |
328 |
|
329 |
public TriangleBoxIntersector(ref BoundingBox box) |
330 |
{ |
331 |
center = (box.Min + box.Max) * 0.5f; |
332 |
size = (box.Max - box.Min) * 0.5f; |
333 |
|
334 |
triangle = new Vector3[3]; |
335 |
edge = Vector3.Zero; |
336 |
} |
337 |
|
338 |
public Vector3[] Triangle => triangle; |
339 |
|
340 |
public bool Intersect() |
341 |
{ |
342 |
for (int i = 0; i < triangle.Length; i++) |
343 |
triangle[i] -= center; |
344 |
|
345 |
edge = triangle[1] - triangle[0]; |
346 |
|
347 |
if (AxisTest(Y, Z, 0, 2) || AxisTest(Z, X, 0, 2) || AxisTest(X, Y, 2, 1)) |
348 |
return false; |
349 |
|
350 |
edge = triangle[2] - triangle[1]; |
351 |
|
352 |
if (AxisTest(Y, Z, 0, 2) || AxisTest(Z, X, 0, 2) || AxisTest(X, Y, 0, 1)) |
353 |
return false; |
354 |
|
355 |
edge = triangle[0] - triangle[2]; |
356 |
|
357 |
if (AxisTest(Y, Z, 0, 1) || AxisTest(Z, X, 0, 1) || AxisTest(X, Y, 2, 1)) |
358 |
return false; |
359 |
|
360 |
return true; |
361 |
} |
362 |
|
363 |
private const int X = 0; |
364 |
private const int Y = 1; |
365 |
private const int Z = 2; |
366 |
|
367 |
private bool AxisTest(int a1, int a2, int p0, int p1) |
368 |
{ |
369 |
Vector3 v0 = triangle[p0]; |
370 |
Vector3 v1 = triangle[p1]; |
371 |
float e1 = edge[a1]; |
372 |
float e2 = edge[a2]; |
373 |
|
374 |
float c0 = e2 * v0[a1] - e1 * v0[a2]; |
375 |
float c1 = e2 * v1[a1] - e1 * v1[a2]; |
376 |
float rad = Math.Abs(e2) * size[a1] + Math.Abs(e1) * size[a2]; |
377 |
|
378 |
return (c0 < c1) ? (c0 > rad || c1 < -rad) : (c1 > rad || c0 < -rad); |
379 |
} |
380 |
} |
381 |
|
382 |
private struct PolygonBoxIntersector |
383 |
{ |
384 |
private BoundingBox bbox; |
385 |
private TriangleBoxIntersector triangleBoxIntersector; |
386 |
|
387 |
public PolygonBoxIntersector(ref BoundingBox bbox) |
388 |
{ |
389 |
this.bbox = bbox; |
390 |
this.triangleBoxIntersector = new TriangleBoxIntersector(ref bbox); |
391 |
} |
392 |
|
393 |
public bool Intersects(Polygon polygon) |
394 |
{ |
395 |
if (!bbox.Intersects(polygon.BoundingBox)) |
396 |
return false; |
397 |
|
398 |
if (!bbox.Intersects(polygon.Plane)) |
399 |
return false; |
400 |
|
401 |
var intersector = new TriangleBoxIntersector(ref bbox); |
402 |
var points = polygon.Mesh.Points; |
403 |
var indices = polygon.PointIndices; |
404 |
|
405 |
intersector.Triangle[0] = points[indices[0]]; |
406 |
intersector.Triangle[1] = points[indices[1]]; |
407 |
intersector.Triangle[2] = points[indices[2]]; |
408 |
|
409 |
if (intersector.Intersect()) |
410 |
return true; |
411 |
|
412 |
if (indices.Length > 3) |
413 |
{ |
414 |
intersector.Triangle[0] = points[indices[2]]; |
415 |
intersector.Triangle[1] = points[indices[3]]; |
416 |
intersector.Triangle[2] = points[indices[0]]; |
417 |
|
418 |
if (intersector.Intersect()) |
419 |
return true; |
420 |
} |
421 |
|
422 |
return false; |
423 |
} |
424 |
} |
425 |
|
426 |
public OctreeNode FindLargestOrEqual(Vector3 point, float largestSize) |
427 |
{ |
428 |
var node = this; |
429 |
|
430 |
while (!node.IsLeaf && node.Size > largestSize) |
431 |
{ |
432 |
Vector3 center = node.Center; |
433 |
|
434 |
int nx = (point.X < center.X) ? 0 : 4; |
435 |
int ny = (point.Y < center.Y) ? 0 : 2; |
436 |
int nz = (point.Z < center.Z) ? 0 : 1; |
437 |
|
438 |
var childNode = node.children[nx + ny + nz]; |
439 |
|
440 |
if (childNode.Size < largestSize) |
441 |
break; |
442 |
|
443 |
node = childNode; |
444 |
} |
445 |
|
446 |
return node; |
447 |
} |
448 |
|
449 |
public OctreeNode FindLeaf(Vector3 point) |
450 |
{ |
451 |
if (!bbox.Contains(point)) |
452 |
return null; |
453 |
|
454 |
if (children == null) |
455 |
return this; |
456 |
|
457 |
Vector3 center = Center; |
458 |
|
459 |
int nx = (point.X < center.X) ? 0 : 4; |
460 |
int ny = (point.Y < center.Y) ? 0 : 2; |
461 |
int nz = (point.Z < center.Z) ? 0 : 1; |
462 |
|
463 |
OctreeNode childNode = children[nx + ny + nz]; |
464 |
|
465 |
return childNode.FindLeaf(point); |
466 |
} |
467 |
|
468 |
public IEnumerable<OctreeNode> FindLeafs(BoundingBox box) |
469 |
{ |
470 |
var stack = new Stack<OctreeNode>(); |
471 |
stack.Push(this); |
472 |
|
473 |
while (stack.Count > 0) |
474 |
{ |
475 |
var node = stack.Pop(); |
476 |
|
477 |
if (node.bbox.Intersects(box)) |
478 |
{ |
479 |
if (node.children != null) |
480 |
{ |
481 |
foreach (OctreeNode child in node.children) |
482 |
stack.Push(child); |
483 |
} |
484 |
else |
485 |
{ |
486 |
yield return node; |
487 |
} |
488 |
} |
489 |
} |
490 |
} |
491 |
} |
492 |
} |