Page MenuHome
Differential D1739 Diff 5899 extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp

Changeset View

Standalone View

View Options

extern/bullet2/src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp

Show First 20 Lines • Show All 815 Lines • ▼ Show 20 Lines
* this processes blocks of 4. * this processes blocks of 4.
*/ */
void btSolveL1T (const btScalar *L, btScalar *B, int n, int lskip1) void btSolveL1T (const btScalar *L, btScalar *B, int n, int lskip1)
{ {
/* declare variables - Z matrix, p and q vectors, etc */ /* declare variables - Z matrix, p and q vectors, etc */
btScalar Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex; btScalar Z11,m11,Z21,m21,Z31,m31,Z41,m41,p1,q1,p2,p3,p4,*ex;
const btScalar *ell; const btScalar *ell;
int lskip2,lskip3,i,j; int lskip2,i,j;
// int lskip3;
/* special handling for L and B because we're solving L1 *transpose* */ /* special handling for L and B because we're solving L1 *transpose* */
L = L + (n-1)*(lskip1+1); L = L + (n-1)*(lskip1+1);
B = B + n-1; B = B + n-1;
lskip1 = -lskip1; lskip1 = -lskip1;
/* compute lskip values */ /* compute lskip values */
lskip2 = 2*lskip1; lskip2 = 2*lskip1;
lskip3 = 3*lskip1; //lskip3 = 3*lskip1;
/* compute all 4 x 1 blocks of X */ /* compute all 4 x 1 blocks of X */
for (i=0; i <= n-4; i+=4) { for (i=0; i <= n-4; i+=4) {
/* compute all 4 x 1 block of X, from rows i..i+4-1 */ /* compute all 4 x 1 block of X, from rows i..i+4-1 */
/* set the Z matrix to 0 */ /* set the Z matrix to 0 */
Z11=0; Z11=0;
Z21=0; Z21=0;
Z31=0; Z31=0;
Z41=0; Z41=0;
▲ Show 20 Lines • Show All 354 Lines • ▼ Show 20 Linesstruct btLCP
BTATYPE const m_A; // A rows BTATYPE const m_A; // A rows
btScalar *const m_x, * const m_b, *const m_w, *const m_lo,* const m_hi; // permuted LCP problem data btScalar *const m_x, * const m_b, *const m_w, *const m_lo,* const m_hi; // permuted LCP problem data
btScalar *const m_L, *const m_d; // L*D*L' factorization of set C btScalar *const m_L, *const m_d; // L*D*L' factorization of set C
btScalar *const m_Dell, *const m_ell, *const m_tmp; btScalar *const m_Dell, *const m_ell, *const m_tmp;
bool *const m_state; bool *const m_state;
int *const m_findex, *const m_p, *const m_C; int *const m_findex, *const m_p, *const m_C;
btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w, btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
btScalar *_lo, btScalar *_hi, btScalar *_L, btScalar *_d, btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
btScalar *_Dell, btScalar *_ell, btScalar *_tmp, btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
bool *_state, int *_findex, int *_p, int *_C, btScalar **Arows); bool *_state, int *_findex, int *p, int *c, btScalar **Arows);
int getNub() const { return m_nub; } int getNub() const { return m_nub; }
void transfer_i_to_C (int i); void transfer_i_to_C (int i);
void transfer_i_to_N (int i) { m_nN++; } // because we can assume C and N span 1:i-1 void transfer_i_to_N (int i) { m_nN++; } // because we can assume C and N span 1:i-1
void transfer_i_from_N_to_C (int i); void transfer_i_from_N_to_C (int i);
void transfer_i_from_C_to_N (int i, btAlignedObjectArray<btScalar>& scratch); void transfer_i_from_C_to_N (int i, btAlignedObjectArray<btScalar>& scratch);
int numC() const { return m_nC; } int numC() const { return m_nC; }
int numN() const { return m_nN; } int numN() const { return m_nN; }
int indexC (int i) const { return i; } int indexC (int i) const { return i; }
int indexN (int i) const { return i+m_nC; } int indexN (int i) const { return i+m_nC; }
btScalar Aii (int i) const { return BTAROW(i)[i]; } btScalar Aii (int i) const { return BTAROW(i)[i]; }
btScalar AiC_times_qC (int i, btScalar *q) const { return btLargeDot (BTAROW(i), q, m_nC); } btScalar AiC_times_qC (int i, btScalar *q) const { return btLargeDot (BTAROW(i), q, m_nC); }
btScalar AiN_times_qN (int i, btScalar *q) const { return btLargeDot (BTAROW(i)+m_nC, q+m_nC, m_nN); } btScalar AiN_times_qN (int i, btScalar *q) const { return btLargeDot (BTAROW(i)+m_nC, q+m_nC, m_nN); }
void pN_equals_ANC_times_qC (btScalar *p, btScalar *q); void pN_equals_ANC_times_qC (btScalar *p, btScalar *q);
void pN_plusequals_ANi (btScalar *p, int i, int sign=1); void pN_plusequals_ANi (btScalar *p, int i, int sign=1);
void pC_plusequals_s_times_qC (btScalar *p, btScalar s, btScalar *q); void pC_plusequals_s_times_qC (btScalar *p, btScalar s, btScalar *q);
void pN_plusequals_s_times_qN (btScalar *p, btScalar s, btScalar *q); void pN_plusequals_s_times_qN (btScalar *p, btScalar s, btScalar *q);
void solve1 (btScalar *a, int i, int dir=1, int only_transfer=0); void solve1 (btScalar *a, int i, int dir=1, int only_transfer=0);
void unpermute(); void unpermute();
}; };
btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w, btLCP::btLCP (int _n, int _nskip, int _nub, btScalar *_Adata, btScalar *_x, btScalar *_b, btScalar *_w,
btScalar *_lo, btScalar *_hi, btScalar *_L, btScalar *_d, btScalar *_lo, btScalar *_hi, btScalar *l, btScalar *_d,
btScalar *_Dell, btScalar *_ell, btScalar *_tmp, btScalar *_Dell, btScalar *_ell, btScalar *_tmp,
bool *_state, int *_findex, int *_p, int *_C, btScalar **Arows): bool *_state, int *_findex, int *p, int *c, btScalar **Arows):
m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0), m_n(_n), m_nskip(_nskip), m_nub(_nub), m_nC(0), m_nN(0),
# ifdef BTROWPTRS # ifdef BTROWPTRS
m_A(Arows), m_A(Arows),
#else #else
m_A(_Adata), m_A(_Adata),
#endif #endif
m_x(_x), m_b(_b), m_w(_w), m_lo(_lo), m_hi(_hi), m_x(_x), m_b(_b), m_w(_w), m_lo(_lo), m_hi(_hi),
m_L(_L), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp), m_L(l), m_d(_d), m_Dell(_Dell), m_ell(_ell), m_tmp(_tmp),
m_state(_state), m_findex(_findex), m_p(_p), m_C(_C) m_state(_state), m_findex(_findex), m_p(p), m_C(c)
{ {
{ {
btSetZero (m_x,m_n); btSetZero (m_x,m_n);
} }
{ {
# ifdef BTROWPTRS # ifdef BTROWPTRS
// make matrix row pointers // make matrix row pointers
▲ Show 20 Lines • Show All 833 Lines • Show Last 20 Lines