// modifies this->delta_f
 void CalcForceDelta(int d, float dir)
 {
     delta_f[d] = dir;
     if (numClamped == 0)
     {
         return;
     }
     // get column d of matrix
     float[] ptr = new float[numClamped];
     for (int i = 0; i < numClamped; i++)
     {
         ptr[i] = rowPtrs[i][d];
     }
     // solve force delta
     SolveClamped(delta_f, ptr);
     // flip force delta based on direction
     if (dir > 0.0f)
     {
         ptr = delta_f.ToArray();
         for (int i = 0; i < numClamped; i++)
         {
             ptr[i] = -ptr[i];
         }
     }
 }
 void SolveClamped(ref idVecX x, float[] b)
 {
     // solve L
     SIMDProcessor.MatX_LowerTriangularSolve(clamped, solveCache1.ToArray(), b, numClamped, clampedChangeStart);
     // solve D
     SIMDProcessor.Mul(solveCache2.ToArray(), solveCache1.ToArray(), diagonal.ToArray(), numClamped);
     // solve Lt
     SIMDProcessor.MatX_LowerTriangularSolveTranspose(clamped, x.ToArray(), solveCache2.ToArray(), numClamped);
     clampedChangeStart = numClamped;
 }
 void SolveClamped(ref idVecX x, float[] b)
 {
     // solve L
     SIMDProcessor.MatX_LowerTriangularSolve(clamped, solveCache1.ToArray(), b, numClamped, clampedChangeStart);
     // solve D
     SIMDProcessor.Mul(solveCache2.ToArray(), solveCache1.ToArray(), diagonal.ToArray(), numClamped);
     // solve Lt
     SIMDProcessor.MatX_LowerTriangularSolveTranspose(clamped, x.ToArray(), solveCache2.ToArray(), numClamped);
     clampedChangeStart = numClamped;
 }
 // modifies this->delta_f
 void CalcForceDelta(int d, float dir)
 {
     delta_f[d] = dir;
     if (numClamped == 0)
     {
         return;
     }
     // solve force delta
     SolveClamped(ref delta_f, rowPtrs[d]);
     // flip force delta based on direction
     if (dir > 0.0f)
     {
         float[] ptr = delta_f.ToArray();
         for (int i = 0; i < numClamped; i++)
         {
             ptr[i] = -ptr[i];
         }
     }
 }
        public override bool Solve(ref idMatX o_m, ref idVecX o_x, ref idVecX o_b, ref idVecX o_lo, ref idVecX o_hi, int[] o_boxIndex)
        {
            int i;
            // true when the matrix rows are 16 byte padded
            padded = ((o_m.GetNumRows() + 3) & ~3) == o_m.GetNumColumns();
            Debug.Assert(padded || o_m.GetNumRows() == o_m.GetNumColumns());
            Debug.Assert(o_x.GetSize() == o_m.GetNumRows());
            Debug.Assert(o_b.GetSize() == o_m.GetNumRows());
            Debug.Assert(o_lo.GetSize() == o_m.GetNumRows());
            Debug.Assert(o_hi.GetSize() == o_m.GetNumRows());
            // allocate memory for permuted input
            f.SetData(o_m.GetNumRows(), VECX_ALLOCA(o_m.GetNumRows()));
            a.SetData(o_b.GetSize(), VECX_ALLOCA(o_b.GetSize()));
            b.SetData(o_b.GetSize(), VECX_ALLOCA(o_b.GetSize()));
            lo.SetData(o_lo.GetSize(), VECX_ALLOCA(o_lo.GetSize()));
            hi.SetData(o_hi.GetSize(), VECX_ALLOCA(o_hi.GetSize()));
            if (o_boxIndex != null)
            {
                boxIndex = new int[o_x.GetSize()];
                Array.Copy(boxIndex, o_boxIndex, o_x.GetSize());
            }
            else
                boxIndex = null;
            // we override the const on o_m here but on exit the matrix is unchanged
            m.SetData(o_m.GetNumRows(), o_m.GetNumColumns(), (float[])o_m[0]);
            f.Zero();
            a.Zero();
            b = o_b;
            lo = o_lo;
            hi = o_hi;
            // pointers to the rows of m
            rowPtrs = new float[m.GetNumRows()][];
            for (i = 0; i < m.GetNumRows(); i++)
                rowPtrs[i] = m[i];
            // tells if a variable is at the low boundary, high boundary or inbetween
            side = new int[m.GetNumRows()];
            // index to keep track of the permutation
            permuted = new int[m.GetNumRows()];
            for (i = 0; i < m.GetNumRows(); i++)
                permuted[i] = i;
            // permute input so all unbounded variables come first
            numUnbounded = 0;
            for (i = 0; i < m.GetNumRows(); i++)
                if (lo[i] == float.NegativeInfinity && hi[i] == float.PositiveInfinity)
                {
                    if (numUnbounded != i)
                        Swap(numUnbounded, i);
                    numUnbounded++;
                }
            // permute input so all variables using the boxIndex come last
            int boxStartIndex = m.GetNumRows();
            if (boxIndex != null)
                for (int i = m.GetNumRows() - 1; i >= numUnbounded; i--)
                    if (boxIndex[i] >= 0 && (lo[i] != float.NegativeInfinity || hi[i] != float.PositiveInfinity))
                    {
                        boxStartIndex--;
                        if (boxStartIndex != i)
                            Swap(boxStartIndex, i);
                    }
            // sub matrix for factorization 
            clamped.SetData(m.GetNumRows(), m.GetNumColumns(), MATX_ALLOCA(m.GetNumRows() * m.GetNumColumns()));
            diagonal.SetData(m.GetNumRows(), VECX_ALLOCA(m.GetNumRows()));
            solveCache1.SetData(m.GetNumRows(), VECX_ALLOCA(m.GetNumRows()));
            solveCache2.SetData(m.GetNumRows(), VECX_ALLOCA(m.GetNumRows()));
            // all unbounded variables are clamped
            numClamped = numUnbounded;
            // if there are unbounded variables
            if (numUnbounded != 0)
            {
                // factor and solve for unbounded variables
                if (!FactorClamped()) { idLib.common.Printf("Solve: unbounded factorization failed\n"); return false; }
                SolveClamped(f, b.ToArray());
                // if there are no bounded variables we are done
                if (numUnbounded == m.GetNumRows()) { o_x = f; return true; } // the vector is not permuted
            }
#if IGNORE_UNSATISFIABLE_VARIABLES
            int numIgnored = 0;
#endif
            // allocate for delta force and delta acceleration
            delta_f.SetData(m.GetNumRows(), VECX_ALLOCA(m.GetNumRows()));
            delta_a.SetData(m.GetNumRows(), VECX_ALLOCA(m.GetNumRows()));
            // solve for bounded variables
            string failed = null;
            for (i = numUnbounded; i < m.GetNumRows(); i++)
            {
                clampedChangeStart = 0;
                // once we hit the box start index we can initialize the low and high boundaries of the variables using the box index
                if (i == boxStartIndex)
                {
                    for (int j = 0; j < boxStartIndex; j++)
                        o_x[permuted[j]] = f[j];
                    for (int j = boxStartIndex; j < m.GetNumRows(); j++)
                    {
                        float s = o_x[boxIndex[j]];
                        if (lo[j] != float.NegativeInfinity)
                            lo[j] = -idMath.Fabs(lo[j] * s);
                        if (hi[j] != float.PositiveInfinity)
                            hi[j] = idMath.Fabs(hi[j] * s);
                    }
                }
                // calculate acceleration for current variable
                float dot;
                SIMDProcessor.Dot(out dot, rowPtrs[i], f.ToArray(), i);
                a[i] = dot - b[i];
                // if already at the low boundary
                if (lo[i] >= -LCP_BOUND_EPSILON && a[i] >= -LCP_ACCEL_EPSILON) { side[i] = -1; continue; }
                // if already at the high boundary
                if (hi[i] <= LCP_BOUND_EPSILON && a[i] <= LCP_ACCEL_EPSILON) { side[i] = 1; continue; }
                // if inside the clamped region
                if (idMath.Fabs(a[i]) <= LCP_ACCEL_EPSILON) { side[i] = 0; AddClamped(i, false); continue; }
                // drive the current variable into a valid region
                int n;
                for (n = 0; n < maxIterations; n++)
                {
                    // direction to move
                    float dir = (a[i] <= 0.0f ? 1.0f : -1.0f);
                    // calculate force delta
                    CalcForceDelta(i, dir);
                    // calculate acceleration delta: delta_a = m * delta_f;
                    CalcAccelDelta(i);
                    // maximum step we can take
                    float maxStep;
                    int limit;
                    int limitSide;
                    GetMaxStep(i, dir, out maxStep, out limit, out limitSide);
                    if (maxStep <= 0.0f)
                    {
#if IGNORE_UNSATISFIABLE_VARIABLES
                        // ignore the current variable completely
                        lo[i] = hi[i] = 0.0f;
                        f[i] = 0.0f;
                        side[i] = -1;
                        numIgnored++;
#else
                        failed = string.Format("invalid step size %.4f", maxStep);
#endif
                        break;
                    }
                    // change force
                    ChangeForce(i, maxStep);
                    // change acceleration
                    ChangeAccel(i, maxStep);
                    // clamp/unclamp the variable that limited this step
                    side[limit] = limitSide;
                    switch (limitSide)
                    {
                        case 0:
                            a[limit] = 0.0f;
                            AddClamped(limit, (limit == i));
                            break;
                        case -1:
                            f[limit] = lo[limit];
                            if (limit != i)
                                RemoveClamped(limit);
                            break;
                        case 1:
                            f[limit] = hi[limit];
                            if (limit != i)
                                RemoveClamped(limit);
                            break;
                    }
                    // if the current variable limited the step we can continue with the next variable
                    if (limit == i)
                        break;
                }
                if (n >= maxIterations)
                {
                    failed = string.Format("max iterations %d", maxIterations);
                    break;
                }
                if (failed != null)
                    break;
            }
#if IGNORE_UNSATISFIABLE_VARIABLES
            if (numIgnored != null)
                if (lcp_showFailures.GetBool())
                    idLib.common.Printf("Solve: %d of %d bounded variables ignored\n", numIgnored, m.GetNumRows() - numUnbounded);
#endif
            // if failed clear remaining forces
            if (failed != null)
            {
                if (lcp_showFailures.GetBool())
                    idLib.common.Printf("Solve: %s (%d of %d bounded variables ignored)\n", failed, m.GetNumRows() - i, m.GetNumRows() - numUnbounded);
                for (int j = i; j < m.GetNumRows(); j++)
                    f[j] = 0.0f;
            }
#if _DEBUG && false
            if (failed == null)
                // test whether or not the solution satisfies the complementarity conditions
                for (i = 0; i < m.GetNumRows(); i++)
                {
                    a[i] = -b[i];
                    for (int j = 0; j < m.GetNumRows(); j++)
                        a[i] += rowPtrs[i][j] * f[j];
                    if (f[i] == lo[i])
                    {
                        if (lo[i] != hi[i] && a[i] < -LCP_ACCEL_EPSILON)
                            int bah1 = 1;
                    }
                    else if (f[i] == hi[i])
                    {
                        if (lo[i] != hi[i] && a[i] > LCP_ACCEL_EPSILON)
                            int bah2 = 1;
                    }
                    else if (f[i] < lo[i] || f[i] > hi[i] || idMath.Fabs(a[i]) > 1.0f)
                    {
                        int bah3 = 1;
                    }
                }
#endif
            // unpermute result
            for (i = 0; i < f.GetSize(); i++)
                o_x[permuted[i]] = f[i];
            // unpermute original matrix
            for (i = 0; i < m.GetNumRows(); i++)
            {
                for (int j = 0; j < m.GetNumRows(); j++)
                {
                    if (permuted[j] == i)
                        break;
                }
                if (i != j)
                {
                    m.SwapColumns(i, j);
                    idSwap(permuted[i], permuted[j]);
                }
            }
            return true;
        }
        public override bool Solve(ref idMatX o_m, ref idVecX o_x, ref idVecX o_b, ref idVecX o_lo, ref idVecX o_hi, int[] o_boxIndex)
        {
            // true when the matrix rows are 16 byte padded
            padded = (((o_m.GetNumRows() + 3) & ~3) == o_m.GetNumColumns());
            Debug.Assert(padded || o_m.GetNumRows() == o_m.GetNumColumns());
            Debug.Assert(o_x.GetSize() == o_m.GetNumRows());
            Debug.Assert(o_b.GetSize() == o_m.GetNumRows());
            Debug.Assert(o_lo.GetSize() == o_m.GetNumRows());
            Debug.Assert(o_hi.GetSize() == o_m.GetNumRows());

            // allocate memory for permuted input
            f.SetData(o_m.GetNumRows(), VECX_ALLOCA(o_m.GetNumRows()));
            a.SetData(o_b.GetSize(), VECX_ALLOCA(o_b.GetSize()));
            b.SetData(o_b.GetSize(), VECX_ALLOCA(o_b.GetSize()));
            lo.SetData(o_lo.GetSize(), VECX_ALLOCA(o_lo.GetSize()));
            hi.SetData(o_hi.GetSize(), VECX_ALLOCA(o_hi.GetSize()));
            if (o_boxIndex != null)
            {
                boxIndex = new int[o_x.GetSize()];
                Array.Copy(boxIndex, o_boxIndex, o_x.GetSize());
            }
            else
            {
                boxIndex = null;
            }
            // we override the const on o_m here but on exit the matrix is unchanged
            m.SetData(o_m.GetNumRows(), o_m.GetNumColumns(), (float[])o_m[0]);
            f.Zero();
            a.Zero();
            b  = o_b;
            lo = o_lo;
            hi = o_hi;
            // pointers to the rows of m
            rowPtrs = new float[m.GetNumRows()][];
            for (int i = 0; i < m.GetNumRows(); i++)
            {
                rowPtrs[i] = m[i];
            }
            // tells if a variable is at the low boundary, high boundary or inbetween
            side = new int[m.GetNumRows()];
            // index to keep track of the permutation
            permuted = new int[m.GetNumRows()];
            for (int i = 0; i < m.GetNumRows(); i++)
            {
                permuted[i] = i;
            }
            // permute input so all unbounded variables come first
            numUnbounded = 0;
            for (int i = 0; i < m.GetNumRows(); i++)
            {
                if (lo[i] == -float.NegativeInfinity && hi[i] == float.PositiveInfinity)
                {
                    if (numUnbounded != i)
                    {
                        Swap(numUnbounded, i);
                    }
                    numUnbounded++;
                }
            }
            // permute input so all variables using the boxIndex come last
            int boxStartIndex = m.GetNumRows();

            if (boxIndex != null)
            {
                for (int i = m.GetNumRows() - 1; i >= numUnbounded; i--)
                {
                    if (boxIndex[i] >= 0 && (lo[i] != float.NegativeInfinity || hi[i] != float.PositiveInfinity))
                    {
                        boxStartIndex--;
                        if (boxStartIndex != i)
                        {
                            Swap(boxStartIndex, i);
                        }
                    }
                }
            }
            // sub matrix for factorization
            clamped.SetData(m.GetNumRows(), m.GetNumColumns(), MATX_ALLOCA(m.GetNumRows() * m.GetNumColumns()));
            diagonal.SetData(m.GetNumRows(), VECX_ALLOCA(m.GetNumRows()));
            // all unbounded variables are clamped
            numClamped = numUnbounded;
            // if there are unbounded variables
            if (numUnbounded != 0)
            {
                // factor and solve for unbounded variables
                if (!FactorClamped())
                {
                    idLib.common.Printf("idLCP_Square::Solve: unbounded factorization failed\n");
                    return(false);
                }
                SolveClamped(f, b.ToArray());
                // if there are no bounded variables we are done
                if (numUnbounded == m.GetNumRows())
                {
                    o_x = f; // the vector is not permuted
                    return(true);
                }
            }
#if IGNORE_UNSATISFIABLE_VARIABLES
            int numIgnored = 0;
#endif
            // allocate for delta force and delta acceleration
            delta_f.SetData(m.GetNumRows(), VECX_ALLOCA(m.GetNumRows()));
            delta_a.SetData(m.GetNumRows(), VECX_ALLOCA(m.GetNumRows()));
            //    // solve for bounded variables
            string failed = null;
            float  dot;
            for (int i = numUnbounded; i < m.GetNumRows(); i++)
            {
                // once we hit the box start index we can initialize the low and high boundaries of the variables using the box index
                if (i == boxStartIndex)
                {
                    for (int j = 0; j < boxStartIndex; j++)
                    {
                        o_x[permuted[j]] = f[j];
                    }
                    for (int j = boxStartIndex; j < m.GetNumRows(); j++)
                    {
                        float s = o_x[boxIndex[j]];
                        if (lo[j] != float.NegativeInfinity)
                        {
                            lo[j] = -idMath.Fabs(lo[j] * s);
                        }
                        if (hi[j] != float.PositiveInfinity)
                        {
                            hi[j] = idMath.Fabs(hi[j] * s);
                        }
                    }
                }
                // calculate acceleration for current variable
                SIMDProcessor.Dot(out dot, rowPtrs[i], f.ToArray(), i);
                a[i] = dot - b[i];
                // if already at the low boundary
                if (lo[i] >= -LCP_BOUND_EPSILON && a[i] >= -LCP_ACCEL_EPSILON)
                {
                    side[i] = -1; continue;
                }
                // if already at the high boundary
                if (hi[i] <= LCP_BOUND_EPSILON && a[i] <= LCP_ACCEL_EPSILON)
                {
                    side[i] = 1; continue;
                }
                // if inside the clamped region
                if (idMath.Fabs(a[i]) <= LCP_ACCEL_EPSILON)
                {
                    side[i] = 0; AddClamped(i); continue;
                }
                // drive the current variable into a valid region
                int n;
                for (n = 0; n < maxIterations; n++)
                {
                    // direction to move
                    float dir = (a[i] <= 0.0f ? 1.0f : -1.0f);
                    // calculate force delta
                    CalcForceDelta(i, dir);
                    // calculate acceleration delta: delta_a = m * delta_f;
                    CalcAccelDelta(i);
                    // maximum step we can take
                    float maxStep;
                    int   limit;
                    int   limitSide;
                    GetMaxStep(i, dir, out maxStep, out limit, out limitSide);
                    if (maxStep <= 0.0f)
                    {
#if IGNORE_UNSATISFIABLE_VARIABLES
                        // ignore the current variable completely
                        lo[i]   = hi[i] = 0.0f;
                        f[i]    = 0.0f;
                        side[i] = -1;
                        numIgnored++;
#else
                        failed = string.Format("invalid step size %.4f", maxStep);
#endif
                        break;
                    }
                    // change force
                    ChangeForce(i, maxStep);
                    // change acceleration
                    ChangeAccel(i, maxStep);
                    // clamp/unclamp the variable that limited this step
                    side[limit] = limitSide;
                    switch (limitSide)
                    {
                    case 0:
                        a[limit] = 0.0f;
                        AddClamped(limit);
                        break;

                    case -1:
                        f[limit] = lo[limit];
                        if (limit != i)
                        {
                            RemoveClamped(limit);
                        }
                        break;

                    case 1:
                        f[limit] = hi[limit];
                        if (limit != i)
                        {
                            RemoveClamped(limit);
                        }
                        break;
                    }
                    // if the current variable limited the step we can continue with the next variable
                    if (limit == i)
                    {
                        break;
                    }
                }
                if (n >= maxIterations)
                {
                    failed = string.Format("max iterations %d", maxIterations);
                    break;
                }
                if (failed != null)
                {
                    break;
                }
            }

#if IGNORE_UNSATISFIABLE_VARIABLES
            if (numIgnored > 0)
            {
                if (lcp_showFailures.GetBool())
                {
                    idLib.common.Printf("idLCP_Symmetric::Solve: %d of %d bounded variables ignored\n", numIgnored, m.GetNumRows() - numUnbounded);
                }
            }
#endif
            // if failed clear remaining forces
            if (failed != null)
            {
                if (lcp_showFailures.GetBool())
                {
                    idLib.common.Printf("idLCP_Square::Solve: %s (%d of %d bounded variables ignored)\n", failed, m.GetNumRows() - i, m.GetNumRows() - numUnbounded);
                }
                for (int j = i; j < m.GetNumRows(); j++)
                {
                    f[j] = 0.0f;
                }
            }
#if _DEBUG && false
            if (failed == null)
            {
                // test whether or not the solution satisfies the complementarity conditions
                for (int i = 0; i < m.GetNumRows(); i++)
                {
                    a[i] = -b[i];
                    for (int j = 0; j < m.GetNumRows(); j++)
                    {
                        a[i] += rowPtrs[i][j] * f[j];
                    }
                    if (f[i] == lo[i])
                    {
                        if (lo[i] != hi[i] && a[i] < -LCP_ACCEL_EPSILON)
                        {
                            int bah1 = 1;
                        }
                    }
                    else if (f[i] == hi[i])
                    {
                        if (lo[i] != hi[i] && a[i] > LCP_ACCEL_EPSILON)
                        {
                            int bah2 = 1;
                        }
                    }
                    else if (f[i] < lo[i] || f[i] > hi[i] || idMath.Fabs(a[i]) > 1.0f)
                    {
                        int bah3 = 1;
                    }
                }
            }
#endif
            // unpermute result
            for (int i = 0; i < f.GetSize(); i++)
            {
                o_x[permuted[i]] = f[i];
            }
            // unpermute original matrix
            for (int i = 0; i < m.GetNumRows(); i++)
            {
                for (int j = 0; j < m.GetNumRows(); j++)
                {
                    if (permuted[j] == i)
                    {
                        break;
                    }
                }
                if (i != j)
                {
                    m.SwapColumns(i, j);
                    idSwap(permuted[i], permuted[j]);
                }
            }
            return(true);
        }
 // modifies this->f and uses this->delta_f
 void ChangeForce(int d, float step)
 {
     // only the clamped variables and current variable have a force delta unequal zero
     SIMDProcessor.MulAdd(f.ToArray(), step, delta_f.ToArray(), numClamped);
     f[d] += step * delta_f[d];
 }