netgen/libsrc/gprim/geom2d.hpp

#ifndef FILE_GEOM2D
#define FILE_GEOM2D

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

#include <mydefs.hpp>

#include <general/template.hpp>
#include "geomobjects.hpp"
#include <meshing/global.hpp>

namespace netgen 
{

  /* Geometric Algorithms */

#define EPSGEOM 1E-5


  DLL_HEADER void MyError (const char * ch);

  class Point2d;
  class Vec2d;

  class LINE2D;
  class Line2d;
  class PLine2d;
  class TRIANGLE2D;
  class PTRIANGLE2D;


  inline Vec2d operator- (const Point2d & p1, const Point2d & p2);
  inline Point2d operator- (const Point2d & p1, const Vec2d & v);
  inline Point2d operator+ (const Point2d & p1, const Vec2d & v);
  inline Point2d Center (const Point2d & p1, const Point2d & p2);

  inline void PpSmV (const Point2d & p1, double s, const Vec2d & v, Point2d & p2);
  inline void PmP (const Point2d & p1, const Point2d & p2, Vec2d & v);
  ostream & operator<<(ostream  & s, const Point2d & p);
  inline Vec2d operator- (const Point2d & p1, const Point2d & p2);
  inline Point2d operator- (const Point2d & p1, const Vec2d & v);
  inline Point2d operator+ (const Point2d & p1, const Vec2d & v);
  inline Vec2d operator- (const Vec2d & p1, const Vec2d & v);
  inline Vec2d operator+ (const Vec2d & p1, const Vec2d & v);
  inline Vec2d operator* (double scal, const Vec2d & v);
  DLL_HEADER double Angle (const Vec2d & v);
  DLL_HEADER double FastAngle (const Vec2d & v);
  DLL_HEADER double Angle (const Vec2d & v1, const Vec2d & v2);
  DLL_HEADER double FastAngle (const Vec2d & v1, const Vec2d & v2);
  ostream & operator<<(ostream  & s, const Vec2d & v);
  double Dist2(const Line2d & g, const Line2d & h );		// GH
  int Near (const Point2d & p1, const Point2d & p2, const double eps);

  int Parallel (const Line2d & l1, const Line2d & l2, double peps = EPSGEOM);
  int IsOnLine (const Line2d & l, const Point2d & p, double heps = EPSGEOM);
  int IsOnLongLine (const Line2d & l, const Point2d & p);
  int Hit (const Line2d & l1, const Line2d & l2, double heps = EPSGEOM);
  ostream & operator<<(ostream  & s, const Line2d & l);
  DLL_HEADER Point2d CrossPoint (const PLine2d & l1, const PLine2d & l2);
  DLL_HEADER Point2d CrossPoint (const Line2d & l1, const Line2d & l2);
  int Parallel (const PLine2d & l1, const PLine2d & l2, double peps = EPSGEOM);
  int IsOnLine (const PLine2d & l, const Point2d & p, double heps = EPSGEOM);
  int IsOnLongLine (const PLine2d & l, const Point2d & p);
  int Hit (const PLine2d & l1, const Line2d & l2, double heps = EPSGEOM);
  ostream & operator<<(ostream  & s, const Line2d & l);
  ostream & operator<<(ostream  & s, const TRIANGLE2D & t); 
  ostream & operator<<(ostream & s, const PTRIANGLE2D & t);
  double Dist2 (const Point2d & p1, const Point2d & p2);

  ///
  class Point2d
  {
    ///
    friend class Vec2d;

  protected:
    ///
    double px, py;

  public:
    ///
    Point2d() { /* px = py = 0; */ }
    ///
    Point2d(double ax, double ay) { px = ax; py = ay; }
    ///
    Point2d(const Point2d & p2) { px = p2.px; py = p2.py; }

    Point2d (const Point<2> & p2)
    {
      px = p2(0);
      py = p2(1);
    }
    ///
    Point2d & operator= (const Point2d & p2)
    { px = p2.px; py = p2.py; return *this; }
    
    ///
    int operator== (const Point2d & p2) const			// GH
    { return (px == p2.px  &&  py == p2.py) ; }

    ///
    double & X() { return px; }
    ///
    double & Y() { return py; }
    ///
    double X() const { return px; }
    ///
    double Y() const { return py; }

