String FileDataCacheItem::VectorAsString( const DVector& v )
{
String s = String().Format( "\n%d", v.Length() );
for ( int i = 0; i < v.Length(); ++i )
s.AppendFormat( "\n%.8e", v[i] );
return s;
}
double LCPPolyDist<Dimension,FVector,DVector>::ProcessLoop (
bool halfspaceConstructor, int& statusCode, FVector closest[2])
{
int i;
double* Q = new1<double>(mNumEquations);
double** M = new2<double>(mNumEquations, mNumEquations);
statusCode = 0;
bool processing = true;
if (!BuildMatrices(M, Q))
{
WM5_LCPPOLYDIST_FUNCTION(CloseLog());
closest[0] = FVector::ZERO;
closest[1] = FVector::ZERO;
processing = false;
}
WM5_LCPPOLYDIST_FUNCTION(PrintMatrices(M, Q));
int tryNumber = 0;
double distance = -Mathd::MAX_REAL;
while (processing)
{
double* zResult = new1<double>(mNumEquations);
double* wResult = new1<double>(mNumEquations);
int lcpStatusCode;
LCPSolver solver(mNumEquations, M, Q, zResult, wResult, lcpStatusCode);
WM5_UNUSED(solver);
switch (lcpStatusCode)
{
case SC_FOUND_TRIVIAL_SOLUTION:
{
statusCode = SC_FOUND_TRIVIAL_SOLUTION;
break;
}
case SC_FOUND_SOLUTION:
{
DVector lcpClosest[2];
for (i = 0; i < mDimension; ++i)
{
lcpClosest[0][i] = (float)zResult[i];
lcpClosest[1][i] = (float)zResult[i + mDimension];
}
DVector diff = lcpClosest[0] - lcpClosest[1];
distance = diff.Length();
statusCode = VerifySolution(lcpClosest);
if (!halfspaceConstructor)
{
WM5_LCPPOLYDIST_FUNCTION(VerifyWithTestPoints(lcpClosest,
statusCode));
}
for (i = 0; i < mDimension; ++i)
{
closest[0][i] = (float)lcpClosest[0][i];
closest[1][i] = (float)lcpClosest[1][i];
}
processing = false;
break;
}
case SC_CANNOT_REMOVE_COMPLEMENTARY:
{
WM5_LCPPOLYDIST_FUNCTION(LogRetries(tryNumber));
if (tryNumber == 3)
{
statusCode = SC_CANNOT_REMOVE_COMPLEMENTARY;
processing = false;
break;
}
MoveHalfspaces(mNumFaces1, mA1, mB1);
WM5_LCPPOLYDIST_FUNCTION(LogHalfspaces(mNumFaces1, mA1, mB1));
MoveHalfspaces(mNumFaces2, mA2, mB2);
WM5_LCPPOLYDIST_FUNCTION(LogHalfspaces(mNumFaces2, mA2, mB2));
BuildMatrices(M, Q);
++tryNumber;
break;
}
case SC_EXCEEDED_MAX_RETRIES:
{
WM5_LCPPOLYDIST_FUNCTION(LCPSolverLoopLimit());
statusCode = SC_EXCEEDED_MAX_RETRIES;
processing = false;
break;
}
}
delete1(wResult);
delete1(zResult);
}
delete2(M);
delete1(Q);
return distance;
}
