C++ Vector_3类代码示例

double geometryUtils::computeVoronoiArea(Vertex_handle vertex) {
    double voronoiArea = 0.0;
    Vertex_circulator j;

    j = vertex->vertex_begin();

    do {
        Point_3 p11 = j->vertex()->point();
        Point_3 p12 = j->next()->vertex()->point();
        Point_3 p13 = j->next()->next()->vertex()->point();
        Vector_3 v11 = p13 - p12;
        Vector_3 v12 = p11 - p12;
        v11 = v11 / sqrt(CGAL::to_double(v11.squared_length()));
        v12 = v12 / sqrt(CGAL::to_double(v12.squared_length()));
        double alpha = acos(CGAL::to_double(v11 * v12));
        Point_3 p22 = j->opposite()->vertex()->point();
        Point_3 p23 = j->opposite()->next()->vertex()->point();
        Vector_3 v21 = p11 - p23;
        Vector_3 v22 = p22 - p23;
        v21 = v21 / sqrt(CGAL::to_double(v21.squared_length()));
        v22 = v22 / sqrt(CGAL::to_double(v22.squared_length()));

        double beta = acos(CGAL::to_double(v21 * v22));
        Vector_3 x = p13 - p11;
        double length = CGAL::to_double(x.squared_length());

        voronoiArea += (1.0 / 8.0) * (1.0 / tan(alpha) + 1.0 / tan(beta)) * length;

    } while (++j != vertex->vertex_begin());

    return voronoiArea;

};

void rotate_points3d(const Eigen::MatrixXd& ptsin, const Point_3& center, const Vector_3& direction, double angle, Eigen::MatrixXd& ptsout)
{
	Eigen::Vector3d c(center.x(), center.y(), center.z());
    Eigen::Vector3d dir(direction.x(), direction.y(), direction.z());
    Eigen::Matrix4d rotMat = create_rotation3d_line_angle(center, direction, angle);

	ptsout.resize(ptsin.rows(), ptsin.cols() );
    ptsout = transform_point3d(ptsin, rotMat);
}

