Skip to content

Commit ca70779

Browse files
author
deseilligny
committed
Code Clino Blokc generated, not tested yet
1 parent 09b783a commit ca70779

File tree

3 files changed

+73
-34
lines changed

3 files changed

+73
-34
lines changed

‎MMVII/src/SymbDerGen/ComonHeaderSymb.h‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -353,6 +353,11 @@ template <class Type> class cRot3dF
353353
{
354354
}
355355

356+
cRot3dF(const std::vector<Type> & aVecUk,size_t * aK0Uk,const std::vector<Type> & aVecObs,size_t * aK0Obs) :
357+
cRot3dF(aVecUk,IndexAutoIncr(aK0Uk,3),aVecObs,IndexAutoIncr(aK0Obs,9))
358+
{
359+
}
360+
356361
tPt Value(const tPt & aPt) const {return mIJK*aPt;}
357362

358363
cRot3dF<Type> Inverse() const {return cRot3dF<Type>( mIJK.Transpose());}
@@ -404,6 +409,25 @@ template <class Type> class cPoseF
404409
mRot( WithAxiator ? tRot(aVecUk,aK0Uk+3,aVecObs,aK0Obs) : tRot(aVecObs,aK0Obs) )
405410
{
406411
}
412+
413+
414+
cPoseF(const std::vector<Type> & aVecUk,size_t * aK0Uk,const std::vector<Type> & aVecObs,size_t * aK0Obs) :
415+
cPoseF(aVecUk,*aK0Uk,aVecObs, *aK0Obs,true)
416+
{
417+
*aK0Uk += 6;
418+
*aK0Obs += 9;
419+
}
420+
421+
422+
423+
cPoseF(const std::vector<Type> & aVecObs,size_t * aK0Obs) :
424+
cPoseF( aVecObs, *aK0Obs,aVecObs,*aK0Obs+3,false)
425+
{
426+
// !!! We cannot use AutoIncr in constructor at it would be used twice with undefined order
427+
*aK0Obs += 12;
428+
}
429+
430+
407431
tPt Value(const tPt & aPt) const {return mCenter + mRot.Value(aPt);}
408432

409433
const cMatF<Type> & IJK() const {return mRot.mIJK;}
@@ -464,6 +488,12 @@ template <class Type> class cP3dNorm
464488
{
465489
}
466490

