/*! calculate inverse */
inline dgemat2 inv(const dgemat2& A)
{VERBOSE_REPORT;
const double Adet( det(A) );
dgemat2 Ainv;
Ainv(0,0)= A(1,1)/Adet; Ainv(0,1)=-A(0,1)/Adet;
Ainv(1,0)=-A(1,0)/Adet; Ainv(1,1)= A(0,0)/Adet;
return Ainv;
}
/*! calculate inverse */
inline dgemat3 inv(const dgemat3& A)
{VERBOSE_REPORT;
const double Adet( det(A) );
dgemat3 Ainv;
Ainv(0,0) =(A(1,1)*A(2,2)-A(1,2)*A(2,1))/Adet;
Ainv(0,1) =(A(0,2)*A(2,1)-A(0,1)*A(2,2))/Adet;
Ainv(0,2) =(A(0,1)*A(1,2)-A(0,2)*A(1,1))/Adet;
Ainv(1,0) =(A(1,2)*A(2,0)-A(1,0)*A(2,2))/Adet;
Ainv(1,1) =(A(0,0)*A(2,2)-A(0,2)*A(2,0))/Adet;
Ainv(1,2) =(A(0,2)*A(1,0)-A(0,0)*A(1,2))/Adet;
Ainv(2,0) =(A(1,0)*A(2,1)-A(1,1)*A(2,0))/Adet;
Ainv(2,1) =(A(0,1)*A(2,0)-A(0,0)*A(2,1))/Adet;
Ainv(2,2) =(A(0,0)*A(1,1)-A(0,1)*A(1,0))/Adet;
return Ainv;
}
bool MxQuadric::optimize(MxVector& v) const
{
MxMatrix Ainv(A.dim());
double det = A.invert(Ainv);
if( FEQ(det, 0.0, 1e-12) )
return false;
v = (Ainv * b);
mxv_neg(v, v.dim());
return true;
}
Foam::gpuDiagonalMatrix<Type> Foam::inv(const gpuDiagonalMatrix<Type>& A)
{
gpuDiagonalMatrix<Type> Ainv = A;
thrust::transform
(
A.begin(),
A.end(),
Ainv(),
DiagonalMatrixInvertFunctor<Type>()
);
return Ainv;
}
/**
* Return the inverse of the given TypeTensor. Algorithm taken from the tensor classes
* of Ton van den Boogaard (http://tmku209.ctw.utwente.nl/~ton/tensor.html)
*/
template <typename T> TypeTensor<T> inv(const TypeTensor<T> &A ) {
double Sub11, Sub12, Sub13;
Sub11 = A._coords[4]*A._coords[8] - A._coords[5]*A._coords[7];
Sub12 = A._coords[3]*A._coords[8] - A._coords[6]*A._coords[5];
Sub13 = A._coords[3]*A._coords[7] - A._coords[6]*A._coords[4];
double detA = A._coords[0]*Sub11 - A._coords[1]*Sub12 + A._coords[2]*Sub13;
libmesh_assert( std::fabs(detA)>1.e-15 );
TypeTensor<T> Ainv(A);
Ainv._coords[0] = Sub11/detA;
Ainv._coords[1] = (-A._coords[1]*A._coords[8]+A._coords[2]*A._coords[7])/detA;
Ainv._coords[2] = ( A._coords[1]*A._coords[5]-A._coords[2]*A._coords[4])/detA;
Ainv._coords[3] = -Sub12/detA;
Ainv._coords[4] = ( A._coords[0]*A._coords[8]-A._coords[2]*A._coords[6])/detA;
Ainv._coords[5] = (-A._coords[0]*A._coords[5]+A._coords[2]*A._coords[3])/detA;
Ainv._coords[6] = Sub13/detA;
Ainv._coords[7] = (-A._coords[0]*A._coords[7]+A._coords[1]*A._coords[6])/detA;
Ainv._coords[8] = ( A._coords[0]*A._coords[4]-A._coords[1]*A._coords[3])/detA;
return Ainv;
}
void OCCSurface :: DefineTangentialPlane (const Point<3> & ap1,
const PointGeomInfo & geominfo1,
const Point<3> & ap2,
const PointGeomInfo & geominfo2)
{
if (projecttype == PLANESPACE)
{
p1 = ap1; p2 = ap2;
//cout << "p1 = " << p1 << endl;
//cout << "p2 = " << p2 << endl;
GetNormalVector (p1, geominfo1, ez);
ex = p2 - p1;
ex -= (ex * ez) * ez;
ex.Normalize();
ey = Cross (ez, ex);
GetNormalVector (p2, geominfo2, n2);
nmid = 0.5*(n2+ez);
ez = nmid;
ez.Normalize();
ex = (p2 - p1).Normalize();
ez -= (ez * ex) * ex;
ez.Normalize();
ey = Cross (ez, ex);
nmid = ez;
//cout << "ex " << ex << " ey " << ey << " ez " << ez << endl;
}
else
{
if ( (geominfo1.u < umin) ||
(geominfo1.u > umax) ||
(geominfo2.u < umin) ||
(geominfo2.u > umax) ||
(geominfo1.v < vmin) ||
(geominfo1.v > vmax) ||
(geominfo2.v < vmin) ||
(geominfo2.v > vmax) ) throw UVBoundsException();
p1 = ap1; p2 = ap2;
psp1 = Point<2>(geominfo1.u, geominfo1.v);
psp2 = Point<2>(geominfo2.u, geominfo2.v);
Vec<3> n;
GetNormalVector (p1, geominfo1, n);
gp_Pnt pnt;
gp_Vec du, dv;
occface->D1 (geominfo1.u, geominfo1.v, pnt, du, dv);
DenseMatrix D1(3,2), D1T(2,3), DDTinv(2,2);
D1(0,0) = du.X(); D1(1,0) = du.Y(); D1(2,0) = du.Z();
D1(0,1) = dv.X(); D1(1,1) = dv.Y(); D1(2,1) = dv.Z();
/*
(*testout) << "DefineTangentialPlane" << endl
<< "---------------------" << endl;
(*testout) << "D1 = " << endl << D1 << endl;
*/
Transpose (D1, D1T);
DenseMatrix D1TD1(3,3);
D1TD1 = D1T*D1;
if (D1TD1.Det() == 0) throw SingularMatrixException();
CalcInverse (D1TD1, DDTinv);
DenseMatrix Y(3,2);
Vec<3> y1 = (ap2-ap1).Normalize();
Vec<3> y2 = Cross(n, y1).Normalize();
for (int i = 0; i < 3; i++)
{
Y(i,0) = y1(i);
Y(i,1) = y2(i);
}
DenseMatrix A(2,2);
A = DDTinv * D1T * Y;
DenseMatrix Ainv(2,2);
if (A.Det() == 0) throw SingularMatrixException();
CalcInverse (A, Ainv);
for (int i = 0; i < 2; i++)
for (int j = 0; j < 2; j++)
{
Amat(i,j) = A(i,j);
Amatinv(i,j) = Ainv(i,j);
}
Vec<2> temp = Amatinv * (psp2-psp1);
double r = temp.Length();
// double alpha = -acos (temp(0)/r);
double alpha = -atan2 (temp(1),temp(0));
DenseMatrix R(2,2);
R(0,0) = cos (alpha);
R(1,0) = -sin (alpha);
R(0,1) = sin (alpha);
R(1,1) = cos (alpha);
A = A*R;
if (A.Det() == 0) throw SingularMatrixException();
CalcInverse (A, Ainv);
for (int i = 0; i < 2; i++)
for (int j = 0; j < 2; j++)
{
Amat(i,j) = A(i,j);
Amatinv(i,j) = Ainv(i,j);
}
temp = Amatinv * (psp2-psp1);
};
}
int VizGeorefSpline2D::solve(void)
{
int r, c, v;
int p;
// No points at all
if ( _nof_points < 1 )
{
type = VIZ_GEOREF_SPLINE_ZERO_POINTS;
return(0);
}
// Only one point
if ( _nof_points == 1 )
{
type = VIZ_GEOREF_SPLINE_ONE_POINT;
return(1);
}
// Just 2 points - it is necessarily 1D case
if ( _nof_points == 2 )
{
_dx = x[1] - x[0];
_dy = y[1] - y[0];
double fact = 1.0 / ( _dx * _dx + _dy * _dy );
_dx *= fact;
_dy *= fact;
type = VIZ_GEOREF_SPLINE_TWO_POINTS;
return(2);
}
// More than 2 points - first we have to check if it is 1D or 2D case
double xmax = x[0], xmin = x[0], ymax = y[0], ymin = y[0];
double delx, dely;
double xx, yy;
double sumx = 0.0f, sumy= 0.0f, sumx2 = 0.0f, sumy2 = 0.0f, sumxy = 0.0f;
double SSxx, SSyy, SSxy;
for ( p = 0; p < _nof_points; p++ )
{
xx = x[p];
yy = y[p];
xmax = MAX( xmax, xx );
xmin = MIN( xmin, xx );
ymax = MAX( ymax, yy );
ymin = MIN( ymin, yy );
sumx += xx;
sumx2 += xx * xx;
sumy += yy;
sumy2 += yy * yy;
sumxy += xx * yy;
}
delx = xmax - xmin;
dely = ymax - ymin;
SSxx = sumx2 - sumx * sumx / _nof_points;
SSyy = sumy2 - sumy * sumy / _nof_points;
SSxy = sumxy - sumx * sumy / _nof_points;
if ( delx < 0.001 * dely || dely < 0.001 * delx ||
fabs ( SSxy * SSxy / ( SSxx * SSyy ) ) > 0.99 )
{
int p1;
type = VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL;
_dx = _nof_points * sumx2 - sumx * sumx;
_dy = _nof_points * sumy2 - sumy * sumy;
double fact = 1.0 / sqrt( _dx * _dx + _dy * _dy );
_dx *= fact;
_dy *= fact;
for ( p = 0; p < _nof_points; p++ )
{
double dxp = x[p] - x[0];
double dyp = y[p] - y[0];
u[p] = _dx * dxp + _dy * dyp;
unused[p] = 1;
}
for ( p = 0; p < _nof_points; p++ )
{
int min_index = -1;
double min_u = 0;
for ( p1 = 0; p1 < _nof_points; p1++ )
{
if ( unused[p1] )
{
if ( min_index < 0 || u[p1] < min_u )
{
min_index = p1;
min_u = u[p1];
}
}
}
index[p] = min_index;
unused[min_index] = 0;
}
return(3);
}
type = VIZ_GEOREF_SPLINE_FULL;
// Make the necessary memory allocations
if ( _AA )
CPLFree(_AA);
if ( _Ainv )
CPLFree(_Ainv);
_nof_eqs = _nof_points + 3;
_AA = ( double * )CPLCalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
_Ainv = ( double * )CPLCalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
// Calc the values of the matrix A
for ( r = 0; r < 3; r++ )
for ( c = 0; c < 3; c++ )
A(r,c) = 0.0;
for ( c = 0; c < _nof_points; c++ )
{
A(0,c+3) = 1.0;
A(1,c+3) = x[c];
A(2,c+3) = y[c];
A(c+3,0) = 1.0;
A(c+3,1) = x[c];
A(c+3,2) = y[c];
}
for ( r = 0; r < _nof_points; r++ )
for ( c = r; c < _nof_points; c++ )
{
A(r+3,c+3) = base_func( x[r], y[r], x[c], y[c] );
if ( r != c )
A(c+3,r+3 ) = A(r+3,c+3);
}
#if VIZ_GEOREF_SPLINE_DEBUG
for ( r = 0; r < _nof_eqs; r++ )
{
for ( c = 0; c < _nof_eqs; c++ )
fprintf(stderr, "%f", A(r,c));
fprintf(stderr, "\n");
}
#endif
// Invert the matrix
int status = matrixInvert( _nof_eqs, _AA, _Ainv );
if ( !status )
{
fprintf(stderr, " There is a problem to invert the interpolation matrix\n");
return 0;
}
// calc the coefs
for ( v = 0; v < _nof_vars; v++ )
for ( r = 0; r < _nof_eqs; r++ )
{
coef[v][r] = 0.0;
for ( c = 0; c < _nof_eqs; c++ )
coef[v][r] += Ainv(r,c) * rhs[v][c];
}
return(4);
}
int VizGeorefSpline2D::solve(void)
{
int r, c;
int p;
// No points at all
if ( _nof_points < 1 )
{
type = VIZ_GEOREF_SPLINE_ZERO_POINTS;
return(0);
}
// Only one point
if ( _nof_points == 1 )
{
type = VIZ_GEOREF_SPLINE_ONE_POINT;
return(1);
}
// Just 2 points - it is necessarily 1D case
if ( _nof_points == 2 )
{
_dx = x[1] - x[0];
_dy = y[1] - y[0];
double fact = 1.0 / ( _dx * _dx + _dy * _dy );
_dx *= fact;
_dy *= fact;
type = VIZ_GEOREF_SPLINE_TWO_POINTS;
return(2);
}
// More than 2 points - first we have to check if it is 1D or 2D case
double xmax = x[0], xmin = x[0], ymax = y[0], ymin = y[0];
double delx, dely;
double xx, yy;
double sumx = 0.0f, sumy= 0.0f, sumx2 = 0.0f, sumy2 = 0.0f, sumxy = 0.0f;
double SSxx, SSyy, SSxy;
for ( p = 0; p < _nof_points; p++ )
{
xx = x[p];
yy = y[p];
xmax = MAX( xmax, xx );
xmin = MIN( xmin, xx );
ymax = MAX( ymax, yy );
ymin = MIN( ymin, yy );
sumx += xx;
sumx2 += xx * xx;
sumy += yy;
sumy2 += yy * yy;
sumxy += xx * yy;
}
delx = xmax - xmin;
dely = ymax - ymin;
SSxx = sumx2 - sumx * sumx / _nof_points;
SSyy = sumy2 - sumy * sumy / _nof_points;
SSxy = sumxy - sumx * sumy / _nof_points;
if ( delx < 0.001 * dely || dely < 0.001 * delx ||
fabs ( SSxy * SSxy / ( SSxx * SSyy ) ) > 0.99 )
{
int p1;
type = VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL;
_dx = _nof_points * sumx2 - sumx * sumx;
_dy = _nof_points * sumy2 - sumy * sumy;
double fact = 1.0 / sqrt( _dx * _dx + _dy * _dy );
_dx *= fact;
_dy *= fact;
for ( p = 0; p < _nof_points; p++ )
{
double dxp = x[p] - x[0];
double dyp = y[p] - y[0];
u[p] = _dx * dxp + _dy * dyp;
unused[p] = 1;
}
for ( p = 0; p < _nof_points; p++ )
{
int min_index = -1;
double min_u = 0;
for ( p1 = 0; p1 < _nof_points; p1++ )
{
if ( unused[p1] )
{
if ( min_index < 0 || u[p1] < min_u )
{
min_index = p1;
min_u = u[p1];
}
}
}
index[p] = min_index;
unused[min_index] = 0;
}
return(3);
}
type = VIZ_GEOREF_SPLINE_FULL;
// Make the necessary memory allocations
_nof_eqs = _nof_points + 3;
if( _nof_eqs > INT_MAX / _nof_eqs )
{
CPLError(CE_Failure, CPLE_AppDefined, "Too many coefficients. Computation aborted.");
return 0;
}
double* _AA = ( double * )VSICalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
double* _Ainv = ( double * )VSICalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
if( _AA == NULL || _Ainv == NULL )
{
CPLError(CE_Failure, CPLE_AppDefined, "Out-of-memory while allocating temporary arrays. Computation aborted.");
VSIFree(_AA);
VSIFree(_Ainv);
return 0;
}
// Calc the values of the matrix A
for ( r = 0; r < 3; r++ )
for ( c = 0; c < 3; c++ )
A(r,c) = 0.0;
for ( c = 0; c < _nof_points; c++ )
{
A(0,c+3) = 1.0;
A(1,c+3) = x[c];
A(2,c+3) = y[c];
A(c+3,0) = 1.0;
A(c+3,1) = x[c];
A(c+3,2) = y[c];
}
for ( r = 0; r < _nof_points; r++ )
for ( c = r; c < _nof_points; c++ )
{
A(r+3,c+3) = VizGeorefSpline2DBase_func( x[r], y[r], x[c], y[c] );
if ( r != c )
A(c+3,r+3 ) = A(r+3,c+3);
}
#if VIZ_GEOREF_SPLINE_DEBUG
for ( r = 0; r < _nof_eqs; r++ )
{
for ( c = 0; c < _nof_eqs; c++ )
fprintf(stderr, "%f", A(r,c));
fprintf(stderr, "\n");
}
#endif
int ret = 4;
#ifdef HAVE_ARMADILLO
try
{
arma::mat matA(_AA,_nof_eqs,_nof_eqs,false);
arma::mat matRHS(_nof_eqs, _nof_vars);
int row, col;
for(row = 0; row < _nof_eqs; row++)
for(col = 0; col < _nof_vars; col++)
matRHS.at(row, col) = rhs[col][row];
arma::mat matCoefs(_nof_vars, _nof_eqs);
if( !arma::solve(matCoefs, matA, matRHS) )
{
CPLError(CE_Failure, CPLE_AppDefined, "There is a problem to invert the interpolation matrix.");
ret = 0;
}
else
{
for(row = 0; row < _nof_eqs; row++)
for(col = 0; col < _nof_vars; col++)
coef[col][row] = matCoefs.at(row, col);
}
}
catch(...)
{
CPLError(CE_Failure, CPLE_AppDefined, "There is a problem to invert the interpolation matrix.");
ret = 0;
}
#else
// Invert the matrix
int status = matrixInvert( _nof_eqs, _AA, _Ainv );
if ( !status )
{
CPLError(CE_Failure, CPLE_AppDefined, "There is a problem to invert the interpolation matrix.");
ret = 0;
}
else
{
// calc the coefs
for ( int v = 0; v < _nof_vars; v++ )
for ( r = 0; r < _nof_eqs; r++ )
{
coef[v][r] = 0.0;
for ( c = 0; c < _nof_eqs; c++ )
coef[v][r] += Ainv(r,c) * rhs[v][c];
}
}
#endif
VSIFree(_AA);
VSIFree(_Ainv);
return(ret);
}
