C++ Vec::Normalize方法代码示例

/*
void Surface :: GetNormalVector (const Point<3> & p, Vec<3> & n) const
{
  CalcGradient (p, n);
  n.Normalize();
}
*/
Vec<3> Surface :: GetNormalVector (const Point<3> & p) const
{
  Vec<3> n;
  CalcGradient (p, n);
  n.Normalize();
  return n;
}

void MeshOptimize2dSurfaces :: 
GetNormalVector(INDEX surfind, const Point<3> & p, Vec<3> & n) const
{
  Vec<3> hn = n;
  geometry.GetSurface(surfind)->CalcGradient (p, hn);
  hn.Normalize();
  n = hn;

  /*
  if (geometry.GetSurface(surfind)->Inverse())
    n *= -1;
  */
}

int Polyhedra :: AddFace (int pi1, int pi2, int pi3, int inputnum)
{
  (*testout) << "polyhedra, add face " << pi1 << ", " << pi2 << ", " << pi3 << endl;

  if(pi1 == pi2 || pi2 == pi3 || pi3 == pi1)
    {
      ostringstream msg;
      msg << "Illegal point numbers for polyhedron face: " << pi1+1 << ", " << pi2+1 << ", " << pi3+1;
      throw NgException(msg.str());
    }

  faces.Append (Face (pi1, pi2, pi3, points, inputnum));
  
  Point<3> p1 = points[pi1];
  Point<3> p2 = points[pi2];
  Point<3> p3 = points[pi3];

  Vec<3> v1 = p2 - p1;
  Vec<3> v2 = p3 - p1;

  Vec<3> n = Cross (v1, v2); 
  n.Normalize();

  Plane pl (p1, n);
//   int inverse;
//   int identicto = -1;
//   for (int i = 0; i < planes.Size(); i++)
//     if (pl.IsIdentic (*planes[i], inverse, 1e-9*max3(v1.Length(),v2.Length(),Dist(p2,p3))))
//       {
// 	if (!inverse)
// 	  identicto = i;
//       }
//   //  cout << "is identic = " << identicto << endl;
//   identicto = -1;    // changed April 10, JS

//   if (identicto != -1)
//     faces.Last().planenr = identicto;
//   else
    {
      planes.Append (new Plane (p1, n));
      surfaceactive.Append (1);
      surfaceids.Append (0);
      faces.Last().planenr = planes.Size()-1;
    }

//  (*testout) << "is plane nr " << faces.Last().planenr << endl;

  return faces.Size();
}

void Brick :: CalcData()
{
  v12 = p2 - p1;
  v13 = p3 - p1;
  v14 = p4 - p1;

  Point<3> pi[8];
  int i1, i2, i3;
  int i, j;
  
  i = 0;
  for (i3 = 0; i3 <= 1; i3++)
    for (i2 = 0; i2 <= 1; i2++)
      for (i1 = 0; i1 <= 1; i1++)
	{
	  pi[i] = p1 + (double)i1 * v12 + (double)i2 * v13 + (double)i3 * v14;
	  i++;
	}

  static int lface[6][4] =
  { { 1, 3, 2, 4 },
    { 5, 6, 7, 8 },
    { 1, 2, 5, 6 },
    { 3, 7, 4, 8 },
    { 1, 5, 3, 7 },
    { 2, 4, 6, 8 } };
  
  Array<double> data(6);
  for (i = 0; i < 6; i++)
    {
      const Point<3> lp1 = pi[lface[i][0]-1];
      const Point<3> lp2 = pi[lface[i][1]-1];
      const Point<3> lp3 = pi[lface[i][2]-1];

      Vec<3> n = Cross ((lp2-lp1), (lp3-lp1));
      n.Normalize();
      
      for (j = 0; j < 3; j++)
	{
	  data[j] = lp1(j);
	  data[j+3] = n(j);
	}
      faces[i] -> SetPrimitiveData (data);
    }
}

