netgen/libsrc/gprim/geom3d.hpp

#ifndef FILE_GEOM3D
#define FILE_GEOM3D

/* *************************************************************************/
/* File:   geom3d.hh                                                       */
/* Author: Joachim Schoeberl                                               */
/* Date:   5. Aug. 95                                                      */
/* *************************************************************************/

#include <mydefs.hpp>
#include "geom2d.hpp"

namespace netgen
{


  extern DLL_HEADER void MyError (const char * ch);

  class Point3d;
  class Vec3d;

  inline Vec3d operator- (const Point3d & p1, const Point3d & p2);
  inline Point3d operator- (const Point3d & p1, const Vec3d & v);
  inline Point3d operator+ (const Point3d & p1, const Vec3d & v);
  Point3d & Add (double d, const Vec3d & v);
  Point3d & Add2 (double d, const Vec3d & v,
		  double d2, const Vec3d & v2);
  inline Point3d Center (const Point3d & p1, const Point3d & p2);
  inline Point3d Center (const Point3d & p1, const Point3d & p2, const Point3d & p3);
  inline Point3d Center (const Point3d & p1, const Point3d & p2, 
			 const Point3d & p3, const Point3d & p4);
  ostream & operator<<(ostream  & s, const Point3d & p);
  inline Vec3d operator- (const Vec3d & p1, const Vec3d & v);
  inline Vec3d operator+ (const Vec3d & p1, const Vec3d & v);
  inline Vec3d operator* (double scal, const Vec3d & v);
  inline double operator* (const Vec3d & v1, const Vec3d & v2);
  inline Vec3d Cross (const Vec3d & v1, const Vec3d & v2);
  inline void Cross (const Vec3d & v1, const Vec3d & v2, Vec3d & prod);
  double Angle (const Vec3d & v);
  double FastAngle (const Vec3d & v);
  double Angle (const Vec3d & v1, const Vec3d & v2);
  double FastAngle (const Vec3d & v1, const Vec3d & v2);
  ostream & operator<<(ostream  & s, const Vec3d & v);
  void Transpose (Vec3d & v1, Vec3d & v2, Vec3d & v3);
  int SolveLinearSystem (const Vec3d & col1,
			 const Vec3d & col2,
			 const Vec3d & col3,
			 const Vec3d & rhs,
			 Vec3d & sol);
  int SolveLinearSystemLS (const Vec3d & col1,
			   const Vec3d & col2,
			   const Vec2d & rhs,
			   Vec3d & sol);
  int SolveLinearSystemLS2 (const Vec3d & col1,
			    const Vec3d & col2,
			    const Vec2d & rhs, 
			    Vec3d & sol,
			    double & x, double & y);
  int PseudoInverse (const Vec3d & col1,
		     const Vec3d & col2,
		     Vec3d & inv1,
		     Vec3d & inv2);
  double Determinant (const Vec3d & col1,
		      const Vec3d & col2,
		      const Vec3d & col3);

  inline double Dist2 (const Point3d & p1, const Point3d & p2);

  /// Point in R3
  class Point3d
  {
  protected:
    ///
    double x[3];
  
  public:
    ///
    Point3d () { x[0] = x[1] = x[2] = 0; }
    ///
    Point3d(double ax, double ay, double az) 
    { x[0] = ax; x[1] = ay; x[2] = az; }
    ///
    Point3d(double ax[3])
    { x[0] = ax[0]; x[1] = ax[1]; x[2] = ax[2]; }

    ///
    Point3d(const Point3d & p2) 
    { x[0] = p2.x[0]; x[1] = p2.x[1]; x[2] = p2.x[2]; }

    Point3d (const Point<3> & p2)
    {
      for (int i = 0; i < 3; i++)
	x[i] = p2(i);
    }
  
    ///
    Point3d & operator= (const Point3d & p2)
    { x[0] = p2.x[0]; x[1] = p2.x[1]; x[2] = p2.x[2]; return *this; }
  
    ///
    int operator== (const Point3d& p) const
    { return (x[0] == p.x[0] && x[1] == p.x[1] && x[2] == p.x[2]); }
  
