doom3-bfg/neo/idlib/math/Lcp.cpp

/*
===========================================================================

Doom 3 BFG Edition GPL Source Code
Copyright (C) 1993-2012 id Software LLC, a ZeniMax Media company.
Copyright (C) 2012 Robert Beckebans

This file is part of the Doom 3 BFG Edition GPL Source Code ("Doom 3 BFG Edition Source Code").

Doom 3 BFG Edition Source Code is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

Doom 3 BFG Edition Source Code is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with Doom 3 BFG Edition Source Code.  If not, see <http://www.gnu.org/licenses/>.

In addition, the Doom 3 BFG Edition Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 BFG Edition Source Code.  If not, please request a copy in writing from id Software at the address below.

If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.

===========================================================================
*/
#pragma hdrstop
#include "precompiled.h"

// this file is full of intentional case fall throughs
//lint -e616

// the code is correct, it can't be a NULL pointer
//lint -e613

static idCVar lcp_showFailures( "lcp_showFailures", "0", CVAR_BOOL, "show LCP solver failures" );

const float LCP_BOUND_EPSILON			= 1e-5f;
const float LCP_ACCEL_EPSILON			= 1e-5f;
const float LCP_DELTA_ACCEL_EPSILON		= 1e-9f;
const float LCP_DELTA_FORCE_EPSILON		= 1e-9f;

#define IGNORE_UNSATISFIABLE_VARIABLES

#if defined(USE_INTRINSICS)
#define LCP_SIMD
#endif

#if defined(LCP_SIMD)
ALIGN16( const __m128 SIMD_SP_zero )							= { 0.0f, 0.0f, 0.0f, 0.0f };
ALIGN16( const __m128 SIMD_SP_one )								= { 1.0f, 1.0f, 1.0f, 1.0f };
ALIGN16( const __m128 SIMD_SP_two )								= { 2.0f, 2.0f, 2.0f, 2.0f };
ALIGN16( const __m128 SIMD_SP_tiny )							= { 1e-10f, 1e-10f, 1e-10f, 1e-10f };
ALIGN16( const __m128 SIMD_SP_infinity )						= { idMath::INFINITY, idMath::INFINITY, idMath::INFINITY, idMath::INFINITY };
ALIGN16( const __m128 SIMD_SP_LCP_DELTA_ACCEL_EPSILON )			= { LCP_DELTA_ACCEL_EPSILON, LCP_DELTA_ACCEL_EPSILON, LCP_DELTA_ACCEL_EPSILON, LCP_DELTA_ACCEL_EPSILON };
ALIGN16( const __m128 SIMD_SP_LCP_DELTA_FORCE_EPSILON )			= { LCP_DELTA_FORCE_EPSILON, LCP_DELTA_FORCE_EPSILON, LCP_DELTA_FORCE_EPSILON, LCP_DELTA_FORCE_EPSILON };
ALIGN16( const __m128 SIMD_SP_LCP_BOUND_EPSILON )				= { LCP_BOUND_EPSILON, LCP_BOUND_EPSILON, LCP_BOUND_EPSILON, LCP_BOUND_EPSILON };
ALIGN16( const __m128 SIMD_SP_neg_LCP_BOUND_EPSILON )			= { -LCP_BOUND_EPSILON, -LCP_BOUND_EPSILON, -LCP_BOUND_EPSILON, -LCP_BOUND_EPSILON };
ALIGN16( const unsigned int SIMD_SP_signBit[4] )				= { IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK, IEEE_FLT_SIGN_MASK };
// DG: cast IEEE_FLT_SIGN_MASK to uint before interting it, as uint and not long values are expected here
#define INV_IEEE_FLT_SIGN_MASK (~((unsigned int)IEEE_FLT_SIGN_MASK))
ALIGN16( const unsigned int SIMD_SP_absMask[4] )				= { INV_IEEE_FLT_SIGN_MASK, INV_IEEE_FLT_SIGN_MASK, INV_IEEE_FLT_SIGN_MASK, INV_IEEE_FLT_SIGN_MASK };
// DG end
ALIGN16( const unsigned int SIMD_SP_indexedStartMask[4][4] )	= { { 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF }, { 0, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF }, { 0, 0, 0xFFFFFFFF, 0xFFFFFFFF }, { 0, 0, 0, 0xFFFFFFFF } };
ALIGN16( const unsigned int SIMD_SP_indexedEndMask[4][4] )		= { { 0, 0, 0, 0 }, { 0xFFFFFFFF, 0, 0, 0 }, { 0xFFFFFFFF, 0xFFFFFFFF, 0, 0 }, { 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0 } };
ALIGN16( const unsigned int SIMD_SP_clearLast1[4] )				= { 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0 };
ALIGN16( const unsigned int SIMD_SP_clearLast2[4] )				= { 0xFFFFFFFF, 0xFFFFFFFF, 0, 0 };
ALIGN16( const unsigned int SIMD_SP_clearLast3[4] )				= { 0xFFFFFFFF, 0, 0, 0 };
ALIGN16( const unsigned int SIMD_DW_zero[4] )					= { 0, 0, 0, 0 };
ALIGN16( const unsigned int SIMD_DW_one[4] )					= { 1, 1, 1, 1 };
ALIGN16( const unsigned int SIMD_DW_four[4] )					= { 4, 4, 4, 4 };
ALIGN16( const unsigned int SIMD_DW_index[4] )					= { 0, 1, 2, 3 };
ALIGN16( const int SIMD_DW_not3[4] )							= { ~3, ~3, ~3, ~3 };
#endif // #if defined(LCP_SIMD)
/*
========================
Multiply_SIMD

dst[i] = src0[i] * src1[i];

Assumes the source and destination have the same memory alignment.
========================
*/
static void Multiply_SIMD( float* dst, const float* src0, const float* src1, const int count )
{
	int i = 0;
	
	// RB: changed unsigned int to uintptr_t
	for( ; ( ( uintptr_t )dst & 0xF ) != 0 && i < count; i++ )
		// RB end
	{
		dst[i] = src0[i] * src1[i];
	}
	
#if defined(LCP_SIMD)
	
	for( ; i + 4 <= count; i += 4 )
	{
		assert_16_byte_aligned( &dst[i] );
		assert_16_byte_aligned( &src0[i] );
		assert_16_byte_aligned( &src1[i] );
		
		__m128 s0 = _mm_load_ps( src0 + i );
		__m128 s1 = _mm_load_ps( src1 + i );
		s0 = _mm_mul_ps( s0, s1 );
		_mm_store_ps( dst + i, s0 );
	}
	
#else
	
	for( ; i + 4 <= count; i += 4 )
	{
		assert_16_byte_aligned( &dst[i] );
		assert_16_byte_aligned( &src0[i] );
		assert_16_byte_aligned( &src1[i] );
	
		dst[i + 0] = src0[i + 0] * src1[i + 0];
		dst[i + 1] = src0[i + 1] * src1[i + 1];
		dst[i + 2] = src0[i + 2] * src1[i + 2];
		dst[i + 3] = src0[i + 3] * src1[i + 3];
	}
	
#endif
	
	for( ; i < count; i++ )
	{
		dst[i] = src0[i] * src1[i];
	}
}

/*
========================
MultiplyAdd_SIMD

dst[i] += constant * src[i];

Assumes the source and destination have the same memory alignment.
========================
*/
static void MultiplyAdd_SIMD( float* dst, const float constant, const float* src, const int count )
{
	int i = 0;
	
	
	// RB: changed unsigned int to uintptr_t
	for( ; ( ( uintptr_t )dst & 0xF ) != 0 && i < count; i++ )
		// RB end
	{
		dst[i] += constant * src[i];
	}
	
#if defined(LCP_SIMD)
	
	__m128 c = _mm_load1_ps( & constant );
	for( ; i + 4 <= count; i += 4 )
	{
		assert_16_byte_aligned( &dst[i] );
		assert_16_byte_aligned( &src[i] );
		
		__m128 s = _mm_load_ps( src + i );
		__m128 d = _mm_load_ps( dst + i );
		s = _mm_add_ps( _mm_mul_ps( s, c ), d );
		_mm_store_ps( dst + i, s );
	}
	
#else
	
	for( ; i + 4 <= count; i += 4 )
	{
		assert_16_byte_aligned( &src[i] );
		assert_16_byte_aligned( &dst[i] );
	
		dst[i + 0] += constant * src[i + 0];
		dst[i + 1] += constant * src[i + 1];
		dst[i + 2] += constant * src[i + 2];
		dst[i + 3] += constant * src[i + 3];
	}
	
#endif
	
	for( ; i < count; i++ )
	{
		dst[i] += constant * src[i];
	}
}

/*
========================
DotProduct_SIMD

dot = src0[0] * src1[0] + src0[1] * src1[1] + src0[2] * src1[2] + ...
========================
*/
static float DotProduct_SIMD( const float* src0, const float* src1, const int count )
{
	assert_16_byte_aligned( src0 );
	assert_16_byte_aligned( src1 );
	
#if defined(LCP_SIMD)
	
	__m128 sum = ( __m128& ) SIMD_SP_zero;
	int i = 0;
	for( ; i < count - 3; i += 4 )
	{
		__m128 s0 = _mm_load_ps( src0 + i );
		__m128 s1 = _mm_load_ps( src1 + i );
		sum = _mm_add_ps( _mm_mul_ps( s0, s1 ), sum );
	}
	__m128 mask = _mm_load_ps( ( float* ) &SIMD_SP_indexedEndMask[count & 3] );
	__m128 s0 = _mm_and_ps( _mm_load_ps( src0 + i ), mask );
	__m128 s1 = _mm_and_ps( _mm_load_ps( src1 + i ), mask );
	sum = _mm_add_ps( _mm_mul_ps( s0, s1 ), sum );
	sum = _mm_add_ps( sum, _mm_shuffle_ps( sum, sum, _MM_SHUFFLE( 1, 0, 3, 2 ) ) );
	sum = _mm_add_ps( sum, _mm_shuffle_ps( sum, sum, _MM_SHUFFLE( 2, 3, 0, 1 ) ) );
	float dot;
	_mm_store_ss( & dot, sum );
	return dot;
	
#else
	
	// RB: the old loop caused completely broken rigid body physics and NaN errors
#if 1
	float s0 = 0.0f;
	float s1 = 0.0f;
	float s2 = 0.0f;
	float s3 = 0.0f;
	
	int i = 0;
	for( ; i + 4 <= count; i += 4 )
	{
		s0 += src0[i + 0] * src1[i + 0];
		s1 += src0[i + 1] * src1[i + 1];
		s2 += src0[i + 2] * src1[i + 2];
		s3 += src0[i + 3] * src1[i + 3];
	}
	
	switch( count - i )
	{
			NODEFAULT;
	
		case 4:
			s3 += src0[i + 3] * src1[i + 3];
		case 3:
			s2 += src0[i + 2] * src1[i + 2];
		case 2:
			s1 += src0[i + 1] * src1[i + 1];
		case 1:
			s0 += src0[i + 0] * src1[i + 0];
		case 0:
			break;
	}
	return s0 + s1 + s2 + s3;
#else
	
	float s = 0;
	for( int i = 0; i < count; i++ )
	{
		s += src0[i] * src1[i];
	}
	
	return s;
#endif
	// RB end
	
#endif
}

