C++ Point3d函数代码示例

void World::build()
{
        //a sample scene with 5 spheres
        Sphere *sph1 = new Sphere();
        sph1->set_color(Pixel(123, 255, 45));
        sph1->set_center(Point3d(-400, 200, -300));
        sph1->set_radius(250);
        add_object(sph1);


        Sphere *sph2 = new Sphere();
        sph2->set_color(Pixel(255, 123, 45));
        sph2->set_center(Point3d(400, -200,-100));
        sph2->set_radius(200);
        add_object(sph2);

        Sphere *sph3 = new Sphere();
        sph3->set_color(Pixel(45, 123, 255));
        sph3->set_center(Point3d(400, 200,-100));
        sph3->set_radius(150);
        add_object(sph3);

        Sphere *sph4 = new Sphere();
        sph4->set_color(Pixel(123, 123, 45));
        sph4->set_center(Point3d(-600, -200,-100));
        sph4->set_radius(150);
        add_object(sph4);

        Sphere *sph5 = new Sphere();
        sph5->set_color(Pixel(235, 34, 45));
        sph5->set_center(Point3d(0, 0,-100));
        sph5->set_radius(50);
        add_object(sph5);
}

void Particle::initializeDefaultPhysics() {
   position = Point3d();
   velocity = Point3d(Utilities::randomNegOneToOne(),
                      Utilities::randomNegOneToOne(),
                      Utilities::randomNegOneToOne());
   force = Point3d(-velocity.x, -velocity.y, -velocity.z);
   mass = DEFAULT_MASS;
}

/**
 * This method returns the target that the camera should be looking at
 */
Point3d ControllerEditor::getCameraTarget(){
	if (conF == NULL)
		return Point3d(0,1,0);
	Character* ch = conF->getCharacter();
	if (ch == NULL)
		return Point3d(0,1,0);
	//we need to get the character's position. We'll hack that a little...
	return Point3d(ch->getRoot()->getCMPosition().x, 1, ch->getRoot()->getCMPosition().z);
}

 std::vector<Point3d> IlluminanceMap_Impl::corners() const
 {
   std::vector<Point3d> result;
   result.push_back(Point3d(0,this->yLength(),0));
   result.push_back(Point3d(0,0,0));
   result.push_back(Point3d(this->xLength(),0,0));
   result.push_back(Point3d(this->xLength(),this->yLength(),0));
   return result;
 }

void Various_Processing_Component::NoiseAdditionUniform (PolyhedronPtr pMesh, double noiseIntensity, bool preserveBoundaries)
{
	// mesh centre calculation based on discrete "moment".
	//TODO: maybe in the future it will be based on volume moment.
	int numVertex = pMesh->size_of_vertices();;
	Vector centroid = Point3d(0,0,0) - CGAL::ORIGIN;
	double distancetoCentroid = 0.0;
	Vertex_iterator	pVertex;
	for (pVertex = pMesh->vertices_begin(); pVertex != pMesh->vertices_end(); pVertex++)
	{
		Vector vectemp = pVertex->point() - CGAL::ORIGIN;
		centroid = centroid + vectemp;
	}
	centroid = centroid/numVertex;

	// calculate the average distance from vertices to mesh centre
	for (pVertex = pMesh->vertices_begin(); pVertex!= pMesh->vertices_end(); pVertex++)
	{
		Vector vectemp = pVertex->point() - CGAL::ORIGIN;
		distancetoCentroid = distancetoCentroid + (double)std::sqrt((vectemp - centroid) * (vectemp - centroid));
	}
	distancetoCentroid = distancetoCentroid/numVertex;

	// add random uniform-distributed (between [-noiseLevel, +noiseLevel])
	srand((unsigned)time(NULL));
	double noisex, noisey, noisez;
	double noiseLevel = distancetoCentroid * noiseIntensity;
	for (pVertex = pMesh->vertices_begin(); pVertex!= pMesh->vertices_end(); pVertex++)
	{
		// keep boundaries untouched if demanded by user
		bool is_border_vertex = false;
		bool stopFlag = false;
		Halfedge_around_vertex_circulator hav = (*pVertex).vertex_begin();
		do
		{
			if (hav->is_border()==true)
			{
				is_border_vertex = true;
				stopFlag = true;
			}
			hav++;
		} while ((hav!=(*pVertex).vertex_begin())&&(stopFlag==false));

		if ((preserveBoundaries==true)&&(is_border_vertex==true))
			continue;

		noisex = noiseLevel * (1.0*rand()/RAND_MAX-0.5)*2;
		noisey = noiseLevel * (1.0*rand()/RAND_MAX-0.5)*2;
		noisez = noiseLevel * (1.0*rand()/RAND_MAX-0.5)*2;
		Vector temp = Point3d(noisex, noisey, noisez) - CGAL::ORIGIN;
		pVertex->point() = pVertex->point() + temp;
	}

	// for correct rendering, we need to update the mesh normals
	pMesh->compute_normals();
}

	Line::Line(const Span& sp){
		// contructor from linear span
		p0 = sp.p0;
		v = sp.vs * sp.length;
		length = sp.length;
		//	box = sp.box;
		box.min = Point3d(sp.box.min);
		box.max = Point3d(sp.box.max);
		ok = !sp.NullSpan;
	}

