netgen/libsrc/csg/spline3d.cpp

356 lines
7.7 KiB
C++
Raw Normal View History

2009-01-13 04:40:13 +05:00
#include <mystdlib.h>
#include <myadt.hpp>
#include <linalg.hpp>
#include <csg.hpp>
namespace netgen
{
splinesegment3d :: splinesegment3d (const Point<3> & ap1, const Point<3> & ap2,
const Point<3> & ap3)
{
p1 = ap1;
p2 = ap2;
p3 = ap3;
}
/*
todo
Tip von Joerg Stiller:
setzt Du in
void splinesegment3d :: Evaluate
Zeilen 54 und 56
b2 = 2 * t * (1-t);
b2 /= sqrt(2);
Das heisst, Du wichtest das zweite Bersteinpolynom mit
w2 = 1 / sqrt(2);
Das ist aber nur fuer 45-Grad-Segmente korrekt. Fuer den
allgemeinen Fall funktioniert
w2 = ( e(p3 - p1), e(p2 - p1) ); // also cos(winkel(p3-p1, p2-p1))
bzw. schoen symmetrisch
w2 = ( e(p3 - p1), e(p2 - p1) )/2 + ( e(p1 - p3), e(p2 - p3) )/2;
Das ist natuerlich kein C++ Code sondern symbolisch, wobei
e(p3 - p1) ist der von p1 zu p3 zeigende Einheitsvektor und
(a, b) steht fuer das Skalarprodukt zweier Vektoren etc.
Eine vergleichbare Information steht auch irgendwo im Hoscheck & Lasser.
Ich habe das Buch aber eben nicht zur Hand.
*/
void splinesegment3d :: Evaluate (double t, Point<3> & p) const
{
double x, y, z, w;
double b1, b2, b3;
b1 = (1-t)*(1-t);
b2 = 2 * t * (1-t);
b3 = t * t;
b2 /= sqrt(double(2));
x = p1(0) * b1 + p2(0) * b2 + p3(0) * b3;
y = p1(1) * b1 + p2(1) * b2 + p3(1) * b3;
z = p1(2) * b1 + p2(2) * b2 + p3(2) * b3;
w = b1 + b2 + b3;
p(0) = x / w;
p(1) = y / w;
p(2) = z / w;
}
void splinesegment3d :: EvaluateTangent (double t, Vec<3> & tang) const
{
double x, y, z, w, xprime, yprime, zprime, wprime;
double b1, b2, b3, b1prime, b2prime, b3prime;
b1 = (1-t)*(1-t);
b2 = 2 * t * (1-t);
b3 = t * t;
b2 /= sqrt(double(2));
b1prime = 2 * t - 2;
b2prime = - 4 * t + 2;
b3prime = 2 * t;
b2prime /= sqrt(double(2));
x = p1(0) * b1 + p2(0) * b2 + p3(0) * b3;
y = p1(1) * b1 + p2(1) * b2 + p3(1) * b3;
z = p1(2) * b1 + p2(2) * b2 + p3(2) * b3;
w = b1 + b2 + b3;
xprime = p1(0) * b1prime + p2(0) * b2prime + p3(0) * b3prime;
yprime = p1(1) * b1prime + p2(1) * b2prime + p3(1) * b3prime;
zprime = p1(2) * b1prime + p2(2) * b2prime + p3(2) * b3prime;
wprime = b1prime + b2prime + b3prime;
tang(0) = (w * xprime - x * wprime) / (w * w);
tang(1) = (w * yprime - y * wprime) / (w * w);
tang(2) = (w * zprime - z * wprime) / (w * w);
}
void spline3d :: AddSegment (const Point<3> & ap1, const Point<3> & ap2,
const Point<3> & ap3)
{
segments.Append (new splinesegment3d (ap1, ap2, ap3));
}
void spline3d :: Evaluate (double t, Point<3> & p) const
{
int nr;
double loct;
static int cnt = 0;
cnt++;
if (cnt % 10000 == 0) (*mycout) << "Evaluate calls: " << cnt << endl;
while (t < 0) t += GetNumSegments();
while (t >= GetNumSegments()) t -= GetNumSegments();
nr = 1 + int (t);
loct = t - nr + 1;
segments.Get(nr)->Evaluate (loct, p);
}
void spline3d :: EvaluateTangent (double t, Vec<3> & tang) const
{
int nr;
double loct;
while (t < 0) t += GetNumSegments();
while (t >= GetNumSegments()) t -= GetNumSegments();
nr = 1 + int (t);
loct = t - nr + 1;
segments.Get(nr)->EvaluateTangent (loct, tang);
}
double spline3d :: ProjectToSpline (Point<3> & p) const
{
double t, tl, tu, dt, dist, mindist, optt(0);
Point<3> hp;
Vec<3> tanx, px;
dt = 0.01;
mindist = 0;
for (t = 0; t <= GetNumSegments() + dt/2; t += dt)
{
Evaluate (t, hp);
dist = Dist (hp, p);
if (t == 0 || dist < mindist)
{
optt = t;
mindist = dist;
}
}
tu = optt + dt;
tl = optt - dt;
while (tu - tl > 1e-2)
{
optt = 0.5 * (tu + tl);
Evaluate (optt, hp);
EvaluateTangent (optt, tanx);
if (tanx * (hp - p) > 0)
tu = optt;
else
tl = optt;
}
optt = 0.5 * (tu + tl);
optt = ProjectToSpline (p, optt);
return optt;
}
double spline3d :: ProjectToSpline (Point<3> & p, double optt) const
{
double tl, tu, dt, val, dval, valu, vall;
Point<3> hp;
Vec<3> tanx, px;
int its = 0;
int cnt = 1000;
do
{
dt = 1e-8;
tl = optt - dt;
tu = optt + dt;
EvaluateTangent (optt, tanx);
Evaluate (optt, hp);
px = hp - p;
val = px * tanx;
EvaluateTangent (tl, tanx);
Evaluate (tl, hp);
px = hp - p;
vall = px * tanx;
EvaluateTangent (tu, tanx);
Evaluate (tu, hp);
px = hp - p;
valu = px * tanx;
dval = (valu - vall) / (2 * dt);
if (its % 100 == 99)
(*testout) << "optt = " << optt
<< " val = " << val
<< " dval = " << dval << endl;
optt -= val / dval;
its++;
if (fabs(val) < 1e-8 && cnt > 5) cnt = 5;
cnt--;
}
while (cnt > 0);
Evaluate (optt, p);
return optt;
}
splinetube :: splinetube (const spline3d & amiddlecurve, double ar)
: Surface(), middlecurve (amiddlecurve), r(ar)
{
(*mycout) << "Splinetube Allocated, r = " << r << endl;
}
void splinetube :: DefineTangentialPlane (const Point<3> & ap1,
const Point<3> & ap2)
{
double t;
double phi, z;
p1 = ap1;
p2 = ap2;
cp = p1;
t = middlecurve.ProjectToSpline (cp);
ex = p1 - cp;
middlecurve.EvaluateTangent (t, ez);
ex.Normalize();
ez.Normalize();
ey = Cross (ez, ex);
phi = r * atan2 (ey * (p2-cp), ex * (p2-cp));
z = ez * (p2 - cp);
e2x(0) = phi;
e2x(1) = z;
e2x.Normalize();
e2y(1) = e2x(0);
e2y(0) = -e2x(1);
// (*testout) << "Defineplane: " << endl
// << "p1 = " << p1 << " p2 = " << p2 << endl
// << "pc = " << cp << endl
// << "ex = " << ex << " ey = " << ey << " ez = " << ez << endl
// << "phi = " << phi << " z = " << z << endl
// << "e2x = " << e2x << " e2y = " << e2y << endl;
}
void splinetube :: ToPlane (const Point<3> & p3d, Point<2> & pplain, double h,
int & zone) const
{
Vec<2> v;
v(0) = r * atan2 (ey * (p3d-cp), ex * (p3d-cp));
v(1) = ez * (p3d - cp);
zone = 0;
if (v(0) > r * 2) zone = 1;
if (v(0) < r * 2) zone = 2;
pplain(0) = (v * e2x) / h;
pplain(1) = (v * e2y) / h;
}
void splinetube :: FromPlane (const Point<2> & pplain, Point<3> & p3d, double h) const
{
Vec<2> v;
v(0) = pplain(0) * h * e2x(0) + pplain(1) * h * e2y(0);
v(1) = pplain(0) * h * e2x(1) + pplain(1) * h * e2y(1);
p3d = p1 + v(0) * ey + v(1) * ez;
Project (p3d);
}
void splinetube :: Project (Point<3> & p3d) const
{
Point<3> hp;
hp = p3d;
middlecurve.ProjectToSpline (hp);
p3d = hp + (r / Dist(p3d, hp)) * (p3d - hp);
}
double splinetube :: CalcFunctionValue (const Point<3> & point) const
{
Point<3> hcp;
double rad;
hcp = point;
middlecurve.ProjectToSpline (hcp);
rad = Dist (hcp, point);
return 0.5 * (rad * rad / r - r);
}
void splinetube :: CalcGradient (const Point<3> & point, Vec<3> & grad) const
{
Point<3> hcp;
hcp = point;
middlecurve.ProjectToSpline (hcp);
grad = point - hcp;
grad /= r;
}
Point<3> splinetube :: GetSurfacePoint () const
{
Point<3> p;
Vec<3> t, n;
middlecurve.Evaluate (0, p);
middlecurve.EvaluateTangent (0, t);
n = t.GetNormal ();
n *= r;
(*mycout) << "p = " << p << " t = " << t << " n = " << n << endl;
return p + n;
}
void splinetube :: Print (ostream & str) const
{
int i;
str << "SplineTube, "
<< middlecurve.GetNumSegments () << " segments, r = " << r << endl;
for (i = 1; i <= middlecurve.GetNumSegments(); i++)
str << middlecurve.P1(i) << " - "
<< middlecurve.P2(i) << " - "
<< middlecurve.P3(i) << endl;
}
int splinetube :: BoxInSolid (const BoxSphere<3> & box) const
// 0 .. no, 1 .. yes, 2 .. maybe
{
Point<3> pc = box.Center();
middlecurve.ProjectToSpline (pc);
double d = Dist (pc, box.Center());
if (d < r - box.Diam()/2) return 1;
if (d > r + box.Diam()/2) return 0;
return 2;
}
}