static void
computeInteractionMatrixFromList (/*const*/ vpList<vpBasicFeature *> & featureList,
/*const*/ vpList<int> & featureSelectionList,
vpMatrix & L)
{
if (featureList.empty())
{
vpERROR_TRACE("feature list empty, cannot compute Ls") ;
throw(vpServoException(vpServoException::noFeatureError,
"feature list empty, cannot compute Ls")) ;
}
/* The matrix dimension is not known before the affectation loop.
* It thus should be allocated on the flight, in the loop.
* The first assumption is that the size has not changed. A double
* reallocation (realloc(dim*2)) is done if necessary. In particulary,
* [log_2(dim)+1] reallocations are done for the first matrix computation.
* If the allocated size is too large, a correction is done after the loop.
* The algotithmic cost is linear in affectation, logarthmic in allocation
* numbers and linear in allocation size.
*/
/* First assumption: matrix dimensions have not changed. If 0, they are
* initialized to dim 1.*/
int rowL = L .getRows();
const int colL = 6;
if (0 == rowL) { rowL = 1; L .resize(rowL, colL);}
/* vectTmp is used to store the return values of functions get_s() and
* error(). */
vpMatrix matrixTmp;
int rowMatrixTmp, colMatrixTmp;
/* The cursor are the number of the next case of the vector array to
* be affected. A memory reallocation should be done when cursor
* is out of the vector-array range.*/
int cursorL = 0;
for (featureList.front() ,featureSelectionList.front() ;
!featureList.outside();
featureSelectionList.next(),featureList.next() )
{
vpBasicFeature * sPTR = featureList.value() ;
const int select = featureSelectionList.value() ;
/* Get s. */
matrixTmp = sPTR->interaction(select);
rowMatrixTmp = matrixTmp .getRows();
colMatrixTmp = matrixTmp .getCols();
/* Check the matrix L size, and realloc if needed. */
while (rowMatrixTmp + cursorL > rowL)
{ rowL *= 2; L .resize (rowL,colL,false); vpDEBUG_TRACE(15,"Realloc!"); }
/* Copy the temporarily matrix into L. */
for (int k = 0; k < rowMatrixTmp; ++k, ++cursorL)
for (int j = 0; j < colMatrixTmp; ++j)
{ L[cursorL][j] = matrixTmp[k][j]; }
}
L.resize (cursorL,colL,false);
return ;
}