    ///
    double & X() { return x[0]; }
    ///
    double & Y() { return x[1]; }
    ///
    double & Z() { return x[2]; }
    ///
    double X() const { return x[0]; }
    ///
    double Y() const { return x[1]; }
    ///
    double Z() const { return x[2]; }
    ///
    double & X(int i) { return x[i-1]; }
    ///
    double X(int i) const { return x[i-1]; }
    ///
    const Point3d & SetToMin (const Point3d & p2)
    {
      if (p2.x[0] < x[0]) x[0] = p2.x[0];
      if (p2.x[1] < x[1]) x[1] = p2.x[1];
      if (p2.x[2] < x[2]) x[2] = p2.x[2];
      return *this;
    }

    ///
    const Point3d & SetToMax (const Point3d & p2)
    {
      if (p2.x[0] > x[0]) x[0] = p2.x[0];
      if (p2.x[1] > x[1]) x[1] = p2.x[1];
      if (p2.x[2] > x[2]) x[2] = p2.x[2];
      return *this;
    }

    ///
    friend inline Vec3d operator- (const Point3d & p1, const Point3d & p2);
    ///
    friend inline Point3d operator- (const Point3d & p1, const Vec3d & v);
    ///
    friend inline Point3d operator+ (const Point3d & p1, const Vec3d & v);
    ///
    inline Point3d & operator+= (const Vec3d & v);
    inline Point3d & operator-= (const Vec3d & v);
    ///
    inline Point3d & Add (double d, const Vec3d & v);
    ///
    inline Point3d & Add2 (double d, const Vec3d & v,
			   double d2, const Vec3d & v2);
    ///
    friend inline double Dist (const Point3d & p1, const Point3d & p2)
    { return sqrt (  (p1.x[0]-p2.x[0]) * (p1.x[0]-p2.x[0]) + 
		     (p1.x[1]-p2.x[1]) * (p1.x[1]-p2.x[1]) +
                     (p1.x[2]-p2.x[2]) * (p1.x[2]-p2.x[2])); }
    ///
    inline friend double Dist2 (const Point3d & p1, const Point3d & p2)
    { return  (  (p1.x[0]-p2.x[0]) * (p1.x[0]-p2.x[0]) + 
		 (p1.x[1]-p2.x[1]) * (p1.x[1]-p2.x[1]) +
		 (p1.x[2]-p2.x[2]) * (p1.x[2]-p2.x[2])); }

    ///
    friend inline Point3d Center (const Point3d & p1, const Point3d & p2);
    ///
    friend inline Point3d Center (const Point3d & p1, const Point3d & p2, const Point3d & p3);
    ///
    friend inline Point3d Center (const Point3d & p1, const Point3d & p2, 
				  const Point3d & p3, const Point3d & p4);
    ///
    friend ostream & operator<<(ostream  & s, const Point3d & p);
  
    ///
    friend class Vec3d;
    ///
    friend class Box3d;


    operator Point<3> () const
    {
      return Point<3> (x[0], x[1], x[2]);
    }
  };


  ///
  class Vec3d
  {
  protected:
    ///
    double x[3];

  public:
    ///
    inline Vec3d() { x[0] = x[1] = x[2] = 0; }
    ///
    inline Vec3d(double ax, double ay, double az)
    { x[0] = ax; x[1] = ay; x[2] = az; }
    ///
    Vec3d(double ax[3])
    { x[0] = ax[0]; x[1] = ax[1]; x[2] = ax[2]; }
    ///
    inline Vec3d(const Vec3d & v2) 
    { x[0] = v2.x[0]; x[1] = v2.x[1]; x[2] = v2.x[2]; }
    ///
    inline Vec3d(const Point3d & p1, const Point3d & p2)
    { 
      x[0] = p2.x[0] - p1.x[0];
      x[1] = p2.x[1] - p1.x[1];
      x[2] = p2.x[2] - p1.x[2]; 
    }
    ///
    inline Vec3d(const Point3d & p1)
    { 
      x[0] = p1.x[0];
      x[1] = p1.x[1];
      x[2] = p1.x[2];
    }
  
    Vec3d (const Vec<3> & v2)
    {
      for (int i = 0; i < 3; i++)
	x[i] = v2(i);
    }

    operator Vec<3> () const
    {
      return Vec<3> (x[0], x[1], x[2]);
    }


