Octree

public class Game1 : Microsoft.Xna.Framework.Game
{
 private GraphicsDeviceManager _graphics;
 private SpriteBatch _spriteBatch;
 private SpriteFont _font;
 private FpsCounter _fpsCounter; 
 
 public Game1()
 {
  _graphics = new GraphicsDeviceManager(this);
  _graphics.SynchronizeWithVerticalRetrace = false;
  this.IsFixedTimeStep = false;
  Content.RootDirectory = "Content";
  _fpsCounter = new FpsCounter();
 }
 
 protected override void Initialize()
 {
  base.Initialize();
 }
 
 protected override void LoadContent()
 {
  _spriteBatch = new SpriteBatch(GraphicsDevice);
  _font = Content.Load<SpriteFont>("Arial");
 }
 
 protected override void UnloadContent()
 {
  Content.Unload();
 }
 
 protected override void Update(GameTime gameTime)
 {
  if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed ||
    Keyboard.GetState().IsKeyDown(Keys.Escape))
   this.Exit();
 
  _fpsCounter.Update(gameTime);
 
  base.Update(gameTime);
 }
 
 protected override void Draw(GameTime gameTime)
 {
  GraphicsDevice.Clear(Color.CornflowerBlue);
 
  _spriteBatch.Begin();
  _spriteBatch.DrawString(_font, string.Format("FPS: {0} Frame time: {1}", _fpsCounter.FramesPerSecond, _fpsCounter.FrameTime), Vector2.Zero, Color.White);
  _spriteBatch.End();
 
  base.Draw(gameTime);
 }
}

public class GameEntity
{
 public Matrix World;
 
 public BoundingBox OriginalBoundingBox;
 
 public BoundingBox TransformedBoundingBox;
 
 public Model Model { get; set; }
}

private Camera _camera;
private List<GameEntity> _entities;
private int _modelsCount;
...
 
public Game1()
{
 _graphics = new GraphicsDeviceManager(this);
 _graphics.SynchronizeWithVerticalRetrace = false;
 this.IsFixedTimeStep = false;
 Content.RootDirectory = "Content";
 
 _fpsCounter = new FpsCounter();
 _camera = new Camera();
 _camera.RotationSpeed = 10;
 _camera.MovingSpeed = 10;
 _entities = new List<GameEntity>();
 _modelsCount = 5000;
}
 
protected override void LoadContent()
{
 _spriteBatch = new SpriteBatch(GraphicsDevice);
 _font = Content.Load<SpriteFont>("Arial");
 Model model = Content.Load<Model>("cats");
 //создаем игровые сущности и располагаем их в случайном порядке в пределах куба {-50, -50, -50; 50, 50, 50}
 Random r = new Random((int)DateTime.Now.Ticks);
 for (int i = 0; i < _modelsCount; i++)
 {
  GameEntity entity = new GameEntity();
  entity.Model = model;
  entity.World = Matrix.CreateTranslation(new Vector3((float)r.NextDouble(), (float)r.NextDouble(), -(float)r.NextDouble()) * 100 - Vector3.One * 50);
  entity.OriginalBoundingBox = BoundingBox.CreateFromSphere(model.Meshes[0].BoundingSphere);
  entity.Update(new GameTime());
  _entities.Add(entity);
 }
}
protected override void Update(GameTime gameTime)
{
 if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed ||
  Keyboard.GetState().IsKeyDown(Keys.Escape))
  this.Exit();
 
 _camera.Update(gameTime);
 
 _fpsCounter.Update(gameTime);
 
 base.Update(gameTime);
}
 
protected override void Draw(GameTime gameTime)
{
 GraphicsDevice.Clear(Color.CornflowerBlue);
 
 GraphicsDevice.DepthStencilState = DepthStencilState.Default;
 
 for (int i = 0; i < _modelsCount; i++)
 {
  _entities[i].Model.Draw(
   _entities[i].World,
   _camera.View,
   _camera.Projection);
 }
 
 _spriteBatch.Begin();
 _spriteBatch.DrawString(_font, string.Format("FPS: {0} Frame time: {1}", _fpsCounter.FramesPerSecond, _fpsCounter.FrameTime), Vector2.Zero, Color.White);
 _spriteBatch.End();
 
 base.Draw(gameTime);
}

public class DebugRenderer
 
{
 private BasicEffect _effect;
 
