curveTools.C

Go to the documentation of this file.

#include "scalar.H"
#include "vector.H"
#include "curveTools.H"
 
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
 
namespace Foam
{
 
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
 
scalar distance(const vector& p1, const vector& p2)
{
    return mag(p2 - p1);
}
 
 
bool stepForwardsToNextPoint
(
    const vector& o,
    vector& n,
    label& i,
    label& ip1,
    scalar l,
    const curve& Curve
)
{
    label ip1n = ip1-1;
    while (++ip1n < Curve.size() && distance(o, Curve[ip1n]) < l);
    label in = ip1n - 1;
 
    bool eoc = true;
 
    if (ip1n < Curve.size() && in >= 0)
    {
        eoc = interpolate(Curve[in], Curve[ip1n], o, n, l);
 
        i = in;
        ip1 = ip1n;
    }
 
    return eoc;
}
 
 
bool stepBackwardsToNextPoint
(
    const vector& o,
    vector& n,
    label& i,
    label& ip1,
    scalar l,
    const curve& Curve
)
{
    label ip1n = ip1+1;
    while (--ip1n >= 0 && distance(o, Curve[ip1n]) < l);
    label in = ip1n + 1;
 
    bool eoc = true;
 
    if (ip1n >= 0 && in < Curve.size())
    {
        eoc = interpolate(Curve[in], Curve[ip1n], o, n, l);
 
        i = in;
        ip1 = ip1n;
    }
 
    return eoc;
}
 
 
bool interpolate
(
    const vector& p1,
    const vector& p2,
    const vector& o,
    vector& n,
    scalar l
)
{
    vector D = p1 - p2;
    scalar a = magSqr(D);
    scalar b = 2.0*(D&(p2 - o));
    scalar c = magSqr(p2) + (o&(o - 2.0*p2)) - l*l;
 
    scalar b2m4ac = b*b - 4.0*a*c;
 
    if (b2m4ac >= 0.0)
    {
        scalar srb2m4ac = sqrt(b2m4ac);
 
        scalar lamda = (-b - srb2m4ac)/(2.0*a);
 
        if (lamda > 1.0+curveSmall || lamda < -curveSmall)
        {
            lamda = (-b + srb2m4ac)/(2.0*a);
        }
 
        if (lamda < 1.0+curveSmall && lamda > -curveSmall)
        {
            n = p2 + lamda*(p1 - p2);
 
            return false;
        }
        else
        {
            return true;
        }
    }
 
    return true;
}
 
 
 
bool XstepForwardsToNextPoint
(
    const vector& o,
    vector& n,
    label& i,
    label& ip1,
    scalar l,
    const curve& Curve
)
{
    label ip1n = ip1-1;
    while (++ip1n < Curve.size() && mag(o.x() - Curve[ip1n].x()) < l);
    label in = ip1n - 1;
 
    bool eoc = true;
 
    if (ip1n < Curve.size() && in >= 0)
    {
        eoc = Xinterpolate(Curve[in], Curve[ip1n], o, n, l);
 
        i = in;
        ip1 = ip1n;
    }
 
    return eoc;
}
 
 
 
bool Xinterpolate
(
    const vector& p1,
    const vector& p2,
    const vector& o,
    vector& n,
    scalar l
)
{
    n.x() = o.x() + l;
 
    if (p2.x() < o.x() + l - curveSmall && p2.x() > o.x() - l + curveSmall)
    {
        return true;
    }
 
    if (p2.x() < o.x() + l)
    {
        n.x() = o.x() - l;
    }
 
    vector D = p2 - p1;
    scalar lamda = (n.x() - p1.x())/D.x();
    n.y() = p1.y() + lamda*D.y();
 
    return false;
}
 
 
// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
 
} // End namespace Foam
 
// ************************************************************************* //