mirror of
https://github.com/NGSolve/netgen.git
synced 2024-09-22 17:52:50 +05:00
745 lines
16 KiB
C++
745 lines
16 KiB
C++
#include <algorithm>
|
|
#include <mystdlib.h>
|
|
|
|
#include <myadt.hpp>
|
|
#include <gprim.hpp>
|
|
|
|
namespace netgen
|
|
{
|
|
ostream & operator<<(ostream & s, const Point3d & p)
|
|
{
|
|
return s << "(" << p.x[0] << ", " << p.x[1] << ", " << p.x[2] << ")";
|
|
}
|
|
|
|
ostream & operator<<(ostream & s, const Vec3d & v)
|
|
{
|
|
return s << "(" << v.x[0] << ", " << v.x[1] << ", " << v.x[2] << ")";
|
|
}
|
|
|
|
double Angle (const Vec3d & v1, const Vec3d & v2)
|
|
{
|
|
double co = (v1 * v2) / (v1.Length() * v2.Length());
|
|
if (co > 1) co = 1;
|
|
if (co < -1) co = -1;
|
|
return acos ( co );
|
|
}
|
|
|
|
|
|
void Vec3d :: GetNormal (Vec3d & n) const
|
|
{
|
|
if (fabs (X()) > fabs (Z()))
|
|
{
|
|
n.X() = -Y();
|
|
n.Y() = X();
|
|
n.Z() = 0;
|
|
}
|
|
else
|
|
{
|
|
n.X() = 0;
|
|
n.Y() = Z();
|
|
n.Z() = -Y();
|
|
}
|
|
double len = n.Length();
|
|
if (len == 0)
|
|
{
|
|
n.X() = 1;
|
|
n.Y() = n.Z() = 0;
|
|
}
|
|
else
|
|
n /= len;
|
|
}
|
|
|
|
/*
|
|
ostream & operator<<(ostream & s, const ROTDenseMatrix3D & r)
|
|
{
|
|
return s << "{ (" << r.txx << ", " << r.txy << ", " << r.txz << ") , ("
|
|
<< r.tyx << ", " << r.tyy << ", " << r.tyz << ") , ("
|
|
<< r.tzx << ", " << r.tzy << ", " << r.tzz << ") }";
|
|
}
|
|
*/
|
|
|
|
/*
|
|
Vec3d operator- (const Point3d & p1, const Point3d & p2)
|
|
{
|
|
return Vec3d (p1.X() - p2.X(), p1.Y() - p2.Y(),p1.Z() - p2.Z());
|
|
}
|
|
|
|
Point3d operator- (const Point3d & p1, const Vec3d & v)
|
|
{
|
|
return Point3d (p1.X() - v.X(), p1.Y() - v.Y(),p1.Z() - v.Z());
|
|
}
|
|
|
|
Point3d operator+ (const Point3d & p1, const Vec3d & v)
|
|
{
|
|
return Point3d (p1.X() + v.X(), p1.Y() + v.Y(),p1.Z() + v.Z());
|
|
}
|
|
|
|
Vec3d operator- (const Vec3d & v1, const Vec3d & v2)
|
|
{
|
|
return Vec3d (v1.X() - v2.X(), v1.Y() - v2.Y(),v1.Z() - v2.Z());
|
|
}
|
|
|
|
Vec3d operator+ (const Vec3d & v1, const Vec3d & v2)
|
|
{
|
|
return Vec3d (v1.X() + v2.X(), v1.Y() + v2.Y(),v1.Z() + v2.Z());
|
|
}
|
|
|
|
Vec3d operator* (double scal, const Vec3d & v)
|
|
{
|
|
return Vec3d (scal * v.X(), scal * v.Y(), scal * v.Z());
|
|
}
|
|
*/
|
|
/*
|
|
double operator* (const Vec3d & v1, const Vec3d & v2)
|
|
{
|
|
return v1.X() * v2.X() + v1.Y() * v2.Y() + v1.Z() * v2.Z();
|
|
}
|
|
|
|
double Cross (const Vec3d & v1, const Vec3d & v2)
|
|
{
|
|
return v1.X() * v2.Y() - v1.Y() * v2.X();
|
|
}
|
|
*/
|
|
|
|
/*
|
|
void ROTDenseMatrix3D :: CalcRotMat(double ag, double bg, double lg, double size2, Vec3d r)
|
|
{
|
|
size = size2;
|
|
txx=size * ( cos(bg) * cos(lg) );
|
|
txy=size * ( cos(bg) * sin(lg) );
|
|
txz=size * (-sin(bg) );
|
|
|
|
tyx=size * ( sin(ag) * sin(bg) * cos(lg) - cos(ag) * sin(lg) );
|
|
tyy=size * ( sin(ag) * sin(bg) * sin(lg) + cos(ag) * cos(lg) );
|
|
tyz=size * ( sin(ag) * cos(bg) );
|
|
|
|
tzx=size * ( cos(ag) * sin(bg) * cos(lg) + sin(ag) * sin(lg) );
|
|
tzy=size * ( cos(ag) * sin(bg) * sin(lg) - sin(ag) * cos(lg) );
|
|
tzz=size * ( cos(ag) * cos(bg) );
|
|
|
|
deltaR=r;
|
|
}
|
|
ROTDenseMatrix3D :: ROTDenseMatrix3D(double ag, double bg, double lg, double size2, Vec3d r)
|
|
{CalcRotMat(ag, bg, lg, size2, r); }
|
|
|
|
ROTDenseMatrix3D :: ROTDenseMatrix3D(Vec3d rot2)
|
|
{
|
|
Vec3d r2(0,0,0);
|
|
CalcRotMat(rot2.X(), rot2.Y(), rot2.Z(), 1, r2);
|
|
}
|
|
|
|
ROTDenseMatrix3D ROTDenseMatrix3D :: INV()
|
|
{
|
|
ROTDenseMatrix3D rinv(txx/sqr(size),tyx/sqr(size),tzx/sqr(size),
|
|
txy/sqr(size),tyy/sqr(size),tzy/sqr(size),
|
|
txz/sqr(size),tyz/sqr(size),tzz/sqr(size),
|
|
1/size,deltaR);
|
|
return rinv;
|
|
}
|
|
|
|
Vec3d operator* (const ROTDenseMatrix3D & r, const Vec3d & v)
|
|
{
|
|
return Vec3d (r.XX() * v.X() + r.XY() * v.Y() + r.XZ() * v.Z(),
|
|
r.YX() * v.X() + r.YY() * v.Y() + r.YZ() * v.Z(),
|
|
r.ZX() * v.X() + r.ZY() * v.Y() + r.ZZ() * v.Z() );
|
|
}
|
|
|
|
Point3d operator* (const ROTDenseMatrix3D & r, const Point3d & p)
|
|
{
|
|
return Point3d (r.XX() * p.X() + r.XY() * p.Y() + r.XZ() * p.Z(),
|
|
r.YX() * p.X() + r.YY() * p.Y() + r.YZ() * p.Z(),
|
|
r.ZX() * p.X() + r.ZY() * p.Y() + r.ZZ() * p.Z() );
|
|
}
|
|
*/
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
Box3d :: Box3d ( double aminx, double amaxx,
|
|
double aminy, double amaxy,
|
|
double aminz, double amaxz )
|
|
{
|
|
minx[0] = aminx; maxx[0] = amaxx;
|
|
minx[1] = aminy; maxx[1] = amaxy;
|
|
minx[2] = aminz; maxx[2] = amaxz;
|
|
}
|
|
|
|
Box3d :: Box3d ( const Box3d & b2 )
|
|
{
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
minx[i] = b2.minx[i];
|
|
maxx[i] = b2.maxx[i];
|
|
}
|
|
}
|
|
|
|
Box3d :: Box3d ( const Box<3> & b2 )
|
|
{
|
|
for (int i = 0; i < 3; i++)
|
|
{
|
|
minx[i] = b2.PMin()(i);
|
|
maxx[i] = b2.PMax()(i);
|
|
}
|
|
}
|
|
|
|
|
|
/*
|
|
int Box3d :: Intersect (const Box3d & box2) const
|
|
{
|
|
int i;
|
|
for (i = 0; i <= 2; i++)
|
|
if (minx[i] > box2.maxx[i] || maxx[i] < box2.minx[i])
|
|
return 0;
|
|
return 1;
|
|
}
|
|
*/
|
|
|
|
/*
|
|
void Box3d :: SetPoint (const Point3d & p)
|
|
{
|
|
minx[0] = maxx[0] = p.X();
|
|
minx[1] = maxx[1] = p.Y();
|
|
minx[2] = maxx[2] = p.Z();
|
|
}
|
|
|
|
void Box3d :: AddPoint (const Point3d & p)
|
|
{
|
|
if (p.X() < minx[0]) minx[0] = p.X();
|
|
if (p.X() > maxx[0]) maxx[0] = p.X();
|
|
if (p.Y() < minx[1]) minx[1] = p.Y();
|
|
if (p.Y() > maxx[1]) maxx[1] = p.Y();
|
|
if (p.Z() < minx[2]) minx[2] = p.Z();
|
|
if (p.Z() > maxx[2]) maxx[2] = p.Z();
|
|
}
|
|
*/
|
|
|
|
void Box3d :: GetPointNr (int i, Point3d & point) const
|
|
{
|
|
i--;
|
|
point.X() = (i & 1) ? maxx[0] : minx[0];
|
|
point.Y() = (i & 2) ? maxx[1] : minx[1];
|
|
point.Z() = (i & 4) ? maxx[2] : minx[2];
|
|
}
|
|
|
|
|
|
void Box3d :: Increase (double d)
|
|
{
|
|
for (int i = 0; i <= 2; i++)
|
|
{
|
|
minx[i] -= d;
|
|
maxx[i] += d;
|
|
}
|
|
}
|
|
|
|
void Box3d :: IncreaseRel (double /* rel */)
|
|
{
|
|
for (int i = 0; i <= 2; i++)
|
|
{
|
|
double d = 0.5 * (maxx[i] - minx[i]);
|
|
minx[i] -= d;
|
|
maxx[i] += d;
|
|
}
|
|
}
|
|
|
|
|
|
Box3d :: Box3d (const Point3d& p1, const Point3d& p2)
|
|
{
|
|
minx[0] = min2 (p1.X(), p2.X());
|
|
minx[1] = min2 (p1.Y(), p2.Y());
|
|
minx[2] = min2 (p1.Z(), p2.Z());
|
|
maxx[0] = max2 (p1.X(), p2.X());
|
|
maxx[1] = max2 (p1.Y(), p2.Y());
|
|
maxx[2] = max2 (p1.Z(), p2.Z());
|
|
}
|
|
|
|
const Box3d& Box3d :: operator+=(const Box3d& b)
|
|
{
|
|
minx[0] = min2 (minx[0], b.minx[0]);
|
|
minx[1] = min2 (minx[1], b.minx[1]);
|
|
minx[2] = min2 (minx[2], b.minx[2]);
|
|
maxx[0] = max2 (maxx[0], b.maxx[0]);
|
|
maxx[1] = max2 (maxx[1], b.maxx[1]);
|
|
maxx[2] = max2 (maxx[2], b.maxx[2]);
|
|
|
|
return *this;
|
|
}
|
|
|
|
Point3d Box3d :: MaxCoords() const
|
|
{
|
|
return Point3d(maxx[0], maxx[1], maxx[2]);
|
|
}
|
|
|
|
Point3d Box3d :: MinCoords() const
|
|
{
|
|
return Point3d(minx[0], minx[1], minx[2]);
|
|
}
|
|
|
|
/*
|
|
void Box3d :: CreateNegMinMaxBox()
|
|
{
|
|
minx[0] = MAXDOUBLE;
|
|
minx[1] = MAXDOUBLE;
|
|
minx[2] = MAXDOUBLE;
|
|
maxx[0] = MINDOUBLE;
|
|
maxx[1] = MINDOUBLE;
|
|
maxx[2] = MINDOUBLE;
|
|
|
|
}
|
|
*/
|
|
|
|
void Box3d :: WriteData(ofstream& fout) const
|
|
{
|
|
for(int i = 0; i < 3; i++)
|
|
{
|
|
fout << minx[i] << " " << maxx[i] << " ";
|
|
}
|
|
fout << "\n";
|
|
}
|
|
|
|
void Box3d :: ReadData(ifstream& fin)
|
|
{
|
|
for(int i = 0; i < 3; i++)
|
|
{
|
|
fin >> minx[i];
|
|
fin >> maxx[i];
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
Box3dSphere :: Box3dSphere ( double aminx, double amaxx,
|
|
double aminy, double amaxy,
|
|
double aminz, double amaxz )
|
|
: Box3d (aminx, amaxx, aminy, amaxy, aminz, amaxz)
|
|
{
|
|
CalcDiamCenter ();
|
|
}
|
|
|
|
|
|
void Box3dSphere :: CalcDiamCenter ()
|
|
{
|
|
diam = sqrt( sqr (maxx[0] - minx[0]) +
|
|
sqr (maxx[1] - minx[1]) +
|
|
sqr (maxx[2] - minx[2]));
|
|
|
|
c.X() = 0.5 * (minx[0] + maxx[0]);
|
|
c.Y() = 0.5 * (minx[1] + maxx[1]);
|
|
c.Z() = 0.5 * (minx[2] + maxx[2]);
|
|
|
|
inner = min2 ( min2 (maxx[0] - minx[0], maxx[1] - minx[1]), maxx[2] - minx[2]) / 2;
|
|
}
|
|
|
|
|
|
void Box3dSphere :: GetSubBox (int i, Box3dSphere & sbox) const
|
|
{
|
|
i--;
|
|
if (i & 1)
|
|
{
|
|
sbox.minx[0] = c.X();
|
|
sbox.maxx[0] = maxx[0];
|
|
}
|
|
else
|
|
{
|
|
sbox.minx[0] = minx[0];
|
|
sbox.maxx[0] = c.X();
|
|
}
|
|
if (i & 2)
|
|
{
|
|
sbox.minx[1] = c.Y();
|
|
sbox.maxx[1] = maxx[1];
|
|
}
|
|
else
|
|
{
|
|
sbox.minx[1] = minx[1];
|
|
sbox.maxx[1] = c.Y();
|
|
}
|
|
if (i & 4)
|
|
{
|
|
sbox.minx[2] = c.Z();
|
|
sbox.maxx[2] = maxx[2];
|
|
}
|
|
else
|
|
{
|
|
sbox.minx[2] = minx[2];
|
|
sbox.maxx[2] = c.Z();
|
|
}
|
|
|
|
// sbox.CalcDiamCenter ();
|
|
|
|
sbox.c.X() = 0.5 * (sbox.minx[0] + sbox.maxx[0]);
|
|
sbox.c.Y() = 0.5 * (sbox.minx[1] + sbox.maxx[1]);
|
|
sbox.c.Z() = 0.5 * (sbox.minx[2] + sbox.maxx[2]);
|
|
sbox.diam = 0.5 * diam;
|
|
sbox.inner = 0.5 * inner;
|
|
}
|
|
|
|
|
|
|
|
|
|
/*
|
|
double Determinant (const Vec3d & col1,
|
|
const Vec3d & col2,
|
|
const Vec3d & col3)
|
|
{
|
|
return
|
|
col1.x[0] * ( col2.x[1] * col3.x[2] - col2.x[2] * col3.x[1]) +
|
|
col1.x[1] * ( col2.x[2] * col3.x[0] - col2.x[0] * col3.x[2]) +
|
|
col1.x[2] * ( col2.x[0] * col3.x[1] - col2.x[1] * col3.x[0]);
|
|
}
|
|
*/
|
|
|
|
void Transpose (Vec3d & v1, Vec3d & v2, Vec3d & v3)
|
|
{
|
|
Swap (v1.Y(), v2.X());
|
|
Swap (v1.Z(), v3.X());
|
|
Swap (v2.Z(), v3.Y());
|
|
}
|
|
|
|
|
|
/*
|
|
gcc4.8.3 warning: array subscript is above array bounds [-Warray-bounds]
|
|
*/
|
|
#ifdef __GNUC__
|
|
#pragma GCC diagnostic push
|
|
#pragma GCC diagnostic ignored "-Warray-bounds"
|
|
#endif
|
|
|
|
int SolveLinearSystem (const Vec3d & col1, const Vec3d & col2,
|
|
const Vec3d & col3, const Vec3d & rhs,
|
|
Vec3d & sol)
|
|
{
|
|
// changed by MW
|
|
double matrix[3][3];
|
|
double locrhs[3];
|
|
int retval = 0;
|
|
|
|
for(int i=0; i<3; i++)
|
|
{
|
|
matrix[i][0] = col1.X(i+1);
|
|
matrix[i][1] = col2.X(i+1);
|
|
matrix[i][2] = col3.X(i+1);
|
|
locrhs[i] = rhs.X(i+1);
|
|
}
|
|
|
|
for(int i=0; i<2; i++)
|
|
{
|
|
int pivot = i;
|
|
double maxv = fabs(matrix[i][i]);
|
|
for(int j=i+1; j<3; j++)
|
|
if(fabs(matrix[j][i]) > maxv)
|
|
{
|
|
maxv = fabs(matrix[j][i]);
|
|
pivot = j;
|
|
}
|
|
|
|
if(fabs(maxv) > 1e-40)
|
|
{
|
|
if(pivot != i)
|
|
{
|
|
swap(matrix[i][0],matrix[pivot][0]);
|
|
swap(matrix[i][1],matrix[pivot][1]);
|
|
swap(matrix[i][2],matrix[pivot][2]);
|
|
swap(locrhs[i],locrhs[pivot]);
|
|
}
|
|
for(int j=i+1; j<3; j++)
|
|
{
|
|
double fac = matrix[j][i] / matrix[i][i];
|
|
|
|
for(int k=i+1; k<3; k++)
|
|
matrix[j][k] -= fac*matrix[i][k];
|
|
locrhs[j] -= fac*locrhs[i];
|
|
}
|
|
}
|
|
else
|
|
retval = 1;
|
|
}
|
|
|
|
if(fabs(matrix[2][2]) < 1e-40)
|
|
retval = 1;
|
|
|
|
if(retval != 0)
|
|
return retval;
|
|
|
|
|
|
for(int i=2; i>=0; i--)
|
|
{
|
|
double sum = locrhs[i];
|
|
for(int j=2; j>i; j--)
|
|
sum -= matrix[i][j]*sol.X(j+1);
|
|
|
|
sol.X(i+1) = sum/matrix[i][i];
|
|
}
|
|
|
|
return 0;
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
double det = Determinant (col1, col2, col3);
|
|
if (fabs (det) < 1e-40)
|
|
return 1;
|
|
|
|
sol.X() = Determinant (rhs, col2, col3) / det;
|
|
sol.Y() = Determinant (col1, rhs, col3) / det;
|
|
sol.Z() = Determinant (col1, col2, rhs) / det;
|
|
|
|
return 0;
|
|
*/
|
|
/*
|
|
Vec3d cr;
|
|
Cross (col1, col2, cr);
|
|
double det = cr * col3;
|
|
|
|
if (fabs (det) < 1e-40)
|
|
return 1;
|
|
|
|
if (fabs(cr.Z()) > 1e-12)
|
|
{
|
|
// solve for 3. component
|
|
sol.Z() = (cr * rhs) / det;
|
|
|
|
// 2x2 system for 1. and 2. component
|
|
double res1 = rhs.X() - sol.Z() * col3.X();
|
|
double res2 = rhs.Y() - sol.Z() * col3.Y();
|
|
|
|
sol.X() = (col2.Y() * res1 - col2.X() * res2) / cr.Z();
|
|
sol.Y() = (col1.X() * res2 - col1.Y() * res1) / cr.Z();
|
|
|
|
}
|
|
else
|
|
{
|
|
det = Determinant (col1, col2, col3);
|
|
if (fabs (det) < 1e-40)
|
|
return 1;
|
|
|
|
sol.X() = Determinant (rhs, col2, col3) / det;
|
|
sol.Y() = Determinant (col1, rhs, col3) / det;
|
|
sol.Z() = Determinant (col1, col2, rhs) / det;
|
|
}
|
|
|
|
return 0;
|
|
*/
|
|
}
|
|
|
|
#ifdef __GNUC__
|
|
#pragma GCC diagnostic pop
|
|
#endif
|
|
|
|
|
|
|
|
int SolveLinearSystemLS (const Vec3d & col1,
|
|
const Vec3d & col2,
|
|
const Vec2d & rhs,
|
|
Vec3d & sol)
|
|
{
|
|
double a11 = col1 * col1;
|
|
double a12 = col1 * col2;
|
|
double a22 = col2 * col2;
|
|
|
|
double det = a11 * a22 - a12 * a12;
|
|
|
|
if (det*det <= 1e-24 * a11 * a22)
|
|
{
|
|
sol = Vec3d (0, 0, 0);
|
|
return 1;
|
|
}
|
|
|
|
Vec2d invrhs;
|
|
invrhs.X() = ( a22 * rhs.X() - a12 * rhs.Y()) / det;
|
|
invrhs.Y() = (-a12 * rhs.X() + a11 * rhs.Y()) / det;
|
|
|
|
sol.X() = invrhs.X() * col1.X() + invrhs.Y() * col2.X();
|
|
sol.Y() = invrhs.X() * col1.Y() + invrhs.Y() * col2.Y();
|
|
sol.Z() = invrhs.X() * col1.Z() + invrhs.Y() * col2.Z();
|
|
|
|
return 0;
|
|
|
|
/*
|
|
Vec3d inv1, inv2;
|
|
int err =
|
|
PseudoInverse (col1, col2, inv1, inv2);
|
|
|
|
sol = rhs.X() * inv1 + rhs.Y() * inv2;
|
|
return err;
|
|
*/
|
|
}
|
|
|
|
int SolveLinearSystemLS2 (const Vec3d & col1,
|
|
const Vec3d & col2,
|
|
const Vec2d & rhs,
|
|
Vec3d & sol, double & x, double & y)
|
|
{
|
|
double a11 = col1 * col1;
|
|
double a12 = col1 * col2;
|
|
double a22 = col2 * col2;
|
|
|
|
double det = a11 * a22 - a12 * a12;
|
|
|
|
if (fabs (det) <= 1e-12 * col1.Length() * col2.Length() ||
|
|
col1.Length2() == 0 || col2.Length2() == 0)
|
|
{
|
|
sol = Vec3d (0, 0, 0);
|
|
x = 0; y = 0;
|
|
return 1;
|
|
}
|
|
|
|
Vec2d invrhs;
|
|
invrhs.X() = ( a22 * rhs.X() - a12 * rhs.Y()) / det;
|
|
invrhs.Y() = (-a12 * rhs.X() + a11 * rhs.Y()) / det;
|
|
|
|
sol.X() = invrhs.X() * col1.X() + invrhs.Y() * col2.X();
|
|
sol.Y() = invrhs.X() * col1.Y() + invrhs.Y() * col2.Y();
|
|
sol.Z() = invrhs.X() * col1.Z() + invrhs.Y() * col2.Z();
|
|
|
|
x = invrhs.X();
|
|
y = invrhs.Y();
|
|
|
|
return 0;
|
|
|
|
/*
|
|
Vec3d inv1, inv2;
|
|
int err =
|
|
PseudoInverse (col1, col2, inv1, inv2);
|
|
|
|
sol = rhs.X() * inv1 + rhs.Y() * inv2;
|
|
return err;
|
|
*/
|
|
}
|
|
|
|
int PseudoInverse (const Vec3d & col1,
|
|
const Vec3d & col2,
|
|
Vec3d & inv1,
|
|
Vec3d & inv2)
|
|
{
|
|
double a11 = col1 * col1;
|
|
double a12 = col1 * col2;
|
|
double a22 = col2 * col2;
|
|
|
|
double det = a11 * a22 - a12 * a12;
|
|
|
|
if (fabs (det) < 1e-12 * col1.Length() * col2.Length())
|
|
{
|
|
inv1 = Vec3d (0, 0, 0);
|
|
inv2 = Vec3d (0, 0, 0);
|
|
return 1;
|
|
}
|
|
|
|
double ia11 = a22 / det;
|
|
double ia12 = -a12 / det;
|
|
double ia22 = a11 / det;
|
|
|
|
inv1 = ia11 * col1 + ia12 * col2;
|
|
inv2 = ia12 * col1 + ia22 * col2;
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
|
|
|
|
QuadraticFunction3d ::
|
|
QuadraticFunction3d (const Point3d & p, const Vec3d & v)
|
|
{
|
|
Vec3d hv(v);
|
|
hv /= (hv.Length() + 1e-12);
|
|
Vec3d t1, t2;
|
|
hv.GetNormal (t1);
|
|
Cross (hv, t1, t2);
|
|
|
|
double t1p = t1.X() * p.X() + t1.Y() * p.Y() + t1.Z() * p.Z();
|
|
double t2p = t2.X() * p.X() + t2.Y() * p.Y() + t2.Z() * p.Z();
|
|
c0 = sqr (t1p) + sqr (t2p);
|
|
cx = -2 * (t1p * t1.X() + t2p * t2.X());
|
|
cy = -2 * (t1p * t1.Y() + t2p * t2.Y());
|
|
cz = -2 * (t1p * t1.Z() + t2p * t2.Z());
|
|
|
|
cxx = t1.X() * t1.X() + t2.X() * t2.X();
|
|
cyy = t1.Y() * t1.Y() + t2.Y() * t2.Y();
|
|
czz = t1.Z() * t1.Z() + t2.Z() * t2.Z();
|
|
|
|
cxy = 2 * t1.X() * t1.Y() + 2 * t2.X() * t2.Y();
|
|
cxz = 2 * t1.X() * t1.Z() + 2 * t2.X() * t2.Z();
|
|
cyz = 2 * t1.Y() * t1.Z() + 2 * t2.Y() * t2.Z();
|
|
|
|
/*
|
|
(*testout) << "c0 = " << c0
|
|
<< " clin = " << cx << " " << cy << " " << cz
|
|
<< " cq = " << cxx << " " << cyy << " " << czz
|
|
<< cxy << " " << cyz << " " << cyz << endl;
|
|
*/
|
|
}
|
|
|
|
// QuadraticFunction3d gqf (Point3d (0,0,0), Vec3d (1, 0, 0));
|
|
|
|
|
|
|
|
|
|
|
|
void referencetransform :: Set (const Point3d & p1, const Point3d & p2,
|
|
const Point3d & p3, double ah)
|
|
{
|
|
ex = p2 - p1;
|
|
ex /= ex.Length();
|
|
ey = p3 - p1;
|
|
ey -= (ex * ey) * ex;
|
|
ey /= ey.Length();
|
|
ez = Cross (ex, ey);
|
|
rp = p1;
|
|
h = ah;
|
|
|
|
exh = ah * ex;
|
|
eyh = ah * ey;
|
|
ezh = ah * ez;
|
|
ah = 1 / ah;
|
|
ex_h = ah * ex;
|
|
ey_h = ah * ey;
|
|
ez_h = ah * ez;
|
|
}
|
|
|
|
void referencetransform :: ToPlain (const Point3d & p, Point3d & pp) const
|
|
{
|
|
Vec3d v;
|
|
v = p - rp;
|
|
pp.X() = (ex_h * v);
|
|
pp.Y() = (ey_h * v);
|
|
pp.Z() = (ez_h * v);
|
|
}
|
|
|
|
void referencetransform :: ToPlain (const NgArray<Point3d> & p,
|
|
NgArray<Point3d> & pp) const
|
|
{
|
|
Vec3d v;
|
|
int i;
|
|
|
|
pp.SetSize (p.Size());
|
|
for (i = 1; i <= p.Size(); i++)
|
|
{
|
|
v = p.Get(i) - rp;
|
|
pp.Elem(i).X() = (ex_h * v);
|
|
pp.Elem(i).Y() = (ey_h * v);
|
|
pp.Elem(i).Z() = (ez_h * v);
|
|
}
|
|
}
|
|
|
|
void referencetransform :: FromPlain (const Point3d & pp, Point3d & p) const
|
|
{
|
|
Vec3d v;
|
|
// v = (h * pp.X()) * ex + (h * pp.Y()) * ey + (h * pp.Z()) * ez;
|
|
// p = rp + v;
|
|
v.X() = pp.X() * exh.X() + pp.Y() * eyh.X() + pp.Z() * ezh.X();
|
|
v.Y() = pp.X() * exh.Y() + pp.Y() * eyh.Y() + pp.Z() * ezh.Y();
|
|
v.Z() = pp.X() * exh.Z() + pp.Y() * eyh.Z() + pp.Z() * ezh.Z();
|
|
p.X() = rp.X() + v.X();
|
|
p.Y() = rp.Y() + v.Y();
|
|
p.Z() = rp.Z() + v.Z();
|
|
}
|
|
|
|
|
|
}
|