    Vec3d & operator= (const Vec3d & v2)
    { x[0] = v2.x[0]; x[1] = v2.x[1]; x[2] = v2.x[2]; return *this; }
    ///
    Vec3d & operator= (double val)
    { x[0] = x[1] = x[2] = val; return *this; }
    ///
    double & X() { return x[0]; }
    ///
    double & Y() { return x[1]; }
    ///
    double & Z() { return x[2]; }
    ///
    double & X(int i) { return x[i-1]; }

    ///
    double X() const { return x[0]; }
    ///
    double Y() const { return x[1]; }
    ///
    double Z() const { return x[2]; }
    ///
    double X(int i) const { return x[i-1]; }

    ///
    double Length() const 
    { return sqrt (x[0] * x[0] + x[1] * x[1] + x[2] * x[2]); }
    ///
    double Length2() const 
    { return x[0] * x[0] + x[1] * x[1] + x[2] * x[2]; }

    ///
    inline friend double Dist (const Vec3d & v1, const Vec3d & v2)
    { return sqrt (  (v1.x[0]-v2.x[0]) * (v1.x[0]-v2.x[0]) + 
		     (v1.x[1]-v2.x[1]) * (v1.x[1]-v2.x[1]) +
                     (v1.x[2]-v2.x[2]) * (v1.x[2]-v2.x[2])); }
    ///
    inline friend double Dist2 (const Vec3d & v1, const Vec3d & v2)
    { return  (  (v1.x[0]-v2.x[0]) * (v1.x[0]-v2.x[0]) + 
		 (v1.x[1]-v2.x[1]) * (v1.x[1]-v2.x[1]) +
		 (v1.x[2]-v2.x[2]) * (v1.x[2]-v2.x[2])); }

    ///
    Vec3d & operator+= (const Vec3d & v2);
    ///
    Vec3d & operator-= (const Vec3d & v2);
    ///
    Vec3d & operator*= (double s);
    ///
    Vec3d & operator/= (double s);
    ///
    inline Vec3d & Add (double d, const Vec3d & v);
    ///
    inline Vec3d & Add2 (double d, const Vec3d & v,
			 double d2, const Vec3d & v2);

    ///
    friend inline Vec3d operator- (const Point3d & p1, const Point3d & p2);
    ///
    friend inline Point3d operator- (const Point3d & p1, const Vec3d & v);
    ///
    friend inline Point3d operator+ (const Point3d & p1, const Vec3d & v);
    ///
    friend inline Vec3d operator- (const Vec3d & p1, const Vec3d & v);
    ///
    friend inline Vec3d operator+ (const Vec3d & p1, const Vec3d & v);
    ///
    friend inline Vec3d operator* (double scal, const Vec3d & v);

    ///
    friend inline double operator* (const Vec3d & v1, const Vec3d & v2);
    ///
    friend inline Vec3d Cross (const Vec3d & v1, const Vec3d & v2);
    ///
    friend inline void Cross (const Vec3d & v1, const Vec3d & v2, Vec3d & prod);

    /// Returns one normal-vector to n
    DLL_HEADER void GetNormal (Vec3d & n) const;
    ///
    friend double Angle (const Vec3d & v);
    ///
    friend double FastAngle (const Vec3d & v);
    ///
    friend double Angle (const Vec3d & v1, const Vec3d & v2);
    ///
    friend double FastAngle (const Vec3d & v1, const Vec3d & v2);

    void Normalize() 
    {
      double len = (x[0] * x[0] + x[1] * x[1] + x[2] * x[2]);
      if (len == 0) return;
      len = sqrt (len);
      x[0] /= len; x[1] /= len; x[2] /= len;
    }

    ///
    friend ostream & operator<<(ostream  & s, const Vec3d & v);

