ElMatrix<double> cOriFromBundle::CalculRot(const cOFB_Sol1S1T & aSol,bool Show)
{
// R [mDir1B ...] = [mV1 ...]
ElMatrix<double> aMatB = MatFromCol(mDir1B,mDir2B,mDir3B);
ElMatrix<double> aMatBinA = MatFromCol(aSol.mV1,aSol.mV2,aSol.mV3);
ElMatrix<double> aMat = aMatBinA * gaussj(aMatB);
// std::cout << mDir1B << mDir2B << mDir3B << "\n";
// std::cout << aSol.mV1 << aSol.mV2 << aSol.mV3 << "\n";
// std::cout << "xxxxxxxxxxxx\n";
/*
for (int anY=0 ; anY<3 ; anY++)
{
for (int anX=0 ; anX<3 ; anX++)
{
std::cout << aMat(anX,anY) << " ";
}
std::cout << "\n";
}
*/
// getchar();
ElMatrix<double> aR = NearestRotation(aMat);
// std::cout << "yyyyyyyyyyyyyy\n";
if (Show)
{
std::cout << "DET " << aR.Det() -1.0 << " Eucl " << ElMatrix<double>(3,true).L2(aR*aR.transpose())<< "\n";
std::cout << " SCAL " << scal(mBase^mDir1A,aR*mDir1B) << " "
<< scal(mBase^mDir2A,aR*mDir2B) << " "
<< scal(mBase^mDir3A,aR*mDir3B) << "\n";
}
return aR;
}
ElRotation3D RotationCart2RTL(Pt3dr aP, double aZ,Pt3dr aDirX )
{
Pt3dr aDirZ = vunit(aP);
aP = aP - aDirZ * aZ;
// Pt3dr aDirX = vunit(Pt3dr(0,0,1)^aDirZ);
Pt3dr aDirY = vunit(aDirZ^aDirX);
aDirX = vunit(aDirY^aDirZ);
ElRotation3D aRTL2Cart(aP,MatFromCol(aDirX,aDirY,aDirZ),true);
// std::cout << "TES-RTL :: " << aRTL2Cart.inv().ImAff(aP *1.001) << "\n";
// std::cout << "TES-RTL :: " << aRTL2Cart.inv().ImAff(aP) << "\n";
return aRTL2Cart.inv();
}
Pt3dr cPushB_PhysMod::Im2GeoC_Refined(const Pt2dr & aP0Im,Pt3dr aPTer,const Pt3dr &aU,const Pt3dr & aV) const
{
Pt2dr aPIm = GeoC2Im_Init(aPTer);
Pt2dr aGradX = GeoC2Im_Init(aPTer+aU) - aPIm;
Pt2dr aGradY = GeoC2Im_Init(aPTer+aV) - aPIm;
ElMatrix<double> aGrad = MatFromCol(aGradX,aGradY);
aGrad = gaussj(aGrad);
int aNbIter =0 ;
while ((euclid(aPIm-aP0Im) > TheEpsilonRefine) && (aNbIter<5))
{
aNbIter ++;
Pt2dr aSol = aGrad * (aP0Im-aPIm);
aPTer = aPTer + aU*aSol.x + aV*aSol.y;
aPIm = GeoC2Im_Init(aPTer);
}
return aPTer;
}
Pt2dr cPushB_PhysMod::GeoCToIm_Refined(const Pt3dr & aPTer) const
{
Pt2dr aPIm = GeoC2Im_Init(aPTer);
ElSeg3D aSeg0 = Im2GeoC_Init(aPIm);
Pt3dr aProj = aSeg0.ProjOrtho(aPTer);
Pt3dr aGradX = Im2GeoC_Init(aPIm+Pt2dr(1,0)).ProjOrtho(aPTer) -aProj;
Pt3dr aGradY = Im2GeoC_Init(aPIm+Pt2dr(0,1)).ProjOrtho(aPTer) -aProj;
Pt3dr aGradZ = aSeg0.TgNormee();
ElMatrix<double> aGrad = MatFromCol(aGradX,aGradY,aGradZ);
aGrad = gaussj(aGrad);
int aNbIter =0 ;
while ((euclid(aProj-aPTer) > TheEpsilonRefine) && (aNbIter<5))
{
aNbIter ++;
Pt3dr aSol = aGrad * (aPTer-aProj);
aPIm = aPIm + Pt2dr(aSol.x,aSol.y);
aProj = Im2GeoC_Init(aPIm).ProjOrtho(aPTer);
}
return aPIm;
}
void correctPlanarPolygon( vector<Pt3dr> &aPolygon )
{
if (aPolygon.size() < 3) ELISE_ERROR_EXIT("aPolygon.size() = " << aPolygon.size() << " < 3");
Pt3dr u = vunit(aPolygon[1] - aPolygon[0]), minV = vunit(aPolygon[2] - aPolygon[0]);
size_t minI = 2;
REAL minScal = ElAbs(scal(u, minV));
for (size_t i = 3; i < aPolygon.size(); i++)
{
Pt3dr v = vunit(aPolygon[i] - aPolygon[0]);
REAL d = ElAbs(scal(u, v));
if (d < minScal)
{
minScal = d;
minI = i;
minV = v;
}
}
cout << "minI = " << minI << endl;
cout << "minScal = " << minScal << endl;
cout << "minV = " << minV << endl;
Pt3dr n = u ^ minV;
cout << "minV = " << minV << endl;
ElMatrix<REAL> planToGlobalMatrix = MatFromCol(u, minV, n);
if (planToGlobalMatrix.Det() < 1e-10) ELISE_ERROR_EXIT("matrix is not inversible");
ElRotation3D planToGlobalRot(aPolygon[0], planToGlobalMatrix, true);
ElRotation3D globalToPlanRot = planToGlobalRot.inv();
//~ const size_t nbVertices = aPolygon.size();
//~ ostringstream ss;
//~ static int ii = 0;
//~ const REAL extrudSize = 1e4;
//~ ss << "polygon_" << (ii++) << ".ply";
//~ ofstream f(ss.str().c_str());
//~ f << "ply" << endl;
//~ f << "format ascii 1.0" << endl;
//~ f << "element vertex " << 4 * nbVertices << endl;
//~ f << "property float x" << endl;
//~ f << "property float y" << endl;
//~ f << "property float z" << endl;
//~ f << "property uchar red" << endl;
//~ f << "property uchar green" << endl;
//~ f << "property uchar blue" << endl;
//~ f << "element face " << nbVertices << endl;
//~ f << "property list uchar int vertex_indices" << endl;
//~ f << "end_header" << endl;
REAL zDiffSum = 0.;
for (size_t i = 0; i < aPolygon.size(); i++)
{
Pt3dr p = globalToPlanRot.ImAff(aPolygon[i]);
zDiffSum += ElAbs(p.z);
aPolygon[i] = planToGlobalRot.ImAff(Pt3dr(p.x, p.y, 0.));
//~ Pt3dr p0 = (i == 0) ? aPolygon[aPolygon.size() - 1] : aPolygon[i - 1], p1 = aPolygon[i], p2 = p1 + n * extrudSize, p3 = p0 + n * extrudSize;
//~ f << p0.x << ' ' << p0.y << ' ' << p0.z << " 128 128 128" << endl;
//~ f << p1.x << ' ' << p1.y << ' ' << p1.z << " 128 128 128" << endl;
//~ f << p2.x << ' ' << p2.y << ' ' << p2.z << " 128 128 128" << endl;
//~ f << p3.x << ' ' << p3.y << ' ' << p3.z << " 128 128 128" << endl;
}
//~ for (size_t i = 0; i < aPolygon.size(); i++)
//~ f << 4 << ' ' << i * 4 << ' ' << i * 4 + 1 << ' ' << i * 4 + 2 << ' ' << i* 4 + 3 << endl;
//~ f.close();
cout << "zDiffSum = " << zDiffSum << endl;
}
