double LCPPolyDist<VectorIn,TVector,TTuple>::ProcessLoop (bool bHS,
int& riStatusCode, VectorIn* akRes)
{
int i;
double* adQ = new double[m_iNumEquations];;
double** aadM = new double*[m_iNumEquations];
for (i = 0; i < m_iNumEquations; i++)
aadM[i] = new double[m_iNumEquations];
riStatusCode = 0;
bool bContinue = true;
if ( !BuildMatrices(aadM,adQ) )
{
LOGFUNCTION(CloseLog());
akRes[0] = VectorIn::ZERO;
akRes[1] = VectorIn::ZERO;
bContinue = false;
}
LOGFUNCTION(PrintMatrices(aadM,adQ));
int iTryNumber = 0;
float fDistance = -10.0f;
while ( bContinue )
{
double* adZResult = new double[m_iNumEquations];
double* adWResult = new double[m_iNumEquations];
int iReturn;
LCPSolver::LCPSolver(m_iNumEquations,aadM,adQ,adZResult,adWResult,
iReturn);
switch ( iReturn )
{
case SC_FOUND_TRIVIAL_SOLUTION:
{
riStatusCode = SC_FOUND_TRIVIAL_SOLUTION;
break;
}
case SC_FOUND_SOLUTION:
{
// solution found
TVector* akResD = new TVector[2];
for (i = 0; i < m_iDimension; i++)
{
akResD[0][i] = (float) adZResult[i];
akResD[1][i] = (float) adZResult[i+m_iDimension];
}
TVector kDiff = akResD[0]-akResD[1];
fDistance = (float) kDiff.Length();
riStatusCode = VerifySolution(akResD);
if ( !bHS )
{
LOGFUNCTION(VerifyWithTestPoints(akResD,riStatusCode));
}
for (i = 0; i < m_iDimension; i++)
{
akRes[0][i] = (float) akResD[0][i];
akRes[1][i] = (float) akResD[1][i];
}
delete[] akResD;
bContinue = false;
break;
}
case SC_CANNOT_REMOVE_COMPLEMENTARY:
{
LOGFUNCTION(LogRetries(iTryNumber))
if ( iTryNumber == 3 )
{
riStatusCode = SC_CANNOT_REMOVE_COMPLEMENTARY;
bContinue = false;
break;
}
MoveHalfspaces(m_iNumFaces1,m_adB1,m_akA1);
LOGFUNCTION(LogHalfspaces(m_adB1,m_akA1,m_iNumFaces1));
MoveHalfspaces(m_iNumFaces2,m_adB2,m_akA2);
LOGFUNCTION(LogHalfspaces(m_adB2,m_akA2,m_iNumFaces2));
BuildMatrices(aadM,adQ);
iTryNumber++;
break;
}
case SC_EXCEEDED_MAX_RETRIES:
{
LOGFUNCTION(LCPSolverLoopLimit());
riStatusCode = SC_EXCEEDED_MAX_RETRIES;
bContinue = false;
break;
}
}
delete[] adWResult;
delete[] adZResult;
}
for (i = 0; i < m_iNumEquations; i++)
delete[] aadM[i];
delete[] aadM;
delete[] adQ;
return fDistance;
}
