src/foam/graph/curve/curveTools.C

   1 #include "scalar.H"
   2 #include "vector.H"
   3 #include "curveTools.H"
   4
   5 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
   6
   7 namespace Foam
   8 {
   9
  10 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
  11
  12 scalar distance(const vector& p1, const vector& p2)
  13 {
  14     return mag(p2 - p1);
  15 }
  16
  17
  18 bool stepForwardsToNextPoint
  19 (
  20     const vector& o,
  21     vector& n,
  22     label& i,
  23     label& ip1,
  24     scalar l,
  25     const curve& Curve
  26 )
  27 {
  28     label ip1n = ip1-1;
  29     while (++ip1n < Curve.size() && distance(o, Curve[ip1n]) < l);
  30     label in = ip1n - 1;
  31
  32     bool eoc = true;
  33
  34     if (ip1n < Curve.size() && in >= 0)
  35     {
  36         eoc = interpolate(Curve[in], Curve[ip1n], o, n, l);
  37
  38         i = in;
  39         ip1 = ip1n;
  40     }
  41
  42     return eoc;
  43 }
  44
  45
  46 bool stepBackwardsToNextPoint
  47 (
  48     const vector& o,
  49     vector& n,
  50     label& i,
  51     label& ip1,
  52     scalar l,
  53     const curve& Curve
  54 )
  55 {
  56     label ip1n = ip1+1;
  57     while (--ip1n >= 0 && distance(o, Curve[ip1n]) < l);
  58     label in = ip1n + 1;
  59
  60     bool eoc = true;
  61
  62     if (ip1n >= 0 && in < Curve.size())
  63     {
  64         eoc = interpolate(Curve[in], Curve[ip1n], o, n, l);
  65
  66         i = in;
  67         ip1 = ip1n;
  68     }
  69
  70     return eoc;
  71 }
  72
  73
  74 bool interpolate
  75 (
  76     const vector& p1,
  77     const vector& p2,
  78     const vector& o,
  79     vector& n,
  80     scalar l
  81 )
  82 {
  83     vector D = p1 - p2;
  84     scalar a = magSqr(D);
  85     scalar b = 2.0*(D&(p2 - o));
  86     scalar c = magSqr(p2) + (o&(o - 2.0*p2)) - l*l;
  87
  88     scalar b2m4ac = b*b - 4.0*a*c;
  89
  90     if (b2m4ac >= 0.0)
  91     {
  92         scalar srb2m4ac = sqrt(b2m4ac);
  93
  94         scalar lamda = (-b - srb2m4ac)/(2.0*a);
  95
  96         if (lamda > 1.0+curveSmall || lamda < -curveSmall)
  97         {
  98             lamda = (-b + srb2m4ac)/(2.0*a);
  99         }
 100
 101         if (lamda < 1.0+curveSmall && lamda > -curveSmall)
 102         {
 103             n = p2 + lamda*(p1 - p2);
 104
 105             return false;
 106         }
 107         else
 108         {
 109             return true;
 110         }
 111     }
 112     else
 113     {
 114         return true;
 115     }
 116 }
 117
 118
 119
 120 bool XstepForwardsToNextPoint
 121 (
 122     const vector& o,
 123     vector& n,
 124     label& i,
 125     label& ip1,
 126     scalar l,
 127     const curve& Curve
 128 )
 129 {
 130     label ip1n = ip1-1;
 131     while (++ip1n < Curve.size() && mag(o.x() - Curve[ip1n].x()) < l);
 132     label in = ip1n - 1;
 133
 134     bool eoc = true;
 135
 136     if (ip1n < Curve.size() && in >= 0)
 137     {
 138         eoc = Xinterpolate(Curve[in], Curve[ip1n], o, n, l);
 139
 140         i = in;
 141         ip1 = ip1n;
 142     }
 143
 144     return eoc;
 145 }
 146
 147
 148
 149 bool Xinterpolate
 150 (
 151     const vector& p1,
 152     const vector& p2,
 153     const vector& o,
 154     vector& n,
 155     scalar l
 156 )
 157 {
 158     n.x() = o.x() + l;
 159
 160     if (p2.x() < o.x() + l - curveSmall && p2.x() > o.x() - l + curveSmall)
 161     {
 162         return true;
 163     }
 164
 165     if (p2.x() < o.x() + l)
 166     {
 167         n.x() = o.x() - l;
 168     }
 169
 170     vector D = p2 - p1;
 171     scalar lamda = (n.x() - p1.x())/D.x();
 172     n.y() = p1.y() + lamda*D.y();
 173     return false;
 174
 175 }
 176
 177
 178 // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
 179
 180 } // End namespace Foam
 181
 182 // ************************************************************************* //