 private VertexPositionColor[] _bboxVerts;
 
 private short[] _bboxIndices;
 
 public GraphicsDevice Device { get; private set; }
 
 public DebugRenderer(GraphicsDevice device)
 {
  Device = device;
 
  _effect = new BasicEffect(Device);
  _effect.VertexColorEnabled = true;
  _effect.LightingEnabled = false;
 
  _bboxVerts = new VertexPositionColor[8];
  _bboxIndices = new short[]
  {
   0, 1,
   1, 2,
   2, 3,
   3, 0,
   0, 4,
   1, 5,
   2, 6,
   3, 7,
   4, 5,
   5, 6,
   6, 7,
   7, 4,
  };
 }
 
 public void DrawBoundingBox(ref BoundingBox boundingBox, ref Matrix view, ref Matrix projection, ref Color color)
 {
  Vector3[] corners = boundingBox.GetCorners();
  for (int i = 0; i < 8; i++)
  {
   _bboxVerts[i].Position = corners[i];
   _bboxVerts[i].Color = color;
  }
 
  _effect.View = view;
  _effect.Projection = projection;
 
  foreach (EffectPass pass in _effect.CurrentTechnique.Passes)
  {
   pass.Apply();
 
   Device.DrawUserIndexedPrimitives(
    PrimitiveType.LineList,
    _bboxVerts,
    0,
    8,
    _bboxIndices,
    0,
    _bboxIndices.Length / 2);
  }
 }
}

public static class Utilities
{
 public static void TransformBox(ref BoundingBox inBox, ref Matrix mat, out BoundingBox result)
 {
  Vector3 position = (Vector3)(0.5f * (inBox.Min + inBox.Max));
  Vector3 offset;
  Vector3 dim = new Vector3();
 
  Vector3.Subtract(ref inBox.Max, ref position, out offset);
 
  dim.X = ((offset.X * Math.Abs(mat.M11)) + (offset.Y * Math.Abs(mat.M21))) + (offset.Z * Math.Abs(mat.M31));
  dim.Y = ((offset.X * Math.Abs(mat.M12)) + (offset.Y * Math.Abs(mat.M22))) + (offset.Z * Math.Abs(mat.M32));
  dim.Z = ((offset.X * Math.Abs(mat.M13)) + (offset.Y * Math.Abs(mat.M23))) + (offset.Z * Math.Abs(mat.M33));
 
  Vector3.Transform(ref position, ref mat, out position);
 
  Vector3.Subtract(ref position, ref dim, out result.Min);
  Vector3.Add(ref position, ref dim, out result.Max);
 }
}

public void Update(GameTime gameTime)
{
 //трансформируем ограничивающий объем
 Utilities.TransformBox(ref OriginalBoundingBox, ref World, out TransformedBoundingBox);
}

private DebugRenderer _debugRenderer;
private Color _bboxColor;
private KeyboardState _prevKeyboardState;
private bool _drawDebug;
...
 
//в конструкторе
_bboxColor = Color.Blue;
...
 
protected override void Initialize()
{
 base.Initialize();
 
 _debugRenderer = new DebugRenderer(GraphicsDevice);
}
protected override void Update(GameTime gameTime)
{
 if (GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed ||
  Keyboard.GetState().IsKeyDown(Keys.Escape))
  this.Exit();
 
 KeyboardState currentKeyboardState = Keyboard.GetState();
 if (currentKeyboardState != _prevKeyboardState)
 {
  //переключаем флаг отрисовки ограничивающих объемов
  if (currentKeyboardState.IsKeyDown(Keys.B))
  {
   _drawDebug = !_drawDebug;
  }
 }
 
 _prevKeyboardState = currentKeyboardState;
 
 //обновляем все игровые сущности
 for (int i = 0; i < _entities.Count; i++)
 {
  _entities[i].Update(gameTime);
 }
 
 //обновляем камеру
 _camera.Update(gameTime);
 
 //считаем FPS
 _fpsCounter.Update(gameTime);
 
 base.Update(gameTime);
}
 
