C++ Octree类代码示例

sure::Scalar sure::keypoints::calculateEntropyWithCrossproducts(const Octree& octree, Node* node, Scalar normalSamplingrate, Scalar radius, Scalar influenceRadius)
{
  FixedPayload mainNormalIntegrate;
  octree.integratePayload(node->fixed().getMeanPosition(), radius, mainNormalIntegrate);
  Normal mainNormal = mainNormalIntegrate.calculateNormal();

  if( mainNormal.isStable() )
  {
    sure::normal::CrossProductHistogram histogram;
    histogram.setInfluenceRadius(influenceRadius);
    NodeVector nodes = octree.getNodes(node->fixed().getMeanPosition(), radius, normalSamplingrate);

    for(unsigned int i=0; i<nodes.size(); ++i)
    {
      Node* currNode = nodes[i];
      NormalPayload* currPayload = static_cast<CrossProductPayload*>(currNode->opt());
      if( currPayload->normal_.isStable())
      {
        histogram.insertCrossProduct(mainNormal.vector(), currPayload->normal_.vector());
      }
    }

    return histogram.calculateEntropy();
  }
  return 0.0;
}

void TestOctree()
{
	std::vector<vgl_point_3d<double> > Points;
	Points.push_back(vgl_point_3d<double>(1.0, 0.0, 0.0));
	Points.push_back(vgl_point_3d<double>(0.0, 0.0, 0.0));
	Points.push_back(vgl_point_3d<double>(0.0, 1.0, 0.0));
	
	std::vector<std::vector<unsigned int> > VertexLists;
	std::vector<unsigned int> VertexList;
	VertexList.push_back(0);
	VertexList.push_back(1);
	VertexList.push_back(2);
	VertexLists.push_back(VertexList);
	
	Octree Tree;
	Tree.Build(Points, VertexLists);
	
	OrientedPoint Intersection;
	Ray R(vgl_point_3d<double>(0.5, 0.5, -1.0), vgl_vector_3d<double> (0.0, 0.0, 1.0));
	bool intersect = Tree.IntersectRay(R, Intersection);
	
	std::cout << "Intersect? " << intersect << std::endl;
	if(intersect)
		std::cout << "Intersection: " << Intersection << std::endl;
}

void test1()
{
    Octree* mytree = new Octree();
    std::cout << mytree->getOrigin().getX() << std::endl;
    std::cout << mytree->getRadii().getX() << std::endl;
    delete mytree;
}

void Octree::updateParents() {
	Octree* node = parent;
	while (node) {
		node->subtree_colors.push_back(std::make_pair(color_id, getColor()));
		node = node->getParent();
	}
}

void Octree::testIntersectRayBoth( const Ray& ray, HitResult & hitRes )
{
	stack<Octree*> trees;

	Ray inverseRay = ray.inverse();

	if (boundingBox.intersects(ray) || boundingBox.intersects(inverseRay))
		trees.push(this);

	while(!trees.empty())
	{
		Octree * t = trees.top(); trees.pop();

		if (t->testIntersectHit(ray, hitRes))
		{
			return;
		}
		else
		{
			for (StdVector<Octree>::iterator child = t->children.begin(); child != t->children.end(); child++)
			{
				if (child->boundingBox.intersects(ray) || child->boundingBox.intersects(inverseRay))
					trees.push(&(*child));
			}
		}
	}
}

void Octree::newNode( int depth, double x, double y, double z )
{
	double extent = boundingBox.xExtent / 2.0;

	Vec3d center;

	center.x() = boundingBox.center.x() + (extent * x);
	center.y() = boundingBox.center.y() + (extent * y);
	center.z() = boundingBox.center.z() + (extent * z);

	BoundingBox bb(center, extent, extent, extent);

	// Add child
	children.push_back(Octree());
	Octree * child = &children.back();

	child->boundingBox = bb;
	child->trianglePerNode = this->trianglePerNode;

	// Collect triangles inside child's bounding box
	for(StdVector<BaseTriangle*>::iterator it = this->triangleData.begin(); it != this->triangleData.end(); it++)
	{
		BaseTriangle* face = *it;

		if( bb.containsTriangle(face->vec(0), face->vec(1), face->vec(2)) )
		{
			child->triangleData.push_back(face);
		}
	}

	child->build(depth + 1); // build it
}

void GameEngine::Generate(Entity *node, Graphic::Object *obj)
{
  Vector2f	size(40, 40);
  Octree	*oct = new Octree(Box3f(Vector3f(0,0,0),Vector3f(50,50,50)));
  Box3f		box;

  node->data = oct;
  std::list<std::pair<Box3f,ISceneNode*> > listToInsert;
  for (unsigned int i = 0; i < size[0]; ++i)
    {
      for (unsigned int j = 0; j < size[1]; ++j)
        {
          Graphic::Object *newObj = obj->Clone()->As<Graphic::Object>();
          newObj->Matrix.Translation(i,j,0);
          newObj->GetReelBox(box);
          listToInsert.push_front(std::pair<Box3f,ISceneNode*>(box, newObj));
        }
    }
  obj->Matrix.Translation(42,42,42);
  obj->GetReelBox(box);
  listToInsert.push_front(std::pair<Box3f,ISceneNode*>(box, obj));

  oct->Insert(listToInsert, 16);
  oct->DrawOutlines(true);
}

