//-----------------------------------------------------------------------------
// computeDualView
//-----------------------------------------------------------------------------
void computeDualView(int a, int b, int c, int d, float tx, float ty, float tz) {
for (int i=0; i<NSAMPLES; i++)
for (int j=0; j<NSAMPLES; j++)
intersection_count[i][j] = 0;
for (int i=0; i<NLINES; i++)
if (displayL[i])
computeDual(lines[i][0], lines[i][1], lines[i][2], lines[i][3], a, b, c, d);
if (displayV)
computeDual(v_x[0], v_y[0], v_x[1], v_y[1], a, b, c, d);
displayDual2();
displayBox();
}
double RigidBodies3DSobogusInterface::solve( Eigen::VectorXd& r, Eigen::VectorXd& v, unsigned& num_iterations, const unsigned max_threads, const double& tol, const unsigned max_iters, const unsigned eval_every, const bool use_infinity_norm )
{
assert( m_primal );
assert( r.size() == 3 * m_primal->H.rowsOfBlocks() );
assert( v.size() == m_primal->H.cols() );
// If dual has not been computed yet
if( !m_dual )
{
computeDual();
}
// r to local coords
Eigen::VectorXd r_loc{ m_primal->E.transpose() * r };
// Setup GS parameters
bogus::DualFrictionProblem<3u>::GaussSeidelType gs;
if( tol != 0.0 ) { gs.setTol( tol ); }
if( max_iters != 0 ) { gs.setMaxIters( max_iters ); }
assert( eval_every > 0 ); assert( eval_every <= max_iters );
gs.setEvalEvery( eval_every );
gs.setMaxThreads( max_threads );
gs.setAutoRegularization( 0.0 );
gs.useInfinityNorm( use_infinity_norm );
// Compute coloring if multithreading
gs.coloring().update( max_threads > 1, m_dual->W );
m_dual->undoPermutation();
if( max_threads > 1 )
{
m_dual->applyPermutation( gs.coloring().permutation );
gs.coloring().resetPermutation();
}
m_dual->W.cacheTranspose();
const bool try_zero{ false };
const double res{ m_dual->solveWith( gs, r_loc.data(), num_iterations, try_zero ) };
// Compute the outgoing velocity
v = m_primal->MInv * ( m_primal->H.transpose() * r_loc - Eigen::VectorXd::Map( m_primal->f, m_primal->H.cols() ) );
// Convert r to world coords
r = m_primal->E * r_loc;
return res;
}
double RigidBodies3DSobogusInterface::evalInfNormError( const Eigen::VectorXd& r )
{
if( !m_dual )
{
computeDual();
}
// Convert r to local coords
assert( m_primal != nullptr );
const Eigen::VectorXd r_loc{ m_primal->E.transpose() * r };
// Setup GS parameters
bogus::DualFrictionProblem<3u>::GaussSeidelType gs;
gs.useInfinityNorm( true );
assert( m_dual != nullptr );
gs.setMatrix( m_dual->W );
return m_dual->evalWith( gs, r_loc.data() );
}