    operator Point<2> () const
    {
      return Point<2> (px, py);
    }


    ///
    friend inline Vec2d operator- (const Point2d & p1, const Point2d & p2);
    ///
    friend inline Point2d operator- (const Point2d & p1, const Vec2d & v);
    ///
    friend inline Point2d operator+ (const Point2d & p1, const Vec2d & v);

    ///
    friend inline Point2d Center (const Point2d & p1, const Point2d & p2);

    const Point2d & SetToMin (const Point2d & p2)
    {
      if (p2.px < px) px = p2.px;
      if (p2.py < py) py = p2.py;
      return *this;
    }


    ///
    const Point2d & SetToMax (const Point2d & p2)
    {
      if (p2.px > px) px = p2.px;
      if (p2.py > py) py = p2.py;
      return *this;
    }

    ///
    friend double Dist (const Point2d & p1, const Point2d & p2)
    { return sqrt ( (p1.px - p2.px) * (p1.px - p2.px) +
		    (p1.py - p2.py) * (p1.py - p2.py) ); }
    //    { return sqrt ( sqr (p1.X()-p2.X()) + sqr (p1.Y()-p2.Y()) ); }

    ///
    friend double Dist2 (const Point2d & p1, const Point2d & p2)
    { return ( (p1.px - p2.px) * (p1.px - p2.px) +
	       (p1.py - p2.py) * (p1.py - p2.py) ); }
    //    { return sqr (p1.X()-p2.X()) + sqr (p1.Y()-p2.Y()) ; }


    /**
       Points clock-wise ?
       Are the points (p1, p2, p3) clock-wise ?
    */
    friend inline int CW (const Point2d & p1, const Point2d & p2, const Point2d & p3)
    {
      //      return Cross (p2 - p1, p3 - p2) < 0;      
      return
	(p2.px - p1.px) * (p3.py - p2.py) - 
	(p2.py - p1.py) * (p3.px - p2.px) < 0;
    }
    /**
       Points counter-clock-wise ?
       Are the points (p1, p2, p3) counter-clock-wise ?
    */
    friend inline bool CCW (const Point2d & p1, const Point2d & p2, const Point2d & p3)
    {
      //      return Cross (p2 - p1, p3 - p2) > 0;
      return
	(p2.px - p1.px) * (p3.py - p2.py) - 
	(p2.py - p1.py) * (p3.px - p2.px) > 0;
    }  /**
	  Points counter-clock-wise ?
	  Are the points (p1, p2, p3) counter-clock-wise ?
       */
    friend inline bool CCW (const Point2d & p1, const Point2d & p2, const Point2d & p3, double eps)
    {
      //      return Cross (p2 - p1, p3 - p2) > 0;
      double ax = p2.px - p1.px;
      double ay = p2.py - p1.py;
      double bx = p3.px - p2.px;
      double by = p3.py - p2.py;

      return ax*by - ay*bx > eps*eps*max2(ax*ax+ay*ay,bx*bx+by*by);
    }

    ///
    friend inline void PpSmV (const Point2d & p1, double s, const Vec2d & v, Point2d & p2);
    ///
    friend inline void PmP (const Point2d & p1, const Point2d & p2, Vec2d & v);

    ///
    friend ostream & operator<<(ostream  & s, const Point2d & p);
  };


  inline int Near (const Point2d & p1, const Point2d & p2, 
		   const double eps = 1e-4 )
  { 
    return  Dist2(p1,p2) <= eps*eps; 
  }






  ///
  class Vec2d
  {
  protected:
    ///
    double vx, vy;

  public:
    ///
    Vec2d() { /* vx = vy = 0; */ }
    ///
    Vec2d(double ax, double ay)
    { vx = ax; vy = ay; }
    ///
    Vec2d(const Vec2d & v2) { vx = v2.vx; vy = v2.vy; }

    ///
    explicit Vec2d(const Vec<2> & v2) { vx = v2(0); vy = v2(1); }

    ///
    Vec2d(const Point2d & p1, const Point2d & p2)
    { vx = p2.px - p1.px; vy = p2.py - p1.py; }
    
    ///
    Vec2d & operator= (const Vec2d & p2)
    { vx = p2.vx; vy = p2.vy; return *this; }

