// Compute the parameter of the transformation of Point in L coordinate
// to point in R coordinates
void CalcParamEqRel
(
Pt3d<Fonc_Num> & aTr,
ElMatrix<Fonc_Num> & aMat,
cRotationFormelle & aRotR,
cRotationFormelle & aRotL,
int aRNumGl,
int aLNumGl
)
{
#if (HandleGL != 1)
ELISE_ASSERT(! aRotR.IsGL(),"Guimbal lock in Eq Rig still unsupported");
ELISE_ASSERT(! aRotL.IsGL(),"Guimbal lock in Eq Rig still unsupported");
aRNumGl=-1;
aLNumGl=-1;
#endif
ElMatrix<Fonc_Num> aRMat = aRotR.MatFGLComplete(aRNumGl);
ElMatrix<Fonc_Num> aLMat = aRotL.MatFGLComplete(aLNumGl);
ElMatrix<Fonc_Num> aRMatInv = aRMat.transpose();
aMat = aRMatInv * aLMat;
aTr = aRMatInv * (aRotL.COpt() - aRotR.COpt());
}
static Pt3dr PInBCoord(const Pt3dr & aBase,const ElMatrix<double> & aRot,const Pt3dr & aPA)
{
// mDirOr1A (vunit(mBase^mDir1A)),
Pt3dr aRes = aRot.transpose() * (aBase+aPA);
std::cout << "PInBCoord " << scal(aBase^aPA,aRot*aRes) << "\n";
return aRes;
}
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;
}