protected override void Draw(GameTime gameTime)
{
 GraphicsDevice.Clear(Color.CornflowerBlue);
 
 GraphicsDevice.DepthStencilState = DepthStencilState.Default;
 
 for (int i = 0; i < _modelsCount; i++)
 {
  _entities[i].Model.Draw(
   _entities[i].World,
   _camera.View,
   _camera.Projection);
 
  if (_drawDebug)
  {
   _debugRenderer.DrawBoundingBox(ref _entities[i].TransformedBoundingBox, ref _camera.View, ref _camera.Projection, ref _bboxColor);
  }    
 }
 
 _spriteBatch.Begin();
 _spriteBatch.DrawString(_font, string.Format("FPS: {0} Frame time: {1}", _fpsCounter.FramesPerSecond, _fpsCounter.FrameTime), Vector2.Zero, Color.White);
 _spriteBatch.End();
 
 base.Draw(gameTime);
}

public class Node
{
 public BoundingBox BoundingBox;
 
 public List<GameEntity> Entities { get; private set; }
 
 public List<Node> Children { get; private set; }
 
 public Node Parent { get; private set; }
 
 public int NestingLevel { get; private set; }
 
 public Node(Node parent, BoundingBox bbox, int nestingLevel)
 {
  Parent = parent;
  BoundingBox = bbox;
  NestingLevel = nestingLevel;
  Entities = new List<GameEntity>();
  Children = new List<Node>();
 }
}

public static void GetCenter(this BoundingBox bbox, out Vector3 center)
{
 center = new Vector3(
  (bbox.Min.X + bbox.Max.X) / 2,
  (bbox.Min.Y + bbox.Max.Y) / 2,
  (bbox.Min.Z + bbox.Max.Z) / 2);
}
 
public static void GetSize(this BoundingBox bbox, out Vector3 size)
{
 size = new Vector3(
  bbox.Max.X - bbox.Min.X,
  bbox.Max.Y - bbox.Min.Y,
  bbox.Max.Z - bbox.Min.Z);
}
 
public static double Round(double value, int digits)
{
 double scale = Math.Pow(10.0, digits);
 double round = Math.Floor(Math.Abs(value) * scale + 0.5);
 return (Math.Sign(value) * round / scale);
}

private void SplitNode(Node parent)
{
 Vector3 min = parent.BoundingBox.Min;
 Vector3 max = parent.BoundingBox.Max;
 
 //получаем координаты центра нода
 Vector3 c = (min + max) / 2.0f;
 
 //получаем дочерние ноды
 BoundingBox tempBbox = new BoundingBox();
 
 //левый верхний передний
 tempBbox.Min = new Vector3(min.X, c.Y, min.Z);
 tempBbox.Max = new Vector3(c.X, max.Y, c.Z);
 parent.Children.Add(new Node(parent, tempBbox, parent.NestingLevel + 1));
 
 //правый верхний передний
 tempBbox.Min = new Vector3(c.X, c.Y, min.Z);
 tempBbox.Max = new Vector3(max.X, max.Y, c.Z);
 parent.Children.Add(new Node(parent, tempBbox, parent.NestingLevel + 1));
 
 //левый нижний передний
 tempBbox.Min = new Vector3(min.X, min.Y, min.Z);
 tempBbox.Max = new Vector3(c.X, c.Y, c.Z);
 parent.Children.Add(new Node(parent, tempBbox, parent.NestingLevel + 1));
 
 //правый нижний передний
 tempBbox.Min = new Vector3(c.X, min.Y, min.Z);
 tempBbox.Max = new Vector3(max.X, c.Y, c.Z);
 parent.Children.Add(new Node(parent, tempBbox, parent.NestingLevel + 1));
 
 
 //левый верхний задний
 tempBbox.Min = new Vector3(min.X, c.Y, c.Z);
 tempBbox.Max = new Vector3(c.X, max.Y, max.Z);
 parent.Children.Add(new Node(parent, tempBbox, parent.NestingLevel + 1));
 
 //правый верхний задний
 tempBbox.Min = new Vector3(c.X, c.Y, c.Z);
 tempBbox.Max = new Vector3(max.X, max.Y, max.Z);
 parent.Children.Add(new Node(parent, tempBbox, parent.NestingLevel + 1));
 
 //левый нижний задний
 tempBbox.Min = new Vector3(min.X, min.Y, c.Z);
 tempBbox.Max = new Vector3(c.X, c.Y, max.Z);
 parent.Children.Add(new Node(parent, tempBbox, parent.NestingLevel + 1));
 
 //правый нижний задний
 tempBbox.Min = new Vector3(c.X, min.Y, c.Z);
 tempBbox.Max = new Vector3(max.X, c.Y, max.Z);
 parent.Children.Add(new Node(parent, tempBbox, parent.NestingLevel + 1));
 
 //заносим новые узлы в массив листьев, если уровень вложенности достиг максимального
 if (parent.NestingLevel + 1 >= _maxNestingLevel)
 {
  if (_leafs == null)
  {
   _leafSize = parent.Children[0].BoundingBox.Max.X - parent.Children[0].BoundingBox.Min.X;
   _leafMassiveSize = (int)Utilities.Round(((Root.BoundingBox.Max.X - Root.BoundingBox.Min.X) / _leafSize), 0);
   _leafs = new Node[_leafMassiveSize, _leafMassiveSize, _leafMassiveSize];
  }
  for (int i = 0; i < 8; i++)
  {
   //заносим листы в массив
   Vector3 center;
   parent.Children[i].BoundingBox.GetCenter(out center);
   int x = (int)(Math.Abs(Root.BoundingBox.Min.X - center.X) / _leafSize);
   int y = (int)(Math.Abs(Root.BoundingBox.Min.Y - center.Y) / _leafSize);
   int z = (int)(Math.Abs(Root.BoundingBox.Min.Z - center.Z) / _leafSize);
   _leafs[x, y, z] = parent.Children[i];
  }
 }
}

