C++ Vec3d::Length方法代码示例

int PseudoInverse (const Vec3d & col1,
		   const Vec3d & col2,
		   Vec3d & inv1,
		   Vec3d & inv2)
{
  double a11 = col1 * col1;
  double a12 = col1 * col2;
  double a22 = col2 * col2;
  
  double det = a11 * a22 - a12 * a12;

  if (fabs (det) < 1e-12 * col1.Length() * col2.Length())
    {
      inv1 = Vec3d (0, 0, 0);
      inv2 = Vec3d (0, 0, 0);
      return 1;
    }

  double ia11 = a22 / det;
  double ia12 = -a12 / det;
  double ia22 = a11 / det;

  inv1 = ia11 * col1 + ia12 * col2;
  inv2 = ia12 * col1 + ia22 * col2;

  return 0;
}

int SolveLinearSystemLS2 (const Vec3d & col1,
			 const Vec3d & col2,
			 const Vec2d & rhs,
			 Vec3d & sol, double & x, double & y)
{
  double a11 = col1 * col1;
  double a12 = col1 * col2;
  double a22 = col2 * col2;
  
  double det = a11 * a22 - a12 * a12;

  if (fabs (det) <= 1e-12 * col1.Length() * col2.Length() || 
      col1.Length2() == 0 || col2.Length2() == 0)
    {
      sol = Vec3d (0, 0, 0);
      x = 0; y = 0;
      return 1;
    }
  
  Vec2d invrhs;
  invrhs.X() = ( a22 * rhs.X() - a12 * rhs.Y()) / det;
  invrhs.Y() = (-a12 * rhs.X() + a11 * rhs.Y()) / det;

  sol.X() = invrhs.X() * col1.X() + invrhs.Y() * col2.X();
  sol.Y() = invrhs.X() * col1.Y() + invrhs.Y() * col2.Y();
  sol.Z() = invrhs.X() * col1.Z() + invrhs.Y() * col2.Z();

  x = invrhs.X();
  y = invrhs.Y();

  return 0;

  /*
  Vec3d inv1, inv2;
  int err = 
    PseudoInverse (col1, col2, inv1, inv2);

   sol = rhs.X() * inv1 + rhs.Y() * inv2;
   return err;
  */
}

void NGInterface::getMeshNormal(int i,double* x)
{
    Element2d &face = ((Mesh*)mesh)->SurfaceElement(i);
    const Point3d &lp1 = ((Mesh*)mesh)->Point (face.PNum(1)),
            &lp2 = ((Mesh*)mesh)->Point (face.PNum(2)),
            &lp3 = ((Mesh*)mesh)->Point (face.PNum(3));
    Vec3d n = Cross (Vec3d (lp1, lp2), Vec3d (lp1, lp3));

    n /= (n.Length()+1e-12);
    x[0] = n.X();
    x[1] = n.Y();
    x[2] = n.Z();
}

void WriteSTLFormat (const Mesh & mesh,
                     const string & filename)
{
    cout << "\nWrite STL Surface Mesh" << endl;

    ofstream outfile (filename.c_str());

    int i, j, k;

    outfile.precision(10);

    outfile << "solid" << endl;

    for (i = 1; i <= mesh.GetNSE(); i++)
    {
        outfile << "facet normal ";
        const Point3d& p1 = mesh.Point(mesh.SurfaceElement(i).PNum(1));
        const Point3d& p2 = mesh.Point(mesh.SurfaceElement(i).PNum(2));
        const Point3d& p3 = mesh.Point(mesh.SurfaceElement(i).PNum(3));

        Vec3d normal = Cross(p2-p1,p3-p1);
        if (normal.Length() != 0)
        {
            normal /= (normal.Length());
        }

        outfile << normal.X() << " " << normal.Y() << " " << normal.Z() << "\n";
        outfile << "outer loop\n";

        outfile << "vertex " << p1.X() << " " << p1.Y() << " " << p1.Z() << "\n";
        outfile << "vertex " << p2.X() << " " << p2.Y() << " " << p2.Z() << "\n";
        outfile << "vertex " << p3.X() << " " << p3.Y() << " " << p3.Z() << "\n";

        outfile << "endloop\n";
        outfile << "endfacet\n";
    }
    outfile << "endsolid" << endl;
}

void Vec3d :: GetNormal (Vec3d & n) const
  {
  if (fabs (X()) > fabs (Z()))
    {
    n.X() = -Y();
    n.Y() = X();
    n.Z() = 0;
    }
  else
    {
    n.X() = 0;
    n.Y() = Z();
    n.Z() = -Y();
    }
  double len = n.Length();
  if (len == 0)
    {
    n.X() = 1;
    n.Y() = n.Z() = 0;
    }
  else
    n /= len;
  }

