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;
}
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;
}