void add_neighbourhood_to_hullPoints(pointVector& hullPoints, const Vertex_const_handle& vert, const double& tapeSize) {
	Point_3 pnt = vert->point();
	hullPoints.push_back(pnt);								// Add vertex point
	Nef_polyhedron::SVertex_const_iterator svcIt = vert->svertices_begin(), svcItEND = vert->svertices_end();
	CGAL_For_all(svcIt,svcItEND) {
		Vector_3 vecR(pnt,svcIt->target()->point());
		Vector_3 vecRnew = vecR * tapeSize / std::sqrt(CGAL::to_double(vecR.squared_length()));
		if ((vecR.squared_length()-OVERLAP_DIST_THRESHOLD) > vecRnew.squared_length())
			hullPoints.push_back(pnt+vecRnew);						// Add svertex neighbourhood point (tapesize away from vertex)
		else
			hullPoints.push_back(svcIt->target()->point());
	}

//compute the cross point
Point_3 compute_cross_point(Plane_3 plane, Point_3 start, Point_3 end)
{
	Vector_3 normal = plane.orthogonal_vector();
	Vector_3 line_direction = end - start;
	Point_3  p= plane.point();
	double t;
	double a = (start.x() - p.x()) * normal.x() + (start.y() - p.y()) * normal.y() + (start.z() - p.z()) * normal.z();
	double b = line_direction.x() * normal.x() + line_direction.y() * normal.y() + line_direction.z() * normal.z();

	assert(b != 0);
	t = -a / b;

	return start + t * line_direction;
}

/**
 * True if u is steeper than v. Uses the square of slope to avoid sqrt.
 */
bool is_steeper(Vector_3 u, Vector_3 v)
{
    Vector_2 u_2 = Vector_2(u.x(), u.y());
    Vector_2 v_2 = Vector_2(v.x(), v.y());
    return ((u.z() * u.z() / u_2.squared_length()) > 
            (v.z() * v.z() / v_2.squared_length())); 
}

// Assign either ceiling, floor or the default semantic based on input booleans and normal vector
void assignCeilVloor(std::set<Polyhedron::Facet_handle>& fhSet, bool canBeUp, bool canBeDown) {
	pointVector facetPoints;
	Vector_3 ortVec;
	for (std::set<Polyhedron::Facet_handle>::iterator sfIt=fhSet.begin();sfIt!=fhSet.end();++sfIt) {
		if (!canBeUp && !canBeDown) {
			(*sfIt)->semanticBLA = DEFAULT_HOR_SEMANTIC; continue;
		}

		facetPoints = comp_facetPoints(*sfIt);
		CGAL::normal_vector_newell_3(facetPoints.begin(),facetPoints.end(),ortVec);
		if (!normalizeVector(ortVec)) continue;
		if (canBeDown && ortVec.z() <= -HORIZONTAL_ANGLE_RANGE )	(*sfIt)->semanticBLA = DEFAULT_DOWN_SEMANTIC;
		else if (canBeUp && ortVec.z() >= HORIZONTAL_ANGLE_RANGE )	(*sfIt)->semanticBLA = DEFAULT_UP_SEMANTIC;
		else														(*sfIt)->semanticBLA = DEFAULT_HOR_SEMANTIC;
	}
}

int PlasmaBunch::interaction(int m, int sum){
 register int i, j,k;
 static Vector_3 r;

 int ret=Plasma::interaction(m,sum);

 // now interacting with bunch particles
 int type=0; // ion-ion
 double dEcoul,dEpotent,dQuant;
 double df;

 for(i=0;i<(m<0 ? n : m+1);i++){

   if(i>=ni)type|=0x1;// setting electron-? interaction type
   else type&=0x2;// setting ion-? interaction type
   if(is_ion)type&=0x1;         // setting ?-ion interaction type
   else  type|=0x2;// setting ?-electron interaction type

   for(j=0;j<nb;j++){

     for(k=0;k<3;k++){ // determining the closest
                       //distance and correspondent direction
       r[k]=xx[i][k]-xb[j][k];
       if(r[k]>L/2)r[k]-=L;
       if(r[k]<-L/2)r[k]+=L;
     }

     double R=r.norm();
     if(R<1e-20)printf("Got small distance to bunch (%d,b%d) !\n",i,j);
     r/=R;

     dEcoul=qb/R;

     df=potential(type,R,dEpotent,dQuant);

     for(k=0;k<3;k++){ // to avoid vector copying
       f[i][k]+=df*r[k];
       //f[j][k]-=df*r[k];
     }
     Ecoul+=dEcoul;
     Quant+=dQuant;
     Epotent+=dEpotent;
   }

 }
 return ret;
}

void Viewer::alphaChanged()
{

    normals.resize(0);
    pos_poly.resize(0);

    std::list<Facet> facets;
    scene->alpha_shape.get_alpha_shape_facets(std::back_inserter(facets), Alpha_shape_3::REGULAR);

    for(std::list<Facet>::iterator fit = facets.begin();
        fit != facets.end();
        ++fit) {
      const Cell_handle& ch = fit->first;
      const int index = fit->second;

      //const Vector_3& n = ch->normal(index); // must be unit vector

      const Point_3& a = ch->vertex((index+1)&3)->point();
      const Point_3& b = ch->vertex((index+2)&3)->point();
      const Point_3& c = ch->vertex((index+3)&3)->point();

      Vector_3 v = CGAL::unit_normal(a,b,c);

      normals.push_back(v.x()); normals.push_back(v.y()); normals.push_back(v.z());
      normals.push_back(v.x()); normals.push_back(v.y()); normals.push_back(v.z());
      normals.push_back(v.x()); normals.push_back(v.y()); normals.push_back(v.z());
      pos_poly.push_back(a.x()); pos_poly.push_back(a.y()); pos_poly.push_back(a.z());
      pos_poly.push_back(b.x()); pos_poly.push_back(b.y()); pos_poly.push_back(b.z());
      pos_poly.push_back(c.x()); pos_poly.push_back(c.y()); pos_poly.push_back(c.z());

    }

    initialize_buffers();

}

static std::vector<Point_3> mapPointsFromOXYplane(std::vector<Point_2> points,
		Vector_3 nu)
{
	DEBUG_START;

	ASSERT(!!Vector3d(nu.x(), nu.y(), nu.z()) && "nu is null vector");

	Vector_3 ez(0., 0, 1.);
	double length = sqrt(nu.squared_length());
	ASSERT(std::fpclassify(length) != FP_ZERO);
	nu = nu * 1. / length; /* Normalize std::vector \nu. */
	ASSERT(std::isfinite(nu.x()));
	ASSERT(std::isfinite(nu.y()));
	ASSERT(std::isfinite(nu.z()));
	Vector_3 tau = cross_product(nu, ez);

	std::vector<Point_3> pointsMapped;
	CGAL::Origin o;
	for (auto &point : points)
	{
		pointsMapped.push_back(o + tau * point.x() + ez * point.y());
	}
	DEBUG_END;
	return pointsMapped;
}

// Réalise un impact sur la face fs à partir d'une liste de points à déplacer, répartis par couronne (pts[0] = première couronne intérieure, pts[0][0] = premier point de la première couronne)
void DegradeAnObject::impactTheFacetArea(std::vector< std::vector<Point_3> > pts, Facet fs, double ray, int index) {
	double str = 0.02;
	Vector_3 normal = normalizeVector(getNormalOfFacet(fs));
	Kernel::Plane_3 pl(fs.halfedge()->vertex()->point(), normal);
	for(int i = 0 ; i < pts.size() ; i++) {
		for(int j = 0 ; j < pts[i].size() ; j++) {
			bool chk = false;
			Point_iterator pi = polys[index].points_begin();
			while(!chk) {
				++pi;
				if(*pi == pts[i][j]) {
					*pi = Point_3(pi->x() - (impactStrengh(str, i))*normal.x(), pi->y() - (impactStrengh(str, i))*normal.y(), pi->z() - (impactStrengh(str, i))*normal.z());
					chk = true;
				}
			}
		}
	}
}

static Vector_3 leastSquaresPoint(const std::vector<unsigned> activeGroup,
		const std::vector<SupportItem> &items)
{
	DEBUG_START;
	Eigen::Matrix3d matrix;
	Eigen::Vector3d vector;
	for (unsigned i = 0; i < 3; ++i)
	{
		vector(i) = 0.;
		for (unsigned j = 0; j < 3; ++j)
			matrix(i, j) = 0.;
	}

	std::cout << "Calculating least squares points for the following items"
		<< std::endl;
	for (unsigned iPlane : activeGroup)
	{
		SupportItem item = items[iPlane];
		Vector_3 u = item.direction;
		double value = item.value;
		std::cout << "  Item #" << iPlane << ": u = " << u << "; h = "
			<< value << std::endl;
		for (unsigned i = 0; i < 3; ++i)
		{
			for (unsigned j = 0; j < 3; ++j)
				matrix(i, j) += u.cartesian(i)
					* u.cartesian(j);
			vector(i) += u.cartesian(i) * value;
		}
	}
	Eigen::Vector3d solution = matrix.inverse() * vector;

	Vector_3 result(solution(0), solution(1), solution(2));
	std::cout << "Result: " << result << std::endl;
	for (unsigned iPlane : activeGroup)
	{
		SupportItem item = items[iPlane];
		double delta = item.direction * result - item.value;
		std::cout << "  delta = " << delta << std::endl;
	}
	DEBUG_END;
	return result;
}

void buildFacets(Polyhedron_3 polyhedron,
		std::vector<SimpleEdge_3> &edges,
		std::vector<Vector_3> &U, std::vector<double> &H,
		std::map<int, int> &indices)
{
	DEBUG_START;
	std::vector<Plane_3> planes = renumerateFacets(polyhedron, edges,
			indices);
	for (const Plane_3 &plane : planes)
	{
		Vector_3 norm = plane.orthogonal_vector();
		double length = sqrt(norm.squared_length());
		norm = norm * (1. / length);
		double value = -plane.d() / length;
		ASSERT(value > 0.);
		U.push_back(norm);
		H.push_back(value);
	}
	DEBUG_END;
}

// Computes the intersection C+uV of the planes M*x+D=0 and N*x+E=0.
// Returns -1 if there are no intersections (parallel), 0 for a line
// intersection and 1 for co-planarity.
// precondition: A, B are normalized (hessian form)
int plane_plane_intersection(const Vector_3& M, const double D,
                             const Vector_3& N, const double E,
                             Point_3& C, Vector_3& V)
{
  typedef CGAL::Cartesian<double> K;
  typedef CGAL::Plane_3<K> Plane_3;
  typedef CGAL::Line_3<K> Line_3;

  Plane_3 P1(M.x(), M.y(), M.z(), D);
  Plane_3 P2(N.x(), N.y(), N.z(), E);
  CGAL::Object result = CGAL::intersection(P1, P2);
  if (const Line_3 *iline = CGAL::object_cast<Line_3>(&result)) {
    CGAL::Point_3<K> p = iline->point(0);
    CGAL::Vector_3<K> v = iline->to_vector();
    C = Point_3(p.x(), p.y(), p.z());
    V = Vector_3(v.x(), v.y(), v.z());
    return 0;
  } 
  else if (const Plane_3 *iplane = CGAL::object_cast<Plane_3>(&result)) {
    return 1;
  } 
  else {
    return -1;
  }
}

int line_plane_intersection(const Point_3& B, const Vector_3& M,
                            const Vector_3& N, const double D,
                            Point_3& p)
{
  typedef CGAL::Cartesian<double> K;
  typedef CGAL::Plane_3<K> Plane_3;
  typedef CGAL::Line_3<K> Line_3;

  CGAL::Point_3<K> CB(B.x(), B.y(), B.z());
  CGAL::Vector_3<K> CM(M.x(), M.y(), M.z());

  Line_3 L(CB, CM);
  Plane_3 P(N.x(), N.y(), N.z(), D);
  CGAL::Object result = CGAL::intersection(L, P);
  if (const CGAL::Point_3<K> *ipoint = CGAL::object_cast<CGAL::Point_3<K> >(&result)) {
    p = Point_3(ipoint->x(), ipoint->y(), ipoint->z());
    return 0;
  } 
  else if (const Line_3 *iline = CGAL::object_cast<Line_3>(&result)) {
    return 1;
  } 
  else {
    return -1;
  }
}

Polyhedron_3 obtainPolyhedron(Polyhedron_3 initialP, std::map<int, int> map,
		IpoptTopologicalCorrector *FTNLP)
{
	DEBUG_START;
	std::vector<Vector_3> directions = FTNLP->getDirections();
	std::vector<double> values = FTNLP->getValues();
	std::vector<Plane_3> planes(initialP.size_of_facets());
	unsigned iFacet = 0;
	for (auto I = initialP.facets_begin(), E = initialP.facets_end();
			I != E; ++I)
	{
		auto it = map.find(iFacet);
		if (it != map.end())
		{
			int i = it->second;
			Vector_3 u = directions[i];
			double h = values[i];
			ASSERT(h > 0);
			planes[iFacet] = Plane_3(-u.x(), -u.y(), -u.z(), h);
			std::cout << "Changing plane #" << iFacet << ": "
				<< I->plane() << " |--> " << planes[iFacet]
				<< std::endl;
		}
		else
		{
			planes[iFacet] = I->plane();
		}
		++iFacet;
	}

	Polyhedron_3 intersection(planes);
	std::cout << "Change in facets number: " << initialP.size_of_facets()
		<< " -> " << intersection.size_of_facets() << std::endl;
	ASSERT(initialP.size_of_facets() - intersection.size_of_facets()
			< map.size() &&
			"It seems that all extracted facets have gone");
	DEBUG_END;
	return intersection;
}