Пример #1
0
int CalcTriangleCenter (const Point3d ** pts, Point3d & c)
{
  static DenseMatrix a(2), inva(2);
  static Vector rs(2), sol(2);
  double h = Dist(*pts[0], *pts[1]);

  Vec3d v1(*pts[0], *pts[1]);
  Vec3d v2(*pts[0], *pts[2]);

  rs.Elem(1) = v1 * v1;
  rs.Elem(2) = v2 * v2;

  a.Elem(1,1) = 2 * rs.Get(1);
  a.Elem(1,2) = a.Elem(2,1) = 2 * (v1 * v2);
  a.Elem(2,2) = 2 * rs.Get(2);

  if (fabs (a.Det()) <= 1e-12 * h * h)
    {
      (*testout) << "CalcTriangleCenter: degenerated" << endl;
      return 1;
    }

  CalcInverse (a, inva);
  inva.Mult (rs, sol);

  c = *pts[0];
  v1 *= sol.Get(1);
  v2 *= sol.Get(2);

  c += v1;
  c += v2;

  return 0;
}
Пример #2
0
void Cholesky (const DenseMatrix & a,
	       DenseMatrix & l, Vector & d)
{
  // Factors   A = L D L^T

  double x;

  int i, j, k;
  int n = a.Height();
  
  //  (*testout) << "a = " << a << endl;

  l = a;

  for (i = 1; i <= n; i++)
    {
      for (j = i; j <= n; j++)
	{
	  x = l.Get(i, j);

	  for (k = 1; k < i; k++)
	    x -= l.Get(i, k) * l.Get(j, k) * d.Get(k); 
	  
	  if (i == j)
	    {
	      d.Elem(i) = x;
	    }
	  else
	    {
	      l.Elem(j, i) = x / d.Get(k);
	    }
	}
    }

  for (i = 1; i <= n; i++)
    {
      l.Elem(i, i) = 1;
      for (j = i+1; j <= n; j++)
	l.Elem(i, j) = 0;
    }

  /*
  // Multiply:
  (*testout) << "multiplied factors: " << endl;
  for (i = 1; i <= n; i++)
    for (j = 1; j <= n; j++)
      {
	x = 0;
	for (k = 1; k <= n; k++)
	  x += l.Get(i, k) * l.Get(j, k) * d.Get(k);
	(*testout) << x << " ";
      }
  (*testout) << endl;
  */
}
Пример #3
0
void MinFunction :: ApproximateHesse (const Vector & x,
				      DenseMatrix & hesse) const
{
  int n = x.Size();
  int i, j;

  static Vector hx;
  hx.SetSize(n);

  double eps = 1e-6;
  double f, f11, f12, f21, f22;
  
  for (i = 1; i <= n; i++)
    {
      for (j = 1; j < i; j++)
	{
	  hx = x;
	  hx.Elem(i) = x.Get(i) + eps;
	  hx.Elem(j) = x.Get(j) + eps;
	  f11 = Func(hx);
	  hx.Elem(i) = x.Get(i) + eps;
	  hx.Elem(j) = x.Get(j) - eps;
	  f12 = Func(hx);
	  hx.Elem(i) = x.Get(i) - eps;
	  hx.Elem(j) = x.Get(j) + eps;
	  f21 = Func(hx);
	  hx.Elem(i) = x.Get(i) - eps;
	  hx.Elem(j) = x.Get(j) - eps;
	  f22 = Func(hx);

	  hesse.Elem(i, j) = hesse.Elem(j, i) =
	    (f11 + f22 - f12 - f21) / (2 * eps * eps);
	}

      hx = x;
      f = Func(x);
      hx.Elem(i) = x.Get(i) + eps;
      f11 = Func(hx);
      hx.Elem(i) = x.Get(i) - eps;
      f22 = Func(hx);

      hesse.Elem(i, i) = (f11 + f22 - 2 * f) / (eps * eps);
    }
  //  (*testout) << "hesse = " << hesse << endl;
}
Пример #4
0
int LDLtUpdate (DenseMatrix & l, Vector & d, double a, const Vector & u)
{
  // Bemerkung: Es wird a aus R erlaubt
  // Rueckgabewert: 0 .. D bleibt positiv definit
  //                1 .. sonst

  int i, j, n;

  n = l.Height();

  Vector v(n);
  double t, told, xi;

  told = 1;
  v = u;

  for (j = 1; j <= n; j++)
    {
      t = told + a * sqr (v.Elem(j)) / d.Get(j);

      if (t <= 0) 
	{
	  (*testout) << "update err, t = " << t << endl;
	  return 1;
	}

      xi = a * v.Elem(j) / (d.Get(j) * t);

      d.Elem(j) *= t / told;

      for (i = j + 1; i <= n; i++)
	{
	  v.Elem(i) -= v.Elem(j) * l.Elem(i, j);
	  l.Elem(i, j) += xi * v.Elem(i);
	}

      told = t;
    }

  return 0;
}
Пример #5
0
int
IntersectTriangleLine (const Point<3> ** tri, const Point<3> ** line)
{
  Vec3d vl(*line[0], *line[1]);
  Vec3d vt1(*tri[0], *tri[1]);
  Vec3d vt2(*tri[0], *tri[2]);
  Vec3d vrs(*tri[0], *line[0]);

  static DenseMatrix a(3), ainv(3);
  static Vector rs(3), lami(3);
  int i;

  /*
  (*testout) << "Tri-Line inters: " << endl
	     << "tri = " << *tri[0] << ", " << *tri[1] << ", " << *tri[2] << endl
	     << "line = " << *line[0] << ", " << *line[1] << endl;
  */
  for (i = 1; i <= 3; i++)
    {
      a.Elem(i, 1) = -vl.X(i);
      a.Elem(i, 2) = vt1.X(i);
      a.Elem(i, 3) = vt2.X(i);
      rs.Elem(i) = vrs.X(i);
    }

  double det = a.Det();

  double arel = vl.Length() * vt1.Length() * vt2.Length();
  /*
  double amax = 0;
  for (i = 1; i <= 9; i++)
    if (fabs (a.Get(i)) > amax)
      amax = fabs(a.Get(i));
  */
  // new !!!!
  if (fabs (det) <= 1e-10 * arel)
    {
#ifdef DEVELOP      
      // line parallel to triangle !
      // cout << "ERROR: IntersectTriangleLine degenerated" << endl;
      //      (*testout) << "WARNING: IntersectTriangleLine degenerated\n";
      /*
      (*testout) << "lin-tri intersection: " << endl
		 << "line = " << *line[0] << " - " << *line[1] << endl
		 << "tri = " << *tri[0] << " - " << *tri[1] << " - " << *tri[2] << endl
		 << "lami = " << lami << endl
		 << "pc = " << ( *line[0] + lami.Get(1) * vl ) << endl
		 << "   = " << ( *tri[0] + lami.Get(2) * vt1 + lami.Get(3) * vt2) << endl
		 << " a = " << a << endl
		 << " ainv = " << ainv << endl
		 << " det(a) = " << det << endl
		 << " rs = " << rs << endl;
      */
#endif
      return 0;
    }

  CalcInverse (a, ainv);
  ainv.Mult (rs, lami);

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

  double eps = 1e-6;
  if (
      (lami.Get(1) >= -eps && lami.Get(1) <= 1+eps && 
       lami.Get(2) >= -eps && lami.Get(3) >= -eps && 
       lami.Get(2) + lami.Get(3) <= 1+eps)  && !
      (lami.Get(1) >= eps && lami.Get(1) <= 1-eps && 
       lami.Get(2) >= eps && lami.Get(3) >= eps && 
       lami.Get(2) + lami.Get(3) <= 1-eps) )


     {
#ifdef DEVELOP
       //      cout << "WARNING: IntersectTriangleLine degenerated" << endl;
      (*testout) << "WARNING: IntersectTriangleLine numerical inexact" << endl;

      (*testout) << "lin-tri intersection: " << endl
		 << "line = " << *line[0] << " - " << *line[1] << endl
		 << "tri = " << *tri[0] << " - " << *tri[1] << " - " << *tri[2] << endl
		 << "lami = " << lami << endl
		 << "pc = " << ( *line[0] + lami.Get(1) * vl ) << endl
		 << "   = " << ( *tri[0] + lami.Get(2) * vt1 + lami.Get(3) * vt2) << endl
		 << " a = " << a << endl
		 << " ainv = " << ainv << endl
		 << " det(a) = " << det << endl
		 << " rs = " << rs << endl;
#endif
    }
      

  if (lami.Get(1) >= 0 && lami.Get(1) <= 1 && 
      lami.Get(2) >= 0 && lami.Get(3) >= 0 && lami.Get(2) + lami.Get(3) <= 1)
    {

      return 1;
    }

  return 0;
}