/*
========================
LowerTriangularSolve_SIMD

Solves x in Lx = b for the n * n sub-matrix of L.
	* if skip > 0 the first skip elements of x are assumed to be valid already
	* L has to be a lower triangular matrix with (implicit) ones on the diagonal
	* x == b is allowed
========================
*/
static void LowerTriangularSolve_SIMD( const idMatX& L, float* x, const float* b, const int n, int skip )
{
	if( skip >= n )
	{
		return;
	}
	
	const float* lptr = L.ToFloatPtr();
	int nc = L.GetNumColumns();
	
	assert( ( nc & 3 ) == 0 );
	
	// unrolled cases for n < 8
	if( n < 8 )
	{
#define NSKIP( n, s )	((n<<3)|(s&7))
		switch( NSKIP( n, skip ) )
		{
			case NSKIP( 1, 0 ):
				x[0] = b[0];
				return;
			case NSKIP( 2, 0 ):
				x[0] = b[0];
			case NSKIP( 2, 1 ):
				x[1] = b[1] - lptr[1 * nc + 0] * x[0];
				return;
			case NSKIP( 3, 0 ):
				x[0] = b[0];
			case NSKIP( 3, 1 ):
				x[1] = b[1] - lptr[1 * nc + 0] * x[0];
			case NSKIP( 3, 2 ):
				x[2] = b[2] - lptr[2 * nc + 0] * x[0] - lptr[2 * nc + 1] * x[1];
				return;
			case NSKIP( 4, 0 ):
				x[0] = b[0];
			case NSKIP( 4, 1 ):
				x[1] = b[1] - lptr[1 * nc + 0] * x[0];
			case NSKIP( 4, 2 ):
				x[2] = b[2] - lptr[2 * nc + 0] * x[0] - lptr[2 * nc + 1] * x[1];
			case NSKIP( 4, 3 ):
				x[3] = b[3] - lptr[3 * nc + 0] * x[0] - lptr[3 * nc + 1] * x[1] - lptr[3 * nc + 2] * x[2];
				return;
			case NSKIP( 5, 0 ):
				x[0] = b[0];
			case NSKIP( 5, 1 ):
				x[1] = b[1] - lptr[1 * nc + 0] * x[0];
			case NSKIP( 5, 2 ):
				x[2] = b[2] - lptr[2 * nc + 0] * x[0] - lptr[2 * nc + 1] * x[1];
			case NSKIP( 5, 3 ):
				x[3] = b[3] - lptr[3 * nc + 0] * x[0] - lptr[3 * nc + 1] * x[1] - lptr[3 * nc + 2] * x[2];
			case NSKIP( 5, 4 ):
				x[4] = b[4] - lptr[4 * nc + 0] * x[0] - lptr[4 * nc + 1] * x[1] - lptr[4 * nc + 2] * x[2] - lptr[4 * nc + 3] * x[3];
				return;
			case NSKIP( 6, 0 ):
				x[0] = b[0];
			case NSKIP( 6, 1 ):
				x[1] = b[1] - lptr[1 * nc + 0] * x[0];
			case NSKIP( 6, 2 ):
				x[2] = b[2] - lptr[2 * nc + 0] * x[0] - lptr[2 * nc + 1] * x[1];
			case NSKIP( 6, 3 ):
				x[3] = b[3] - lptr[3 * nc + 0] * x[0] - lptr[3 * nc + 1] * x[1] - lptr[3 * nc + 2] * x[2];
			case NSKIP( 6, 4 ):
				x[4] = b[4] - lptr[4 * nc + 0] * x[0] - lptr[4 * nc + 1] * x[1] - lptr[4 * nc + 2] * x[2] - lptr[4 * nc + 3] * x[3];
			case NSKIP( 6, 5 ):
				x[5] = b[5] - lptr[5 * nc + 0] * x[0] - lptr[5 * nc + 1] * x[1] - lptr[5 * nc + 2] * x[2] - lptr[5 * nc + 3] * x[3] - lptr[5 * nc + 4] * x[4];
				return;
			case NSKIP( 7, 0 ):
				x[0] = b[0];
			case NSKIP( 7, 1 ):
				x[1] = b[1] - lptr[1 * nc + 0] * x[0];
			case NSKIP( 7, 2 ):
				x[2] = b[2] - lptr[2 * nc + 0] * x[0] - lptr[2 * nc + 1] * x[1];
			case NSKIP( 7, 3 ):
				x[3] = b[3] - lptr[3 * nc + 0] * x[0] - lptr[3 * nc + 1] * x[1] - lptr[3 * nc + 2] * x[2];
			case NSKIP( 7, 4 ):
				x[4] = b[4] - lptr[4 * nc + 0] * x[0] - lptr[4 * nc + 1] * x[1] - lptr[4 * nc + 2] * x[2] - lptr[4 * nc + 3] * x[3];
			case NSKIP( 7, 5 ):
				x[5] = b[5] - lptr[5 * nc + 0] * x[0] - lptr[5 * nc + 1] * x[1] - lptr[5 * nc + 2] * x[2] - lptr[5 * nc + 3] * x[3] - lptr[5 * nc + 4] * x[4];
			case NSKIP( 7, 6 ):
				x[6] = b[6] - lptr[6 * nc + 0] * x[0] - lptr[6 * nc + 1] * x[1] - lptr[6 * nc + 2] * x[2] - lptr[6 * nc + 3] * x[3] - lptr[6 * nc + 4] * x[4] - lptr[6 * nc + 5] * x[5];
				return;
		}
#undef NSKIP
		return;
	}
	
	// process first 4 rows
	switch( skip )
	{
		case 0:
			x[0] = b[0];
		case 1:
			x[1] = b[1] - lptr[1 * nc + 0] * x[0];
		case 2:
			x[2] = b[2] - lptr[2 * nc + 0] * x[0] - lptr[2 * nc + 1] * x[1];
		case 3:
			x[3] = b[3] - lptr[3 * nc + 0] * x[0] - lptr[3 * nc + 1] * x[1] - lptr[3 * nc + 2] * x[2];
			skip = 4;
	}
	
	lptr = L[skip];
	
	int i = skip;
	
#if defined(LCP_SIMD)
	
	// work up to a multiple of 4 rows
	for( ; ( i & 3 ) != 0 && i < n; i++ )
	{
		__m128 sum = _mm_load_ss( & b[i] );
		int j = 0;
		for( ; j < i - 3; j += 4 )
		{
			__m128 s0 = _mm_load_ps( lptr + j );
			__m128 s1 = _mm_load_ps( x + j );
			sum = _mm_sub_ps( sum, _mm_mul_ps( s0, s1 ) );
		}
		__m128 mask = _mm_load_ps( ( float* ) & SIMD_SP_indexedEndMask[i & 3] );
		__m128 s0 = _mm_and_ps( _mm_load_ps( lptr + j ), mask );
		__m128 s1 = _mm_and_ps( _mm_load_ps( x + j ), mask );
		sum = _mm_sub_ps( sum, _mm_mul_ps( s0, s1 ) );
		sum = _mm_add_ps( sum, _mm_shuffle_ps( sum, sum, _MM_SHUFFLE( 1, 0, 3, 2 ) ) );
		sum = _mm_add_ps( sum, _mm_shuffle_ps( sum, sum, _MM_SHUFFLE( 2, 3, 0, 1 ) ) );
		_mm_store_ss( & x[i], sum );
		lptr += nc;
	}
	
	for( ; i + 3 < n; i += 4 )
	{
		const float* lptr0 = &lptr[0 * nc];
		const float* lptr1 = &lptr[1 * nc];
		const float* lptr2 = &lptr[2 * nc];
		const float* lptr3 = &lptr[3 * nc];
		
		assert_16_byte_aligned( lptr0 );
		assert_16_byte_aligned( lptr1 );
		assert_16_byte_aligned( lptr2 );
		assert_16_byte_aligned( lptr3 );
		
		__m128 va = _mm_load_ss( & b[i + 0] );
		__m128 vb = _mm_load_ss( & b[i + 1] );
		__m128 vc = _mm_load_ss( & b[i + 2] );
		__m128 vd = _mm_load_ss( & b[i + 3] );
		
		__m128 x0 = _mm_load_ps( & x[0] );
		
		va = _mm_sub_ps( va, _mm_mul_ps( x0, _mm_load_ps( lptr0 + 0 ) ) );
		vb = _mm_sub_ps( vb, _mm_mul_ps( x0, _mm_load_ps( lptr1 + 0 ) ) );
		vc = _mm_sub_ps( vc, _mm_mul_ps( x0, _mm_load_ps( lptr2 + 0 ) ) );
		vd = _mm_sub_ps( vd, _mm_mul_ps( x0, _mm_load_ps( lptr3 + 0 ) ) );
		
		for( int j = 4; j < i; j += 4 )
		{
			__m128 xj = _mm_load_ps( &x[j] );
			
			va = _mm_sub_ps( va, _mm_mul_ps( xj, _mm_load_ps( lptr0 + j ) ) );
			vb = _mm_sub_ps( vb, _mm_mul_ps( xj, _mm_load_ps( lptr1 + j ) ) );
			vc = _mm_sub_ps( vc, _mm_mul_ps( xj, _mm_load_ps( lptr2 + j ) ) );
			vd = _mm_sub_ps( vd, _mm_mul_ps( xj, _mm_load_ps( lptr3 + j ) ) );
		}
		
		vb = _mm_sub_ps( vb, _mm_mul_ps( va, _mm_load1_ps( lptr1 + i + 0 ) ) );
		vc = _mm_sub_ps( vc, _mm_mul_ps( va, _mm_load1_ps( lptr2 + i + 0 ) ) );
		vc = _mm_sub_ps( vc, _mm_mul_ps( vb, _mm_load1_ps( lptr2 + i + 1 ) ) );
		vd = _mm_sub_ps( vd, _mm_mul_ps( va, _mm_load1_ps( lptr3 + i + 0 ) ) );
		vd = _mm_sub_ps( vd, _mm_mul_ps( vb, _mm_load1_ps( lptr3 + i + 1 ) ) );
		vd = _mm_sub_ps( vd, _mm_mul_ps( vc, _mm_load1_ps( lptr3 + i + 2 ) ) );
		
		__m128 ta = _mm_unpacklo_ps( va, vc );		// x0, z0, x1, z1
		__m128 tb = _mm_unpackhi_ps( va, vc );		// x2, z2, x3, z3
		__m128 tc = _mm_unpacklo_ps( vb, vd );		// y0, w0, y1, w1
		__m128 td = _mm_unpackhi_ps( vb, vd );		// y2, w2, y3, w3
		
		va = _mm_unpacklo_ps( ta, tc );				// x0, y0, z0, w0
		vb = _mm_unpackhi_ps( ta, tc );				// x1, y1, z1, w1
		vc = _mm_unpacklo_ps( tb, td );				// x2, y2, z2, w2
		vd = _mm_unpackhi_ps( tb, td );				// x3, y3, z3, w3
		
		va = _mm_add_ps( va, vb );
		vc = _mm_add_ps( vc, vd );
		va = _mm_add_ps( va, vc );
		
		_mm_store_ps( & x[i], va );
		
		lptr += 4 * nc;
	}
	
	// go through any remaining rows
	for( ; i < n; i++ )
	{
		__m128 sum = _mm_load_ss( & b[i] );
		int j = 0;
		for( ; j < i - 3; j += 4 )
		{
			__m128 s0 = _mm_load_ps( lptr + j );
			__m128 s1 = _mm_load_ps( x + j );
			sum = _mm_sub_ps( sum, _mm_mul_ps( s0, s1 ) );
		}
		__m128 mask = _mm_load_ps( ( float* ) & SIMD_SP_indexedEndMask[i & 3] );
		__m128 s0 = _mm_and_ps( _mm_load_ps( lptr + j ), mask );
		__m128 s1 = _mm_and_ps( _mm_load_ps( x + j ), mask );
		sum = _mm_sub_ps( sum, _mm_mul_ps( s0, s1 ) );
		sum = _mm_add_ps( sum, _mm_shuffle_ps( sum, sum, _MM_SHUFFLE( 1, 0, 3, 2 ) ) );
		sum = _mm_add_ps( sum, _mm_shuffle_ps( sum, sum, _MM_SHUFFLE( 2, 3, 0, 1 ) ) );
		_mm_store_ss( & x[i], sum );
		lptr += nc;
	}
	
#else
	
	// work up to a multiple of 4 rows
	for( ; ( i & 3 ) != 0 && i < n; i++ )
	{
		float sum = b[i];
		for( int j = 0; j < i; j++ )
		{
			sum -= lptr[j] * x[j];
		}
		x[i] = sum;
		lptr += nc;
	}
	
	assert_16_byte_aligned( x );
	
	for( ; i + 3 < n; i += 4 )
	{
		const float* lptr0 = &lptr[0 * nc];
		const float* lptr1 = &lptr[1 * nc];
		const float* lptr2 = &lptr[2 * nc];
		const float* lptr3 = &lptr[3 * nc];
	
		assert_16_byte_aligned( lptr0 );
		assert_16_byte_aligned( lptr1 );
		assert_16_byte_aligned( lptr2 );
		assert_16_byte_aligned( lptr3 );
	
		float a0 = - lptr0[0] * x[0] + b[i + 0];
		float a1 = - lptr0[1] * x[1];
		float a2 = - lptr0[2] * x[2];
		float a3 = - lptr0[3] * x[3];
	
		float b0 = - lptr1[0] * x[0] + b[i + 1];
		float b1 = - lptr1[1] * x[1];
		float b2 = - lptr1[2] * x[2];
		float b3 = - lptr1[3] * x[3];
	
		float c0 = - lptr2[0] * x[0] + b[i + 2];
		float c1 = - lptr2[1] * x[1];
		float c2 = - lptr2[2] * x[2];
		float c3 = - lptr2[3] * x[3];
	
		float d0 = - lptr3[0] * x[0] + b[i + 3];
		float d1 = - lptr3[1] * x[1];
		float d2 = - lptr3[2] * x[2];
		float d3 = - lptr3[3] * x[3];
	
		for( int j = 4; j < i; j += 4 )
		{
			a0 -= lptr0[j + 0] * x[j + 0];
			a1 -= lptr0[j + 1] * x[j + 1];
			a2 -= lptr0[j + 2] * x[j + 2];
			a3 -= lptr0[j + 3] * x[j + 3];
	
			b0 -= lptr1[j + 0] * x[j + 0];
			b1 -= lptr1[j + 1] * x[j + 1];
			b2 -= lptr1[j + 2] * x[j + 2];
			b3 -= lptr1[j + 3] * x[j + 3];
	
			c0 -= lptr2[j + 0] * x[j + 0];
			c1 -= lptr2[j + 1] * x[j + 1];
			c2 -= lptr2[j + 2] * x[j + 2];
			c3 -= lptr2[j + 3] * x[j + 3];
	
			d0 -= lptr3[j + 0] * x[j + 0];
			d1 -= lptr3[j + 1] * x[j + 1];
			d2 -= lptr3[j + 2] * x[j + 2];
			d3 -= lptr3[j + 3] * x[j + 3];
		}
	
		b0 -= lptr1[i + 0] * a0;
		b1 -= lptr1[i + 0] * a1;
		b2 -= lptr1[i + 0] * a2;
		b3 -= lptr1[i + 0] * a3;
	
		c0 -= lptr2[i + 0] * a0;
		c1 -= lptr2[i + 0] * a1;
		c2 -= lptr2[i + 0] * a2;
		c3 -= lptr2[i + 0] * a3;
	
		c0 -= lptr2[i + 1] * b0;
		c1 -= lptr2[i + 1] * b1;
		c2 -= lptr2[i + 1] * b2;
		c3 -= lptr2[i + 1] * b3;
	
		d0 -= lptr3[i + 0] * a0;
		d1 -= lptr3[i + 0] * a1;
		d2 -= lptr3[i + 0] * a2;
		d3 -= lptr3[i + 0] * a3;
	
		d0 -= lptr3[i + 1] * b0;
		d1 -= lptr3[i + 1] * b1;
		d2 -= lptr3[i + 1] * b2;
		d3 -= lptr3[i + 1] * b3;
	
		d0 -= lptr3[i + 2] * c0;
		d1 -= lptr3[i + 2] * c1;
		d2 -= lptr3[i + 2] * c2;
		d3 -= lptr3[i + 2] * c3;
	
		x[i + 0] = a0 + a1 + a2 + a3;
		x[i + 1] = b0 + b1 + b2 + b3;
		x[i + 2] = c0 + c1 + c2 + c3;
		x[i + 3] = d0 + d1 + d2 + d3;
	
		lptr += 4 * nc;
	}
	
	// go through any remaining rows
	for( ; i < n; i++ )
	{
		float sum = b[i];
		for( int j = 0; j < i; j++ )
		{
			sum -= lptr[j] * x[j];
		}
		x[i] = sum;
		lptr += nc;
	}
	
#endif
	
}

