mirror of
https://github.com/NGSolve/netgen.git
synced 2024-09-22 17:52:50 +05:00
886 lines
20 KiB
C++
886 lines
20 KiB
C++
#ifndef FILE_GEOM2D
|
|
#define FILE_GEOM2D
|
|
|
|
/* *************************************************************************/
|
|
/* File: geom2d.hh */
|
|
/* Author: Joachim Schoeberl */
|
|
/* Date: 5. Aug. 95 */
|
|
/* *************************************************************************/
|
|
|
|
|
|
|
|
/* Geometric Algorithms */
|
|
|
|
#define EPSGEOM 1E-5
|
|
|
|
|
|
// extern 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);
|
|
double Angle (const Vec2d & v);
|
|
double FastAngle (const Vec2d & v);
|
|
double Angle (const Vec2d & v1, const Vec2d & v2);
|
|
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);
|
|
Point2d CrossPoint (const PLine2d & l1, const PLine2d & l2);
|
|
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 double Angle (const Vec2d & v);
|
|
///
|
|
friend double FastAngle (const Vec2d & v);
|
|
///
|
|
friend double Angle (const Vec2d & v1, const Vec2d & v2);
|
|
///
|
|
friend 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 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);
|
|
|
|
///
|
|
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
|
|
|
|
|
|
|
|
class Polygon2d
|
|
{
|
|
protected:
|
|
Array<Point2d> points;
|
|
|
|
public:
|
|
Polygon2d ();
|
|
~Polygon2d ();
|
|
|
|
void AddPoint (const Point2d & p);
|
|
int GetNP() const { return points.Size(); }
|
|
void GetPoint (int i, Point2d & p) const
|
|
{ p = points.Get(i); }
|
|
void GetLine (int i, Point2d & p1, Point2d & p2) const
|
|
{ p1 = points.Get(i); p2 = points.Get(i%points.Size()+1); }
|
|
|
|
double Area () const { return fabs (HArea()); }
|
|
int CW () const { return HArea() > 0; }
|
|
int CCW () const { return HArea() < 0; }
|
|
|
|
int IsOn (const Point2d & p) const;
|
|
int IsIn (const Point2d & p) const;
|
|
|
|
int IsConvex () const;
|
|
|
|
int IsStarPoint (const Point2d & p) const;
|
|
Point2d Center() const;
|
|
Point2d EqualAreaPoint () const;
|
|
private:
|
|
double HArea () const;
|
|
};
|
|
|
|
|
|
/** 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
|