netgen-mesher 6.2.2506.post35.dev0__cp314-cp314-win_amd64.whl

netgen/include/gprim/geom2d.hpp

@@ -0,0 +1,858 @@
+#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