    ///
    double & X() { return vx; }
    ///
    double & Y() { return vy; }
    ///
    double X() const { return vx; }
    ///
    double Y() const { return vy; }

    ///
    double Length() const { return sqrt (vx * vx + vy * vy); }
    ///
    double Length2() const { return vx * vx + vy * vy; }

    void GetNormal (Vec2d & n) const { n.vx=-vy; n.vy=vx; }		// GH

    ///
    inline Vec2d & operator+= (const Vec2d & v2);
    ///
    inline Vec2d & operator-= (const Vec2d & v2);
    ///
    inline Vec2d & operator*= (double s);
    ///
    inline Vec2d & operator/= (double s);

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

    ///
    friend double operator* (const Vec2d & v1, const Vec2d & v2)
    { return v1.X() * v2.X() + v1.Y() * v2.Y(); }


    ///
    friend double Cross (const Vec2d & v1, const Vec2d & v2)
    { return double(v1.X()) * double(v2.Y()) -
	double(v1.Y()) * double(v2.X()); }

    ///
    friend inline void PpSmV (const Point2d & p1, double s, const Vec2d & v, Point2d & p2);
    ///
    friend inline void PmP (const Point2d & p1, const Point2d & p2, Vec2d & v);

    ///						Angle in [0,2*PI)

    ///
    friend DLL_HEADER double Angle (const Vec2d & v);
    ///
    friend DLL_HEADER double FastAngle (const Vec2d & v);
    ///
    friend DLL_HEADER double Angle (const Vec2d & v1, const Vec2d & v2);
    ///
    friend DLL_HEADER double FastAngle (const Vec2d & v1, const Vec2d & v2);

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



  ///
  class Line2d
  {
  protected:
    ///
    Point2d p1, p2;

  public:
    ///
    Line2d() : p1(), p2() { };
    ///
    Line2d(const Point2d & ap1, const Point2d & ap2)
    { p1 = ap1; p2 = ap2; }

    ///
    Line2d & operator= (const Line2d & l2)
    { p1 = l2.p1; p2 = l2.p2; return *this;}

    ///
    Point2d & P1() { return p1; }
    ///
    Point2d & P2() { return p2; }
    ///
    const Point2d & P1() const { return p1; }
    ///
    const Point2d & P2() const { return p2; }

    ///
    double XMax() const { return max2 (p1.X(), p2.X()); }
    ///
    double YMax() const { return max2 (p1.Y(), p2.Y()); }
    ///
    double XMin() const { return min2 (p1.X(), p2.X()); }
    ///
    double YMin() const { return min2 (p1.Y(), p2.Y()); }

    ///
    Vec2d Delta () const { return Vec2d (p2.X()-p1.X(), p2.Y()-p1.Y()); }
    ///
    double Length () const { return Delta().Length(); }
    ///
    double Length2 () const
    { return sqr (p1.X() - p2.X()) +
	sqr (p1.Y() - p2.Y()); }

    void GetNormal (Line2d & n) const;					// GH
    Vec2d NormalDelta () const;						// GH

    /// square of the distance between two 2d-lines.
    friend double Dist2(const Line2d & g, const Line2d & h );		// GH

    ///
    friend DLL_HEADER Point2d CrossPoint (const Line2d & l1, const Line2d & l2);
    /// returns 1 iff parallel
    friend int CrossPointBarycentric (const Line2d & l1, const Line2d & l2,
				      double & lam1, double & lam2, double eps);
    
    ///
    friend int Parallel (const Line2d & l1, const Line2d & l2, double peps);
    ///
    friend int IsOnLine (const Line2d & l, const Point2d & p, double heps);
    ///
    friend int IsOnLongLine (const Line2d & l, const Point2d & p);
    ///
    friend int Hit (const Line2d & l1, const Line2d & l2, double heps);

    ///
    friend ostream & operator<<(ostream  & s, const Line2d & l);
  };


#ifdef NONE
  ///
  class PLine2d
  {
  protected:
    ///
    Point2d const * p1, *p2;

  public:
    ///
    PLine2d() { };
    ///
    PLine2d(Point2d const * ap1, Point2d const * ap2)
    { p1 = ap1; p2 = ap2; }

    ///
    PLine2d & operator= (const PLine2d & l2)
    { p1 = l2.p1; p2 = l2.p2; return *this;}

