SQuaternion Average( const CSymmetryType & oSym, Iterator pBegin, Iterator pEnd,
PropertyMap oMap )
{
vector<SQuaternion> oSymOpList = oSym.GetQuatOperatorList();
vector<SQuaternion> oCloudAverage( oSymOpList.size() );
for( Size_Type n = 0; n < oSymOpList.size(); n ++ )
{
oCloudAverage[n] = oMap.GetQuaternion( *pBegin ) * oSymOpList[n];
oCloudAverage[n].ToConvention();
}
vector<Int> oCloudPointsList( oSymOpList.size(), 1 );
for( Iterator pCur = (pBegin + 1); pCur != pEnd; ++ pCur )
{
for( Size_Type n = 0; n < oSymOpList.size(); n ++ )
{
Int nCloudIndex = 0;
SQuaternion q = oMap.GetQuaternion( *pCur ) * oSymOpList[ n ];;
Float fMinDist = 200;
for( Size_Type m = 0; m < oSymOpList.size(); m ++ ) // find cloud
{
SQuaternion qProd = q.Inverse() * oCloudAverage[m];
Float fDist = Float( 2 ) * acos( std::min( Float( 1 ), qProd.m_fW ) );
if( fDist < fMinDist )
{
fMinDist = fDist;
nCloudIndex = m;
}
}
Int nPoints = oCloudPointsList[ nCloudIndex ];
SQuaternion & qAve = oCloudAverage[ nCloudIndex ];
q.ToConvention();
qAve.m_fX = ( ( qAve.m_fX * nPoints ) + q.m_fX );
qAve.m_fY = ( ( qAve.m_fY * nPoints ) + q.m_fY );
qAve.m_fZ = ( ( qAve.m_fZ * nPoints ) + q.m_fZ );
qAve.m_fW = ( ( qAve.m_fW * nPoints ) + q.m_fW );
qAve = qAve / qAve.EuclideanNorm();
qAve.ToConvention();
oCloudPointsList[ nCloudIndex ] ++;
}
}
SQuaternion oOrientationAverage;
oOrientationAverage.Set( 0, 0, 0, 0 );
for( Size_Type n = 0; n < oSymOpList.size(); n ++ )
{
oCloudAverage[n] = ReduceToFundamentalZone( oSym, oCloudAverage[n] );
oCloudAverage[n].ToConvention();
oOrientationAverage += oCloudAverage[n];
}
oOrientationAverage = oOrientationAverage / oOrientationAverage.EuclideanNorm();
return oOrientationAverage;
}