//.........这里部分代码省略.........
	*/

	// test 3 tet-faces:
	for (i = 1; i <= 3; i++)
	  {
	    Vec3d vtet1, vtet2;
	    switch (i)
	      {
	      case 1:
		{
		  vtet1 = *tet[tetp2] - *tet[tetp1];
		  vtet2 = *tet[tetp3] - *tet[tetp1];
		  break;
		}
	      case 2:
		{
		  vtet1 = *tet[tetp3] - *tet[tetp1];
		  vtet2 = *tet[tetp4] - *tet[tetp1];
		  break;
		}
	      case 3:
		{
		  vtet1 = *tet[tetp4] - *tet[tetp1];
		  vtet2 = *tet[tetp2] - *tet[tetp1];
		  break;
		}
	      }
	    
	    Vec3d ntet;
	    Cross (vtet1, vtet2, ntet);
	    
	    Vec3d crline = Cross (ntri, ntet);

	    double lcrline = crline.Length();
	    if (lcrline < eps * eps)
	      continue;


	    if (vtri1 * crline + vtri2 * crline < 0)
	      crline *= -1;

	    double lam1, lam2, lam3, lam4;
	    LocalCoordinates (vtri1, vtri2, crline, lam1, lam2);
	    LocalCoordinates (vtet1, vtet2, crline, lam3, lam4);
	    
	    if (lam1 > -eps && lam2 > -eps &&
		lam3 > -eps && lam4 > -eps)
	      {
		//		(*testout) << "hit, cnt == 1" << "\n";
		return 1;
	      }
	  }

	return 0;
	break;
      }
    case 2:
      {
	// common edge
	tetp3 = 0;
	while (tetp3 == tetp1 || tetp3 == tetp2)
	  tetp3++;
	tetp4 = 6 - tetp1 - tetp2 - tetp3;
	trip3 = 3 - trip1 - trip2;

	//	(*testout) << "trip1,2,3 = " << trip1 << ", " << trip2 << ", " << trip3 << endl;

int IntersectTetTriangle (const Point<3> ** tet, const Point<3> ** tri,
			  const int * tetpi, const int * tripi)
{
  int i, j;
  double diam = Dist (*tri[0], *tri[1]);
  double epsrel = 1e-8;
  double eps = diam * epsrel;

  double eps2 = eps * eps;
  int cnt = 0;

  int tetp1 = -1, tetp2 = -1;
  int trip1 = -1, trip2 = -1;
  int tetp3, tetp4, trip3;

  /*
  for (i = 0; i < 4; i++)
    loctetpi[i] = -1;
  */


  if (!tetpi)
    {
      for (i = 0; i <= 2; i++)
	{
	  //	  loctripi[i] = -1;
	  for (j = 0; j <= 3; j++)
	    {
	      if (Dist2 (*tet[j], *tri[i]) < eps2)
		{
		  //		  loctripi[i] = j;
		  //		  loctetpi[j] = i;
		  cnt++;
		  tetp2 = tetp1;
		  tetp1 = j;
		  trip2 = trip1;
		  trip1 = i;
		  break;
		}
	    }
	}
    }
  else
    {
      for (i = 0; i <= 2; i++)
	{
	  //	  loctripi[i] = -1;
	  for (j = 0; j <= 3; j++)
	    {
	      if (tetpi[j] == tripi[i])
		{
		  //		  loctripi[i] = j;
		  //		  loctetpi[j] = i;
		  cnt++;
		  tetp2 = tetp1;
		  tetp1 = j;
		  trip2 = trip1;
		  trip1 = i;
		  break;
		}
	    }
	}
    }  
  
  //  (*testout) << "cnt = " << cnt << endl;


  //  (*testout) << "tet-trig inters, cnt = " << cnt << endl;
  
  // cnt .. number of common points
  switch (cnt)
    {
    case 0:
      {
	Vec3d no, n;
	int inpi[3];

	// check, if some trigpoint is in tet:

	for (j = 0; j < 3; j++)
	  inpi[j] = 1;

	for (i = 1; i <= 4; i++)
	  {
	    int pi1 = i % 4;
	    int pi2 = (i+1) % 4;
	    int pi3 = (i+2) % 4;
	    int pi4 = (i+3) % 4;

	    Vec3d v1 (*tet[pi1], *tet[pi2]);
	    Vec3d v2 (*tet[pi1], *tet[pi3]);
	    Vec3d v3 (*tet[pi1], *tet[pi4]);
	    Cross (v1, v2, n);

	    // n /= n.Length();
	    double nl = n.Length();

	    if (v3 * n > 0)
	      n *= -1;

//.........这里部分代码省略.........