ForkMaster/netgen
1
0
Fork 0
mirror of https://github.com/NGSolve/netgen.git synced 2024-12-29 23:30:33 +05:00
Code Issues Packages Projects Releases Wiki Activity
86342534c1
netgen/libsrc/gprim/spline.cpp
Matthias Hochsteger cb87362f64 Rename Array to NgArray
2019-07-09 10:39:16 +02:00

561 lines
12 KiB
C++
Raw Blame History

/*
Spline curve for Mesh generator
*/
#include <mystdlib.h>
#include <linalg.hpp>
#include <gprim.hpp>
#include "spline.hpp"
namespace netgen
{
// just for testing (JS)
template <int D>
void ProjectTrivial (const SplineSeg3<D> & seg,
const Point<D> point, Point<D> & point_on_curve, double & t)
{
double mindist = -1;
for (int i = 0; i <= 1000; i++)
{
double ht = double(i)/1000;
Point<D> p = seg.GetPoint(ht);
double dist = Dist2 (p, point);
if (i == 0 || dist < mindist)
{
mindist = dist;
t = ht;
}
}
point_on_curve = seg.GetPoint(t);
}
template <>
void CircleSeg<3> :: LineIntersections (const double a, const double b, const double c,
NgArray < Point<3> > & points, const double eps) const
{
cerr << "CircleSeg<3>::LineIntersections not implemented" << endl;
}
template <>
void CircleSeg<2> :: LineIntersections (const double a, const double b, const double c,
NgArray < Point<2> > & points, const double eps) const
{
points.SetSize(0);
double px=0,py=0;
if(fabs(b) > 1e-20)
py = -c/b;
else
px = -c/a;
const double c1 = a*a + b*b;
const double c2 = 2. * ( a*(py-pm(1)) - b*(px-pm(0)));
const double c3 = pow(px-pm(0),2) + pow(py-pm(1),2) - pow(Radius(),2);
const double discr = c2*c2 - 4*c1*c3;
if(discr < 0)
return;
NgArray<double> t;
if(fabs(discr) < 1e-20)
t.Append(-0.5*c2/c1);
else
{
t.Append((-c2+sqrt(discr))/(2.*c1));
t.Append((-c2-sqrt(discr))/(2.*c1));
}
for(int i=0; i<t.Size(); i++)
{
Point<2> p (px-t[i]*b,py+t[i]*a);
double angle = atan2(p(1),p(0))+M_PI;
if(angle > StartAngle()-eps && angle < EndAngle()+eps)
points.Append(p);
}
}
template<int D>
SplineSeg3<D> :: SplineSeg3 (const GeomPoint<D> & ap1,
const GeomPoint<D> & ap2,
const GeomPoint<D> & ap3)
: p1(ap1), p2(ap2), p3(ap3)
{
weight = Dist (p1, p3) / sqrt (0.5 * (Dist2 (p1, p2) + Dist2 (p2, p3)));
// weight = sqrt(2);
// cout << "weight = " << weight << endl;
proj_latest_t = 0.5;
}
template<int D>
Point<D> SplineSeg3<D> :: GetPoint (double t) const
{
double b1, b2, b3;
b1 = (1-t)*(1-t);
b2 = weight * t * (1-t);
b3 = t * t;
Vec<D> hp = b1 * Vec<D>(p1) + b2 * Vec<D>(p2) + b3 * Vec<D>(p3);
double w = b1+b2+b3;
return Point<D> ((1.0/w)*hp);
/*
double x, y, w;
x = p1(0) * b1 + p2(0) * b2 + p3(0) * b3;
y = p1(1) * b1 + p2(1) * b2 + p3(1) * b3;
w = b1 + b2 + b3;
if(D==3)
{
double z = p1(2) * b1 + p2(2) * b2 + p3(2) * b3;
return Point<D> (x/w, y/w, z/w);
}
else
return Point<D> (x/w, y/w);
*/
}
template<int D>
Vec<D> SplineSeg3<D> :: GetTangent (const double t) const
{
const double b1 = (1.-t)*((weight-2.)*t-weight);
const double b2 = weight*(1.-2.*t);
const double b3 = t*((weight-2)*t+2.);
Vec<D> retval;
for(int i=0; i<D; i++)
retval(i) = b1*p1(i) + b2*p2(i) + b3*p3(i);
return retval;
}
template<int D>
void SplineSeg3<D> :: GetCoeff (Vector & u) const
{
DenseMatrix a(6, 6);
DenseMatrix ata(6, 6);
Vector f(6);
u.SetSize(6);
// ata.SetSymmetric(1);
double t = 0;
for (int i = 0; i < 5; i++, t += 0.25)
{
Point<D> p = GetPoint (t);
a(i, 0) = p(0) * p(0);
a(i, 1) = p(1) * p(1);
a(i, 2) = p(0) * p(1);
a(i, 3) = p(0);
a(i, 4) = p(1);
a(i, 5) = 1;
}
a(5, 0) = 1;
CalcAtA (a, ata);
u = 0;
u(5) = 1;
a.MultTrans (u, f);
ata.Solve (f, u);
// the sign
Point<D> p0 = GetPoint(0);
Vec<D> ht = GetTangent(0);
Vec<2> tang(ht(0), ht(1));
double gradx = 2.*u(0)*p0(0) + u(2)*p0(1) + u(3);
double grady = 2.*u(1)*p0(1) + u(2)*p0(0) + u(4);
Vec<2> gradn (grady, -gradx);
if (tang * gradn < 0) u *= -1;
}
template<int D>
void SplineSeg3<D> :: GetCoeff (Vector & u, Point<D> pref) const
{
DenseMatrix a(6, 6);
DenseMatrix ata(6, 6);
Vector f(6);
u.SetSize(6);
// ata.SetSymmetric(1);
double t = 0;
for (int i = 0; i < 5; i++, t += 0.25)
{
Vec<D> p = GetPoint (t)-pref;
a(i, 0) = p(0) * p(0);
a(i, 1) = p(1) * p(1);
a(i, 2) = p(0) * p(1);
a(i, 3) = p(0);
a(i, 4) = p(1);
a(i, 5) = 1;
}
a(5, 0) = 1;
CalcAtA (a, ata);
u = 0;
u(5) = 1;
a.MultTrans (u, f);
ata.Solve (f, u);
// the sign
// Point<D> p0 = GetPoint(0);
Vec<D> ht = GetTangent(0);
Vec<2> tang(ht(0), ht(1));
double gradx = u(3);
double grady = u(4);
// double gradx = 2.*u(0)*p0(0) + u(2)*p0(1) + u(3);
// double grady = 2.*u(1)*p0(1) + u(2)*p0(0) + u(4);
Vec<2> gradn (grady, -gradx);
if (tang * gradn < 0) u *= -1;
}
template<int D>
void SplineSeg3<D> :: Project (const Point<D> point, Point<D> & point_on_curve, double & t) const
{
double t_old = -1;
/*
if(proj_latest_t > 0. && proj_latest_t < 1.)
t = proj_latest_t;
else
t = 0.5;
*/
double tmin = 1;
double dist_min2 = Dist2 (GetPoint(tmin), point);
for (double ti = 0; ti < 0.99; ti += 0.25)
{
double di = Dist2(GetPoint(ti), point);
if (di < dist_min2)
{
tmin = ti;
dist_min2 = di;
}
}
t = tmin;
Point<D> phi;
Vec<D> phip,phipp,phimp;
int i=0;
while(t > -0.5 && t < 1.5 && i<20 && fabs(t-t_old) > 1e-15 )
{
GetDerivatives(t,phi,phip,phipp);
t_old = t;
phimp = phi-point;
//t = min2(max2(t-(phip*phimp)/(phipp*phimp + phip*phip),0.),1.);
t -= (phip*phimp)/(phipp*phimp + phip*phip);
i++;
}
//if(i<10 && t > 0. && t < 1.)
if(i<20 && t > -0.4 && t < 1.4)
{
if(t < 0)
{
t = 0.;
}
if(t > 1)
{
t = 1.;
}
point_on_curve = SplineSeg3<D>::GetPoint(t);
double dist = Dist(point,point_on_curve);
phi = SplineSeg3<D> ::GetPoint(0);
double auxdist = Dist(phi,point);
if(auxdist < dist)
{
t = 0.;
point_on_curve = phi;
dist = auxdist;
}
phi = SplineSeg3<D> ::GetPoint(1);
auxdist = Dist(phi,point);
if(auxdist < dist)
{
t = 1.;
point_on_curve = phi;
dist = auxdist;
}
}
else
{
double t0 = 0;
double t1 = 0.5;
double t2 = 1.;
double d0,d1,d2;
//(*testout) << "newtonersatz" << endl;
while(t2-t0 > 1e-8)
{
phi = SplineSeg3<D> ::GetPoint(t0); d0 = Dist(phi,point);
phi = SplineSeg3<D> ::GetPoint(t1); d1 = Dist(phi,point);
phi = SplineSeg3<D> ::GetPoint(t2); d2 = Dist(phi,point);
double a = (2.*d0 - 4.*d1 +2.*d2)/pow(t2-t0,2);
if(a <= 0)
{
if(d0 < d2)
t2 -= 0.3*(t2-t0);
else
t0 += 0.3*(t2-t0);
t1 = 0.5*(t2+t0);
}
else
{
double b = (d1-d0-a*(t1*t1-t0*t0))/(t1-t0);
double auxt1 = -0.5*b/a;
if(auxt1 < t0)
{
t2 -= 0.4*(t2-t0);
t0 = max2(0.,t0-0.1*(t2-t0));
}
else if (auxt1 > t2)
{
t0 += 0.4*(t2-t0);
t2 = min2(1.,t2+0.1*(t2-t0));
}
else
{
t1 = auxt1;
auxt1 = 0.25*(t2-t0);
t0 = max2(0.,t1-auxt1);
t2 = min2(1.,t1+auxt1);
}
t1 = 0.5*(t2+t0);
}
}
phi = SplineSeg3<D> ::GetPoint(t0); d0 = Dist(phi,point);
phi = SplineSeg3<D> ::GetPoint(t1); d1 = Dist(phi,point);
phi = SplineSeg3<D> ::GetPoint(t2); d2 = Dist(phi,point);
double mind = d0;
t = t0;
if(d1 < mind)
{
t = t1;
mind = d1;
}
if(d2 < mind)
{
t = t2;
mind = d2;
}
point_on_curve = SplineSeg3<D> ::GetPoint(t);
}
//(*testout) << " latest_t " << proj_latest_t << " t " << t << endl;
proj_latest_t = t;
/*
// test it by trivial sampling
double ht;
Point<D> hp;
ProjectTrivial (*this, point, hp, ht);
if (fabs (t-ht) > 1e-3)
{
// if (Dist2 (point, hp) < Dist2 (point, point_on_curve))
cout << "project is wrong" << endl;
cout << "t = " << t << ", ht = " << ht << endl;
cout << "dist org = " << Dist(point, point_on_curve) << endl;
cout << "dist trivial = " << Dist(point, hp) << endl;
}
*/
}
template<int D>
void SplineSeg3<D> :: GetDerivatives (const double t,
Point<D> & point,
Vec<D> & first,
Vec<D> & second) const
{
Vec<D> v1(p1), v2(p2), v3(p3);
double b1 = (1.-t)*(1.-t);
double b2 = weight*t*(1.-t);
double b3 = t*t;
double w = b1+b2+b3;
b1 *= 1./w; b2 *= 1./w; b3 *= 1./w;
double b1p = 2.*(t-1.);
double b2p = weight*(1.-2.*t);
double b3p = 2.*t;
const double wp = b1p+b2p+b3p;
const double fac1 = wp/w;
b1p *= 1./w; b2p *= 1./w; b3p *= 1./w;
const double b1pp = 2.;
const double b2pp = -2.*weight;
const double b3pp = 2.;
const double wpp = b1pp+b2pp+b3pp;
const double fac2 = (wpp*w-2.*wp*wp)/(w*w);
for(int i=0; i<D; i++)
point(i) = b1*p1(i) + b2*p2(i) + b3*p3(i);
first = (b1p - b1*fac1) * v1 +
(b2p - b2*fac1) * v2 +
(b3p - b3*fac1) * v3;
second = (b1pp/w - 2*b1p*fac1 - b1*fac2) * v1 +
(b2pp/w - 2*b2p*fac1 - b2*fac2) * v2 +
(b3pp/w - 2*b3p*fac1 - b3*fac2) * v3;
}
template<>
double SplineSeg3<2> :: MaxCurvature(void) const
{
Vec<2> v1 = p1-p2;
Vec<2> v2 = p3-p2;
double l1 = v1.Length();
double l2 = v2.Length();
double cosalpha = (v1*v2)/(l1*l2);
return sqrt(cosalpha + 1.)/(min2(l1,l2)*(1.-cosalpha));
}
template<>
double SplineSeg3<3> :: MaxCurvature(void) const
{
Vec<3> v1 = p1-p2;
Vec<3> v2 = p3-p2;
double l1 = v1.Length();
double l2 = v2.Length();
double cosalpha = v1*v2/(l1*l2);
return sqrt(cosalpha + 1.)/(min2(l1,l2)*(1.-cosalpha));
}
template<int D>
void SplineSeg3<D> :: LineIntersections (const double a, const double b, const double c,
NgArray < Point<D> > & points, const double eps) const
{
points.SetSize(0);
double t;
const double c1 = a*p1(0) - weight*a*p2(0) + a*p3(0)
+ b*p1(1) - weight*b*p2(1) + b*p3(1)
+ (2.-weight)*c;
const double c2 = -2.*a*p1(0) + weight*a*p2(0) -2.*b*p1(1) + weight*b*p2(1) + (weight-2.)*c;
const double c3 = a*p1(0) + b*p1(1) + c;
if(fabs(c1) < 1e-20)
{
if(fabs(c2) < 1e-20)
return;
t = -c3/c2;
if((t > -eps) && (t < 1.+eps))
points.Append(GetPoint(t));
return;
}
const double discr = c2*c2-4.*c1*c3;
if(discr < 0)
return;
if(fabs(discr/(c1*c1)) < 1e-14)
{
t = -0.5*c2/c1;
if((t > -eps) && (t < 1.+eps))
points.Append(GetPoint(t));
return;
}
t = (-c2 + sqrt(discr))/(2.*c1);
if((t > -eps) && (t < 1.+eps))
points.Append(GetPoint(t));
t = (-c2 - sqrt(discr))/(2.*c1);
if((t > -eps) && (t < 1.+eps))
points.Append(GetPoint(t));
}
template < int D >
void SplineSeg3<D> :: GetRawData (NgArray<double> & data) const
{
data.Append(3);
for(int i=0; i<D; i++)
data.Append(p1[i]);
for(int i=0; i<D; i++)
data.Append(p2[i]);
for(int i=0; i<D; i++)
data.Append(p3[i]);
}
template class SplineSeg3<2>;
template class SplineSeg3<3>;
RegisterClassForArchive<SplineSeg<2>> regss2;
RegisterClassForArchive<SplineSeg<3>> regss3;
RegisterClassForArchive<LineSeg<2>, SplineSeg<2>> regls2;
RegisterClassForArchive<LineSeg<3>, SplineSeg<3>> regls3;
RegisterClassForArchive<SplineSeg3<2>, SplineSeg<2>> regsss2;
RegisterClassForArchive<SplineSeg3<3>, SplineSeg<3>> regsss3;
}
Reference in New Issue View Git Blame Copy Permalink