/*
========================
LowerTriangularSolveTranspose_SIMD

Solves x in L'x = b for the n * n sub-matrix of L.
	* L has to be a lower triangular matrix with (implicit) ones on the diagonal
	* x == b is allowed
========================
*/
static void LowerTriangularSolveTranspose_SIMD( const idMatX& L, float* x, const float* b, const int n )
{
	int nc = L.GetNumColumns();
	
	assert( ( nc & 3 ) == 0 );
	
	int m = n;
	int r = n & 3;
	
	if( ( m & 3 ) != 0 )
	{
		const float* lptr = L.ToFloatPtr() + m * nc + m;
		if( ( m & 3 ) == 1 )
		{
			x[m - 1] = b[m - 1];
			m -= 1;
		}
		else if( ( m & 3 ) == 2 )
		{
			x[m - 1] = b[m - 1];
			x[m - 2] = b[m - 2] - lptr[-1 * nc - 2] * x[m - 1];
			m -= 2;
		}
		else
		{
			x[m - 1] = b[m - 1];
			x[m - 2] = b[m - 2] - lptr[-1 * nc - 2] * x[m - 1];
			x[m - 3] = b[m - 3] - lptr[-1 * nc - 3] * x[m - 1] - lptr[-2 * nc - 3] * x[m - 2];
			m -= 3;
		}
	}
	
	const float* lptr = L.ToFloatPtr() + m * nc + m - 4;
	float* xptr = x + m;
	
#if defined(LCP_SIMD)
	
	// process 4 rows at a time
	for( int i = m; i >= 4; i -= 4 )
	{
		assert_16_byte_aligned( b );
		assert_16_byte_aligned( xptr );
		assert_16_byte_aligned( lptr );
		
		__m128 s0 = _mm_load_ps( &b[i - 4] );
		__m128 s1 = ( __m128& )SIMD_SP_zero;
		__m128 s2 = ( __m128& )SIMD_SP_zero;
		__m128 s3 = ( __m128& )SIMD_SP_zero;
		
		// process 4x4 blocks
		const float* xptr2 = xptr;	// x + i;
		const float* lptr2 = lptr;	// ptr = L[i] + i - 4;
		for( int j = i; j < m; j += 4 )
		{
			__m128 xj = _mm_load_ps( xptr2 );
			
			s0 = _mm_sub_ps( s0, _mm_mul_ps( _mm_splat_ps( xj, 0 ), _mm_load_ps( lptr2 + 0 * nc ) ) );
			s1 = _mm_sub_ps( s1, _mm_mul_ps( _mm_splat_ps( xj, 1 ), _mm_load_ps( lptr2 + 1 * nc ) ) );
			s2 = _mm_sub_ps( s2, _mm_mul_ps( _mm_splat_ps( xj, 2 ), _mm_load_ps( lptr2 + 2 * nc ) ) );
			s3 = _mm_sub_ps( s3, _mm_mul_ps( _mm_splat_ps( xj, 3 ), _mm_load_ps( lptr2 + 3 * nc ) ) );
			
			lptr2 += 4 * nc;
			xptr2 += 4;
		}
		for( int j = 0; j < r; j++ )
		{
			s0 = _mm_sub_ps( s0, _mm_mul_ps( _mm_load_ps( lptr2 ), _mm_load1_ps( &xptr2[j] ) ) );
			lptr2 += nc;
		}
		s0 = _mm_add_ps( s0, s1 );
		s2 = _mm_add_ps( s2, s3 );
		s0 = _mm_add_ps( s0, s2 );
		// process left over of the 4 rows
		lptr -= 4 * nc;
		__m128 t0 = _mm_and_ps( _mm_load_ps( lptr + 3 * nc ), ( __m128& )SIMD_SP_clearLast1 );
		__m128 t1 = _mm_and_ps( _mm_load_ps( lptr + 2 * nc ), ( __m128& )SIMD_SP_clearLast2 );
		__m128 t2 = _mm_load_ss( lptr + 1 * nc );
		s0 = _mm_sub_ps( s0, _mm_mul_ps( t0, _mm_splat_ps( s0, 3 ) ) );
		s0 = _mm_sub_ps( s0, _mm_mul_ps( t1, _mm_splat_ps( s0, 2 ) ) );
		s0 = _mm_sub_ps( s0, _mm_mul_ps( t2, _mm_splat_ps( s0, 1 ) ) );
		// store result
		_mm_store_ps( &xptr[-4], s0 );
		// update pointers for next four rows
		lptr -= 4;
		xptr -= 4;
	}
	
#else
	
	// process 4 rows at a time
	for( int i = m; i >= 4; i -= 4 )
	{
		assert_16_byte_aligned( b );
		assert_16_byte_aligned( xptr );
		assert_16_byte_aligned( lptr );
	
		float s0 = b[i - 4];
		float s1 = b[i - 3];
		float s2 = b[i - 2];
		float s3 = b[i - 1];
		// process 4x4 blocks
		const float* xptr2 = xptr;	// x + i;
		const float* lptr2 = lptr;	// ptr = L[i] + i - 4;
		for( int j = i; j < m; j += 4 )
		{
			float t0 = xptr2[0];
			s0 -= lptr2[0] * t0;
			s1 -= lptr2[1] * t0;
			s2 -= lptr2[2] * t0;
			s3 -= lptr2[3] * t0;
			lptr2 += nc;
			float t1 = xptr2[1];
			s0 -= lptr2[0] * t1;
			s1 -= lptr2[1] * t1;
			s2 -= lptr2[2] * t1;
			s3 -= lptr2[3] * t1;
			lptr2 += nc;
			float t2 = xptr2[2];
			s0 -= lptr2[0] * t2;
			s1 -= lptr2[1] * t2;
			s2 -= lptr2[2] * t2;
			s3 -= lptr2[3] * t2;
			lptr2 += nc;
			float t3 = xptr2[3];
			s0 -= lptr2[0] * t3;
			s1 -= lptr2[1] * t3;
			s2 -= lptr2[2] * t3;
			s3 -= lptr2[3] * t3;
			lptr2 += nc;
			xptr2 += 4;
		}
		for( int j = 0; j < r; j++ )
		{
			float t = xptr2[j];
			s0 -= lptr2[0] * t;
			s1 -= lptr2[1] * t;
			s2 -= lptr2[2] * t;
			s3 -= lptr2[3] * t;
			lptr2 += nc;
		}
		// process left over of the 4 rows
		lptr -= nc;
		s0 -= lptr[0] * s3;
		s1 -= lptr[1] * s3;
		s2 -= lptr[2] * s3;
		lptr -= nc;
		s0 -= lptr[0] * s2;
		s1 -= lptr[1] * s2;
		lptr -= nc;
		s0 -= lptr[0] * s1;
		lptr -= nc;
		// store result
		xptr[-4] = s0;
		xptr[-3] = s1;
		xptr[-2] = s2;
		xptr[-1] = s3;
		// update pointers for next four rows
		lptr -= 4;
		xptr -= 4;
	}
	
#endif
	
}

/*
========================
UpperTriangularSolve_SIMD

Solves x in Ux = b for the n * n sub-matrix of U.
	* U has to be a upper triangular matrix
	* invDiag is the reciprical of the diagonal of the upper triangular matrix.
	* x == b is allowed
========================
*/
static void UpperTriangularSolve_SIMD( const idMatX& U, const float* invDiag, float* x, const float* b, const int n )
{
	for( int i = n - 1; i >= 0; i-- )
	{
		float sum = b[i];
		const float* uptr = U[i];
		for( int j = i + 1; j < n; j++ )
		{
			sum -= uptr[j] * x[j];
		}
		x[i] = sum * invDiag[i];
	}
}

/*
========================
LU_Factor_SIMD

In-place factorization LU of the n * n sub-matrix of mat. The reciprocal of the diagonal
elements of U are stored in invDiag. No pivoting is used.
========================
*/
static bool LU_Factor_SIMD( idMatX& mat, idVecX& invDiag, const int n )
{
	for( int i = 0; i < n; i++ )
	{
	
		float d1 = mat[i][i];
		
		if( fabs( d1 ) < idMath::FLT_SMALLEST_NON_DENORMAL )
		{
			return false;
		}
		
		invDiag[i] = d1 = 1.0f / d1;
		
		float* ptr1 = mat[i];
		
		for( int j = i + 1; j < n; j++ )
		{
		
			float* ptr2 = mat[j];
			float d2 = ptr2[i] * d1;
			ptr2[i] = d2;
			
			int k;
			for( k = n - 1; k > i + 15; k -= 16 )
			{
				ptr2[k - 0] -= d2 * ptr1[k - 0];
				ptr2[k - 1] -= d2 * ptr1[k - 1];
				ptr2[k - 2] -= d2 * ptr1[k - 2];
				ptr2[k - 3] -= d2 * ptr1[k - 3];
				ptr2[k - 4] -= d2 * ptr1[k - 4];
				ptr2[k - 5] -= d2 * ptr1[k - 5];
				ptr2[k - 6] -= d2 * ptr1[k - 6];
				ptr2[k - 7] -= d2 * ptr1[k - 7];
				ptr2[k - 8] -= d2 * ptr1[k - 8];
				ptr2[k - 9] -= d2 * ptr1[k - 9];
				ptr2[k - 10] -= d2 * ptr1[k - 10];
				ptr2[k - 11] -= d2 * ptr1[k - 11];
				ptr2[k - 12] -= d2 * ptr1[k - 12];
				ptr2[k - 13] -= d2 * ptr1[k - 13];
				ptr2[k - 14] -= d2 * ptr1[k - 14];
				ptr2[k - 15] -= d2 * ptr1[k - 15];
			}
			switch( k - i )
			{
					NODEFAULT;
				case 15:
					ptr2[k - 14] -= d2 * ptr1[k - 14];
				case 14:
					ptr2[k - 13] -= d2 * ptr1[k - 13];
				case 13:
					ptr2[k - 12] -= d2 * ptr1[k - 12];
				case 12:
					ptr2[k - 11] -= d2 * ptr1[k - 11];
				case 11:
					ptr2[k - 10] -= d2 * ptr1[k - 10];
				case 10:
					ptr2[k - 9] -= d2 * ptr1[k - 9];
				case 9:
					ptr2[k - 8] -= d2 * ptr1[k - 8];
				case 8:
					ptr2[k - 7] -= d2 * ptr1[k - 7];
				case 7:
					ptr2[k - 6] -= d2 * ptr1[k - 6];
				case 6:
					ptr2[k - 5] -= d2 * ptr1[k - 5];
				case 5:
					ptr2[k - 4] -= d2 * ptr1[k - 4];
				case 4:
					ptr2[k - 3] -= d2 * ptr1[k - 3];
				case 3:
					ptr2[k - 2] -= d2 * ptr1[k - 2];
				case 2:
					ptr2[k - 1] -= d2 * ptr1[k - 1];
				case 1:
					ptr2[k - 0] -= d2 * ptr1[k - 0];
				case 0:
					break;
			}
		}
	}
	
	return true;
}