Camera::Camera(){
    at = Point3d(0.0,5.0,0.0);
	up = Point3d(0.0,1.0,0.0);
	theta = M_PI/3;
	phi = M_PI/3;
	R = 43.0;
    eye = transf_coord();
	//glMatrixMode(GL_MODELVIEW);
	//glLoadIdentity();
    //gluLookAt(eye.x,eye.y,eye.z,at.x,at.y,at.z,up.x,up.y,up.z);
};

  TEST_F(spelHelperTest, distSquared3D)
  {
    double b = sqrt(1.0 / 3.0);

    Point3d A = Point3d(0.0, 0.0, 0.0);
    Point3d B = Point3d(b, b, b);
    Point3f C = Point3f(-b, -b, -b);

    EXPECT_DOUBLE_EQ(0.0, spelHelper::distSquared3d(A, A));
    EXPECT_DOUBLE_EQ(1.0, spelHelper::distSquared3d(A, B));
    EXPECT_FLOAT_EQ(1.0, (float)spelHelper::distSquared3d(Point3f(0.0, 0.0, 0.0), C));
  }

	Camera::~Camera(void)
	{
		cameraCenter = Point3d();
		topLeft = Point3d();
		topRight = Point3d();
		bottomLeft = Point3d();
		fovX = 0;
		fovY = 0;
		rotation = 0;
		xResolution = 0;
		yResolution = 0;
	}

void drawBoundingBox(const Point3d &pMin, const Point3d &pMax)
{
    Point3d pts[8];
    pts[0] = Point3d(pMin.x, pMin.y, pMin.z);
    pts[1] = Point3d(pMax.x, pMin.y, pMin.z);
    pts[2] = Point3d(pMin.x, pMax.y, pMin.z);
    pts[3] = Point3d(pMin.x, pMin.y, pMax.z);

    pts[4] = Point3d(pMax.x, pMax.y, pMin.z);
    pts[5] = Point3d(pMax.x, pMin.y, pMax.z);
    pts[6] = Point3d(pMax.x, pMax.y, pMax.z);
    pts[7] = Point3d(pMin.x, pMax.y, pMax.z);

    drawLine(pts[0], pts[1]);
    drawLine(pts[0], pts[2]);
    drawLine(pts[0], pts[3]);

    drawLine(pts[6], pts[5]);
    drawLine(pts[6], pts[4]);
    drawLine(pts[6], pts[7]);

    drawLine(pts[2], pts[4]);
    drawLine(pts[2], pts[7]);
    drawLine(pts[3], pts[5]);

    drawLine(pts[3], pts[7]);
    drawLine(pts[4], pts[1]);
    drawLine(pts[5], pts[1]);
}

  void VisualSceneSpecPoints :: BuildScene (int zoomall)
  {
    if (!mesh) 
      {
	VisualScene::BuildScene(zoomall);
	return;
      }
  
    Box3d box;
  
    if (mesh->GetNSeg())
      {
	box.SetPoint (mesh->Point (mesh->LineSegment(1)[0]));
	for (int i = 1; i <= mesh->GetNSeg(); i++)
	  {
	    box.AddPoint (mesh->Point (mesh->LineSegment(i)[0]));
	    box.AddPoint (mesh->Point (mesh->LineSegment(i)[1]));
	  }
      }
    else if (specpoints.Size() >= 2)
      {
	box.SetPoint (specpoints.Get(1).p);
	for (int i = 2; i <= specpoints.Size(); i++)
	  box.AddPoint (specpoints.Get(i).p);
      }
    else
      {
	box = Box3d (Point3d (0,0,0), Point3d (1,1,1));
      }
  
    if (zoomall == 2 && ((vispar.centerpoint >= 1 && vispar.centerpoint <= mesh->GetNP()) ||
			 vispar.use_center_coords))
      {
	if (vispar.use_center_coords)
	  {
	    center.X() = vispar.centerx; center.Y() = vispar.centery; center.Z() = vispar.centerz; 
	  }
	else
	  center = mesh->Point (vispar.centerpoint);
      }
    else
      center = Center (box.PMin(), box.PMax());
        

    rad = 0.5 * Dist (box.PMin(), box.PMax());
  
  
    CalcTransformationMatrices();
  }

	 void Plane::Mirrored(Matrix* tmMirrored) {
		 // calculates a mirror transformation that mirrors 2d about plane
	 
		Point3d p1 = this->Near(Point3d(0.,0.,0.));
		if(tmMirrored->m_unit == false) tmMirrored->Unit();

		double nx = this->normal.getx();
		double ny = this->normal.gety();
		double nz = this->normal.getz();
	   
		// the translation
		tmMirrored->e[ 3] = -2. * nx * this->d;
		tmMirrored->e[ 7] = -2. * ny * this->d;
		tmMirrored->e[11] = -2. * nz * this->d;
	 
		// the rest
		tmMirrored->e[ 0] = 1. - 2. * nx * nx;
		tmMirrored->e[ 5] = 1. - 2. * ny * ny;
		tmMirrored->e[10] = 1. - 2. * nz * nz;
		tmMirrored->e[ 1] = tmMirrored->e[ 4] = -2. * nx * ny;
		tmMirrored->e[ 2] = tmMirrored->e[ 8] = -2. * nz * nx;
		tmMirrored->e[ 6] = tmMirrored->e[ 9] = -2. * ny * nz;
	 
		tmMirrored->m_unit = false;
		tmMirrored->m_mirrored = true;
	}