    ///
    friend class Point3d;
    friend void Transpose (Vec3d & v1, Vec3d & v2, Vec3d & v3);
    friend int SolveLinearSystem (const Vec3d & col1,
				  const Vec3d & col2,
				  const Vec3d & col3,
				  const Vec3d & rhs,
				  Vec3d & sol);
    friend int SolveLinearSystemLS (const Vec3d & col1,
				    const Vec3d & col2,
				    const Vec2d & rhs,
				    Vec3d & sol);
    friend int SolveLinearSystemLS2 (const Vec3d & col1,
				     const Vec3d & col2,
				     const Vec2d & rhs, 
				     Vec3d & sol,
				     double & x, double & y);
    friend int PseudoInverse (const Vec3d & col1,
			      const Vec3d & col2,
			      Vec3d & inv1,
			      Vec3d & inv2);
    friend double Determinant (const Vec3d & col1,
			       const Vec3d & col2,
			       const Vec3d & col3);
  };



  class QuadraticFunction3d
  {
    double c0, cx, cy, cz;
    double cxx, cyy, czz, cxy, cxz, cyz;

  public:
    QuadraticFunction3d (const Point3d & p, const Vec3d & v);
    double Eval (const Point3d & p)
    {
      return 
	c0 
	+ p.X() * (cx + cxx * p.X() + cxy * p.Y() + cxz * p.Z())
	+ p.Y() * (cy + cyy * p.Y() + cyz * p.Z())
	+ p.Z() * (cz + czz * p.Z());
    }
  };



  inline Point3d Center (const Point3d & p1, const Point3d & p2)
  {
    return Point3d (0.5 * (p1.x[0] + p2.x[0]),
		    0.5 * (p1.x[1] + p2.x[1]),
		    0.5 * (p1.x[2] + p2.x[2]));
  }


  inline Point3d Center (const Point3d & p1, const Point3d & p2,
			 const Point3d & p3)
  {
    return Point3d (1.0/3.0 * (p1.x[0] + p2.x[0] + p3.x[0]),
		    1.0/3.0 * (p1.x[1] + p2.x[1] + p3.x[1]),
		    1.0/3.0 * (p1.x[2] + p2.x[2] + p3.x[2]));
  }

  inline Point3d Center (const Point3d & p1, const Point3d & p2,
			 const Point3d & p3, const Point3d & p4)
  {
    return Point3d (0.25 * (p1.x[0] + p2.x[0] + p3.x[0] + p4.x[0]),
		    0.25 * (p1.x[1] + p2.x[1] + p3.x[1] + p4.x[1]),
		    0.25 * (p1.x[2] + p2.x[2] + p3.x[2] + p4.x[2]));
  }



  inline Vec3d & Vec3d :: operator+= (const Vec3d & v2)
  {
    x[0] += v2.x[0];
    x[1] += v2.x[1];
    x[2] += v2.x[2];
    return *this;
  }

  inline Vec3d & Vec3d :: operator-= (const Vec3d & v2)
  {
    x[0] -= v2.x[0];
    x[1] -= v2.x[1];
    x[2] -= v2.x[2];
    return *this;
  }


  inline Vec3d & Vec3d :: operator*= (double s)
  {
    x[0] *= s;
    x[1] *= s;
    x[2] *= s;
    return *this;
  }


  inline Vec3d & Vec3d :: operator/= (double s)
  {
    if (s != 0)
      {
	x[0] /= s;
	x[1] /= s;
	x[2] /= s;
      }
#ifdef DEBUG
    else
      {
	cerr << "Vec div by 0, v = " << (*this) << endl;
	//      MyError ("Vec3d::operator /=: Divisioin by zero");
      }
#endif
    return *this;
  }

  inline Vec3d & Vec3d::Add (double d, const Vec3d & v)
  {
    x[0] += d * v.x[0]; 
    x[1] += d * v.x[1]; 
    x[2] += d * v.x[2];
    return *this;
  }

  inline Vec3d & Vec3d::Add2 (double d, const Vec3d & v,
			      double d2, const Vec3d & v2)
  {
    x[0] += d * v.x[0] + d2 * v2.x[0]; 
    x[1] += d * v.x[1] + d2 * v2.x[1]; 
    x[2] += d * v.x[2] + d2 * v2.x[2]; 
    return *this;
  }








  inline Vec3d operator- (const Point3d & p1, const Point3d & p2)
  {
    return Vec3d (p1.x[0] - p2.x[0], p1.x[1] - p2.x[1],p1.x[2] - p2.x[2]);
  }


  inline Point3d operator- (const Point3d & p1, const Vec3d & v)
  {
    return Point3d (p1.x[0] - v.x[0], p1.x[1] - v.x[1],p1.x[2] - v.x[2]);
  }