/*
========================
LDLT_Factor_SIMD

In-place factorization LDL' of the n * n sub-matrix of mat. The reciprocal of the diagonal
elements are stored in invDiag.

NOTE:	The number of columns of mat must be a multiple of 4.
========================
*/
static bool LDLT_Factor_SIMD( idMatX& mat, idVecX& invDiag, const int n )
{
	float s0, s1, s2, d;
	
	float* v = ( float* ) _alloca16( ( ( n + 3 ) & ~3 ) * sizeof( float ) );
	float* diag = ( float* ) _alloca16( ( ( n + 3 ) & ~3 ) * sizeof( float ) );
	float* invDiagPtr = invDiag.ToFloatPtr();
	
	int nc = mat.GetNumColumns();
	
	assert( ( nc & 3 ) == 0 );
	
	if( n <= 0 )
	{
		return true;
	}
	
	float* mptr = mat[0];
	
	float sum = mptr[0];
	
	if( fabs( sum ) < idMath::FLT_SMALLEST_NON_DENORMAL )
	{
		return false;
	}
	
	diag[0] = sum;
	invDiagPtr[0] = d = 1.0f / sum;
	
	if( n <= 1 )
	{
		return true;
	}
	
	mptr = mat[0];
	for( int j = 1; j < n; j++ )
	{
		mptr[j * nc + 0] = ( mptr[j * nc + 0] ) * d;
	}
	
	mptr = mat[1];
	
	v[0] = diag[0] * mptr[0];
	s0 = v[0] * mptr[0];
	sum = mptr[1] - s0;
	
	if( fabs( sum ) < idMath::FLT_SMALLEST_NON_DENORMAL )
	{
		return false;
	}
	
	mat[1][1] = sum;
	diag[1] = sum;
	invDiagPtr[1] = d = 1.0f / sum;
	
	if( n <= 2 )
	{
		return true;
	}
	
	mptr = mat[0];
	for( int j = 2; j < n; j++ )
	{
		mptr[j * nc + 1] = ( mptr[j * nc + 1] - v[0] * mptr[j * nc + 0] ) * d;
	}
	
	mptr = mat[2];
	
	v[0] = diag[0] * mptr[0];
	s0 = v[0] * mptr[0];
	v[1] = diag[1] * mptr[1];
	s1 = v[1] * mptr[1];
	sum = mptr[2] - s0 - s1;
	
	if( fabs( sum ) < idMath::FLT_SMALLEST_NON_DENORMAL )
	{
		return false;
	}
	
	mat[2][2] = sum;
	diag[2] = sum;
	invDiagPtr[2] = d = 1.0f / sum;
	
	if( n <= 3 )
	{
		return true;
	}
	
	mptr = mat[0];
	for( int j = 3; j < n; j++ )
	{
		mptr[j * nc + 2] = ( mptr[j * nc + 2] - v[0] * mptr[j * nc + 0] - v[1] * mptr[j * nc + 1] ) * d;
	}
	
	mptr = mat[3];
	
	v[0] = diag[0] * mptr[0];
	s0 = v[0] * mptr[0];
	v[1] = diag[1] * mptr[1];
	s1 = v[1] * mptr[1];
	v[2] = diag[2] * mptr[2];
	s2 = v[2] * mptr[2];
	sum = mptr[3] - s0 - s1 - s2;
	
	if( fabs( sum ) < idMath::FLT_SMALLEST_NON_DENORMAL )
	{
		return false;
	}
	
	mat[3][3] = sum;
	diag[3] = sum;
	invDiagPtr[3] = d = 1.0f / sum;
	
	if( n <= 4 )
	{
		return true;
	}
	
	mptr = mat[0];
	for( int j = 4; j < n; j++ )
	{
		mptr[j * nc + 3] = ( mptr[j * nc + 3] - v[0] * mptr[j * nc + 0] - v[1] * mptr[j * nc + 1] - v[2] * mptr[j * nc + 2] ) * d;
	}
	
#if defined(LCP_SIMD)
	
	__m128 vzero = _mm_setzero_ps();
	for( int i = 4; i < n; i += 4 )
	{
		_mm_store_ps( diag + i, vzero );
	}
	
	for( int i = 4; i < n; i++ )
	{
		mptr = mat[i];
		
		assert_16_byte_aligned( v );
		assert_16_byte_aligned( mptr );
		assert_16_byte_aligned( diag );
		
		__m128 m0 = _mm_load_ps( mptr + 0 );
		__m128 d0 = _mm_load_ps( diag + 0 );
		__m128 v0 = _mm_mul_ps( d0, m0 );
		__m128 t0 = _mm_load_ss( mptr + i );
		t0 = _mm_sub_ps( t0, _mm_mul_ps( m0, v0 ) );
		_mm_store_ps( v + 0, v0 );
		
		int k = 4;
		for( ; k < i - 3; k += 4 )
		{
			m0 = _mm_load_ps( mptr + k );
			d0 = _mm_load_ps( diag + k );
			v0 = _mm_mul_ps( d0, m0 );
			t0 = _mm_sub_ps( t0, _mm_mul_ps( m0, v0 ) );
			_mm_store_ps( v + k, v0 );
		}
		
		__m128 mask = ( __m128& ) SIMD_SP_indexedEndMask[i & 3];
		
		m0 = _mm_and_ps( _mm_load_ps( mptr + k ), mask );
		d0 = _mm_load_ps( diag + k );
		v0 = _mm_mul_ps( d0, m0 );
		t0 = _mm_sub_ps( t0, _mm_mul_ps( m0, v0 ) );
		_mm_store_ps( v + k, v0 );
		
		t0 = _mm_add_ps( t0, _mm_shuffle_ps( t0, t0, _MM_SHUFFLE( 1, 0, 3, 2 ) ) );
		t0 = _mm_add_ps( t0, _mm_shuffle_ps( t0, t0, _MM_SHUFFLE( 2, 3, 0, 1 ) ) );
		
		__m128 tiny = _mm_and_ps( _mm_cmpeq_ps( t0, SIMD_SP_zero ), SIMD_SP_tiny );
		t0 = _mm_or_ps( t0, tiny );
		
		_mm_store_ss( mptr + i, t0 );
		_mm_store_ss( diag + i, t0 );
		
		__m128 d = _mm_rcp32_ps( t0 );
		_mm_store_ss( invDiagPtr + i, d );
		
		if( i + 1 >= n )
		{
			return true;
		}
		
		int j = i + 1;
		for( ; j < n - 3; j += 4 )
		{
			float* ra = mat[j + 0];
			float* rb = mat[j + 1];
			float* rc = mat[j + 2];
			float* rd = mat[j + 3];
			
			assert_16_byte_aligned( v );
			assert_16_byte_aligned( ra );
			assert_16_byte_aligned( rb );
			assert_16_byte_aligned( rc );
			assert_16_byte_aligned( rd );
			
			__m128 va = _mm_load_ss( ra + i );
			__m128 vb = _mm_load_ss( rb + i );
			__m128 vc = _mm_load_ss( rc + i );
			__m128 vd = _mm_load_ss( rd + i );
			
			__m128 v0 = _mm_load_ps( v + 0 );
			
			va = _mm_sub_ps( va, _mm_mul_ps( _mm_load_ps( ra + 0 ), v0 ) );
			vb = _mm_sub_ps( vb, _mm_mul_ps( _mm_load_ps( rb + 0 ), v0 ) );
			vc = _mm_sub_ps( vc, _mm_mul_ps( _mm_load_ps( rc + 0 ), v0 ) );
			vd = _mm_sub_ps( vd, _mm_mul_ps( _mm_load_ps( rd + 0 ), v0 ) );
			
			int k = 4;
			for( ; k < i - 3; k += 4 )
			{
				v0 = _mm_load_ps( v + k );
				
				va = _mm_sub_ps( va, _mm_mul_ps( _mm_load_ps( ra + k ), v0 ) );
				vb = _mm_sub_ps( vb, _mm_mul_ps( _mm_load_ps( rb + k ), v0 ) );
				vc = _mm_sub_ps( vc, _mm_mul_ps( _mm_load_ps( rc + k ), v0 ) );
				vd = _mm_sub_ps( vd, _mm_mul_ps( _mm_load_ps( rd + k ), v0 ) );
			}
			
			v0 = _mm_load_ps( v + k );
			
			va = _mm_sub_ps( va, _mm_mul_ps( _mm_and_ps( _mm_load_ps( ra + k ), mask ), v0 ) );
			vb = _mm_sub_ps( vb, _mm_mul_ps( _mm_and_ps( _mm_load_ps( rb + k ), mask ), v0 ) );
			vc = _mm_sub_ps( vc, _mm_mul_ps( _mm_and_ps( _mm_load_ps( rc + k ), mask ), v0 ) );
			vd = _mm_sub_ps( vd, _mm_mul_ps( _mm_and_ps( _mm_load_ps( rd + k ), mask ), v0 ) );
			
			__m128 ta = _mm_unpacklo_ps( va, vc );		// x0, z0, x1, z1
			__m128 tb = _mm_unpackhi_ps( va, vc );		// x2, z2, x3, z3
			__m128 tc = _mm_unpacklo_ps( vb, vd );		// y0, w0, y1, w1
			__m128 td = _mm_unpackhi_ps( vb, vd );		// y2, w2, y3, w3
			
			va = _mm_unpacklo_ps( ta, tc );				// x0, y0, z0, w0
			vb = _mm_unpackhi_ps( ta, tc );				// x1, y1, z1, w1
			vc = _mm_unpacklo_ps( tb, td );				// x2, y2, z2, w2
			vd = _mm_unpackhi_ps( tb, td );				// x3, y3, z3, w3
			
			va = _mm_add_ps( va, vb );
			vc = _mm_add_ps( vc, vd );
			va = _mm_add_ps( va, vc );
			va = _mm_mul_ps( va, d );
			
			_mm_store_ss( ra + i, _mm_splat_ps( va, 0 ) );
			_mm_store_ss( rb + i, _mm_splat_ps( va, 1 ) );
			_mm_store_ss( rc + i, _mm_splat_ps( va, 2 ) );
			_mm_store_ss( rd + i, _mm_splat_ps( va, 3 ) );
		}
		for( ; j < n; j++ )
		{
			float* mptr = mat[j];
			
			assert_16_byte_aligned( v );
			assert_16_byte_aligned( mptr );
			
			__m128 va = _mm_load_ss( mptr + i );
			__m128 v0 = _mm_load_ps( v + 0 );
			
			va = _mm_sub_ps( va, _mm_mul_ps( _mm_load_ps( mptr + 0 ), v0 ) );
			
			int k = 4;
			for( ; k < i - 3; k += 4 )
			{
				v0 = _mm_load_ps( v + k );
				va = _mm_sub_ps( va, _mm_mul_ps( _mm_load_ps( mptr + k ), v0 ) );
			}
			
			v0 = _mm_load_ps( v + k );
			va = _mm_sub_ps( va, _mm_mul_ps( _mm_and_ps( _mm_load_ps( mptr + k ), mask ), v0 ) );
			
			va = _mm_add_ps( va, _mm_shuffle_ps( va, va, _MM_SHUFFLE( 1, 0, 3, 2 ) ) );
			va = _mm_add_ps( va, _mm_shuffle_ps( va, va, _MM_SHUFFLE( 2, 3, 0, 1 ) ) );
			va = _mm_mul_ps( va, d );
			
			_mm_store_ss( mptr + i, va );
		}
	}
	return true;
	
#else
	
	for( int i = 4; i < n; i += 4 )
	{
		diag[i + 0] = 0.0f;
		diag[i + 1] = 0.0f;
		diag[i + 2] = 0.0f;
		diag[i + 3] = 0.0f;
	}
	
	for( int i = 4; i < n; i++ )
	{
		mptr = mat[i];
	
		assert_16_byte_aligned( v );
		assert_16_byte_aligned( mptr );
		assert_16_byte_aligned( diag );
	
		v[0] = diag[0] * mptr[0];
		v[1] = diag[1] * mptr[1];
		v[2] = diag[2] * mptr[2];
		v[3] = diag[3] * mptr[3];
	
		float t0 = - mptr[0] * v[0] + mptr[i];
		float t1 = - mptr[1] * v[1];
		float t2 = - mptr[2] * v[2];
		float t3 = - mptr[3] * v[3];
	
		int k = 4;
		for( ; k < i - 3; k += 4 )
		{
			v[k + 0] = diag[k + 0] * mptr[k + 0];
			v[k + 1] = diag[k + 1] * mptr[k + 1];
			v[k + 2] = diag[k + 2] * mptr[k + 2];
			v[k + 3] = diag[k + 3] * mptr[k + 3];
	
			t0 -= mptr[k + 0] * v[k + 0];
			t1 -= mptr[k + 1] * v[k + 1];
			t2 -= mptr[k + 2] * v[k + 2];
			t3 -= mptr[k + 3] * v[k + 3];
		}
	
		float m0 = ( i - k > 0 ) ? mptr[k + 0] : 0.0f;
		float m1 = ( i - k > 1 ) ? mptr[k + 1] : 0.0f;
		float m2 = ( i - k > 2 ) ? mptr[k + 2] : 0.0f;
		float m3 = ( i - k > 3 ) ? mptr[k + 3] : 0.0f;
	
		v[k + 0] = diag[k + 0] * m0;
		v[k + 1] = diag[k + 1] * m1;
		v[k + 2] = diag[k + 2] * m2;
		v[k + 3] = diag[k + 3] * m3;
	
		t0 -= m0 * v[k + 0];
		t1 -= m1 * v[k + 1];
		t2 -= m2 * v[k + 2];
		t3 -= m3 * v[k + 3];
	
		sum = t0 + t1 + t2 + t3;
	
		if( fabs( sum ) < idMath::FLT_SMALLEST_NON_DENORMAL )
		{
			return false;
		}
	
		mat[i][i] = sum;
		diag[i] = sum;
		invDiagPtr[i] = d = 1.0f / sum;
	
		if( i + 1 >= n )
		{
			return true;
		}
	
		int j = i + 1;
		for( ; j < n - 3; j += 4 )
		{
			float* ra = mat[j + 0];
			float* rb = mat[j + 1];
			float* rc = mat[j + 2];
			float* rd = mat[j + 3];
	
			assert_16_byte_aligned( v );
			assert_16_byte_aligned( ra );
			assert_16_byte_aligned( rb );
			assert_16_byte_aligned( rc );
			assert_16_byte_aligned( rd );
	
			float a0 = - ra[0] * v[0] + ra[i];
			float a1 = - ra[1] * v[1];
			float a2 = - ra[2] * v[2];
			float a3 = - ra[3] * v[3];
	
			float b0 = - rb[0] * v[0] + rb[i];
			float b1 = - rb[1] * v[1];
			float b2 = - rb[2] * v[2];
			float b3 = - rb[3] * v[3];
	
			float c0 = - rc[0] * v[0] + rc[i];
			float c1 = - rc[1] * v[1];
			float c2 = - rc[2] * v[2];
			float c3 = - rc[3] * v[3];
	
			float d0 = - rd[0] * v[0] + rd[i];
			float d1 = - rd[1] * v[1];
			float d2 = - rd[2] * v[2];
			float d3 = - rd[3] * v[3];
	
			int k = 4;
			for( ; k < i - 3; k += 4 )
			{
				a0 -= ra[k + 0] * v[k + 0];
				a1 -= ra[k + 1] * v[k + 1];
				a2 -= ra[k + 2] * v[k + 2];
				a3 -= ra[k + 3] * v[k + 3];
	
				b0 -= rb[k + 0] * v[k + 0];
				b1 -= rb[k + 1] * v[k + 1];
				b2 -= rb[k + 2] * v[k + 2];
				b3 -= rb[k + 3] * v[k + 3];
	
				c0 -= rc[k + 0] * v[k + 0];
				c1 -= rc[k + 1] * v[k + 1];
				c2 -= rc[k + 2] * v[k + 2];
				c3 -= rc[k + 3] * v[k + 3];
	
				d0 -= rd[k + 0] * v[k + 0];
				d1 -= rd[k + 1] * v[k + 1];
				d2 -= rd[k + 2] * v[k + 2];
				d3 -= rd[k + 3] * v[k + 3];
			}
	
			float ra0 = ( i - k > 0 ) ? ra[k + 0] : 0.0f;
			float ra1 = ( i - k > 1 ) ? ra[k + 1] : 0.0f;
			float ra2 = ( i - k > 2 ) ? ra[k + 2] : 0.0f;
			float ra3 = ( i - k > 3 ) ? ra[k + 3] : 0.0f;
	
			float rb0 = ( i - k > 0 ) ? rb[k + 0] : 0.0f;
			float rb1 = ( i - k > 1 ) ? rb[k + 1] : 0.0f;
			float rb2 = ( i - k > 2 ) ? rb[k + 2] : 0.0f;
			float rb3 = ( i - k > 3 ) ? rb[k + 3] : 0.0f;
	
			float rc0 = ( i - k > 0 ) ? rc[k + 0] : 0.0f;
			float rc1 = ( i - k > 1 ) ? rc[k + 1] : 0.0f;
			float rc2 = ( i - k > 2 ) ? rc[k + 2] : 0.0f;
			float rc3 = ( i - k > 3 ) ? rc[k + 3] : 0.0f;
	
			float rd0 = ( i - k > 0 ) ? rd[k + 0] : 0.0f;
			float rd1 = ( i - k > 1 ) ? rd[k + 1] : 0.0f;
			float rd2 = ( i - k > 2 ) ? rd[k + 2] : 0.0f;
			float rd3 = ( i - k > 3 ) ? rd[k + 3] : 0.0f;
	
			a0 -= ra0 * v[k + 0];
			a1 -= ra1 * v[k + 1];
			a2 -= ra2 * v[k + 2];
			a3 -= ra3 * v[k + 3];
	
			b0 -= rb0 * v[k + 0];
			b1 -= rb1 * v[k + 1];
			b2 -= rb2 * v[k + 2];
			b3 -= rb3 * v[k + 3];
	
			c0 -= rc0 * v[k + 0];
			c1 -= rc1 * v[k + 1];
			c2 -= rc2 * v[k + 2];
			c3 -= rc3 * v[k + 3];
	
			d0 -= rd0 * v[k + 0];
			d1 -= rd1 * v[k + 1];
			d2 -= rd2 * v[k + 2];
			d3 -= rd3 * v[k + 3];
	
			ra[i] = ( a0 + a1 + a2 + a3 ) * d;
			rb[i] = ( b0 + b1 + b2 + b3 ) * d;
			rc[i] = ( c0 + c1 + c2 + c3 ) * d;
			rd[i] = ( d0 + d1 + d2 + d3 ) * d;
		}
		for( ; j < n; j++ )
		{
			mptr = mat[j];
	
			assert_16_byte_aligned( v );
			assert_16_byte_aligned( mptr );
	
			float a0 = - mptr[0] * v[0] + mptr[i];
			float a1 = - mptr[1] * v[1];
			float a2 = - mptr[2] * v[2];
			float a3 = - mptr[3] * v[3];
	
			int k = 4;
			for( ; k < i - 3; k += 4 )
			{
				a0 -= mptr[k + 0] * v[k + 0];
				a1 -= mptr[k + 1] * v[k + 1];
				a2 -= mptr[k + 2] * v[k + 2];
				a3 -= mptr[k + 3] * v[k + 3];
			}
	
			float m0 = ( i - k > 0 ) ? mptr[k + 0] : 0.0f;
			float m1 = ( i - k > 1 ) ? mptr[k + 1] : 0.0f;
			float m2 = ( i - k > 2 ) ? mptr[k + 2] : 0.0f;
			float m3 = ( i - k > 3 ) ? mptr[k + 3] : 0.0f;
	
			a0 -= m0 * v[k + 0];
			a1 -= m1 * v[k + 1];
			a2 -= m2 * v[k + 2];
			a3 -= m3 * v[k + 3];
	
			mptr[i] = ( a0 + a1 + a2 + a3 ) * d;
		}
	}
	return true;
	
#endif
}