void slaveFunc () {
	int nodeRank;
	MPI_Comm_rank(MPI_COMM_WORLD, &nodeRank);  
	
	int treeSize;
	MPI_Bcast (&treeSize, 1, MPI_INT, MASTER, MPI_COMM_WORLD);

	char * treeBuffer = new char[treeSize];
	MPI_Bcast (treeBuffer,  treeSize, MPI_BYTE, MASTER, MPI_COMM_WORLD);
	
	Octree t;
	t.readSerializedData(treeBuffer, treeSize);
	
	MPI_Status status;
	int chunkSize;
	MPI_Recv (&chunkSize, 1, MPI_INT, MASTER, HEADER, MPI_COMM_WORLD, &status);

	cout << "chunk size" << chunkSize << endl;
	RayPixel * chunk = new RayPixel[chunkSize];
	MPI_Recv (chunk, chunkSize * sizeof(RayPixel), MPI_BYTE, MASTER, RAY_ARRAY, MPI_COMM_WORLD, &status);

	TracePixel * traceChunk = new TracePixel[chunkSize];

	for (int i = 0; i < chunkSize; i++) {
		traceChunk[i] = traceRay(chunk[i], t);
	}
	cout << "done tracing client pack" << endl;
	
	MPI_Request request;
	MPI_Isend (&chunkSize, 1, MPI_INT, MASTER, HEADER, MPI_COMM_WORLD, &request);
	MPI_Isend (traceChunk, chunkSize * sizeof(TracePixel), MPI_BYTE, MASTER, NODE_ARRAY, MPI_COMM_WORLD, &request);
}

void Octree::intersectRayBoth( const Ray& ray, IndexSet & tris )
{
	stack<Octree*> trees;

	Ray inverseRay(ray.inverse());

	if (boundingBox.intersects(ray) || boundingBox.intersects(inverseRay))
		trees.push(this);
	else
		return;

	while(!trees.empty())
	{
		Octree * t = trees.top(); trees.pop();

		if (!t->intersectHit(tris))
		{
			for (StdVector<Octree>::iterator child = t->children.begin(); child != t->children.end(); child++)
			{
				if (child->boundingBox.intersects(ray) || child->boundingBox.intersects(inverseRay))
				{
					trees.push(&(*child));
				}
			}
		}
	}
}

int main( int args, char* argv[] ) {
	Octree* tree = new Octree();
	int counts[3];
	tree->countNodes(counts);
	std::cout << " Internal " << counts[0] << "\tPseudo " << counts[1] << "\tLeaf " << counts[2] << "\n";

	delete tree;
}

void test3()
{
    Vec3<double> temp = Vec3<double>(0.0, 0.0, 0.0);
    Vec3<double> origin = Vec3<double>(100.0, 100.0, 100.0);
    Vec3<double> radii = Vec3<double>(10.0, 10.0, 10.0);
    Octree * mytree = new Octree(origin, radii);
    std::cout << mytree->getOctant(temp) << std::endl;
    delete mytree;
}

 // This function finds the first
 // non-leaf octant for a data point
 Octree<T> *findBestChild( const glm::vec3 &pos ){
     int octant = 0;
     Octree<T> *ret = this;
     do {
         octant = ret->getOctantFromPoint( pos );
         ret = ret->children[ octant ];
     } while( !ret->isLeafNode() );
     return ret;
 }

OctreePalette::OctreePalette(const std::vector<RGBAPixel>& colors)
	: colors(colors) {
	// add each color to the octree, assign a palette index and update parents
	for (size_t i = 0; i < colors.size(); i++) {
		RGBAPixel color = colors[i];
		Octree* node = Octree::findOrCreateNode(&octree, color);
		node->setColor(color);
		node->setColorID(i);
		node->updateParents();
	}
}

sure::Scalar sure::keypoints::calculateCornerness(const Octree& octree, Node* node, Scalar radius)
{
  Octree::NodeVector vec;
  vec = octree.getNodes(node, octree.getUnitSize(radius));

  Vector3 mean(Vector3::Zero());
  Scalar weight(0.0);
  for(unsigned int i=0; i<vec.size(); ++i)
  {
    Node* currNode = vec[i];
    sure::payload::EntropyPayload* payload = static_cast<EntropyPayload*>(currNode->opt());
    if( payload->entropy_ > 0.0 )
    {
      mean += (payload->entropy_ * currNode->fixed().getMeanPosition());
//      mean[0] += (payload->entropy_ * currNode->fixed().getMeanPosition()[0]);
//      mean[1] += (payload->entropy_ * currNode->fixed().getMeanPosition()[1]);
//      mean[2] += (payload->entropy_ * currNode->fixed().getMeanPosition()[2]);
      weight += payload->entropy_;
    }
  }

  if( weight > 0.0 )
  {
    mean /= weight;
  }
  else
  {
    return 0.f;
  }

  Matrix3 covariance(Matrix3::Zero());
  for(unsigned int i=0; i<vec.size(); ++i)
  {
    Node* currNode = vec[i];
    EntropyPayload* payload = static_cast<EntropyPayload*>(currNode->opt());

    if( payload->entropy_ > 0.0 )
    {
//      Vector3 d;
//      d[0] = mean[0] - currNode->fixed().getMeanPosition()[0];
//      d[1] = mean[1] - currNode->fixed().getMeanPosition()[1];
//      d[2] = mean[2] - currNode->fixed().getMeanPosition()[2];
      covariance += payload->entropy_ * ( (mean - currNode->fixed().getMeanPosition()) * ((mean - currNode->fixed().getMeanPosition()).transpose()) );
//      covariance += payload->entropy_ * ( d * d.transpose() );
    }
  }
  covariance /= weight;

  Vector3 eigenValues;
  pcl::eigen33(covariance, eigenValues);

  return (Scalar) (eigenValues[0] / eigenValues[2]);
}

void Drawable::RemoveFromOctree()
{
    if (octant_)
    {
        Octree* octree = octant_->GetRoot();
        if (updateQueued_)
            octree->CancelUpdate(this);

        // Perform subclass specific deinitialization if necessary
        OnRemoveFromOctree();

        octant_->RemoveDrawable(this);
    }
}