// 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());
}
cEqOffsetGPS::cEqOffsetGPS(cRotationFormelle & aRF,cBaseGPS & aBase,bool doGenCode) :
mSet (aRF.Set()),
mRot (&aRF),
mBase (&aBase),
mGPS ("GPS"),
mNameType ("cEqObsBaseGPS" + std::string(aRF.IsGL() ? "_GL" : "")),
mResidu (mRot->C2M(mBase->BaseInc())- mGPS.PtF()),
mFoncEqResidu (0)
{
/*
ELISE_ASSERT
(
(! aRF.IsGL()),
"cEqOffsetGPS to complete in Gimbal Lock Mode"
);
*/
AllowUnsortedVarIn_SetMappingCur = true;
ELISE_ASSERT
(
mRot->Set()==mBase->Set(),
"cEqOffsetGPS Rotation & Base do no belong to same set of unknown"
);
mRot->IncInterv().SetName("Orient");
mBase->IncInterv().SetName("Base");
mLInterv.AddInterv(mRot->IncInterv());
mLInterv.AddInterv(mBase->IncInterv());
if (doGenCode)
{
GenCode();
return;
}
mFoncEqResidu = cElCompiledFonc::AllocFromName(mNameType);
ELISE_ASSERT(mFoncEqResidu!=0,"Cannot allocate cEqObsBaseGPS");
mFoncEqResidu->SetMappingCur(mLInterv,mSet);
// GL
mGPS.InitAdr(*mFoncEqResidu);
mSet->AddFonct(mFoncEqResidu);
}
