mirror of
https://github.com/id-Software/DOOM-3-BFG.git
synced 2024-12-11 21:21:27 +00:00
3433 lines
90 KiB
C++
3433 lines
90 KiB
C++
/*
|
|
===========================================================================
|
|
|
|
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
|
|
}
|