+ explicit Octree(const OctreeNode& rootNode)
+ {
+ root_.insert(rootNode);
+ }
+
+ void insert(EntityPtr entity)
+ {
+ insert(root_.root(), entity);
+ }
+
+ void insert(stlplus::ntree<OctreeNode>::iterator node, EntityPtr entity)
+ {
+ Plane::Halfspace halfspace;
+ int octantNum = -1;
+
+ if (!node.valid())
+ {
+ std::cerr << "cannot insert into invalid node" << std::endl;
+ return;
+ }
+
+ Plane xy = node->getAabb().getPlaneXY();
+ halfspace = xy.intersectsSphere(entity->getSphere());
+
+ if (halfspace == Plane::POSITIVE)
+ {
+ Plane xz = node->getAabb().getPlaneXZ();
+ halfspace = xz.intersectsSphere(entity->getSphere());
+
+ if (halfspace == Plane::POSITIVE)
+ {
+ Plane yz = node->getAabb().getPlaneYZ();
+ halfspace = yz.intersectsSphere(entity->getSphere());
+
+ if (halfspace == Plane::POSITIVE)
+ {
+ octantNum = 2;
+ }
+ else if (halfspace == Plane::NEGATIVE)
+ {
+ octantNum = 3;
+ }
+ }
+ else if (halfspace == Plane::NEGATIVE)
+ {
+ Plane yz = node->getAabb().getPlaneYZ();
+ halfspace = yz.intersectsSphere(entity->getSphere());
+
+ if (halfspace == Plane::POSITIVE)
+ {
+ octantNum = 1;
+ }
+ else if (halfspace == Plane::NEGATIVE)
+ {
+ octantNum = 0;
+ }
+ }
+ }
+ else if (halfspace == Plane::NEGATIVE)
+ {
+ Plane xz = node->getAabb().getPlaneXZ();
+ halfspace = xz.intersectsSphere(entity->getSphere());
+
+ if (halfspace == Plane::POSITIVE)
+ {
+ Plane yz = node->getAabb().getPlaneYZ();
+ halfspace = yz.intersectsSphere(entity->getSphere());
+
+ if (halfspace == Plane::POSITIVE)
+ {
+ octantNum = 6;
+ }
+ else if (halfspace == Plane::NEGATIVE)
+ {
+ octantNum = 7;
+ }
+ }
+ else if (halfspace == Plane::NEGATIVE)
+ {
+ Plane yz = node->getAabb().getPlaneYZ();
+ halfspace = yz.intersectsSphere(entity->getSphere());
+
+ if (halfspace == Plane::POSITIVE)
+ {
+ octantNum = 5;
+ }
+ else if (halfspace == Plane::NEGATIVE)
+ {
+ octantNum = 4;
+ }
+ }
+ }
+
+ if (octantNum == -1)
+ {
+ node->objects.push_front(entity);
+ //return node;
+ }
+ else
+ {
+ if (root_.children(node) == 0)
+ {
+ addChildren(node);
+ }
+
+ stlplus::ntree<OctreeNode>::iterator child = root_.child(node, octantNum);
+
+ if (child.valid())
+ {
+ return insert(child, entity);
+ }
+ else
+ {
+ std::cerr << "expected but found no child at index " << octantNum << std::endl;
+ //return stlplus::ntree<OctreeNode>::iterator();
+ }
+ //return WeakPtr();
+ }
+ }
+
+ void addChildren(stlplus::ntree<OctreeNode>::iterator node)
+ {
+ Aabb octant;
+
+ if (!node.valid())
+ {
+ std::cerr << "cannot add children to invalid node" << std::endl;
+ return;
+ }
+
+ for (int i = 0; i < 8; ++i)
+ {
+ node->getAabb().getOctant(octant, i);
+ //OctreeNode octantNode(octant);
+
+ root_.append(node, octant);
+ }
+ }
+
+ void draw(stlplus::ntree<OctreeNode>::iterator node, Scalar alpha)