    ///
    const Point2d *& P1() { return p1; }
    ///
    const Point2d *& P2() { return p2; }
    ///
    const Point2d & P1() const { return *p1; }
    ///
    const Point2d & P2() const { return *p2; }

    ///
    double XMax() const { return max2 (p1->X(), p2->X()); }
    ///
    double YMax() const { return max2 (p1->Y(), p2->Y()); }
    ///
    double XMin() const { return min2 (p1->X(), p2->X()); }
    ///
    double YMin() const { return min2 (p1->Y(), p2->Y()); }


    ///
    Vec2d Delta () const { return Vec2d (p2->X()-p1->X(), p2->Y()-p1->Y()); }
    ///
    double Length () const { return Delta().Length(); }
    ///
    double Length2 () const
    { return sqr (p1->X() - p2->X()) +
	sqr (p1->Y() - p2->Y()); }


    
    ///
    friend Point2d CrossPoint (const PLine2d & l1, const PLine2d & l2);
    ///
    friend int Parallel (const PLine2d & l1, const PLine2d & l2, double peps);
    ///
    friend int IsOnLine (const PLine2d & l, const Point2d & p, double heps);
    ///
    friend int IsOnLongLine (const PLine2d & l, const Point2d & p);
    ///
    friend int Hit (const PLine2d & l1, const Line2d & l2, double heps);

    ///
    friend ostream & operator<<(ostream  & s, const Line2d & l);
  };



  ///
  class ILINE
  {
    ///
    INDEX i[2];

  public:
    ///
    ILINE() {};
    ///
    ILINE(INDEX i1, INDEX i2) { i[0] = i1; i[1] = i2; }
    ///
    ILINE(const ILINE & l) { i[0] = l.i[0]; i[1] = l.i[1]; }

    ///
    ILINE & operator= (const ILINE & l)
    { i[0] = l.i[0]; i[1] = l.i[1]; return *this; }

    ///
    const INDEX & I(int ai) const { return i[ai-1]; }
    ///
    const INDEX & X() const { return i[0]; }
    ///
    const INDEX & Y() const { return i[1]; }
    ///
    const INDEX & I1() const { return i[0]; }
    ///
    const INDEX & I2() const { return i[1]; }

    ///
    INDEX & I(int ai) { return i[ai-1]; }
    ///
    INDEX & X() { return i[0]; }
    ///
    INDEX & Y() { return i[1]; }
    ///
    INDEX & I1() { return i[0]; }
    ///
    INDEX & I2() { return i[1]; }
  };




  ///
  class TRIANGLE2D
  {
  private:
    ///
    Point2d p1, p2, p3;

  public:
    ///
    TRIANGLE2D() { };
    ///
    TRIANGLE2D (const Point2d & ap1, const Point2d & ap2,
		const Point2d & ap3)
    { p1 = ap1; p2 = ap2; p3 = ap3;}

    ///
    TRIANGLE2D & operator= (const TRIANGLE2D & t2)
    { p1 = t2.p1; p2 = t2.p2; p3 = t2.p3; return *this; }

    ///
    Point2d & P1() { return p1; }
    ///
    Point2d & P2() { return p2; }
    ///
    Point2d & P3() { return p3; }
    ///
    const Point2d & P1() const { return p1; }
    ///
    const Point2d & P2() const { return p2; }
    ///
    const Point2d & P3() const { return p3; }

    ///
    double XMax() const { return max3 (p1.X(), p2.X(), p3.X()); }
    ///
    double YMax() const { return max3 (p1.Y(), p2.Y(), p3.Y()); }
    ///
    double XMin() const { return min3 (p1.X(), p2.X(), p3.X()); }
    ///
    double YMin() const { return min3 (p1.Y(), p2.Y(), p3.Y()); }

    ///
    inline Point2d Center () const
    { return Point2d( (p1.X()+p2.X()+p3.X())/3, (p1.Y()+p2.Y()+p3.Y())/3); }

    ///
    int Regular() const;
    /// 
    int CW () const;
    ///
    int CCW () const;

    ///
    int IsOn (const Point2d & p) const;
    ///
    int IsIn (const Point2d & p) const;
    ///
    friend ostream & operator<<(ostream  & s, const TRIANGLE2D & t);
  };