/*
========================
GetMaxStep_SIMD
========================
*/
static void GetMaxStep_SIMD( const float* f, const float* a, const float* delta_f, const float* delta_a,
							 const float* lo, const float* hi, const int* side, int numUnbounded, int numClamped,
							 int d, float dir, float& maxStep, int& limit, int& limitSide )
{

#if defined(LCP_SIMD)
	__m128 vMaxStep;
	__m128i vLimit;
	__m128i vLimitSide;
	
	// default to a full step for the current variable
	{
		__m128 vNegAccel = _mm_xor_ps( _mm_load1_ps( a + d ), ( __m128& ) SIMD_SP_signBit );
		__m128 vDeltaAccel = _mm_load1_ps( delta_a + d );
		__m128 vM0 = _mm_cmpgt_ps( _mm_and_ps( vDeltaAccel, ( __m128& ) SIMD_SP_absMask ), SIMD_SP_LCP_DELTA_ACCEL_EPSILON );
		__m128 vStep = _mm_div32_ps( vNegAccel, _mm_sel_ps( SIMD_SP_one, vDeltaAccel, vM0 ) );
		vMaxStep = _mm_sel_ps( SIMD_SP_zero, vStep, vM0 );
		vLimit = _mm_shuffle_epi32( _mm_cvtsi32_si128( d ), 0 );
		vLimitSide = ( __m128i& ) SIMD_DW_zero;
	}
	
	// test the current variable
	{
		__m128 vDeltaForce = _mm_load1_ps( & dir );
		__m128 vSign = _mm_cmplt_ps( vDeltaForce, SIMD_SP_zero );
		__m128 vForceLimit = _mm_sel_ps( _mm_load1_ps( hi + d ), _mm_load1_ps( lo + d ), vSign );
		__m128 vStep = _mm_div32_ps( _mm_sub_ps( vForceLimit, _mm_load1_ps( f + d ) ), vDeltaForce );
		__m128i vSetSide = _mm_or_si128( __m128c( vSign ), ( __m128i& ) SIMD_DW_one );
		__m128 vM0 = _mm_cmpgt_ps( _mm_and_ps( vDeltaForce, ( __m128& ) SIMD_SP_absMask ), SIMD_SP_LCP_DELTA_FORCE_EPSILON );
		__m128 vM1 = _mm_cmpneq_ps( _mm_and_ps( vForceLimit, ( __m128& ) SIMD_SP_absMask ), SIMD_SP_infinity );
		__m128 vM2 = _mm_cmplt_ps( vStep, vMaxStep );
		__m128 vM3 = _mm_and_ps( _mm_and_ps( vM0, vM1 ), vM2 );
		vMaxStep = _mm_sel_ps( vMaxStep, vStep, vM3 );
		vLimitSide = _mm_sel_si128( vLimitSide, vSetSide, __m128c( vM3 ) );
	}
	
	// test the clamped bounded variables
	{
		__m128 mask = ( __m128& ) SIMD_SP_indexedStartMask[numUnbounded & 3];
		__m128i index = _mm_add_epi32( _mm_and_si128( _mm_shuffle_epi32( _mm_cvtsi32_si128( numUnbounded ), 0 ), ( __m128i& ) SIMD_DW_not3 ), ( __m128i& ) SIMD_DW_index );
		int i = numUnbounded & ~3;
		for( ; i < numClamped - 3; i += 4 )
		{
			__m128 vDeltaForce = _mm_and_ps( _mm_load_ps( delta_f + i ), mask );
			__m128 vSign = _mm_cmplt_ps( vDeltaForce, SIMD_SP_zero );
			__m128 vForceLimit = _mm_sel_ps( _mm_load_ps( hi + i ), _mm_load_ps( lo + i ), vSign );
			__m128 vM0 = _mm_cmpgt_ps( _mm_and_ps( vDeltaForce, ( __m128& ) SIMD_SP_absMask ), SIMD_SP_LCP_DELTA_FORCE_EPSILON );
			__m128 vStep = _mm_div32_ps( _mm_sub_ps( vForceLimit, _mm_load_ps( f + i ) ), _mm_sel_ps( SIMD_SP_one, vDeltaForce, vM0 ) );
			__m128i vSetSide = _mm_or_si128( __m128c( vSign ), ( __m128i& ) SIMD_DW_one );
			__m128 vM1 = _mm_cmpneq_ps( _mm_and_ps( vForceLimit, ( __m128& ) SIMD_SP_absMask ), SIMD_SP_infinity );
			__m128 vM2 = _mm_cmplt_ps( vStep, vMaxStep );
			__m128 vM3 = _mm_and_ps( _mm_and_ps( vM0, vM1 ), vM2 );
			vMaxStep = _mm_sel_ps( vMaxStep, vStep, vM3 );
			vLimit = _mm_sel_si128( vLimit, index, vM3 );
			vLimitSide = _mm_sel_si128( vLimitSide, vSetSide, __m128c( vM3 ) );
			mask = ( __m128& ) SIMD_SP_indexedStartMask[0];
			index = _mm_add_epi32( index, ( __m128i& ) SIMD_DW_four );
		}
		__m128 vDeltaForce = _mm_and_ps( _mm_load_ps( delta_f + i ), _mm_and_ps( mask, ( __m128& ) SIMD_SP_indexedEndMask[numClamped & 3] ) );
		__m128 vSign = _mm_cmplt_ps( vDeltaForce, SIMD_SP_zero );
		__m128 vForceLimit = _mm_sel_ps( _mm_load_ps( hi + i ), _mm_load_ps( lo + i ), vSign );
		__m128 vM0 = _mm_cmpgt_ps( _mm_and_ps( vDeltaForce, ( __m128& ) SIMD_SP_absMask ), SIMD_SP_LCP_DELTA_FORCE_EPSILON );
		__m128 vStep = _mm_div32_ps( _mm_sub_ps( vForceLimit, _mm_load_ps( f + i ) ), _mm_sel_ps( SIMD_SP_one, vDeltaForce, vM0 ) );
		__m128i vSetSide = _mm_or_si128( __m128c( vSign ), ( __m128i& ) SIMD_DW_one );
		__m128 vM1 = _mm_cmpneq_ps( _mm_and_ps( vForceLimit, ( __m128& ) SIMD_SP_absMask ), SIMD_SP_infinity );
		__m128 vM2 = _mm_cmplt_ps( vStep, vMaxStep );
		__m128 vM3 = _mm_and_ps( _mm_and_ps( vM0, vM1 ), vM2 );
		vMaxStep = _mm_sel_ps( vMaxStep, vStep, vM3 );
		vLimit = _mm_sel_si128( vLimit, index, vM3 );
		vLimitSide = _mm_sel_si128( vLimitSide, vSetSide, __m128c( vM3 ) );
	}
	
	// test the not clamped bounded variables
	{
		__m128 mask = ( __m128& ) SIMD_SP_indexedStartMask[numClamped & 3];
		__m128i index = _mm_add_epi32( _mm_and_si128( _mm_shuffle_epi32( _mm_cvtsi32_si128( numClamped ), 0 ), ( __m128i& ) SIMD_DW_not3 ), ( __m128i& ) SIMD_DW_index );
		int i = numClamped & ~3;
		for( ; i < d - 3; i += 4 )
		{
			__m128 vNegAccel = _mm_xor_ps( _mm_load_ps( a + i ), ( __m128& ) SIMD_SP_signBit );
			__m128 vDeltaAccel = _mm_and_ps( _mm_load_ps( delta_a + i ), mask );
			__m128 vSide = _mm_cvtepi32_ps( _mm_load_si128( ( __m128i* )( side + i ) ) );
			__m128 vM0 = _mm_cmpgt_ps( _mm_mul_ps( vSide, vDeltaAccel ), SIMD_SP_LCP_DELTA_ACCEL_EPSILON );
			__m128 vStep = _mm_div32_ps( vNegAccel, _mm_sel_ps( SIMD_SP_one, vDeltaAccel, vM0 ) );
			__m128 vM1 = _mm_or_ps( _mm_cmplt_ps( _mm_load_ps( lo + i ), SIMD_SP_neg_LCP_BOUND_EPSILON ), _mm_cmpgt_ps( _mm_load_ps( hi + i ), SIMD_SP_LCP_BOUND_EPSILON ) );
			__m128 vM2 = _mm_cmplt_ps( vStep, vMaxStep );
			__m128 vM3 = _mm_and_ps( _mm_and_ps( vM0, vM1 ), vM2 );
			vMaxStep = _mm_sel_ps( vMaxStep, vStep, vM3 );
			vLimit = _mm_sel_si128( vLimit, index, vM3 );
			vLimitSide = _mm_sel_si128( vLimitSide, ( __m128i& ) SIMD_DW_zero, __m128c( vM3 ) );
			mask = ( __m128& ) SIMD_SP_indexedStartMask[0];
			index = _mm_add_epi32( index, ( __m128i& ) SIMD_DW_four );
		}
		__m128 vNegAccel = _mm_xor_ps( _mm_load_ps( a + i ), ( __m128& ) SIMD_SP_signBit );
		__m128 vDeltaAccel = _mm_and_ps( _mm_load_ps( delta_a + i ), _mm_and_ps( mask, ( __m128& ) SIMD_SP_indexedEndMask[d & 3] ) );
		__m128 vSide = _mm_cvtepi32_ps( _mm_load_si128( ( __m128i* )( side + i ) ) );
		__m128 vM0 = _mm_cmpgt_ps( _mm_mul_ps( vSide, vDeltaAccel ), SIMD_SP_LCP_DELTA_ACCEL_EPSILON );
		__m128 vStep = _mm_div32_ps( vNegAccel, _mm_sel_ps( SIMD_SP_one, vDeltaAccel, vM0 ) );
		__m128 vM1 = _mm_or_ps( _mm_cmplt_ps( _mm_load_ps( lo + i ), SIMD_SP_neg_LCP_BOUND_EPSILON ), _mm_cmpgt_ps( _mm_load_ps( hi + i ), SIMD_SP_LCP_BOUND_EPSILON ) );
		__m128 vM2 = _mm_cmplt_ps( vStep, vMaxStep );
		__m128 vM3 = _mm_and_ps( _mm_and_ps( vM0, vM1 ), vM2 );
		vMaxStep = _mm_sel_ps( vMaxStep, vStep, vM3 );
		vLimit = _mm_sel_si128( vLimit, index, vM3 );
		vLimitSide = _mm_sel_si128( vLimitSide, ( __m128i& ) SIMD_DW_zero, __m128c( vM3 ) );
	}
	
	{
		__m128 tMaxStep = _mm_shuffle_ps( vMaxStep, vMaxStep, _MM_SHUFFLE( 1, 0, 3, 2 ) );
		__m128i tLimit = _mm_shuffle_epi32( vLimit, _MM_SHUFFLE( 1, 0, 3, 2 ) );
		__m128i tLimitSide = _mm_shuffle_epi32( vLimitSide, _MM_SHUFFLE( 1, 0, 3, 2 ) );
		__m128c mask = _mm_cmplt_ps( tMaxStep, vMaxStep );
		vMaxStep = _mm_min_ps( vMaxStep, tMaxStep );
		vLimit = _mm_sel_si128( vLimit, tLimit, mask );
		vLimitSide = _mm_sel_si128( vLimitSide, tLimitSide, mask );
	}
	
	{
		__m128 tMaxStep = _mm_shuffle_ps( vMaxStep, vMaxStep, _MM_SHUFFLE( 2, 3, 0, 1 ) );
		__m128i tLimit = _mm_shuffle_epi32( vLimit, _MM_SHUFFLE( 2, 3, 0, 1 ) );
		__m128i tLimitSide = _mm_shuffle_epi32( vLimitSide, _MM_SHUFFLE( 2, 3, 0, 1 ) );
		__m128c mask = _mm_cmplt_ps( tMaxStep, vMaxStep );
		vMaxStep = _mm_min_ps( vMaxStep, tMaxStep );
		vLimit = _mm_sel_si128( vLimit, tLimit, mask );
		vLimitSide = _mm_sel_si128( vLimitSide, tLimitSide, mask );
	}
	
	_mm_store_ss( & maxStep, vMaxStep );
	limit = _mm_cvtsi128_si32( vLimit );
	limitSide = _mm_cvtsi128_si32( vLimitSide );
	
#else
	
	// default to a full step for the current variable
	{
		float negAccel = -a[d];
		float deltaAccel = delta_a[d];
		int m0 = ( fabs( deltaAccel ) > LCP_DELTA_ACCEL_EPSILON );
		float step = negAccel / ( m0 ? deltaAccel : 1.0f );
		maxStep = m0 ? step : 0.0f;
		limit = d;
		limitSide = 0;
	}
	
	// test the current variable
	{
		float deltaForce = dir;
		float forceLimit = ( deltaForce < 0.0f ) ? lo[d] : hi[d];
		float step = ( forceLimit - f[d] ) / deltaForce;
		int setSide = ( deltaForce < 0.0f ) ? -1 : 1;
		int m0 = ( fabs( deltaForce ) > LCP_DELTA_FORCE_EPSILON );
		int m1 = ( fabs( forceLimit ) != idMath::INFINITY );
		int m2 = ( step < maxStep );
		int m3 = ( m0 & m1 & m2 );
		maxStep = m3 ? step : maxStep;
		limit = m3 ? d : limit;
		limitSide = m3 ? setSide : limitSide;
	}
	
	// test the clamped bounded variables
	for( int i = numUnbounded; i < numClamped; i++ )
	{
		float deltaForce = delta_f[i];
		float forceLimit = ( deltaForce < 0.0f ) ? lo[i] : hi[i];
		int m0 = ( fabs( deltaForce ) > LCP_DELTA_FORCE_EPSILON );
		float step = ( forceLimit - f[i] ) / ( m0 ? deltaForce : 1.0f );
		int setSide = ( deltaForce < 0.0f ) ? -1 : 1;
		int m1 = ( fabs( forceLimit ) != idMath::INFINITY );
		int m2 = ( step < maxStep );
		int m3 = ( m0 & m1 & m2 );
		maxStep = m3 ? step : maxStep;
		limit = m3 ? i : limit;
		limitSide = m3 ? setSide : limitSide;
	}
	
	// test the not clamped bounded variables
	for( int i = numClamped; i < d; i++ )
	{
		float negAccel = -a[i];
		float deltaAccel = delta_a[i];
		int m0 = ( side[i] * deltaAccel > LCP_DELTA_ACCEL_EPSILON );
		float step = negAccel / ( m0 ? deltaAccel : 1.0f );
		int m1 = ( lo[i] < -LCP_BOUND_EPSILON || hi[i] > LCP_BOUND_EPSILON );
		int m2 = ( step < maxStep );
		int m3 = ( m0 & m1 & m2 );
		maxStep = m3 ? step : maxStep;
		limit = m3 ? i : limit;
		limitSide = m3 ? 0 : limitSide;
	}
	
#endif
}

/*
================================================================================================

	SIMD test code

================================================================================================
*/

//#define ENABLE_TEST_CODE

#ifdef ENABLE_TEST_CODE

#define TEST_TRIANGULAR_SOLVE_SIMD_EPSILON		0.1f
#define TEST_TRIANGULAR_SOLVE_SIZE				50
#define TEST_FACTOR_SIMD_EPSILON				0.1f
#define TEST_FACTOR_SOLVE_SIZE					50
#define NUM_TESTS	50

/*
========================
PrintClocks
========================
*/
static void PrintClocks( const char* string, int dataCount, int64 clocks, int64 otherClocks = 0 )
{
	idLib::Printf( string );
	for( int i = idStr::LengthWithoutColors( string ); i < 48; i++ )
	{
		idLib::Printf( " " );
	}
	if( clocks && otherClocks )
	{
		int p = 0;
		if( clocks <= otherClocks )
		{
			p = idMath::Ftoi( ( float )( otherClocks - clocks ) * 100.0f / ( float ) otherClocks );
		}
		else
		{
			p = - idMath::Ftoi( ( float )( clocks - otherClocks ) * 100.0f / ( float ) clocks );
		}
		idLib::Printf( "c = %4d, clcks = %5lld, %d%%\n", dataCount, clocks, p );
	}
	else
	{
		idLib::Printf( "c = %4d, clcks = %5lld\n", dataCount, clocks );
	}
}

