hesuicong
/
openMVS
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
38 Commits
1 Branch
0 Tags
13 MiB
 
 
 
 
 
 
Tree: 8d428ab5a8
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from '8d428ab5a8'
${ noResults }
openMVS/vcglib/vcg/math/quadric5.h

738 lines
20 KiB
Raw Blame History

/****************************************************************************
* VCGLib o o *
* Visual and Computer Graphics Library o o *
* _ O _ *
* Copyright(C) 2004-2016 \/)\/ *
* Visual Computing Lab /\/| *
* ISTI - Italian National Research Council | *
* \ *
* All rights reserved. *
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, *
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
* GNU General Public License (http://www.gnu.org/licenses/gpl.txt) *
* for more details. *
* *
****************************************************************************/
#ifndef __VCGLIB_QUADRIC5
#define __VCGLIB_QUADRIC5
#include <vcg/math/quadric.h>
namespace vcg
{
namespace math {
typedef double ScalarType;
// r = a-b
void inline sub_vec5(const ScalarType a[5], const ScalarType b[5], ScalarType r[5])
{
r[0] = a[0] - b[0];
r[1] = a[1] - b[1];
r[2] = a[2] - b[2];
r[3] = a[3] - b[3];
r[4] = a[4] - b[4];
}
// returns the in-product a*b
ScalarType inline inproduct5(const ScalarType a[5], const ScalarType b[5])
{
return a[0]*b[0]+a[1]*b[1]+a[2]*b[2]+a[3]*b[3]+a[4]*b[4];
}
// r = out-product of a*b
void inline outproduct5(const ScalarType a[5], const ScalarType b[5], ScalarType r[5][5])
{
for(int i = 0; i < 5; i++)
for(int j = 0; j < 5; j++)
r[i][j] = a[i]*b[j];
}
// r = m*v
void inline prod_matvec5(const ScalarType m[5][5], const ScalarType v[5], ScalarType r[5])
{
r[0] = inproduct5(m[0],v);
r[1] = inproduct5(m[1],v);
r[2] = inproduct5(m[2],v);
r[3] = inproduct5(m[3],v);
r[4] = inproduct5(m[4],v);
}
// r = (v transposed)*m
void inline prod_vecmat5(ScalarType v[5],ScalarType m[5][5], ScalarType r[5])
{
for(int i = 0; i < 5; i++)
for(int j = 0; j < 5; j++)
r[j] = v[j]*m[j][i];
}
void inline normalize_vec5(ScalarType v[5])
{
ScalarType norma = sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]+v[3]*v[3]+v[4]*v[4]);
v[0]/=norma;
v[1]/=norma;
v[2]/=norma;
v[3]/=norma;
v[4]/=norma;
}
void inline normalize_vec3(ScalarType v[3])
{
ScalarType norma = sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
v[0]/=norma;
v[1]/=norma;
v[2]/=norma;
}
// dest -= m
void inline sub_mat5(ScalarType dest[5][5],ScalarType m[5][5])
{
for(int i = 0; i < 5; i++)
for(int j = 0; j < 5; j++)
dest[i][j] -= m[i][j];
}
/* computes the symmetric matrix v*v */
void inline symprod_vvt5(ScalarType dest[15],ScalarType v[5])
{
dest[0] = v[0]*v[0];
dest[1] = v[0]*v[1];
dest[2] = v[0]*v[2];
dest[3] = v[0]*v[3];
dest[4] = v[0]*v[4];
dest[5] = v[1]*v[1];
dest[6] = v[1]*v[2];
dest[7] = v[1]*v[3];
dest[8] = v[1]*v[4];
dest[9] = v[2]*v[2];
dest[10] = v[2]*v[3];
dest[11] = v[2]*v[4];
dest[12] = v[3]*v[3];
dest[13] = v[3]*v[4];
dest[14] = v[4]*v[4];
}
/* subtracts symmetric matrix */
void inline sub_symmat5(ScalarType dest[15],ScalarType m[15])
{
for(int i = 0; i < 15; i++)
dest[i] -= m[i];
}
}
template<typename Scalar>
class Quadric5
{
public:
typedef Scalar ScalarType;
// typedef CMeshO::VertexType::FaceType FaceType;
// the real quadric
ScalarType a[15];
ScalarType b[5];
ScalarType c;
inline Quadric5() { c = -1;}
// Necessari se si utilizza stl microsoft
// inline bool operator < ( const Quadric & q ) const { return false; }
// inline bool operator == ( const Quadric & q ) const { return true; }
bool IsValid() const { return (c>=0); }
void SetInvalid() { c = -1.0; }
void Zero() // Azzera le quadriche
{
a[0] = 0;
a[1] = 0;
a[2] = 0;
a[3] = 0;
a[4] = 0;
a[5] = 0;
a[6] = 0;
a[7] = 0;
a[8] = 0;
a[9] = 0;
a[10] = 0;
a[11] = 0;
a[12] = 0;
a[13] = 0;
a[14] = 0;
b[0] = 0;
b[1] = 0;
b[2] = 0;
b[3] = 0;
b[4] = 0;
c = 0;
}
void swapv(ScalarType *vv, ScalarType *ww)
{
ScalarType tmp;
for(int i = 0; i < 5; i++)
{
tmp = vv[i];
vv[i] = ww[i];
ww[i] = tmp;
}
}
// Add the right subset of the current 5D quadric to a given 3D quadric.
void AddtoQ3(math::Quadric<double> &q3) const
{
q3.a[0] += a[0];
q3.a[1] += a[1];
q3.a[2] += a[2];
q3.a[3] += a[5];
q3.a[4] += a[6];
q3.a[5] += a[9];
q3.b[0] += b[0];
q3.b[1] += b[1];
q3.b[2] += b[2];
q3.c += c;
assert(q3.IsValid());
}
// computes the real quadric and the geometric quadric using the face
// The geometric quadric is added to the parameter qgeo
template <class FaceType>
void byFace(FaceType &f, math::Quadric<double> &q1, math::Quadric<double> &q2, math::Quadric<double> &q3, bool QualityQuadric, ScalarType BorderWeight)
{
typedef typename FaceType::VertexType::CoordType CoordType;
double q = QualityFace(f);
// if quality==0 then the geometrical quadric has just zeroes
if(q)
{
byFace(f,true); // computes the geometrical quadric
AddtoQ3(q1);
AddtoQ3(q2);
AddtoQ3(q3);
byFace(f,false); // computes the real quadric
for(int j=0;j<3;++j)
{
if( f.IsB(j) || QualityQuadric )
{
Quadric5<double> temp;
TexCoord2f newtex;
CoordType newpoint = (f.P0(j)+f.P1(j))/2.0 + (f.N()/f.N().Norm())*Distance(f.P0(j),f.P1(j));
newtex.u() = (f.WT( (j+0)%3 ).u()+f.WT( (j+1)%3 ).u())/2.0;
newtex.v() = (f.WT( (j+0)%3 ).v()+f.WT( (j+1)%3 ).v())/2.0;
CoordType oldpoint = f.P2(j);
TexCoord2f oldtex = f.WT((j+2)%3);
f.P2(j)=newpoint;
f.WT((j+2)%3).u()=newtex.u();
f.WT((j+2)%3).v()=newtex.v();
temp.byFace(f,false); // computes the full quadric
if(! f.IsB(j) ) temp.Scale(0.05);
else temp.Scale(BorderWeight);
*this+=temp;
f.P2(j)=oldpoint;
f.WT((j+2)%3).u()=oldtex.u();
f.WT((j+2)%3).v()=oldtex.v();
}
}
}
else if(
(f.WT(1).u()-f.WT(0).u()) * (f.WT(2).v()-f.WT(0).v()) -
(f.WT(2).u()-f.WT(0).u()) * (f.WT(1).v()-f.WT(0).v())
)
byFace(f,false); // computes the real quadric
else // the area is zero also in the texture space
{
a[0]=a[1]=a[2]=a[3]=a[4]=a[5]=a[6]=a[7]=a[8]=a[9]=a[10]=a[11]=a[12]=a[13]=a[14]=0;
b[0]=b[1]=b[2]=b[3]=b[4]=0;
c=0;
}
}
// Computes the geometrical quadric if onlygeo == true and the real quadric if onlygeo == false
template<class FaceType>
void byFace(FaceType &fi, bool onlygeo)
{
//assert(onlygeo==false);
ScalarType p[5];
ScalarType q[5];
ScalarType r[5];
// ScalarType A[5][5];
ScalarType e1[5];
ScalarType e2[5];
// computes p
p[0] = fi.P(0).X();
p[1] = fi.P(0).Y();
p[2] = fi.P(0).Z();
p[3] = fi.WT(0).u();
p[4] = fi.WT(0).v();
// computes q
q[0] = fi.P(1).X();
q[1] = fi.P(1).Y();
q[2] = fi.P(1).Z();
q[3] = fi.WT(1).u();
q[4] = fi.WT(1).v();
// computes r
r[0] = fi.P(2).X();
r[1] = fi.P(2).Y();
r[2] = fi.P(2).Z();
r[3] = fi.WT(2).u();
r[4] = fi.WT(2).v();
if(onlygeo) {
p[3] = 0; q[3] = 0; r[3] = 0;
p[4] = 0; q[4] = 0; r[4] = 0;
}
ComputeE1E2(p,q,r,e1,e2);
ComputeQuadricFromE1E2(e1,e2,p);
if(IsValid()) return;
// qDebug("Warning: failed to find a good 5D quadric try to permute stuff.");
/*
When c is very close to 0, loss of precision causes it to be computed as a negative number,
which is invalid for a quadric. Vertex switches are performed in order to try to obtain a smaller
loss of precision. The one with the smallest error is chosen.
*/
double minerror = std::numeric_limits<double>::max();
int minerror_index = 0;
for(int i = 0; i < 7; i++) // tries the 6! configurations and chooses the one with the smallest error
{
switch(i)
{
case 0:
break;
case 1:
case 3:
case 5:
swapv(q,r);
break;
case 2:
case 4:
swapv(p,r);
break;
case 6: // every swap has loss of precision
swapv(p,r);
for(int j = 0; j <= minerror_index; j++)
{
switch(j)
{
case 0:
break;
case 1:
case 3:
case 5:
swapv(q,r);
break;
case 2:
case 4:
swapv(p,r);
break;
default:
assert(0);
}
}
minerror_index = -1;
break;
default:
assert(0);
}
ComputeE1E2(p,q,r,e1,e2);
ComputeQuadricFromE1E2(e1,e2,p);
if(IsValid())
return;
else if (minerror_index == -1) // the one with the smallest error has been computed
break;
else if(-c < minerror)
{
minerror = -c;
minerror_index = i;
}
}
// failed to find a valid vertex switch
// assert(-c <= 1e-8); // small error
c = 0; // rounds up to zero
}
// Given three 5D points it compute an orthonormal basis e1 e2
void ComputeE1E2 (const ScalarType p[5], const ScalarType q[5], const ScalarType r[5], ScalarType e1[5], ScalarType e2[5]) const
{
ScalarType diffe[5];
ScalarType tmpmat[5][5];
ScalarType tmpvec[5];
// computes e1
math::sub_vec5(q,p,e1);
math::normalize_vec5(e1);
// computes e2
math::sub_vec5(r,p,diffe);
math::outproduct5(e1,diffe,tmpmat);
math::prod_matvec5(tmpmat,e1,tmpvec);
math::sub_vec5(diffe,tmpvec,e2);
math::normalize_vec5(e2);
}
// Given two orthonormal 5D vectors lying on the plane and one of the three points of the triangle compute the quadric.
// Note it uses the same notation of the original garland 98 paper.
void ComputeQuadricFromE1E2(ScalarType e1[5], ScalarType e2[5], ScalarType p[5] )
{
// computes A
a[0] = 1;
a[1] = 0;
a[2] = 0;
a[3] = 0;
a[4] = 0;
a[5] = 1;
a[6] = 0;
a[7] = 0;
a[8] = 0;
a[9] = 1;
a[10] = 0;
a[11] = 0;
a[12] = 1;
a[13] = 0;
a[14] = 1;
ScalarType tmpsymmat[15]; // a compactly stored 5x5 symmetric matrix.
math::symprod_vvt5(tmpsymmat,e1);
math::sub_symmat5(a,tmpsymmat);
math::symprod_vvt5(tmpsymmat,e2);
math::sub_symmat5(a,tmpsymmat);
ScalarType pe1;
ScalarType pe2;
pe1 = math::inproduct5(p,e1);
pe2 = math::inproduct5(p,e2);
// computes b
ScalarType tmpvec[5];
tmpvec[0] = pe1*e1[0] + pe2*e2[0];
tmpvec[1] = pe1*e1[1] + pe2*e2[1];
tmpvec[2] = pe1*e1[2] + pe2*e2[2];
tmpvec[3] = pe1*e1[3] + pe2*e2[3];
tmpvec[4] = pe1*e1[4] + pe2*e2[4];
math::sub_vec5(tmpvec,p,b);
// computes c
c = math::inproduct5(p,p)-pe1*pe1-pe2*pe2;
}
static bool Gauss55( ScalarType x[], ScalarType C[5][5+1] )
{
const ScalarType keps = (ScalarType)1e-6;
int i,j,k;
ScalarType eps; // Determina valore cond.
eps = math::Abs(C[0][0]);
for(i=1;i<5;++i)
{
ScalarType t = math::Abs(C[i][i]);
if( eps<t ) eps = t;
}
eps *= keps;
for (i = 0; i < 5 - 1; ++i) // Ciclo di riduzione
{
int ma = i; // Ricerca massimo pivot
ScalarType vma = math::Abs( C[i][i] );
for (k = i + 1; k < 5; k++)
{
ScalarType t = math::Abs( C[k][i] );
if (t > vma)
{
vma = t;
ma = k;
}
}
if (vma<eps)
return false; // Matrice singolare
if(i!=ma) // Swap del massimo pivot
for(k=0;k<=5;k++)
{
ScalarType t = C[i][k];
C[i][k] = C[ma][k];
C[ma][k] = t;
}
for (k = i + 1; k < 5; k++) // Riduzione
{
ScalarType s;
s = C[k][i] / C[i][i];
for (j = i+1; j <= 5; j++)
C[k][j] -= C[i][j] * s;
C[k][i] = 0.0;
}
}
// Controllo finale singolarita'
if( math::Abs(C[5-1][5- 1])<eps)
return false;
for (i=5-1; i>=0; i--) // Sostituzione
{
ScalarType t;
for (t = 0.0, j = i + 1; j < 5; j++)
t += C[i][j] * x[j];
x[i] = (C[i][5] - t) / C[i][i];
if(math::IsNAN(x[i])) return false;
assert(!math::IsNAN(x[i]));
}
return true;
}
// computes the minimum of the quadric, imposing the geometrical constraint (geo[3] and geo[4] are obviosly ignored)
bool MinimumWithGeoContraints(ScalarType x[5],const ScalarType geo[5]) const
{
x[0] = geo[0];
x[1] = geo[1];
x[2] = geo[2];
ScalarType C3 = -(b[3]+geo[0]*a[3]+geo[1]*a[7]+geo[2]*a[10]);
ScalarType C4 = -(b[4]+geo[0]*a[4]+geo[1]*a[8]+geo[2]*a[11]);
if(a[12] != 0)
{
double tmp = (a[14]-a[13]*a[13]/a[12]);
if(tmp == 0)
return false;
x[4] = (C4 - a[13]*C3/a[12])/ tmp;
x[3] = (C3 - a[13]*x[4])/a[12];
}
else
{
if(a[13] == 0)
return false;
x[4] = C3/a[13];
x[3] = (C4 - a[14]*x[4])/a[13];
}
for(int i=0;i<5;++i)
if( math::IsNAN(x[i])) return false;
//assert(!math::IsNAN(x[i]));
return true;
}
// computes the minimum of the quadric
bool Minimum(ScalarType x[5]) const
{
ScalarType C[5][6];
C[0][0] = a[0];
C[0][1] = a[1];
C[0][2] = a[2];
C[0][3] = a[3];
C[0][4] = a[4];
C[1][0] = a[1];
C[1][1] = a[5];
C[1][2] = a[6];
C[1][3] = a[7];
C[1][4] = a[8];
C[2][0] = a[2];
C[2][1] = a[6];
C[2][2] = a[9];
C[2][3] = a[10];
C[2][4] = a[11];
C[3][0] = a[3];
C[3][1] = a[7];
C[3][2] = a[10];
C[3][3] = a[12];
C[3][4] = a[13];
C[4][0] = a[4];
C[4][1] = a[8];
C[4][2] = a[11];
C[4][3] = a[13];
C[4][4] = a[14];
C[0][5]=-b[0];
C[1][5]=-b[1];
C[2][5]=-b[2];
C[3][5]=-b[3];
C[4][5]=-b[4];
return Gauss55(&(x[0]),C);
}
void operator = ( const Quadric5<double> & q ) // Assegna una quadrica
{
//assert( IsValid() );
assert( q.IsValid() );
a[0] = q.a[0];
a[1] = q.a[1];
a[2] = q.a[2];
a[3] = q.a[3];
a[4] = q.a[4];
a[5] = q.a[5];
a[6] = q.a[6];
a[7] = q.a[7];
a[8] = q.a[8];
a[9] = q.a[9];
a[10] = q.a[10];
a[11] = q.a[11];
a[12] = q.a[12];
a[13] = q.a[13];
a[14] = q.a[14];
b[0] = q.b[0];
b[1] = q.b[1];
b[2] = q.b[2];
b[3] = q.b[3];
b[4] = q.b[4];
c = q.c;
}
// sums the geometrical and the real quadrics
void operator += ( const Quadric5<double> & q )
{
//assert( IsValid() );
assert( q.IsValid() );
a[0] += q.a[0];
a[1] += q.a[1];
a[2] += q.a[2];
a[3] += q.a[3];
a[4] += q.a[4];
a[5] += q.a[5];
a[6] += q.a[6];
a[7] += q.a[7];
a[8] += q.a[8];
a[9] += q.a[9];
a[10] += q.a[10];
a[11] += q.a[11];
a[12] += q.a[12];
a[13] += q.a[13];
a[14] += q.a[14];
b[0] += q.b[0];
b[1] += q.b[1];
b[2] += q.b[2];
b[3] += q.b[3];
b[4] += q.b[4];
c += q.c;
}
/*
it sums the real quadric of the class with a quadric obtained by the geometrical quadric of the vertex.
This quadric is obtained extending to five dimensions the geometrical quadric and simulating that it has been
obtained by sums of 5-dimension quadrics which were computed using vertexes and faces with always the same values
in the fourth and fifth dimensions (respectly the function parameter u and the function parameter v).
this allows to simulate the inexistant continuity in vertexes with multiple texture coords
however this continuity is still inexistant, so even if the algorithm makes a good collapse with this expedient,it obviously
computes bad the priority......this should be adjusted with the extra weight user parameter through.....
*/
void inline Sum3 (const math::Quadric<double> & q3, float u, float v)
{
assert( q3.IsValid() );
a[0] += q3.a[0];
a[1] += q3.a[1];
a[2] += q3.a[2];
a[5] += q3.a[3];
a[6] += q3.a[4];
a[9] += q3.a[5];
a[12] += 1;
a[14] += 1;
b[0] += q3.b[0];
b[1] += q3.b[1];
b[2] += q3.b[2];
b[3] -= u;
b[4] -= v;
c += q3.c + u*u + v*v;
}
void Scale(ScalarType val)
{
for(int i=0;i<15;++i)
a[i]*=val;
for(int i=0;i<5;++i)
b[i]*=val;
c*=val;
}
// returns the quadric value in v
ScalarType Apply(const ScalarType v[5]) const
{
assert( IsValid() );
ScalarType tmpmat[5][5];
ScalarType tmpvec[5];
tmpmat[0][0] = a[0];
tmpmat[0][1] = tmpmat[1][0] = a[1];
tmpmat[0][2] = tmpmat[2][0] = a[2];
tmpmat[0][3] = tmpmat[3][0] = a[3];
tmpmat[0][4] = tmpmat[4][0] = a[4];
tmpmat[1][1] = a[5];
tmpmat[1][2] = tmpmat[2][1] = a[6];
tmpmat[1][3] = tmpmat[3][1] = a[7];
tmpmat[1][4] = tmpmat[4][1] = a[8];
tmpmat[2][2] = a[9];
tmpmat[2][3] = tmpmat[3][2] = a[10];
tmpmat[2][4] = tmpmat[4][2] = a[11];
tmpmat[3][3] = a[12];
tmpmat[3][4] = tmpmat[4][3] = a[13];
tmpmat[4][4] = a[14];
math::prod_matvec5(tmpmat,v,tmpvec);
return math::inproduct5(v,tmpvec) + 2*math::inproduct5(b,v) + c;
}
};
} // end namespace vcg;
#endif