  ///
  class PTRIANGLE2D
  {
  private:
    ///
    Point2d const *p1, *p2, *p3;

  public:
    ///
    PTRIANGLE2D() { };
    ///
    PTRIANGLE2D (const Point2d * ap1, const Point2d * ap2,
		 const Point2d * ap3)
    { p1 = ap1; p2 = ap2; p3 = ap3;}

    ///
    PTRIANGLE2D & operator= (const PTRIANGLE2D & t2)
    { p1 = t2.p1; p2 = t2.p2; p3 = t2.p3; return *this; }

    ///
    const Point2d *& P1() { return p1; }
    ///
    const Point2d *& P2() { return p2; }
    ///
    const Point2d *& P3() { return p3; }
    ///
    const Point2d * P1() const { return p1; }
    ///
    const Point2d * P2() const { return p2; }
    ///
    const Point2d * P3() const { return p3; }

    ///
    double XMax() const { return max3 (p1->X(), p2->X(), p3->X()); }
    ///
    double YMax() const { return max3 (p1->Y(), p2->Y(), p3->Y()); }
    ///
    double XMin() const { return min3 (p1->X(), p2->X(), p3->X()); }
    ///
    double YMin() const { return min3 (p1->Y(), p2->Y(), p3->Y()); }

    ///
    Point2d Center () const
    { return Point2d( (p1->X()+p2->X()+p3->X())/3, (p1->Y()+p2->Y()+p3->Y())/3); }


    ///
    int Regular() const;
    ///
    int CW () const;
    ///
    int CCW () const;

    ///
    int IsOn (const Point2d & p) const;
    ///
    int IsIn (const Point2d & p) const;
    ///
    friend ostream & operator<<(ostream & s, const PTRIANGLE2D & t);
  };
#endif


  /** Cheap approximation to atan2.
      A monotone function of atan2(x,y) is computed.
  */
  extern double Fastatan2 (double x, double y);


  inline Vec2d & Vec2d :: operator+= (const Vec2d & v2)
  {
    vx += v2.vx;
    vy += v2.vy;
    return *this;
  }

  inline Vec2d & Vec2d :: operator-= (const Vec2d & v2)
  {
    vx -= v2.vx;
    vy -= v2.vy;
    return *this;
  }

  inline Vec2d & Vec2d :: operator*= (double s)
  {
    vx *= s;
    vy *= s;
    return *this;
  }


  inline Vec2d & Vec2d :: operator/= (double s)
  {
    if (s != 0)
      {
	vx /= s;
	vy /= s;
      }
    else
      {
	MyError ("Vec2d::operator /=: Division by zero");
      }
    return *this;
  }



  inline Vec2d operator- (const Point2d & p1, const Point2d & p2)
  {
    return Vec2d (p1.X() - p2.X(), p1.Y() - p2.Y());
  }


  inline Point2d operator- (const Point2d & p1, const Vec2d & v)
  {
    return Point2d (p1.X() - v.X(), p1.Y() - v.Y());
  }


  inline Point2d operator+ (const Point2d & p1, const Vec2d & v)
  {
    return Point2d (p1.X() + v.X(), p1.Y() + v.Y());
  }


  inline Point2d Center (const Point2d & p1, const Point2d & p2)
  {
    return Point2d ((p1.X() + p2.X()) / 2, (p1.Y() + p2.Y()) / 2);
  }


  inline Vec2d operator- (const Vec2d & v1, const Vec2d & v2)
  {
    return Vec2d (v1.X() - v2.X(), v1.Y() - v2.Y());
  }


  inline Vec2d operator+ (const Vec2d & v1, const Vec2d & v2)
  {
    return Vec2d (v1.X() + v2.X(), v1.Y() + v2.Y());
  }


  inline Vec2d operator* (double scal, const Vec2d & v)
  {
    return Vec2d (scal * v.X(), scal * v.Y());
  }


  inline void PpSmV (const Point2d & p1, double s,
		     const Vec2d & v, Point2d & p2)
  {
    p2.X() = p1.X() + s * v.X();
    p2.Y() = p1.Y() + s * v.Y();
  }