/*
========================
DotProduct_Test
========================
*/
static void DotProduct_Test()
{
	ALIGN16( float fsrc0[TEST_TRIANGULAR_SOLVE_SIZE + 1]; )
	ALIGN16( float fsrc1[TEST_TRIANGULAR_SOLVE_SIZE + 1]; )
	
	idRandom srnd( 13 );
	
	for( int i = 0; i < TEST_TRIANGULAR_SOLVE_SIZE; i++ )
	{
		fsrc0[i] = srnd.CRandomFloat() * 10.0f;
		fsrc1[i] = srnd.CRandomFloat() * 10.0f;
	}
	
	idTimer timer;
	
	for( int i = 0; i < TEST_TRIANGULAR_SOLVE_SIZE; i++ )
	{
	
		float dot1 = DotProduct_Generic( fsrc0, fsrc1, i );
		int64 clocksGeneric = 0xFFFFFFFFFFFF;
		for( int j = 0; j < NUM_TESTS; j++ )
		{
			fsrc1[TEST_TRIANGULAR_SOLVE_SIZE] = j;
			timer.Clear();
			timer.Start();
			dot1 = DotProduct_Generic( fsrc0, fsrc1, i );
			timer.Stop();
			clocksGeneric = Min( clocksGeneric, timer.ClockTicks() );
		}
		
		PrintClocks( va( "DotProduct_Generic %d", i ), 1, clocksGeneric );
		
		float dot2 = DotProduct_SIMD( fsrc0, fsrc1, i );
		int64 clocksSIMD = 0xFFFFFFFFFFFF;
		for( int j = 0; j < NUM_TESTS; j++ )
		{
			fsrc1[TEST_TRIANGULAR_SOLVE_SIZE] = j;
			timer.Clear();
			timer.Start();
			dot2 = DotProduct_SIMD( fsrc0, fsrc1, i );
			timer.Stop();
			clocksSIMD = Min( clocksSIMD, timer.ClockTicks() );
		}
		
		const char* result = idMath::Fabs( dot1 - dot2 ) < 1e-4f ? "ok" : S_COLOR_RED"X";
		PrintClocks( va( "DotProduct_SIMD    %d %s", i, result ), 1, clocksSIMD, clocksGeneric );
	}
}

/*
========================
LowerTriangularSolve_Test
========================
*/
static void LowerTriangularSolve_Test()
{
	idMatX L;
	idVecX x, b, tst;
	
	int paddedSize = ( TEST_TRIANGULAR_SOLVE_SIZE + 3 ) & ~3;
	
	L.Random( paddedSize, paddedSize, 0, -1.0f, 1.0f );
	x.SetSize( paddedSize );
	b.Random( paddedSize, 0, -1.0f, 1.0f );
	
	idTimer timer;
	
	const int skip = 0;
	
	for( int i = 1; i < TEST_TRIANGULAR_SOLVE_SIZE; i++ )
	{
	
		x.Zero( i );
		
		LowerTriangularSolve_Generic( L, x.ToFloatPtr(), b.ToFloatPtr(), i, skip );
		int64 clocksGeneric = 0xFFFFFFFFFFFF;
		for( int j = 0; j < NUM_TESTS; j++ )
		{
			timer.Clear();
			timer.Start();
			LowerTriangularSolve_Generic( L, x.ToFloatPtr(), b.ToFloatPtr(), i, skip );
			timer.Stop();
			clocksGeneric = Min( clocksGeneric, timer.ClockTicks() );
		}
		
		tst = x;
		x.Zero();
		
		PrintClocks( va( "LowerTriangularSolve_Generic %dx%d", i, i ), 1, clocksGeneric );
		
		LowerTriangularSolve_SIMD( L, x.ToFloatPtr(), b.ToFloatPtr(), i, skip );
		int64 clocksSIMD = 0xFFFFFFFFFFFF;
		for( int j = 0; j < NUM_TESTS; j++ )
		{
			timer.Clear();
			timer.Start();
			LowerTriangularSolve_SIMD( L, x.ToFloatPtr(), b.ToFloatPtr(), i, skip );
			timer.Stop();
			clocksSIMD = Min( clocksSIMD, timer.ClockTicks() );
		}
		
		const char* result = x.Compare( tst, TEST_TRIANGULAR_SOLVE_SIMD_EPSILON ) ? "ok" : S_COLOR_RED"X";
		PrintClocks( va( "LowerTriangularSolve_SIMD    %dx%d %s", i, i, result ), 1, clocksSIMD, clocksGeneric );
	}
}

/*
========================
LowerTriangularSolveTranspose_Test
========================
*/
static void LowerTriangularSolveTranspose_Test()
{
	idMatX L;
	idVecX x, b, tst;
	
	int paddedSize = ( TEST_TRIANGULAR_SOLVE_SIZE + 3 ) & ~3;
	
	L.Random( paddedSize, paddedSize, 0, -1.0f, 1.0f );
	x.SetSize( paddedSize );
	b.Random( paddedSize, 0, -1.0f, 1.0f );
	
	idTimer timer;
	
	for( int i = 1; i < TEST_TRIANGULAR_SOLVE_SIZE; i++ )
	{
	
		x.Zero( i );
		
		LowerTriangularSolveTranspose_Generic( L, x.ToFloatPtr(), b.ToFloatPtr(), i );
		int64 clocksGeneric = 0xFFFFFFFFFFFF;
		for( int j = 0; j < NUM_TESTS; j++ )
		{
			timer.Clear();
			timer.Start();
			LowerTriangularSolveTranspose_Generic( L, x.ToFloatPtr(), b.ToFloatPtr(), i );
			timer.Stop();
			clocksGeneric = Min( clocksGeneric, timer.ClockTicks() );
		}
		
		tst = x;
		x.Zero();
		
		PrintClocks( va( "LowerTriangularSolveTranspose_Generic %dx%d", i, i ), 1, clocksGeneric );
		
		LowerTriangularSolveTranspose_SIMD( L, x.ToFloatPtr(), b.ToFloatPtr(), i );
		int64 clocksSIMD = 0xFFFFFFFFFFFF;
		for( int j = 0; j < NUM_TESTS; j++ )
		{
			timer.Clear();
			timer.Start();
			LowerTriangularSolveTranspose_SIMD( L, x.ToFloatPtr(), b.ToFloatPtr(), i );
			timer.Stop();
			clocksSIMD = Min( clocksSIMD, timer.ClockTicks() );
		}
		
		const char* result = x.Compare( tst, TEST_TRIANGULAR_SOLVE_SIMD_EPSILON ) ? "ok" : S_COLOR_RED"X";
		PrintClocks( va( "LowerTriangularSolveTranspose_SIMD    %dx%d %s", i, i, result ), 1, clocksSIMD, clocksGeneric );
	}
}

/*
========================
LDLT_Factor_Test
========================
*/
static void LDLT_Factor_Test()
{
	idMatX src, original, mat1, mat2;
	idVecX invDiag1, invDiag2;
	
	int paddedSize = ( TEST_FACTOR_SOLVE_SIZE + 3 ) & ~3;
	
	original.SetSize( paddedSize, paddedSize );
	src.Random( paddedSize, paddedSize, 0, -1.0f, 1.0f );
	src.TransposeMultiply( original, src );
	
	idTimer timer;
	
	for( int i = 1; i < TEST_FACTOR_SOLVE_SIZE; i++ )
	{
	
		int64 clocksGeneric = 0xFFFFFFFFFFFF;
		for( int j = 0; j < NUM_TESTS; j++ )
		{
			mat1 = original;
			invDiag1.Zero( TEST_FACTOR_SOLVE_SIZE );
			timer.Clear();
			timer.Start();
			LDLT_Factor_Generic( mat1, invDiag1, i );
			timer.Stop();
			clocksGeneric = Min( clocksGeneric, timer.ClockTicks() );
		}
		
		PrintClocks( va( "LDLT_Factor_Generic %dx%d", i, i ), 1, clocksGeneric );
		
		int64 clocksSIMD = 0xFFFFFFFFFFFF;
		for( int j = 0; j < NUM_TESTS; j++ )
		{
			mat2 = original;
			invDiag2.Zero( TEST_FACTOR_SOLVE_SIZE );
			timer.Clear();
			timer.Start();
			LDLT_Factor_SIMD( mat2, invDiag2, i );
			timer.Stop();
			clocksSIMD = Min( clocksSIMD, timer.ClockTicks() );
		}
		
		const char* result = mat1.Compare( mat2, TEST_FACTOR_SIMD_EPSILON ) && invDiag1.Compare( invDiag2, TEST_FACTOR_SIMD_EPSILON ) ? "ok" : S_COLOR_RED"X";
		PrintClocks( va( "LDLT_Factor_SIMD    %dx%d %s", i, i, result ), 1, clocksSIMD, clocksGeneric );
	}
}
#endif

#define Multiply						Multiply_SIMD
#define MultiplyAdd						MultiplyAdd_SIMD
#define BigDotProduct					DotProduct_SIMD
#define LowerTriangularSolve			LowerTriangularSolve_SIMD
#define LowerTriangularSolveTranspose	LowerTriangularSolveTranspose_SIMD
#define UpperTriangularSolve			UpperTriangularSolve_SIMD
#define LU_Factor						LU_Factor_SIMD
#define LDLT_Factor						LDLT_Factor_SIMD
#define GetMaxStep						GetMaxStep_SIMD

/*
================================================================================================

	idLCP_Square

================================================================================================
*/

/*
================================================
idLCP_Square
================================================
*/
class idLCP_Square : public idLCP
{
public:
	virtual bool	Solve( const idMatX& o_m, idVecX& o_x, const idVecX& o_b, const idVecX& o_lo, const idVecX& o_hi, const int* o_boxIndex );
	
private:
	idMatX			m;					// original matrix
	idVecX			b;					// right hand side
	idVecX			lo, hi;				// low and high bounds
	idVecX			f, a;				// force and acceleration
	idVecX			delta_f, delta_a;	// delta force and delta acceleration
	idMatX			clamped;			// LU factored sub matrix for clamped variables
	idVecX			diagonal;			// reciprocal of diagonal of U of the LU factored sub matrix for clamped variables
	int				numUnbounded;		// number of unbounded variables
	int				numClamped;			// number of clamped variables
	float** 		rowPtrs;			// pointers to the rows of m
	int* 			boxIndex;			// box index
	int* 			side;				// tells if a variable is at the low boundary = -1, high boundary = 1 or inbetween = 0
	int* 			permuted;			// index to keep track of the permutation
	bool			padded;				// set to true if the rows of the initial matrix are 16 byte padded
	
	bool			FactorClamped();
	void			SolveClamped( idVecX& x, const float* b );
	void			Swap( int i, int j );
	void			AddClamped( int r );
	void			RemoveClamped( int r );
	void			CalcForceDelta( int d, float dir );
	void			CalcAccelDelta( int d );
	void			ChangeForce( int d, float step );
	void			ChangeAccel( int d, float step );
};

/*
========================
idLCP_Square::FactorClamped
========================
*/
bool idLCP_Square::FactorClamped()
{
	for( int i = 0; i < numClamped; i++ )
	{
		memcpy( clamped[i], rowPtrs[i], numClamped * sizeof( float ) );
	}
	return LU_Factor( clamped, diagonal, numClamped );
}

/*
========================
idLCP_Square::SolveClamped
========================
*/
void idLCP_Square::SolveClamped( idVecX& x, const float* b )
{
	// solve L
	LowerTriangularSolve( clamped, x.ToFloatPtr(), b, numClamped, 0 );
	
	// solve U
	UpperTriangularSolve( clamped, diagonal.ToFloatPtr(), x.ToFloatPtr(), x.ToFloatPtr(), numClamped );
}

/*
========================
idLCP_Square::Swap
========================
*/
void idLCP_Square::Swap( int i, int j )
{

	if( i == j )
	{
		return;
	}
	
	SwapValues( rowPtrs[i], rowPtrs[j] );
	m.SwapColumns( i, j );
	b.SwapElements( i, j );
	lo.SwapElements( i, j );
	hi.SwapElements( i, j );
	a.SwapElements( i, j );
	f.SwapElements( i, j );
	if( boxIndex != NULL )
	{
		SwapValues( boxIndex[i], boxIndex[j] );
	}
	SwapValues( side[i], side[j] );
	SwapValues( permuted[i], permuted[j] );
}

/*
========================
idLCP_Square::AddClamped
========================
*/
void idLCP_Square::AddClamped( int r )
{

	assert( r >= numClamped );
	
	// add a row at the bottom and a column at the right of the factored
	// matrix for the clamped variables
	
	Swap( numClamped, r );
	
	// add row to L
	for( int i = 0; i < numClamped; i++ )
	{
		float sum = rowPtrs[numClamped][i];
		for( int j = 0; j < i; j++ )
		{
			sum -= clamped[numClamped][j] * clamped[j][i];
		}
		clamped[numClamped][i] = sum * diagonal[i];
	}
	
	// add column to U
	for( int i = 0; i <= numClamped; i++ )
	{
		float sum = rowPtrs[i][numClamped];
		for( int j = 0; j < i; j++ )
		{
			sum -= clamped[i][j] * clamped[j][numClamped];
		}
		clamped[i][numClamped] = sum;
	}
	
	diagonal[numClamped] = 1.0f / clamped[numClamped][numClamped];
	
	numClamped++;
}

