void ImplicitStepper::solveLinear()
{
computeRHS();
computeLHS();
Lhs().multiply( m_rhs, 1.0, m_futureVelocities );
m_strand.dynamics().getScriptingController()->fixLHSAndRHS( Lhs(), m_rhs, m_dt );
m_linearSolver.store( Lhs() );
}
VecXx& ImplicitStepper::impulse_rhs()
{
StrandDynamics& dynamics = m_strand.dynamics();
m_impulseRhs = m_futureVelocities;
dynamics.multiplyByMassMatrix( m_impulseRhs );
computeLHS();
dynamics.getScriptingController()->fixLHSAndRHS( Lhs(), m_impulseRhs, m_dt );
m_linearSolver.store( Lhs() );
return m_impulseRhs;
}
void ImplicitStepper::solveNonLinear()
{
StrandDynamics& dynamics = m_strand.dynamics() ;
Scalar minErr = 1.e99, prevErr = 1.e99;
m_newtonIter = 0;
JacobianMatrixType bestLHS;
VecXx bestRhs, prevRhs;
Scalar alpha = 0.5; // Current step length
const Scalar minAlpha = 0.1; // Minimum step length
bool foundOneSPD = false;
m_strand.requireExactJacobian( false );
// Newton loop -- try to zero-out m_rhs
for( m_newtonIter = 0; m_newtonIter < m_params.m_maxNewtonIterations; ++m_newtonIter )
{
dynamics.setDisplacements( m_dt * m_futureVelocities );
prevRhs = m_rhs;
computeRHS();
if( m_newtonIter )
{
VecXx residual = m_rhs;
dynamics.getScriptingController()->fixRHS( residual );
const Scalar err = residual.squaredNorm() / residual.size();
if( err < minErr || ( !foundOneSPD && !m_linearSolver.notSPD() ) )
{
foundOneSPD = !m_linearSolver.notSPD();
minErr = err;
if( isSmall( err ) || ( m_newtonIter > 3 && minErr < 1.e-6 ) )
{
m_rhs = prevRhs;
break;
}
bestLHS = Lhs();
bestRhs = prevRhs;
}
// Decrease or increase the step length based on current convergence
if( err < prevErr ){
alpha = std::min( 1.0, 1.5 * alpha );
}
else{
alpha = std::max( minAlpha, 0.5 * alpha );
}
prevErr = err;
}
computeLHS();
m_rhs = m_rhs * alpha;
Lhs().multiply( m_rhs, 1., m_futureVelocities );
dynamics.getScriptingController()->fixLHSAndRHS( Lhs(), m_rhs, m_dt );
m_linearSolver.store( Lhs() );
m_notSPD = m_linearSolver.notSPD();
m_linearSolver.solve( m_futureVelocities, m_rhs );
}
// If the non-linear solve failed, returns to the the least problematic step
if( m_newtonIter == m_params.m_maxNewtonIterations )
{
m_rhs = bestRhs;
Lhs() = bestLHS;
m_linearSolver.store( Lhs() );
m_linearSolver.solve( m_futureVelocities, rhs() );
m_notSPD = m_linearSolver.notSPD();
}
m_usedNonlinearSolver = true;
}
static void AssignmentStatement(void)
{
Int32 type,count;
SymPtr sym,clas_init=NULL;
SymPtr parents[16];
sym = Lhs();
if( sym==NULL )
{
return;
}
CheckClassMember(sym);
SkipToken(tEQUALS);
if(sym->flags&SYM_DEFINED)
{
compileError("cannot reassign constant");
return;
}
else if(sym->flags&SYM_CONSTANT)
{
sym->flags |= SYM_DEFINED;
}
/* CheckClassMember(sym); */
new_class=NULL;
type=Expr();
sym->object.type = type;
switch(sym->kind)
{
case LOCAL_KIND:
vm_genI(op_setlocal,sym->num);
break;
case GLOBAL_KIND:
vm_genI(op_setglobal,sym->num);
break;
case FIELD_KIND:
vm_genI(op_setfield,sym->num);
break;
default:
compileError("invalid assignment");
break;
}
if( type==CLASS_TYPE && new_class != NULL )
{
sym->clas = new_class;
sym->super = new_class->super;
sym->locals = new_class->locals;
/* First build a list of all super classes */
count=0;
clas_init=sym;
while( clas_init!=NULL )
{
parents[count++] = clas_init;
clas_init = clas_init->super;
}
/* Now call all constructors in reverse order */
count--;
while( count>=0 )
{
clas_init = SymFindLocal(NEW,parents[count]);
FunctionCall(clas_init,sym);
vm_gen0(op_pop);
count--;
}
/* call user defined constructor */
clas_init=SymFindLocal("init",sym);
if( clas_init!=NULL )
{
if( (Token==tLPAREN && clas_init->nargs!=0) ||
(Token!=tLPAREN && clas_init->nargs==0) )
{
FunctionCall(clas_init,sym);
vm_gen0(op_pop);
}
}
}
}