//.........这里部分代码省略.........
      {
	cerr << "No 2D Optimizer!" << endl;
	return;
      }    

    while ((facokedge < 1.-1e-8 || facokface < 1.-1e-8) && 
	   cnttrials < maxtrials &&
	   multithread.terminate != 1)
      {
	(*testout) << "   facokedge " << facokedge << " facokface " << facokface << " cnttrials " << cnttrials << endl
		   << " perc. " << 95. * max2( min2(facokedge,facokface),
					       double(cnttrials)/double(maxtrials)) << endl;

	SetThreadPercent(95. * max2( min2(facokedge,facokface),
				     double(cnttrials)/double(maxtrials)));

	ostrstr.str("");
	ostrstr << "max. worsening " << max_worsening;
	PrintMessage(5,ostrstr.str());
	oldlamedge = lamedge;
	lamedge *= 6;
	if (lamedge > 2)
	  lamedge = 2;
	   
	if(1==1 || facokedge < 1.-1e-8)
	  {
	    for(int i=0; i<nv.Size(); i++)
	      *nv[i] = Vec<3>(0,0,0);
	    for (int i = 1; i <= mesh.GetNSE(); i++)
	      {
		const Element2d & sel = mesh.SurfaceElement(i);
		Vec<3> auxvec = Cross(mesh.Point(sel.PNum(2))-mesh.Point(sel.PNum(1)),
                                      mesh.Point(sel.PNum(3))-mesh.Point(sel.PNum(1)));
		auxvec.Normalize();
		for (int j = 1; j <= sel.GetNP(); j++)
		  if(!isedgepoint.Test(sel.PNum(j)))
		    *nv[sel.PNum(j) - PointIndex::BASE] += auxvec;
	      }
	    for(int i=0; i<nv.Size(); i++)
	      nv[i]->Normalize();
	    
	    
	    do  // move edges
	      {
		lamedge *= 0.5;
		cnttrials++;
		if(cnttrials % 10 == 0)
		  max_worsening *= 1.1;
		
		
		factryedge = lamedge + (1.-lamedge) * facokedge;

		ostrstr.str("");
		ostrstr << "lamedge = " << lamedge << ", trying: " << factryedge;
		PrintMessage(5,ostrstr.str());
		

		for (int i = 1; i <= np; i++)
		  {
		    if (isedgepoint.Test(i))
		      {
			for (int j = 0; j < 3; j++)
			  mesh.Point(i)(j) = 
			    lamedge * (*should.Get(i))(j) +
			    (1.-lamedge) * (*can.Get(i))(j);
		      }

  void OCCSurface :: GetNormalVector (const Point<3> & p, 
				      const PointGeomInfo & geominfo,
				      Vec<3> & n) const
  {
    gp_Pnt pnt;
    gp_Vec du, dv;

    /*
      double gu = geominfo.u;
      double gv = geominfo.v;

      if (fabs (gu) < 1e-3) gu = 0;
      if (fabs (gv) < 1e-3) gv = 0;

      occface->D1(gu,gv,pnt,du,dv);
    */

    /*
      occface->D1(geominfo.u,geominfo.v,pnt,du,dv);

      n = Cross (Vec<3>(du.X(), du.Y(), du.Z()),
      Vec<3>(dv.X(), dv.Y(), dv.Z()));
      n.Normalize();
    */



    GeomLProp_SLProps lprop(occface,geominfo.u,geominfo.v,1,1e-5);
    double setu=geominfo.u,setv=geominfo.v;

    if(lprop.D1U().Magnitude() < 1e-5 || lprop.D1V().Magnitude() < 1e-5)
      {
	double ustep = 0.01*(umax-umin);
	double vstep = 0.01*(vmax-vmin);

	n=0;

	while(setu < umax && (lprop.D1U().Magnitude() < 1e-5 || lprop.D1V().Magnitude() < 1e-5))
	  setu += ustep;
	if(setu < umax)
	  {
	    lprop.SetParameters(setu,setv);
	    n(0)+=lprop.Normal().X();
	    n(1)+=lprop.Normal().Y();
	    n(2)+=lprop.Normal().Z();
	  }
	setu = geominfo.u;
	while(setu > umin && (lprop.D1U().Magnitude() < 1e-5 || lprop.D1V().Magnitude() < 1e-5))
	  setu -= ustep;
	if(setu > umin)
	  {
	    lprop.SetParameters(setu,setv);
	    n(0)+=lprop.Normal().X();
	    n(1)+=lprop.Normal().Y();
	    n(2)+=lprop.Normal().Z();
	  }
	setu = geominfo.u;

	while(setv < vmax && (lprop.D1U().Magnitude() < 1e-5 || lprop.D1V().Magnitude() < 1e-5))
	  setv += ustep;
	if(setv < vmax)
	  {
	    lprop.SetParameters(setu,setv);
	    n(0)+=lprop.Normal().X();
	    n(1)+=lprop.Normal().Y();
	    n(2)+=lprop.Normal().Z();
	  }
	setv = geominfo.v;
	while(setv > vmin && (lprop.D1U().Magnitude() < 1e-5 || lprop.D1V().Magnitude() < 1e-5))
	  setv -= ustep;
	if(setv > vmin)
	  {
	    lprop.SetParameters(setu,setv);
	    n(0)+=lprop.Normal().X();
	    n(1)+=lprop.Normal().Y();
	    n(2)+=lprop.Normal().Z();
	  }
	setv = geominfo.v;

	n.Normalize();
      }
    else
      {
	n(0)=lprop.Normal().X();
	n(1)=lprop.Normal().Y();
	n(2)=lprop.Normal().Z();
      }

    if(glob_testout)
      {
	(*testout) << "u " << geominfo.u << " v " << geominfo.v 
		   << " du " << lprop.D1U().X() << " "<< lprop.D1U().Y() << " "<< lprop.D1U().Z()
		   << " dv " << lprop.D1V().X() << " "<< lprop.D1V().Y() << " "<< lprop.D1V().Z() << endl;
      }



    if (orient == TopAbs_REVERSED) n = -1*n;
    //  (*testout) << "GetNormalVector" << endl;
  }

Vec<2> ExplicitCurve2d :: Normal (double t) const
{
  Vec<2> tan = EvalPrime (t);
  tan.Normalize();
  return Vec<2> (tan(1), -tan(0));
}

//Finds the intersection between a ray and the cone
//Stores information about intersection in the Hit data structure if its the first intersection
bool Cone::intersect(const Ray &r,Hit &h) const{
	//Find intersection between ray and infinite cone of same parameters
	Vec p = r.getOrigin();
	Vec d = r.getDirection();
	double gamma = cos(theta);
	Matrix M;
	M.setToIdentity();
	M*=gamma*gamma;
	M=Matrix::MakeOuterProduct(dir) - M;
	M.set(3,3,1);
	Vec delta = p - origin;

	double a = d.Dot(M*d);
	double b = 2*d.Dot(M*delta);
	double c = delta.Dot(M*delta);
	double determinant=b*b-4.0*a*c;
	bool intersect = false;
	double t;
	Vec norm;
	//Checks if there are any real intersections with infinite cone
	if(determinant>0){
		intersect = true;
		t = (-b + sqrt(determinant)) / (2.0 * a);
		double t2 = (-b - sqrt(determinant)) / (2.0 * a);
		//Finds the intersection with a smaller t, and checks to see if the intersection is with the portion of the specified cone
		if(t2 < t || t < INTERSECT_EPSILON || !validIntersection(t,r)){
			t = t2;
			if(t < INTERSECT_EPSILON || !validIntersection(t,r)){
				intersect = false;
			}
		}
		if(intersect){
			if(h.getT()>t){
				norm = getNormal(r.getDirection()*t+r.getOrigin());
				norm.Normalize();
				if(norm.Dot(d)>0){
					norm *= -1;
				}
			}
		}
	}
	//checks for intersection with the end cap
	double num = dir.Dot(origin+dir*height-p);
	double den = dir.Dot(d);
	if(den!=0){
		double tc = num/den;
		double dist = (d*tc+p-(origin+dir*height)).Length();
		if(radius()>=dist  && tc > INTERSECT_EPSILON && tc < t){
			t = tc;
			intersect = true;
			norm = dir;
			if(norm.Dot(d)>0){
				norm *= -1;
			}
		}
	}
	//Checks if the intersection is closer than the one stored in the hit data structure
	if(intersect && h.getT() > t){
		h.set(t,material,norm);
		h.setObject(this);
	}
	return intersect;
}

Vec<3> Polyhedra :: SpecialPointTangentialVector (const Point<3> & p, int s1, int s2) const
{
  const double eps = 1e-10*poly_bbox.Diam();

  for (int fi1 = 0; fi1 < faces.Size(); fi1++)
    for (int fi2 = 0; fi2 < faces.Size(); fi2++)
      {
	int si1 = faces[fi1].planenr;
	int si2 = faces[fi2].planenr;

	if (surfaceids[si1] != s1 || surfaceids[si2] != s2) continue;

	//(*testout) << "check pair fi1/fi2 " << fi1 << "/" << fi2 << endl;
	
	Vec<3> n1 = GetSurface(si1) . GetNormalVector (p);
	Vec<3> n2 = GetSurface(si2) . GetNormalVector (p);
	Vec<3> t = Cross (n1, n2);

	//(*testout) << "t = " << t << endl;


	/*
	int samepts = 0;
	for (int j = 0; j < 3; j++)
	  for (int k = 0; k < 3; k++)
	    if (Dist(points[faces[fi1].pnums[j]],
		     points[faces[fi2].pnums[k]]) < eps)
	      samepts++;
	if (samepts < 2) continue;
	*/

	bool shareedge = false;
	for(int j = 0; !shareedge && j < 3; j++)
	  {
	    Vec<3> v1 = points[faces[fi1].pnums[(j+1)%3]] - points[faces[fi1].pnums[j]];
	    double smax = v1.Length();
	    v1 *= 1./smax;
	    
	    int pospos;
	    if(fabs(v1(0)) > 0.5)
	      pospos = 0;
	    else if(fabs(v1(1)) > 0.5)
	      pospos = 1;
	    else
	      pospos = 2;

	    double sp = (p(pospos) - points[faces[fi1].pnums[j]](pospos)) / v1(pospos);
	    if(sp < -eps || sp > smax+eps)
	      continue;

	    for (int k = 0; !shareedge && k < 3; k ++)
	      {
		 Vec<3> v2 = points[faces[fi2].pnums[(k+1)%3]] - points[faces[fi2].pnums[k]];
		 v2.Normalize();
		 if(v2 * v1 > 0)
		   v2 -= v1;
		 else
		   v2 += v1;
		 
		 //(*testout) << "v2.Length2() " << v2.Length2() << endl;

		 if(v2.Length2() > 1e-18)
		   continue;

		 double sa,sb;

		 sa = (points[faces[fi2].pnums[k]](pospos) - points[faces[fi1].pnums[j]](pospos)) / v1(pospos);
		 sb = (points[faces[fi2].pnums[(k+1)%3]](pospos) - points[faces[fi1].pnums[j]](pospos)) / v1(pospos);
		 

		 if(Dist(points[faces[fi1].pnums[j]] + sa*v1, points[faces[fi2].pnums[k]]) > eps)
		   continue;

		 if(sa > sb)
		   {
		     double aux = sa; sa = sb; sb = aux;
		   }

		 //testout->precision(20);
		 //(*testout) << "sa " << sa << " sb " << sb << " smax " << smax << " sp " << sp  << " v1 " << v1 << endl;
		 //testout->precision(8);


		 shareedge = (sa < -eps && sb > eps) ||
		   (sa < smax-eps && sb > smax+eps) ||
		   (sa > -eps && sb < smax+eps);

		 if(!shareedge)
		   continue;

		 sa = max2(sa,0.);
		 sb = min2(sb,smax);

		 if(sp < sa+eps)
		   shareedge = (t * v1 > 0);
		 else if (sp > sb-eps)
		   shareedge = (t * v1 < 0);
		   
	      }
	  }
//.........这里部分代码省略.........

INSOLID_TYPE Polyhedra :: VecInSolid (const Point<3> & p,
				      const Vec<3> & v,
				      double eps) const
{
  ARRAY<int> point_on_faces;
  INSOLID_TYPE res(DOES_INTERSECT);

  Vec<3> vn = v;
  vn.Normalize();
  for (int i = 0; i < faces.Size(); i++)
    {
      const Point<3> & p1 = points[faces[i].pnums[0]];
      
      Vec<3> v0 = p - p1;
      double lam3 = -(faces[i].nn * v0); // n->nn 


      if (fabs (lam3) > eps) continue;
      //(*testout) << "lam3 <= eps" << endl;

      double lam1 = (faces[i].w1 * v0);
      double lam2 = (faces[i].w2 * v0);

      if (lam1 >= -eps_base1 && lam2 >= -eps_base1 && lam1+lam2 <= 1+eps_base1)
	{
	  point_on_faces.Append(i);

	  double scal = vn * faces[i].nn; // n->nn
	
	  res = DOES_INTERSECT;
	  if (scal > eps_base1) res = IS_OUTSIDE;
	  if (scal < -eps_base1) res = IS_INSIDE;
	}
    }
  
  //(*testout) << "point_on_faces.Size() " << point_on_faces.Size() 
  //	     << " res " << res << endl;

  if (point_on_faces.Size() == 0)
    return PointInSolid (p, 0);
  if (point_on_faces.Size() == 1)
    return res;



  
  double mindist(0);
  bool first = true;

  for(int i=0; i<point_on_faces.Size(); i++)
    {
      for(int j=0; j<3; j++)
	{
	  double dist = Dist(p,points[faces[point_on_faces[i]].pnums[j]]);
	  if(dist > eps && (first || dist < mindist))
	    {
	      mindist = dist;
	      first = false;
	    }
	}
    }
  
  Point<3> p2 = p + (1e-2*mindist) * vn;
  res = PointInSolid (p2, eps);

  //  (*testout) << "mindist " << mindist << " res " << res << endl;

  return res;
  
 
}