  inline Point3d operator+ (const Point3d & p1, const Vec3d & v)
  {
    return Point3d (p1.x[0] + v.x[0], p1.x[1] + v.x[1],p1.x[2] + v.x[2]);
  }

  inline Point3d & Point3d::operator+= (const Vec3d & v) 
  {
    x[0] += v.x[0]; 
    x[1] += v.x[1]; 
    x[2] += v.x[2];
    return *this;
  }

  inline Point3d & Point3d::operator-= (const Vec3d & v) 
  {
    x[0] -= v.x[0]; 
    x[1] -= v.x[1]; 
    x[2] -= v.x[2];
    return *this;
  }

  inline Point3d & Point3d::Add (double d, const Vec3d & v)
  {
    x[0] += d * v.x[0]; 
    x[1] += d * v.x[1]; 
    x[2] += d * v.x[2];
    return *this;
  }

  inline Point3d & Point3d::Add2 (double d, const Vec3d & v,
				  double d2, const Vec3d & v2)
  {
    x[0] += d * v.x[0] + d2 * v2.x[0]; 
    x[1] += d * v.x[1] + d2 * v2.x[1]; 
    x[2] += d * v.x[2] + d2 * v2.x[2]; 
    return *this;
  }


  inline Vec3d operator- (const Vec3d & v1, const Vec3d & v2)
  {
    return Vec3d (v1.x[0] - v2.x[0], v1.x[1] - v2.x[1],v1.x[2] - v2.x[2]);
  }


  inline Vec3d operator+ (const Vec3d & v1, const Vec3d & v2)
  {
    return Vec3d (v1.x[0] + v2.x[0], v1.x[1] + v2.x[1],v1.x[2] + v2.x[2]);
  }


  inline Vec3d operator* (double scal, const Vec3d & v)
  {
    return Vec3d (scal * v.x[0], scal * v.x[1], scal * v.x[2]);
  }



  inline double operator* (const Vec3d & v1, const Vec3d & v2)
  {
    return v1.x[0] * v2.x[0] + v1.x[1] * v2.x[1] + v1.x[2] * v2.x[2];
  }



  inline Vec3d Cross (const Vec3d & v1, const Vec3d & v2)
  {
    return Vec3d
      ( v1.x[1] * v2.x[2] - v1.x[2] * v2.x[1],
	v1.x[2] * v2.x[0] - v1.x[0] * v2.x[2],
	v1.x[0] * v2.x[1] - v1.x[1] * v2.x[0]);
  }

  inline void Cross (const Vec3d & v1, const Vec3d & v2, Vec3d & prod)
  {
    prod.x[0] = v1.x[1] * v2.x[2] - v1.x[2] * v2.x[1];
    prod.x[1] = v1.x[2] * v2.x[0] - v1.x[0] * v2.x[2];
    prod.x[2] = v1.x[0] * v2.x[1] - v1.x[1] * v2.x[0];
  }

  inline double Determinant (const Vec3d & col1,
			     const Vec3d & col2,
			     const Vec3d & col3)
  {
    return
      col1.x[0] * ( col2.x[1] * col3.x[2] - col2.x[2] * col3.x[1]) +
      col1.x[1] * ( col2.x[2] * col3.x[0] - col2.x[0] * col3.x[2]) +
      col1.x[2] * ( col2.x[0] * col3.x[1] - col2.x[1] * col3.x[0]);
  }


  ///
  class Box3d
  {
  protected:
    ///
    double minx[3], maxx[3];

  public:
    ///
    Box3d () { };
    ///
    DLL_HEADER Box3d ( double aminx, double amaxx,
	    double aminy, double amaxy,
	    double aminz, double amaxz );
    ///
    DLL_HEADER Box3d ( const Box3d & b2 );
    ///
    DLL_HEADER Box3d (const Point3d& p1, const Point3d& p2);
    ///
    DLL_HEADER Box3d (const Box<3> & b2);
    ///
    double MinX () const { return minx[0]; }
    ///
    double MaxX () const { return maxx[0]; }
    ///
    double MinY () const { return minx[1]; }
    ///
    double MaxY () const { return maxx[1]; }
    ///
    double MinZ () const { return minx[2]; }
    ///
    double MaxZ () const { return maxx[2]; }