491+
cP3dNorm(const std::vector<Type> & aVecUk,size_t* aK0Uk,const std::vector<Type> & aVecObs,size_t* aK0Obs) :
492+
cP3dNorm(aVecUk,IndexAutoIncr(aK0Uk,2),aVecObs,IndexAutoIncr(aK0Obs,9))
493+
{
494+
}
495+
496+
467497
tPt3 CurPt() const
468498
{
469499
return mPNorm + mDuDv.x()*mU + mDuDv.y() * mV;

‎MMVII/src/SymbDerGen/Formulas_BlockRigid.h‎

Lines changed: 41 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -98,12 +98,19 @@ class cFormulaBlocRigid
9898
const std::vector<tUk> & aVObs
9999
) const
100100
{
101-
// create the 4 formal poses, using unkonw (C/W) and obs
101+
// create the 4 formal poses, using unkonw (C/W) and obs, Use auto incremenation by passing adresses
102+
#if (1)
103+
size_t aK0Uk=0,aK0Obs=0;
104+
cPoseF<tUk> aPoseA(aVUk,&aK0Uk,aVObs,&aK0Obs);
105+
cPoseF<tUk> aPoseB(aVUk,&aK0Uk,aVObs,&aK0Obs);
106+
cPoseF<tUk> aPose1(aVUk,&aK0Uk,aVObs,&aK0Obs);
107+
cPoseF<tUk> aPose2(aVUk,&aK0Uk,aVObs,&aK0Obs);
108+
#else
102109
cPoseF<tUk> aPoseA(aVUk,0*NbUk,aVObs,0*NbObs,true);
103-
cPoseF<tUk> aPoseB(aVUk,1*NbUk,aVObs,1*NbObs,true);
104-
cPoseF<tUk> aPose1(aVUk,2*NbUk,aVObs,2*NbObs,true);
105-
cPoseF<tUk> aPose2(aVUk,3*NbUk,aVObs,3*NbObs,true);
106-
110+
cPoseF<tUk> aPoseB(aVUk,1*NbUk,aVObs,1*NbObs,true);
111+
cPoseF<tUk> aPose1(aVUk,2*NbUk,aVObs,2*NbObs,true);
112+
cPoseF<tUk> aPose2(aVUk,3*NbUk,aVObs,3*NbObs,true);
113+
#endif
107114
// compute relative poses B/A and 2/1
108115
cPoseF<tUk> aRelAB = aPoseA.PoseRel(aPoseB);
109116
cPoseF<tUk> aRel12 = aPose1.PoseRel(aPose2);
@@ -159,8 +166,15 @@ class cFormulaRattBRExist
159166
const std::vector<tUk> & aVObs
160167
) const
161168
{
169+
#if (1)
170+
// Use new auto incr func
171+
size_t aK0Uk=0,aK0Obs=0;
172+
cPoseF<tUk> aPoseA(aVUk,&aK0Uk,aVObs,&aK0Obs);
173+
cPoseF<tUk> aPose1(aVObs,&aK0Obs);
174+
#else
162175
cPoseF<tUk> aPoseA(aVUk,0,aVObs,0,true);
163176
cPoseF<tUk> aPose1(aVObs,9,aVObs,12,false);
177+
#endif
164178

165179
cPtxd<tUk,3> aDeltaC = aPoseA.mCenter - aPose1.mCenter;
166180
cMatF<tUk> aDeltaR = aPoseA.IJK()- aPose1.IJK();
@@ -194,8 +208,7 @@ class cFormulaClino
194208
// vector for correction of angular
195209
std::vector<std::string> aVCor;
196210
for (int aD=mD0Corr; aD<=mD1Corr ; aD++)
197-
if (aD!=1)
198-
aVCor.push_back("DCorrAng_"+ToStr(aD));
211+
aVCor.push_back("DCorrAng_"+ToStr(aD));
199212

200213
return Append
201214
(
@@ -225,40 +238,34 @@ class cFormulaClino
225238
const std::vector<tUk> & aVObs
226239
) const
227240
{
228-
cRot3dF<tUk> aRotC2M(aVUk,0,aVObs,0);
229-
cP3dNorm<tUk> aClinoC(aVUk,3,aVObs,9); //
230-
cP3dNorm<tUk> aClinoM = aRotC2M.Value(aClinoC);
231-
232-
233-
//return std::abs(aDirLoc.z() - std::sin(mSetClino.KthMeasure(aKClino).Angle()) );
241+
// IndexAutoIncr(&anInd,3)
242+
size_t aK0Uk=0,aK0Obs=0;
243+
// rotation that is linked to clino, can be a camera rotation Cam->Word
244+
cRot3dF<tUk> aRotC2M(aVUk,&aK0Uk,aVObs,&aK0Obs);
234245

246+
cP3dNorm<tUk> aClinoC(aVUk,&aK0Uk,aVObs,&aK0Obs); //
235247

248+
cPtxd<tUk,3> aClinoM = aRotC2M.Value(aClinoC.CurPt());
249+
tUk aSinT = sin(aVObs.at(aK0Obs++));
236250

237-
// cRot3dF(const std::vector<Type> & aVecUk,size_t aK0Uk,const std::vector<Type> & aVecObs,size_t aK0Obs) :
238-
// cP3dNorm(const std::vector<Type> & aVecUk,size_t aK0Uk,const std::vector<Type> & aVecObs,size_t aK0Obs) :
251+
tUk aSumTeta = CreateCste(0.0,aVUk.at(0));
252+
for (int aD=mD0Corr ; aD<=mD1Corr ; aD++)
253+
{
254+
aSumTeta = aSumTeta + aVUk.at(aK0Uk++) * powI(aSinT,aD);
255+
}
239256

240-
//cPoseF<tUk> aPoseA(aVUk,0,aVObs,0,true);
241-
return aVUk[0];
257+
return { aClinoM.z() - aSumTeta} ;
242258
}
243-
/*
244-
cPtxd<tUk,3> aDeltaC = aPoseA.mCenter - aPose1.mCenter;
245-
cMatF<tUk> aDeltaR = aPoseA.IJK()- aPose1.IJK();
246-
*/
247-
// ...
248-
// extract PoseA,PoseB,pose1, pose2
249-
250-
// compute pose rel B to A, pose rel 2 to 1
251-
// compute the difference
252-
253-
254-
255-
// cPoseF<tUk> aPose1(aVUk,2*NbUk,aVObs,2*NbObs);
256-
// cPoseF<tUk> aRelAB = aPoseA.PoseRel(aPoseB);
257-
// (ToVect(aDeltaC),aDeltaM.ToVect()
258259

260+
cFormulaClino(int aD0Corr,int aD1Corr) :
261+
mD0Corr (aD0Corr),
262+
mD1Corr (aD1Corr)
263+
{
264+
}
259265

260-
int mD0Corr;
261-
int mD1Corr;
266+
private :
267+
int mD0Corr;
268+
int mD1Corr;
262269

263270
};
264271

‎MMVII/src/SymbDerGen/GenerateCodes.cpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -842,6 +842,8 @@ int cAppliGenCode::Exe()
842842

843843
GenCodesFormula((tREAL8*)nullptr,cFormulaBlocRigid(),WithDer); // RIGIDBLOC
844844
GenCodesFormula((tREAL8*)nullptr,cFormulaRattBRExist(),WithDer); // RIGIDBLOC
845+
GenCodesFormula((tREAL8*)nullptr,cFormulaClino(0,3),WithDer);
846+
845847
GenCodesFormula((tREAL8*)nullptr,cFormulaClinoBloc(),WithDer); // CLINOBLOC
846848
GenCodesFormula((tREAL8*)nullptr,cFormulaClinoRot(),WithDer); // CLINOBLOC
847849

0 commit comments

Comments
 (0)