private Node GetLeaf(GameEntity entity)
{
 Vector3 leafSize;
 BoundingBox bbox = entity.TransformedBoundingBox;
 BoundingBox rootBbox = Root.BoundingBox;
 _leafs[0, 0, 0].BoundingBox.GetSize(out leafSize);
 
 //получаем индексы листа, в котором расположена точка Min bbox'а объекта
 int minX = (int)((bbox.Min.X - rootBbox.Min.X) / leafSize.X);
 if (minX < 0)
 {
  minX = 0;
 }
 
 if (minX > _leafMassiveSize - 1)
 {
  minX = _leafMassiveSize - 1;
 }
 
 int minY = (int)((bbox.Min.Y - rootBbox.Min.Y) / leafSize.Y);
 if (minY < 0)
 {
  minY = 0;
 }
 
 if (minY > _leafMassiveSize - 1)
 {
  minY = _leafMassiveSize - 1;
 }
 
 int minZ = (int)((bbox.Min.Z - rootBbox.Min.Z) / leafSize.Z);
 if (minZ < 0)
 {
  minZ = 0;
 }
 
 if (minZ > _leafMassiveSize - 1)
 {
  minZ = _leafMassiveSize - 1;
 }
 
 return _leafs[minX, minY, minZ];
}

private void RegistrateEntity(GameEntity entity, Node node)
{
 node.Entities.Add(entity);
 int hashCode = entity.GetHashCode();
 if (_objectNodeMap.ContainsKey(hashCode))
 {
  _objectNodeMap[hashCode] = node;
 }
 else
 {
  _objectNodeMap.Add(hashCode, node);
 }
}

private void AddEntity(GameEntity entity)
{
 //получаем лист, в котором находится объект
 Node leaf = GetLeaf(entity);
 
 _nodeStack.Clear();
 _nodeStack.Push(leaf);
 
 while (_nodeStack.Count > 0)
 {
  Node node = _nodeStack.Pop();
 
  ContainmentType containmentType = ContainmentType.Disjoint;
 
  node.BoundingBox.Contains(ref entity.TransformedBoundingBox, out containmentType);
  switch (containmentType)
  {
   case ContainmentType.Contains:
    //если объект полностью помещается в узел, то регистрируем его в этом узле
    RegistrateEntity(entity, node);
    break;
 
   case ContainmentType.Disjoint:
    //если объект не пересекается с узлом вообще, то значит объект вышел за пределы дерева
    //поэтому регистрируем его в листе, который вычислили ранее
    RegistrateEntity(entity, leaf);
    break;
 
   case ContainmentType.Intersects:
    //если объект пересекается с узлом, то нужно проверить является ли узел корневым
    if (node.Parent == null)
    {
     //если да, то объект лежит на границе дерева, поэтому регистрируем его
     //в ЛИСТЕ
     RegistrateEntity(entity, leaf);
    }
    else
    {
     //если нет, то передаем объект в родительский узел
     _nodeStack.Push(node.Parent);
    }
    break;
  }
 }
}