    ///
    double Mini (int i) const { return minx[i-1]; }
    ///
    double Maxi (int i) const { return maxx[i-1]; }

    ///
    Point3d PMin () const { return Point3d(minx[0], minx[1], minx[2]); }
    ///
    Point3d PMax () const { return Point3d(maxx[0], maxx[1], maxx[2]); }

    ///
    void GetPointNr (int i, Point3d & point) const;
    /// increase Box at each side with dist 
    void Increase (double dist);
    /// increase Box by factor rel
    void IncreaseRel (double rel);
    /// return 1 if closures are intersecting
    int Intersect (const Box3d & box2) const
    {
      if (minx[0] > box2.maxx[0] || maxx[0] < box2.minx[0] ||
	  minx[1] > box2.maxx[1] || maxx[1] < box2.minx[1] ||
	  minx[2] > box2.maxx[2] || maxx[2] < box2.minx[2])
	return 0;
      return 1;
    }
    /// return 1 if point p in closure
    int IsIn (const Point3d & p) const
    {
      if (minx[0] <= p.x[0] && maxx[0] >= p.x[0] &&
	  minx[1] <= p.x[1] && maxx[1] >= p.x[1] &&
	  minx[2] <= p.x[2] && maxx[2] >= p.x[2])
	return 1;
      return 0;
    }
    ///
    inline void SetPoint (const Point3d & p)
    {
      minx[0] = maxx[0] = p.X();
      minx[1] = maxx[1] = p.Y();
      minx[2] = maxx[2] = p.Z();    
    }

    ///
    inline void AddPoint (const Point3d & p)
    {
      if (p.x[0] < minx[0]) minx[0] = p.x[0];
      if (p.x[0] > maxx[0]) maxx[0] = p.x[0];
      if (p.x[1] < minx[1]) minx[1] = p.x[1];
      if (p.x[1] > maxx[1]) maxx[1] = p.x[1];
      if (p.x[2] < minx[2]) minx[2] = p.x[2];
      if (p.x[2] > maxx[2]) maxx[2] = p.x[2];
    }

    ///
    const Box3d& operator+=(const Box3d& b);

    ///
    Point3d MaxCoords() const;
    ///
    Point3d MinCoords() const;

    /// Make a negative sized box;
    //  void CreateNegMinMaxBox();
  
    ///
    Point3d CalcCenter () const { return Point3d(0.5*(minx[0] + maxx[0]),
						 0.5*(minx[1] + maxx[1]),
						 0.5*(minx[2] + maxx[2])); }
    ///
    double CalcDiam () const { return sqrt(sqr(maxx[0]-minx[0])+
					   sqr(maxx[1]-minx[1])+
					   sqr(maxx[2]-minx[2])); }

    ///
    void WriteData(ofstream& fout) const;
    ///
    void ReadData(ifstream& fin);
  };


  class Box3dSphere : public Box3d
  {
  protected:
    ///
    double diam, inner;
    ///
    Point3d c;
  public:
    ///
    Box3dSphere () { };
    ///
    Box3dSphere ( double aminx, double amaxx,
		  double aminy, double amaxy,
		  double aminz, double amaxz);
    ///
    const Point3d & Center () const { return c; }

    ///
    double Diam () const { return diam; }
    ///
    double Inner () const { return inner; }
    ///
    void GetSubBox (int i, Box3dSphere & sbox) const;

    // private:
    ///
    void CalcDiamCenter ();
  };




  ///
  class referencetransform
  {
    ///
    Vec3d ex, ey, ez;
    ///
    Vec3d exh, eyh, ezh;
    ///
    Vec3d ex_h, ey_h, ez_h;
    ///
    Point3d rp;
    ///
    double h;

  public:

    ///
    void Set (const Point3d & p1, const Point3d & p2,
	      const Point3d & p3, double ah);

    ///
    void ToPlain (const Point3d & p, Point3d & pp) const;
    ///
    void ToPlain (const NgArray<Point3d> & p, NgArray<Point3d> & pp) const;
    ///
    void FromPlain (const Point3d & pp, Point3d & p) const;
  };

}


#endif