void rgb_pcl::getCloudFeatures(cv::Point3d &position, double &hue, PointCloudPtr cloudCluster){
	double R, G, B;
	double size = cloudCluster->points.size();
	
	R = 0; G = 0; B = 0;
	double maxZ = 0;
	for(auto cloud_point: cloudCluster->points){
		position += Point3d(cloud_point.x, cloud_point.y, cloud_point.z);
		R += cloud_point.r; G += cloud_point.g; B += cloud_point.b; 
		if(cloud_point.z > maxZ){
			maxZ = cloud_point.z; //We want to grab the object at the highest z
		}
	}
	position.x /= size; position.y /= size; position.z = maxZ;
	R /= size; G /= size; B /= size; 
	
	double Cmax = maximum(R, G, B);
	double Cmin = minimum(R, G, B);
	double delta = Cmax - Cmin;
	if(R > G && R > B){
		hue = 60*((double)((G-B)/delta));
	}else if(G > R && G > B){
		hue = 60*((double)((B-R)/delta)+2);
	}else if(B > R && B > G){
		hue = 60*((double)((R-G)/delta)+4);
	}
	
	if(hue < 0)
		hue += 360;
}

float CameraProjections::ComputeError(vector<OptimizorParam> &d)
{
	double totalDis = 0;
	double maxDis = -9999999;
	int worseCoef = 3;
	int totalCount = 0;
	for (size_t i = 0; i < params.camCalibrator.opticalAngle.size(); i++)
	{
		cameraLocation = params.camCalibrator.cameraLocation[i];

		cameraOrintation = params.camCalibrator.opticalAngle[i]
				+ Point3d(d[0].data, d[1].data, d[2].data);
		diagnalAngleView = d[3].data;
		CalculateProjection();

		Point2f res;
		if (GetOnRealCordinate(params.camCalibrator.clicked[i], res))
		{
			double t = Distance2point(res, params.camCalibrator.rvizClicked[i]);
			totalDis += t;
			maxDis = std::max(maxDis, t);
			totalCount++;
		}
		else
		{
			return 10000000000;
		}
	}

	return (totalDis + (maxDis * worseCoef)) / (totalCount + worseCoef);
}

Point3d Point3d::centerOf(Point3d l, Point3d r)
{
    return Point3d(
        l.x / 2.0 + r.x / 2.0,
        l.y / 2.0 + r.y / 2.0,
        l.z / 2.0 + r.z / 2.0);
}