/*
========================
idLCP_Square::RemoveClamped
========================
*/
void idLCP_Square::RemoveClamped( int r )
{

	if( !verify( r < numClamped ) )
	{
		// complete fail, most likely due to exceptional floating point values
		return;
	}
	
	numClamped--;
	
	// no need to swap and update the factored matrix when the last row and column are removed
	if( r == numClamped )
	{
		return;
	}
	
	float* y0 = ( float* ) _alloca16( numClamped * sizeof( float ) );
	float* z0 = ( float* ) _alloca16( numClamped * sizeof( float ) );
	float* y1 = ( float* ) _alloca16( numClamped * sizeof( float ) );
	float* z1 = ( float* ) _alloca16( numClamped * sizeof( float ) );
	
	// the row/column need to be subtracted from the factorization
	for( int i = 0; i < numClamped; i++ )
	{
		y0[i] = -rowPtrs[i][r];
	}
	
	memset( y1, 0, numClamped * sizeof( float ) );
	y1[r] = 1.0f;
	
	memset( z0, 0, numClamped * sizeof( float ) );
	z0[r] = 1.0f;
	
	for( int i = 0; i < numClamped; i++ )
	{
		z1[i] = -rowPtrs[r][i];
	}
	
	// swap the to be removed row/column with the last row/column
	Swap( r, numClamped );
	
	// the swapped last row/column need to be added to the factorization
	for( int i = 0; i < numClamped; i++ )
	{
		y0[i] += rowPtrs[i][r];
	}
	
	for( int i = 0; i < numClamped; i++ )
	{
		z1[i] += rowPtrs[r][i];
	}
	z1[r] = 0.0f;
	
	// update the beginning of the to be updated row and column
	for( int i = 0; i < r; i++ )
	{
		float p0 = y0[i];
		float beta1 = z1[i] * diagonal[i];
		
		clamped[i][r] += p0;
		for( int j = i + 1; j < numClamped; j++ )
		{
			z1[j] -= beta1 * clamped[i][j];
		}
		for( int j = i + 1; j < numClamped; j++ )
		{
			y0[j] -= p0 * clamped[j][i];
		}
		clamped[r][i] += beta1;
	}
	
	// update the lower right corner starting at r,r
	for( int i = r; i < numClamped; i++ )
	{
		float diag = clamped[i][i];
		
		float p0 = y0[i];
		float p1 = z0[i];
		diag += p0 * p1;
		
		if( fabs( diag ) < idMath::FLT_SMALLEST_NON_DENORMAL )
		{
			idLib::Printf( "idLCP_Square::RemoveClamped: updating factorization failed\n" );
			diag = idMath::FLT_SMALLEST_NON_DENORMAL;
		}
		
		float beta0 = p1 / diag;
		
		float q0 = y1[i];
		float q1 = z1[i];
		diag += q0 * q1;
		
		if( fabs( diag ) < idMath::FLT_SMALLEST_NON_DENORMAL )
		{
			idLib::Printf( "idLCP_Square::RemoveClamped: updating factorization failed\n" );
			diag = idMath::FLT_SMALLEST_NON_DENORMAL;
		}
		
		float d = 1.0f / diag;
		float beta1 = q1 * d;
		
		clamped[i][i] = diag;
		diagonal[i] = d;
		
		for( int j = i + 1; j < numClamped; j++ )
		{
		
			d = clamped[i][j];
			
			d += p0 * z0[j];
			z0[j] -= beta0 * d;
			
			d += q0 * z1[j];
			z1[j] -= beta1 * d;
			
			clamped[i][j] = d;
		}
		
		for( int j = i + 1; j < numClamped; j++ )
		{
		
			d = clamped[j][i];
			
			y0[j] -= p0 * d;
			d += beta0 * y0[j];
			
			y1[j] -= q0 * d;
			d += beta1 * y1[j];
			
			clamped[j][i] = d;
		}
	}
	return;
}

/*
========================
idLCP_Square::CalcForceDelta

Modifies this->delta_f.
========================
*/
void idLCP_Square::CalcForceDelta( int d, float dir )
{

	delta_f[d] = dir;
	
	if( numClamped <= 0 )
	{
		return;
	}
	
	// get column d of matrix
	float* ptr = ( float* ) _alloca16( numClamped * sizeof( float ) );
	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.ToFloatPtr();
		for( int i = 0; i < numClamped; i++ )
		{
			ptr[i] = - ptr[i];
		}
	}
}

/*
========================
idLCP_Square::CalcAccelDelta

Modifies this->delta_a and uses this->delta_f.
========================
*/
ID_INLINE void idLCP_Square::CalcAccelDelta( int d )
{
	// only the not clamped variables, including the current variable, can have a change in acceleration
	for( int j = numClamped; j <= d; j++ )
	{
		// only the clamped variables and the current variable have a force delta unequal zero
		float dot = BigDotProduct( rowPtrs[j], delta_f.ToFloatPtr(), numClamped );
		delta_a[j] = dot + rowPtrs[j][d] * delta_f[d];
	}
}

/*
========================
idLCP_Square::ChangeForce

Modifies this->f and uses this->delta_f.
========================
*/
ID_INLINE void idLCP_Square::ChangeForce( int d, float step )
{
	// only the clamped variables and current variable have a force delta unequal zero
	MultiplyAdd( f.ToFloatPtr(), step, delta_f.ToFloatPtr(), numClamped );
	f[d] += step * delta_f[d];
}

/*
========================
idLCP_Square::ChangeAccel

Modifies this->a and uses this->delta_a.
========================
*/
ID_INLINE void idLCP_Square::ChangeAccel( int d, float step )
{
	// only the not clamped variables, including the current variable, can have an acceleration unequal zero
	MultiplyAdd( a.ToFloatPtr() + numClamped, step, delta_a.ToFloatPtr() + numClamped, d - numClamped + 1 );
}

/*
========================
idLCP_Square::Solve
========================
*/
bool idLCP_Square::Solve( const idMatX& o_m, idVecX& o_x, const idVecX& o_b, const idVecX& o_lo, const idVecX& o_hi, const int* o_boxIndex )
{

	// true when the matrix rows are 16 byte padded
	padded = ( ( o_m.GetNumRows() + 3 )&~3 ) == o_m.GetNumColumns();
	
	assert( padded || o_m.GetNumRows() == o_m.GetNumColumns() );
	assert( o_x.GetSize() == o_m.GetNumRows() );
	assert( o_b.GetSize() == o_m.GetNumRows() );
	assert( o_lo.GetSize() == o_m.GetNumRows() );
	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 = ( int* )_alloca16( o_x.GetSize() * sizeof( int ) );
		memcpy( boxIndex, o_boxIndex, o_x.GetSize() * sizeof( int ) );
	}
	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(), const_cast<float*>( o_m[0] ) );
	f.Zero();
	a.Zero();
	b = o_b;
	lo = o_lo;
	hi = o_hi;
	
	// pointers to the rows of m
	rowPtrs = ( float** ) _alloca16( m.GetNumRows() * sizeof( float* ) );
	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 = ( int* ) _alloca16( m.GetNumRows() * sizeof( int ) );
	
	// index to keep track of the permutation
	permuted = ( int* ) _alloca16( m.GetNumRows() * sizeof( int ) );
	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] == -idMath::INFINITY && hi[i] == idMath::INFINITY )
		{
			if( numUnbounded != i )
			{
				Swap( numUnbounded, i );
			}
			numUnbounded++;
		}
	}
	
	// permute input so all variables using the boxIndex come last
	int boxStartIndex = m.GetNumRows();
	if( boxIndex )
	{
		for( int i = m.GetNumRows() - 1; i >= numUnbounded; i-- )
		{
			if( boxIndex[i] >= 0 )
			{
				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 )
	{
	
		// factor and solve for unbounded variables
		if( !FactorClamped() )
		{
			idLib::Printf( "idLCP_Square::Solve: unbounded factorization failed\n" );
			return false;
		}
		SolveClamped( f, b.ToFloatPtr() );
		
		// if there are no bounded variables we are done
		if( numUnbounded == m.GetNumRows() )
		{
			o_x = f;	// the vector is not permuted
			return true;
		}
	}
	
	int numIgnored = 0;
	
	// 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
	idStr failed;
	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] != -idMath::INFINITY )
				{
					lo[j] = - idMath::Fabs( lo[j] * s );
				}
				if( hi[j] != idMath::INFINITY )
				{
					hi[j] = idMath::Fabs( hi[j] * s );
				}
			}
		}
		
		// calculate acceleration for current variable
		float dot = BigDotProduct( rowPtrs[i], f.ToFloatPtr(), 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 = 0;
		for( ; 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 );
			
			float maxStep;
			int limit;
			int limitSide;
			
			// maximum step we can take
			GetMaxStep( f.ToFloatPtr(), a.ToFloatPtr(), delta_f.ToFloatPtr(), delta_a.ToFloatPtr(),
						lo.ToFloatPtr(), hi.ToFloatPtr(), side, numUnbounded, numClamped,
						i, dir, maxStep, limit, limitSide );
						
			if( maxStep <= 0.0f )
			{
#ifdef IGNORE_UNSATISFIABLE_VARIABLES
				// ignore the current variable completely
				lo[i] = hi[i] = 0.0f;
				f[i] = 0.0f;
				side[i] = -1;
				numIgnored++;
#else
				failed.Format( "invalid step size %.4f", maxStep );
				for( int j = i; j < m.GetNumRows(); j++ )
				{
					f[j] = 0.0f;
				}
				numIgnored = m.GetNumRows() - i;
#endif
				break;
			}
			
			// change force
			ChangeForce( i, maxStep );
			
			// change acceleration
			ChangeAccel( i, maxStep );
			
			// clamp/unclamp the variable that limited this step
			side[limit] = limitSide;
			if( limitSide == 0 )
			{
				a[limit] = 0.0f;
				AddClamped( limit );
			}
			else if( limitSide == -1 )
			{
				f[limit] = lo[limit];
				if( limit != i )
				{
					RemoveClamped( limit );
				}
			}
			else if( limitSide == 1 )
			{
				f[limit] = hi[limit];
				if( limit != i )
				{
					RemoveClamped( limit );
				}
			}
			
			// if the current variable limited the step we can continue with the next variable
			if( limit == i )
			{
				break;
			}
		}
		
		if( n >= maxIterations )
		{
			failed.Format( "max iterations %d", maxIterations );
			break;
		}
		
		if( failed.Length() )
		{
			break;
		}
	}
	
#ifdef IGNORE_UNSATISFIABLE_VARIABLES
	if( numIgnored )
	{
		if( lcp_showFailures.GetBool() )
		{
			idLib::Printf( "idLCP_Square::Solve: %d of %d bounded variables ignored\n", numIgnored, m.GetNumRows() - numUnbounded );
		}
	}
#endif
	
	// if failed clear remaining forces
	if( failed.Length() )
	{
		if( lcp_showFailures.GetBool() )
		{
			idLib::Printf( "idLCP_Square::Solve: %s (%d of %d bounded variables ignored)\n", failed.c_str(), numIgnored, m.GetNumRows() - numUnbounded );
		}
	}
	
#if defined(_DEBUG) && 0
	if( failed.Length() )
	{
		// 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];
	}
	
	return true;
}

/*
================================================================================================

	idLCP_Symmetric

================================================================================================
*/

/*
================================================
idLCP_Symmetric
================================================
*/
class idLCP_Symmetric : public idLCP
{
public:
	virtual bool	Solve( const idMatX& o_m, idVecX& o_x, const idVecX& o_b, const idVecX& o_lo, const idVecX& o_hi, const int* o_boxIndex );
	
private:
	idMatX			m;					// original matrix
	idVecX			b;					// right hand side
	idVecX			lo, hi;				// low and high bounds
	idVecX			f, a;				// force and acceleration
	idVecX			delta_f, delta_a;	// delta force and delta acceleration
	idMatX			clamped;			// LDLt factored sub matrix for clamped variables
	idVecX			diagonal;			// reciprocal of diagonal of LDLt factored sub matrix for clamped variables
	idVecX			solveCache1;		// intermediate result cached in SolveClamped
	idVecX			solveCache2;		// "
	int				numUnbounded;		// number of unbounded variables
	int				numClamped;			// number of clamped variables
	int				clampedChangeStart;	// lowest row/column changed in the clamped matrix during an iteration
	float** 		rowPtrs;			// pointers to the rows of m
	int* 			boxIndex;			// box index
	int* 			side;				// tells if a variable is at the low boundary = -1, high boundary = 1 or inbetween = 0
	int* 			permuted;			// index to keep track of the permutation
	bool			padded;				// set to true if the rows of the initial matrix are 16 byte padded
	
	bool			FactorClamped();
	void			SolveClamped( idVecX& x, const float* b );
	void			Swap( int i, int j );
	void			AddClamped( int r, bool useSolveCache );
	void			RemoveClamped( int r );
	void			CalcForceDelta( int d, float dir );
	void			CalcAccelDelta( int d );
	void			ChangeForce( int d, float step );
	void			ChangeAccel( int d, float step );
};

/*
========================
idLCP_Symmetric::FactorClamped
========================
*/
bool idLCP_Symmetric::FactorClamped()
{

	clampedChangeStart = 0;
	
	for( int i = 0; i < numClamped; i++ )
	{
		memcpy( clamped[i], rowPtrs[i], numClamped * sizeof( float ) );
	}
	return LDLT_Factor( clamped, diagonal, numClamped );
}

/*
========================
idLCP_Symmetric::SolveClamped
========================
*/
void idLCP_Symmetric::SolveClamped( idVecX& x, const float* b )
{

	// solve L
	LowerTriangularSolve( clamped, solveCache1.ToFloatPtr(), b, numClamped, clampedChangeStart );
	
	// scale with D
	Multiply( solveCache2.ToFloatPtr(), solveCache1.ToFloatPtr(), diagonal.ToFloatPtr(), numClamped );
	
	// solve Lt
	LowerTriangularSolveTranspose( clamped, x.ToFloatPtr(), solveCache2.ToFloatPtr(), numClamped );
	
	clampedChangeStart = numClamped;
}

/*
========================
idLCP_Symmetric::Swap
========================
*/
void idLCP_Symmetric::Swap( int i, int j )
{

	if( i == j )
	{
		return;
	}
	
	SwapValues( rowPtrs[i], rowPtrs[j] );
	m.SwapColumns( i, j );
	b.SwapElements( i, j );
	lo.SwapElements( i, j );
	hi.SwapElements( i, j );
	a.SwapElements( i, j );
	f.SwapElements( i, j );
	if( boxIndex != NULL )
	{
		SwapValues( boxIndex[i], boxIndex[j] );
	}
	SwapValues( side[i], side[j] );
	SwapValues( permuted[i], permuted[j] );
}

/*
========================
idLCP_Symmetric::AddClamped
========================
*/
void idLCP_Symmetric::AddClamped( int r, bool useSolveCache )
{

	assert( r >= numClamped );
	
	if( numClamped < clampedChangeStart )
	{
		clampedChangeStart = numClamped;
	}
	
	// add a row at the bottom and a column at the right of the factored
	// matrix for the clamped variables
	
	Swap( numClamped, r );
	
	// solve for v in L * v = rowPtr[numClamped]
	float dot;
	if( useSolveCache )
	{
	
		// the lower triangular solve was cached in SolveClamped called by CalcForceDelta
		memcpy( clamped[numClamped], solveCache2.ToFloatPtr(), numClamped * sizeof( float ) );
		// calculate row dot product
		dot = BigDotProduct( solveCache2.ToFloatPtr(), solveCache1.ToFloatPtr(), numClamped );
		
	}
	else
	{
	
		float* v = ( float* ) _alloca16( numClamped * sizeof( float ) );
		
		LowerTriangularSolve( clamped, v, rowPtrs[numClamped], numClamped, 0 );
		// add bottom row to L
		Multiply( clamped[numClamped], v, diagonal.ToFloatPtr(), numClamped );
		// calculate row dot product
		dot = BigDotProduct( clamped[numClamped], v, numClamped );
	}
	
	// update diagonal[numClamped]
	float d = rowPtrs[numClamped][numClamped] - dot;
	
	if( fabs( d ) < idMath::FLT_SMALLEST_NON_DENORMAL )
	{
		idLib::Printf( "idLCP_Symmetric::AddClamped: updating factorization failed\n" );
		d = idMath::FLT_SMALLEST_NON_DENORMAL;
	}
	
	clamped[numClamped][numClamped] = d;
	diagonal[numClamped] = 1.0f / d;
	
	numClamped++;
}

