| 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 | 
 } |