  inline void PmP (const Point2d & p1, const Point2d & p2, Vec2d & v)
  {
    v.X() = p1.X() - p2.X();
    v.Y() = p1.Y() - p2.Y();
  }





#ifdef none
  inline int TRIANGLE2D :: Regular() const
  {
    return fabs(Cross ( p2 - p1, p3 - p2)) > EPSGEOM;
  }


  inline int TRIANGLE2D :: CW () const
  {
    return Cross ( p2 - p1, p3 - p2) < 0;
  }


  inline int TRIANGLE2D :: CCW () const
  {
    return Cross ( p2 - p1, p3 - p2) > 0;
  }




  inline int PTRIANGLE2D :: Regular() const
  {
    return fabs(Cross ( *p2 - *p1, *p3 - *p2)) > EPSGEOM;
  }


  inline int PTRIANGLE2D :: CW () const
  {
    return Cross ( *p2 - *p1, *p3 - *p2) < 0;
  }


  inline int PTRIANGLE2D :: CCW () const
  {
    return Cross ( *p2 - *p1, *p3 - *p2) > 0;
  }


#endif


  ///
  class Mat2d
  {
  protected:
    ///
    double coeff[4];

  public:
    ///
    Mat2d() { coeff[0] = coeff[1] = coeff[2] = coeff[3] = 0; }
    ///
    Mat2d(double a11, double a12, double a21, double a22)
    { coeff[0] = a11; coeff[1] = a12; coeff[2] = a21; coeff[3] = a22; }
    ///
    Mat2d(const Mat2d & m2)
    { for (int i = 0; i < 4; i++) coeff[i] = m2.Get(i); }

    ///
    double & Elem (INDEX i, INDEX j) { return coeff[2*(i-1)+j-1]; }
    ///
    double & Elem (INDEX i) {return coeff[i]; }
    ///
    double Get (INDEX i, INDEX j) const { return coeff[2*(i-1)+j-1]; }
    ///
    double Get (INDEX i) const {return coeff[i]; }

    ///  
    double Det () const { return coeff[0] * coeff[3] - coeff[1] * coeff[2]; }

    ///
    void Mult (const Vec2d & v, Vec2d & prod) const;
    ///
    void MultTrans (const Vec2d & v , Vec2d & prod) const;
    ///
    void Solve (const Vec2d & rhs, Vec2d & x) const;
    /// Solves mat * x = rhs, but using a positive definite matrix instead of mat
    void SolvePositiveDefinite (const Vec2d & rhs, Vec2d & x) const;
    /// add a term \alpha * v * v^T
    void AddDiadicProduct (double alpha, Vec2d & v);
  };



  inline void Mat2d :: Mult (const Vec2d & v, Vec2d & prod) const
  {
    prod.X() = coeff[0] * v.X() + coeff[1] * v.Y();
    prod.Y() = coeff[2] * v.X() + coeff[3] * v.Y();
  }


  inline  void Mat2d :: MultTrans (const Vec2d & v, Vec2d & prod) const
  {
    prod.X() = coeff[0] * v.X() + coeff[2] * v.Y();
    prod.Y() = coeff[1] * v.X() + coeff[3] * v.Y();
  }



  inline void Mat2d :: Solve (const Vec2d & rhs, Vec2d & x) const
  {
    double det = Det();
  
    if (det == 0)
      MyError ("Mat2d::Solve: zero determinant");
    else
      {
	x.X() = (coeff[3] * rhs.X() - coeff[1] * rhs.Y()) / det;
	x.Y() = (-coeff[2] * rhs.X() + coeff[0] * rhs.Y()) / det;
      }
  }


  inline void Mat2d :: SolvePositiveDefinite (const Vec2d & rhs, Vec2d & x) const
  {
    double a = max2(coeff[0], 1e-8);
    double b = coeff[1] / a;
    double c = coeff[2] / a;
    double d = max2(coeff[3] - a *b * c, 1e-8);

    x.X() = (rhs.X() - b * rhs.Y()) / a;
    x.Y() = rhs.Y() / d - c * x.X();
  }


  inline void Mat2d :: AddDiadicProduct (double alpha, Vec2d & v)
  {
    coeff[0] += alpha * v.X() * v.X();
    coeff[1] += alpha * v.X() * v.Y();
    coeff[2] += alpha * v.Y() * v.X();
    coeff[3] += alpha * v.Y() * v.Y();
  }

}

#endif