/*
========================
idLCP_Symmetric::RemoveClamped
========================
*/
void idLCP_Symmetric::RemoveClamped( int r )
{

	if( !verify( r < numClamped ) )
	{
		// complete fail, most likely due to exceptional floating point values
		return;
	}
	
	if( r < clampedChangeStart )
	{
		clampedChangeStart = r;
	}
	
	numClamped--;
	
	// no need to swap and update the factored matrix when the last row and column are removed
	if( r == numClamped )
	{
		return;
	}
	
	// swap the to be removed row/column with the last row/column
	Swap( r, numClamped );
	
	// update the factored matrix
	float* addSub = ( float* ) _alloca16( numClamped * sizeof( float ) );
	
	if( r == 0 )
	{
	
		if( numClamped == 1 )
		{
			float diag = rowPtrs[0][0];
			if( fabs( diag ) < idMath::FLT_SMALLEST_NON_DENORMAL )
			{
				idLib::Printf( "idLCP_Symmetric::RemoveClamped: updating factorization failed\n" );
				diag = idMath::FLT_SMALLEST_NON_DENORMAL;
			}
			clamped[0][0] = diag;
			diagonal[0] = 1.0f / diag;
			return;
		}
		
		// calculate the row/column to be added to the lower right sub matrix starting at (r, r)
		float* original = rowPtrs[numClamped];
		float* ptr = rowPtrs[r];
		addSub[0] = ptr[0] - original[numClamped];
		for( int i = 1; i < numClamped; i++ )
		{
			addSub[i] = ptr[i] - original[i];
		}
		
	}
	else
	{
	
		float* v = ( float* ) _alloca16( numClamped * sizeof( float ) );
		
		// solve for v in L * v = rowPtr[r]
		LowerTriangularSolve( clamped, v, rowPtrs[r], r, 0 );
		
		// update removed row
		Multiply( clamped[r], v, diagonal.ToFloatPtr(), r );
		
		// if the last row/column of the matrix is updated
		if( r == numClamped - 1 )
		{
			// only calculate new diagonal
			float dot = BigDotProduct( clamped[r], v, r );
			float diag = rowPtrs[r][r] - dot;
			if( fabs( diag ) < idMath::FLT_SMALLEST_NON_DENORMAL )
			{
				idLib::Printf( "idLCP_Symmetric::RemoveClamped: updating factorization failed\n" );
				diag = idMath::FLT_SMALLEST_NON_DENORMAL;
			}
			clamped[r][r] = diag;
			diagonal[r] = 1.0f / diag;
			return;
		}
		
		// calculate the row/column to be added to the lower right sub matrix starting at (r, r)
		for( int i = 0; i < r; i++ )
		{
			v[i] = clamped[r][i] * clamped[i][i];
		}
		for( int i = r; i < numClamped; i++ )
		{
			float sum;
			if( i == r )
			{
				sum = clamped[r][r];
			}
			else
			{
				sum = clamped[r][r] * clamped[i][r];
			}
			float* ptr = clamped[i];
			for( int j = 0; j < r; j++ )
			{
				sum += ptr[j] * v[j];
			}
			addSub[i] = rowPtrs[r][i] - sum;
		}
	}
	
	// add row/column to the lower right sub matrix starting at (r, r)
	
	float* v1 = ( float* ) _alloca16( numClamped * sizeof( float ) );
	float* v2 = ( float* ) _alloca16( numClamped * sizeof( float ) );
	
	float diag = idMath::SQRT_1OVER2;
	v1[r] = ( 0.5f * addSub[r] + 1.0f ) * diag;
	v2[r] = ( 0.5f * addSub[r] - 1.0f ) * diag;
	for( int i = r + 1; i < numClamped; i++ )
	{
		v1[i] = v2[i] = addSub[i] * diag;
	}
	
	float alpha1 = 1.0f;
	float alpha2 = -1.0f;
	
	// simultaneous update/downdate of the sub matrix starting at (r, r)
	int n = clamped.GetNumColumns();
	for( int i = r; i < numClamped; i++ )
	{
	
		diag = clamped[i][i];
		float p1 = v1[i];
		float newDiag = diag + alpha1 * p1 * p1;
		
		if( fabs( newDiag ) < idMath::FLT_SMALLEST_NON_DENORMAL )
		{
			idLib::Printf( "idLCP_Symmetric::RemoveClamped: updating factorization failed\n" );
			newDiag = idMath::FLT_SMALLEST_NON_DENORMAL;
		}
		
		alpha1 /= newDiag;
		float beta1 = p1 * alpha1;
		alpha1 *= diag;
		
		diag = newDiag;
		float p2 = v2[i];
		newDiag = diag + alpha2 * p2 * p2;
		
		if( fabs( newDiag ) < idMath::FLT_SMALLEST_NON_DENORMAL )
		{
			idLib::Printf( "idLCP_Symmetric::RemoveClamped: updating factorization failed\n" );
			newDiag = idMath::FLT_SMALLEST_NON_DENORMAL;
		}
		
		clamped[i][i] = newDiag;
		float invNewDiag = 1.0f / newDiag;
		diagonal[i] = invNewDiag;
		
		alpha2 *= invNewDiag;
		float beta2 = p2 * alpha2;
		alpha2 *= diag;
		
		// update column below diagonal (i,i)
		float* ptr = clamped.ToFloatPtr() + i;
		
		int j;
		for( j = i + 1; j < numClamped - 1; j += 2 )
		{
		
			float sum0 = ptr[( j + 0 ) * n];
			float sum1 = ptr[( j + 1 ) * n];
			
			v1[j + 0] -= p1 * sum0;
			v1[j + 1] -= p1 * sum1;
			
			sum0 += beta1 * v1[j + 0];
			sum1 += beta1 * v1[j + 1];
			
			v2[j + 0] -= p2 * sum0;
			v2[j + 1] -= p2 * sum1;
			
			sum0 += beta2 * v2[j + 0];
			sum1 += beta2 * v2[j + 1];
			
			ptr[( j + 0 )*n] = sum0;
			ptr[( j + 1 )*n] = sum1;
		}
		
		for( ; j < numClamped; j++ )
		{
		
			float sum = ptr[j * n];
			
			v1[j] -= p1 * sum;
			sum += beta1 * v1[j];
			
			v2[j] -= p2 * sum;
			sum += beta2 * v2[j];
			
			ptr[j * n] = sum;
		}
	}
}

/*
========================
idLCP_Symmetric::CalcForceDelta

Modifies this->delta_f.
========================
*/
ID_INLINE void idLCP_Symmetric::CalcForceDelta( int d, float dir )
{

	delta_f[d] = dir;
	
	if( numClamped == 0 )
	{
		return;
	}
	
	// solve force delta
	SolveClamped( delta_f, rowPtrs[d] );
	
	// flip force delta based on direction
	if( dir > 0.0f )
	{
		float* ptr = delta_f.ToFloatPtr();
		for( int i = 0; i < numClamped; i++ )
		{
			ptr[i] = - ptr[i];
		}
	}
}

/*
========================
idLCP_Symmetric::CalcAccelDelta

Modifies this->delta_a and uses this->delta_f.
========================
*/
ID_INLINE void idLCP_Symmetric::CalcAccelDelta( int d )
{
	// only the not clamped variables, including the current variable, can have a change in acceleration
	for( int j = numClamped; j <= d; j++ )
	{
		// only the clamped variables and the current variable have a force delta unequal zero
		float dot = BigDotProduct( rowPtrs[j], delta_f.ToFloatPtr(), numClamped );
		delta_a[j] = dot + rowPtrs[j][d] * delta_f[d];
	}
}

/*
========================
idLCP_Symmetric::ChangeForce

Modifies this->f and uses this->delta_f.
========================
*/
ID_INLINE void idLCP_Symmetric::ChangeForce( int d, float step )
{
	// only the clamped variables and current variable have a force delta unequal zero
	MultiplyAdd( f.ToFloatPtr(), step, delta_f.ToFloatPtr(), numClamped );
	f[d] += step * delta_f[d];
}

/*
========================
idLCP_Symmetric::ChangeAccel

Modifies this->a and uses this->delta_a.
========================
*/
ID_INLINE void idLCP_Symmetric::ChangeAccel( int d, float step )
{
	// only the not clamped variables, including the current variable, can have an acceleration unequal zero
	MultiplyAdd( a.ToFloatPtr() + numClamped, step, delta_a.ToFloatPtr() + numClamped, d - numClamped + 1 );
}

/*
========================
idLCP_Symmetric::Solve
========================
*/
bool idLCP_Symmetric::Solve( const idMatX& o_m, idVecX& o_x, const idVecX& o_b, const idVecX& o_lo, const idVecX& o_hi, const int* o_boxIndex )
{

	// true when the matrix rows are 16 byte padded
	padded = ( ( o_m.GetNumRows() + 3 )&~3 ) == o_m.GetNumColumns();
	
	assert( padded || o_m.GetNumRows() == o_m.GetNumColumns() );
	assert( o_x.GetSize() == o_m.GetNumRows() );
	assert( o_b.GetSize() == o_m.GetNumRows() );
	assert( o_lo.GetSize() == o_m.GetNumRows() );
	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 = ( int* )_alloca16( o_x.GetSize() * sizeof( int ) );
		memcpy( boxIndex, o_boxIndex, o_x.GetSize() * sizeof( int ) );
	}
	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(), const_cast< float* >( o_m[0] ) );
	f.Zero();
	a.Zero();
	b = o_b;
	lo = o_lo;
	hi = o_hi;
	
	// pointers to the rows of m
	rowPtrs = ( float** ) _alloca16( m.GetNumRows() * sizeof( float* ) );
	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 = ( int* ) _alloca16( m.GetNumRows() * sizeof( int ) );
	
	// index to keep track of the permutation
	permuted = ( int* ) _alloca16( m.GetNumRows() * sizeof( int ) );
	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] == -idMath::INFINITY && hi[i] == idMath::INFINITY )
		{
			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 )
			{
				boxStartIndex--;
				if( boxStartIndex != i )
				{
					Swap( boxStartIndex, i );
				}
			}
		}
	}
	
	// sub matrix for factorization
	clamped.SetDataCacheLines( m.GetNumRows(), m.GetNumColumns(), MATX_ALLOCA_CACHE_LINES( m.GetNumRows() * m.GetNumColumns() ), true );
	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 )
	{
	
		// factor and solve for unbounded variables
		if( !FactorClamped() )
		{
			idLib::Printf( "idLCP_Symmetric::Solve: unbounded factorization failed\n" );
			return false;
		}
		SolveClamped( f, b.ToFloatPtr() );
		
		// if there are no bounded variables we are done
		if( numUnbounded == m.GetNumRows() )
		{
			o_x = f;	// the vector is not permuted
			return true;
		}
	}
	
	int numIgnored = 0;
	
	// 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
	idStr failed;
	for( int 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] != -idMath::INFINITY )
				{
					lo[j] = - idMath::Fabs( lo[j] * s );
				}
				if( hi[j] != idMath::INFINITY )
				{
					hi[j] = idMath::Fabs( hi[j] * s );
				}
			}
		}
		
		// calculate acceleration for current variable
		float dot = BigDotProduct( rowPtrs[i], f.ToFloatPtr(), 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 = 0;
		for( ; 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 );
			
			float maxStep;
			int limit;
			int limitSide;
			
			// maximum step we can take
			GetMaxStep( f.ToFloatPtr(), a.ToFloatPtr(), delta_f.ToFloatPtr(), delta_a.ToFloatPtr(),
						lo.ToFloatPtr(), hi.ToFloatPtr(), side, numUnbounded, numClamped,
						i, dir, maxStep, limit, limitSide );
						
			if( maxStep <= 0.0f )
			{
#ifdef IGNORE_UNSATISFIABLE_VARIABLES
				// ignore the current variable completely
				lo[i] = hi[i] = 0.0f;
				f[i] = 0.0f;
				side[i] = -1;
				numIgnored++;
#else
				failed.Format( "invalid step size %.4f", maxStep );
				for( int j = i; j < m.GetNumRows(); j++ )
				{
					f[j] = 0.0f;
				}
				numIgnored = m.GetNumRows() - i;
#endif
				break;
			}
			
			// change force
			ChangeForce( i, maxStep );
			
			// change acceleration
			ChangeAccel( i, maxStep );
			
			// clamp/unclamp the variable that limited this step
			side[limit] = limitSide;
			if( limitSide == 0 )
			{
				a[limit] = 0.0f;
				AddClamped( limit, ( limit == i ) );
			}
			else if( limitSide == -1 )
			{
				f[limit] = lo[limit];
				if( limit != i )
				{
					RemoveClamped( limit );
				}
			}
			else if( limitSide == 1 )
			{
				f[limit] = hi[limit];
				if( limit != i )
				{
					RemoveClamped( limit );
				}
			}
			
			// if the current variable limited the step we can continue with the next variable
			if( limit == i )
			{
				break;
			}
		}
		
		if( n >= maxIterations )
		{
			failed.Format( "max iterations %d", maxIterations );
			break;
		}
		
		if( failed.Length() )
		{
			break;
		}
	}
	
#ifdef IGNORE_UNSATISFIABLE_VARIABLES
	if( numIgnored )
	{
		if( lcp_showFailures.GetBool() )
		{
			idLib::Printf( "idLCP_Symmetric::Solve: %d of %d bounded variables ignored\n", numIgnored, m.GetNumRows() - numUnbounded );
		}
	}
#endif
	
	// if failed clear remaining forces
	if( failed.Length() )
	{
		if( lcp_showFailures.GetBool() )
		{
			idLib::Printf( "idLCP_Symmetric::Solve: %s (%d of %d bounded variables ignored)\n", failed.c_str(), numIgnored, m.GetNumRows() - numUnbounded );
		}
	}
	
#if defined(_DEBUG) && 0
	if( failed.Length() )
	{
		// test whether or not the solution satisfies the complementarity conditions
		for( int i = 0; i < m.GetNumRows(); i++ )
		{
			a[i] = -b[i];
			for( 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];
	}
	
	return true;
}

/*
================================================================================================

	idLCP

================================================================================================
*/

/*
========================
idLCP::AllocSquare
========================
*/
idLCP* idLCP::AllocSquare()
{
	idLCP* lcp = new idLCP_Square;
	lcp->SetMaxIterations( 32 );
	return lcp;
}

/*
========================
idLCP::AllocSymmetric
========================
*/
idLCP* idLCP::AllocSymmetric()
{
	idLCP* lcp = new idLCP_Symmetric;
	lcp->SetMaxIterations( 32 );
	return lcp;
}

/*
========================
idLCP::~idLCP
========================
*/
idLCP::~idLCP()
{
}

/*
========================
idLCP::SetMaxIterations
========================
*/
void idLCP::SetMaxIterations( int max )
{
	maxIterations = max;
}

/*
========================
idLCP::GetMaxIterations
========================
*/
int idLCP::GetMaxIterations()
{
	return maxIterations;
}

/*
========================
idLCP::Test_f
========================
*/
void idLCP::Test_f( const idCmdArgs& args )
{
#ifdef ENABLE_TEST_CODE
	DotProduct_Test();
	LowerTriangularSolve_Test();
	LowerTriangularSolveTranspose_Test();
	LDLT_Factor_Test();
#endif
}