Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
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;
}