public void ShurBvectorCompute(ref ChSystemDescriptor sysd)
{
// ***TO DO*** move the following thirty lines in a short function ChSystemDescriptor::ShurBvectorCompute() ?
// Compute the b_shur vector in the Shur complement equation N*l = b_shur
// with
// N_shur = D'* (M^-1) * D
// b_shur = - c + D'*(M^-1)*k = b_i + D'*(M^-1)*k
// but flipping the sign of lambdas, b_shur = - b_i - D'*(M^-1)*k
// Do this in three steps:
// Put (M^-1)*k in q sparse vector of each variable..
for (int iv = 0; iv < sysd.GetVariablesList().Count; iv++)
{
if (sysd.GetVariablesList()[iv].IsActive())
{
sysd.GetVariablesList()[iv].Compute_invMb_v(sysd.GetVariablesList()[iv].Get_qb().matrix,
sysd.GetVariablesList()[iv].Get_fb().matrix); // q = [M]'*fb
}
}
// ...and now do b_shur = - D'*q = - D'*(M^-1)*k ..
int s_i = 0;
for (int ic = 0; ic < sysd.GetConstraintsList().Count; ic++)
{
if (sysd.GetConstraintsList()[ic].IsActive())
{
r.matrix[s_i, 0] = sysd.GetConstraintsList()[ic].Compute_Cq_q();
++s_i;
}
}
// ..and finally do b_shur = b_shur - c
sysd.BuildBiVector(tmp.matrix); // b_i = -c = phi/h
r.matrix.MatrInc(tmp.matrix);
}
// Override/implement system functions of ChPhysicsItem
// (to assemble/manage data for system solver)
public override void InjectConstraints(ref ChSystemDescriptor mdescriptor)
{
// if (!IsActive())
// return;
mdescriptor.InsertConstraint(constraint);
}
/// Inject Jacobian blocks into the system descriptor.
/// Tell to a system descriptor that there are item(s) of type ChKblock in this object
/// (for further passing it to a solver)
public override void ContInjectKRMmatrices(ref ChSystemDescriptor mdescriptor)
{
if (m_Jac != null)
{
mdescriptor.InsertKblock(m_Jac.m_KRM);
}
}
// Use this for initialization
public override void Start()
{
out_step = 2000 * step;
base.Start();
if (init_sys)
{
m_use_mat_props = true;
m_contact_model = contactForceModel;
m_adhesion_model = adhesionForceModel;
m_tdispl_model = tangentialDisplacementModel;
m_stiff_contact = false;
descriptor = new ChSystemDescriptor();
descriptor.SetNumThreads(parallel_thread_number);
solver_speed = new ChSolverSMC();
solver_stab = new ChSolverSMC();
collision_system = new collision.ChCollisionSystemBullet((uint)max_objects, scene_size);
// For default SMC there is no need to create contacts 'in advance'
// when models are closer than the safety envelope, so set default envelope to 0
collision.ChCollisionModel.SetDefaultSuggestedEnvelope(0);
contact_container = gameObject.AddComponent <ChContactContainerSMC>();
contact_container.SetSystem(this);
m_minSlipVelocity = 1e-4;
m_characteristicVelocity = 1;
}
}
public override void Start()
{
base.Start();
if (init_sys)
{
SetSolverWarmStarting(true);
// Set default contact container
contact_container = gameObject.AddComponent <ChContactContainerNSC>() as ChContactContainerNSC;//new ChContactContainerNSC();
contact_container.SetSystem(this);
// Set default collision engine
collision_system = new collision.ChCollisionSystemBullet((uint)max_objects, scene_size);
// Set the system descriptor
descriptor = new ChSystemDescriptor();
descriptor.SetNumThreads(parallel_thread_number);
// Set default solver
SetSolverType(solverType);
// Set default collision envelope and margin.
collision.ChCollisionModel.SetDefaultSuggestedEnvelope(0.03);
// collision.ChCollisionModel.SetDefaultSuggestedMargin(0.01);
}
}
//
// SOLVER INTERFACE
//
public override void InjectConstraints(ref ChSystemDescriptor mdescriptor)
{
if (!IsActive())
{
return;
}
mdescriptor.InsertConstraint(Cx);
}
public override void InjectConstraints(ref ChSystemDescriptor mdescriptor)
{
// base behaviour too
base.InjectConstraints(ref mdescriptor);
mdescriptor.InsertConstraint(Rx);
mdescriptor.InsertConstraint(Ru);
mdescriptor.InsertConstraint(Rv);
}
//
// SOLVER INTERFACE
//
public override void InjectConstraints(ref ChSystemDescriptor descriptor)
{
if (!IsActive())
{
return;
}
descriptor.InsertConstraint(m_cnstr_dist);
descriptor.InsertConstraint(m_cnstr_dot);
}
public override void InjectVariables(ref ChSystemDescriptor mdescriptor)
{
// First, inherit to parent class
base.InjectVariables(ref mdescriptor);
if (eng_mode == eCh_eng_mode.ENG_MODE_TO_POWERTRAIN_SHAFT)
{
innershaft1.InjectVariables(ref mdescriptor);
innershaft2.InjectVariables(ref mdescriptor);
}
}
public void _InjectKRMmatrices <Tcont>(List <Tcont> contactlist, ref ChSystemDescriptor mdescriptor)
where Tcont : ChContactSMC <ChContactable_1vars <IntInterface.Six>, ChContactable_1vars <IntInterface.Six> >
{
List <Tcont> .Enumerator itercontact = contactlist.GetEnumerator();
//itercontact.MoveNext();
while (itercontact.Current != contactlist.LastOrDefault())
{
itercontact.MoveNext();
((IEnumerator <ChContactSMC <ChContactable_1vars <IntInterface.Six>, ChContactable_1vars <IntInterface.Six> > >)itercontact).Current.ContInjectKRMmatrices(ref mdescriptor);
}
}
public override void InjectKRMmatrices(ref ChSystemDescriptor mdescriptor)
{
// _InjectKRMmatrices(contactlist_3_3, mdescriptor);
// _InjectKRMmatrices(contactlist_6_3, mdescriptor);
_InjectKRMmatrices(contactlist_6_6, ref mdescriptor);
/*_InjectKRMmatrices(contactlist_333_3, mdescriptor);
* _InjectKRMmatrices(contactlist_333_6, mdescriptor);
* _InjectKRMmatrices(contactlist_333_333, mdescriptor);
* _InjectKRMmatrices(contactlist_666_3, mdescriptor);
* _InjectKRMmatrices(contactlist_666_6, mdescriptor);
* _InjectKRMmatrices(contactlist_666_333, mdescriptor);
* _InjectKRMmatrices(contactlist_666_666, mdescriptor);*/
}
//
// SOLVER INTERFACE
//
public override void InjectConstraints(ref ChSystemDescriptor mdescriptor)
{
if (!this.IsActive())
{
return;
}
for (int i = 0; i < mask.nconstr; i++)
{
if (mask.Constr_N(i).IsActive())
{
mdescriptor.InsertConstraint(mask.Constr_N(i));
}
}
}
public override void InjectKRMmatrices(ref ChSystemDescriptor mdescriptor)
{
for (int i = 0; i < bodylist.Count; i++)
{
bodylist[i].InjectKRMmatrices(ref mdescriptor);
}
for (int i = 0; i < linklist.Count; i++)
{
linklist[i].InjectKRMmatrices(ref mdescriptor);
}
for (int i = 0; i < otherphysicslist.Count; i++)
{
otherphysicslist[i].InjectKRMmatrices(ref mdescriptor);
}
}
public double Res4(ref ChSystemDescriptor sysd)
{
// Project the gradient (for rollback strategy)
// g_proj = (l-project_orthogonal(l - gdiff*g, fric))/gdiff;
double gdiff = 1.0 / Math.Pow(nc, 2.0);
sysd.ShurComplementProduct(tmp.matrix, gammaNew.matrix);
tmp.matrix.MatrInc(r.matrix);
tmp.matrix.MatrScale(-gdiff);
tmp.matrix.MatrInc(gammaNew.matrix);
sysd.ConstraintsProject(tmp.matrix);
tmp.matrix.MatrSub(gammaNew.matrix, tmp.matrix);
tmp.matrix.MatrScale(1.0 / gdiff);
return(tmp.matrix.NormTwo());
}
public override double Solve(ref ChSystemDescriptor sysd //< system description with constraints and variables
)
{
return(0);
}
public override void InjectVariables(ref ChSystemDescriptor mdescriptor)
{
variable.SetDisabled(!IsActive());
mdescriptor.InsertVariables(variable);
}
/// Performs the solution of the problem.
/// \return the maximum constraint violation after termination.
public override double Solve(ref ChSystemDescriptor sysd //< system description with constraints and variables
)
{
List <ChConstraint> mconstraints = sysd.GetConstraintsList();
List <ChVariables> mvariables = sysd.GetVariablesList();
// 1) Update auxiliary data in all constraints before starting,
// that is: g_i=[Cq_i]*[invM_i]*[Cq_i]' and [Eq_i]=[invM_i]*[Cq_i]'
for (int ic = 0; ic < mconstraints.Count; ic++)
{
mconstraints[ic].Update_auxiliary();
}
// 2) Compute, for all items with variables, the initial guess for
// still unconstrained system:
for (int iv = 0; iv < mvariables.Count; iv++)
{
if (mvariables[iv].IsActive())
{
mvariables[iv].Compute_invMb_v(mvariables[iv].Get_qb().matrix, mvariables[iv].Get_fb().matrix); // q = [M]'*fb
}
}
// 3) For all items with variables, add the effect of initial (guessed)
// lagrangian reactions of constraints, if a warm start is desired.
// Otherwise, if no warm start, simply resets initial lagrangians to zero.
if (warm_start)
{
for (int ic = 0; ic < mconstraints.Count; ic++)
{
if (mconstraints[ic].IsActive())
{
mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i());
}
}
}
else
{
for (int ic = 0; ic < mconstraints.Count; ic++)
{
mconstraints[ic].Set_l_i(0.0);
}
}
// 4) Perform the iteration loops (if there are any constraints)
double maxviolation = 0.0;
double maxdeltalambda = 0.0;
if (mconstraints.Count == 0)
{
return(maxviolation);
}
for (int iter = 0; iter < max_iterations; iter++)
{
maxviolation = 0;
maxdeltalambda = 0;
// The iteration on all constraints
for (int ic = 0; ic < mconstraints.Count; ic++)
{
// skip computations if constraint not active.
if (mconstraints[ic].IsActive())
{
// compute residual c_i = [Cq_i]*q + b_i
double mresidual = mconstraints[ic].Compute_Cq_q() + mconstraints[ic].Get_b_i();
// true constraint violation may be different from 'mresidual' (ex:clamped if unilateral)
double candidate_violation = Math.Abs(mconstraints[ic].Violation(mresidual));
// compute: delta_lambda = -(omega/g_i) * ([Cq_i]*q + b_i )
double deltal = (omega / mconstraints[ic].Get_g_i()) * (-mresidual);
// update: lambda += delta_lambda;
double old_lambda = mconstraints[ic].Get_l_i();
mconstraints[ic].Set_l_i(old_lambda + deltal);
// If new lagrangian multiplier does not satisfy inequalities, project
// it into an admissible orthant (or, in general, onto an admissible set)
mconstraints[ic].Project();
// After projection, the lambda may have changed a bit..
double new_lambda = mconstraints[ic].Get_l_i();
// Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old
if (this.shlambda != 1.0)
{
new_lambda = shlambda * new_lambda + (1.0 - shlambda) * old_lambda;
mconstraints[ic].Set_l_i(new_lambda);
}
double true_delta = new_lambda - old_lambda;
// For all items with variables, add the effect of incremented
// (and projected) lagrangian reactions:
mconstraints[ic].Increment_q(true_delta);
if (this.record_violation_history)
{
maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta));
}
maxviolation = ChMaths.ChMax(maxviolation, Math.Abs(candidate_violation));
} // end IsActive()
} // end loop on constraints
// For recording into violation history, if debugging
if (this.record_violation_history)
{
AtIterationEnd(maxviolation, maxdeltalambda, iter);
}
// Terminate the loop if violation in constraints has been successfully limited.
if (maxviolation < tolerance)
{
break;
}
} // end iteration loop
return(maxviolation);
}
/// Tell to a system descriptor that there are variables of type
/// ChVariables in this object (for further passing it to a solver)
/// Basically does nothing, but maybe that inherited classes may specialize this.
public virtual void InjectVariables(ref ChSystemDescriptor mdescriptor)
{
}
/// Performs the solution of the problem.
public override double Solve(ref ChSystemDescriptor sysd)
{
bool verbose = false;
List <ChConstraint> mconstraints = sysd.GetConstraintsList();
// List<ChVariables> mvariables;// = sysd.GetVariablesList();
// if (verbose)
// Debug.Log("Number of constraints: " + mconstraints.Count + "nNumber of variables :" + mvariables.Count);
// Update auxiliary data in all constraints before starting,
// that is: g_i=[Cq_i]*[invM_i]*[Cq_i]' and [Eq_i]=[invM_i]*[Cq_i]'
for (int ic = 0; ic < mconstraints.Count; ic++)
{
mconstraints[ic].Update_auxiliary();
}
double L, t;
double theta;
double thetaNew;
double Beta;
double obj1, obj2;
nc = sysd.CountActiveConstraints();
gamma_hat.Resize(nc, 1);
gammaNew.Resize(nc, 1);
g.Resize(nc, 1);
y.Resize(nc, 1);
gamma.Resize(nc, 1);
yNew.Resize(nc, 1);
r.Resize(nc, 1);
tmp.Resize(nc, 1);
residual = 10e30;
Beta = 0.0;
obj1 = 0.0;
obj2 = 0.0;
// Compute the b_shur vector in the Shur complement equation N*l = b_shur
ShurBvectorCompute(ref sysd);
// Optimization: backup the q sparse data computed above,
// because (M^-1)*k will be needed at the end when computing primals.
ChMatrixDynamic <double> Minvk = new ChMatrixDynamic <double>();
sysd.FromVariablesToVector(Minvk.matrix, true);
// (1) gamma_0 = zeros(nc,1)
if (warm_start)
{
for (int ic = 0; ic < mconstraints.Count; ic++)
{
if (mconstraints[ic].IsActive())
{
mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i());
}
}
}
else
{
for (int ic = 0; ic < mconstraints.Count; ic++)
{
mconstraints[ic].Set_l_i(0.0);
}
}
sysd.FromConstraintsToVector(gamma.matrix);
// (2) gamma_hat_0 = ones(nc,1)
gamma_hat.matrix.FillElem(1);
// (3) y_0 = gamma_0
y.matrix.CopyFromMatrix(gamma.matrix);
// (4) theta_0 = 1
theta = 1.0;
// (5) L_k = norm(N * (gamma_0 - gamma_hat_0)) / norm(gamma_0 - gamma_hat_0)
tmp.matrix.MatrSub(gamma.matrix, gamma_hat.matrix);
L = tmp.matrix.NormTwo();
sysd.ShurComplementProduct(yNew.matrix, tmp.matrix);
L = yNew.matrix.NormTwo() / L;
yNew.matrix.FillElem(0); // reset yNew to be all zeros
// (6) t_k = 1 / L_k
t = 1.0 / L;
// (7) for k := 0 to N_max
for (tot_iterations = 0; tot_iterations < max_iterations; tot_iterations++)
{
// (8) g = N * y_k - r
sysd.ShurComplementProduct(g.matrix, y.matrix);
g.matrix.MatrInc(r.matrix);
// (9) gamma_(k+1) = ProjectionOperator(y_k - t_k * g)
gammaNew.matrix.CopyFromMatrix(g.matrix);
gammaNew.matrix.MatrScale(-t);
gammaNew.matrix.MatrInc(y.matrix);
sysd.ConstraintsProject(gammaNew.matrix);
// (10) while 0.5 * gamma_(k+1)' * N * gamma_(k+1) - gamma_(k+1)' * r >= 0.5 * y_k' * N * y_k - y_k' * r + g' *
// (gamma_(k+1) - y_k) + 0.5 * L_k * norm(gamma_(k+1) - y_k)^2
sysd.ShurComplementProduct(tmp.matrix, gammaNew.matrix); // Here tmp is equal to N*gammaNew;
tmp.matrix.MatrScale(0.5);
tmp.matrix.MatrInc(r.matrix);
obj1 = tmp.matrix.MatrDot(gammaNew.matrix, tmp.matrix);
sysd.ShurComplementProduct(tmp.matrix, y.matrix); // Here tmp is equal to N*y;
tmp.matrix.MatrScale(0.5);
tmp.matrix.MatrInc(r.matrix);
obj2 = tmp.matrix.MatrDot(y.matrix, tmp.matrix);
tmp.matrix.MatrSub(gammaNew.matrix, y.matrix); // Here tmp is equal to gammaNew - y
obj2 = obj2 + tmp.matrix.MatrDot(tmp.matrix, g.matrix) + 0.5 * L * tmp.matrix.MatrDot(tmp.matrix, tmp.matrix);
while (obj1 >= obj2)
{
// (11) L_k = 2 * L_k
L = 2.0 * L;
// (12) t_k = 1 / L_k
t = 1.0 / L;
// (13) gamma_(k+1) = ProjectionOperator(y_k - t_k * g)
gammaNew.matrix.CopyFromMatrix(g.matrix);
gammaNew.matrix.MatrScale(-t);
gammaNew.matrix.MatrInc(y.matrix);
sysd.ConstraintsProject(gammaNew.matrix);
// Update the components of the while condition
sysd.ShurComplementProduct(tmp.matrix, gammaNew.matrix); // Here tmp is equal to N*gammaNew;
tmp.matrix.MatrScale(0.5);
tmp.matrix.MatrInc(r.matrix);
obj1 = tmp.matrix.MatrDot(gammaNew.matrix, tmp.matrix);
sysd.ShurComplementProduct(tmp.matrix, y.matrix); // Here tmp is equal to N*y;
tmp.matrix.MatrScale(0.5);
tmp.matrix.MatrInc(r.matrix);
obj2 = tmp.matrix.MatrDot(y.matrix, tmp.matrix);
tmp.matrix.MatrSub(gammaNew.matrix, y.matrix); // Here tmp is equal to gammaNew - y
obj2 = obj2 + tmp.matrix.MatrDot(tmp.matrix, g.matrix) + 0.5 * L * tmp.matrix.MatrDot(tmp.matrix, tmp.matrix);
// (14) endwhile
}
// (15) theta_(k+1) = (-theta_k^2 + theta_k * sqrt(theta_k^2 + 4)) / 2
thetaNew = (-Math.Pow(theta, 2.0) + theta * Math.Sqrt(Math.Pow(theta, 2.0) + 4.0)) / 2.0;
// (16) Beta_(k+1) = theta_k * (1 - theta_k) / (theta_k^2 + theta_(k+1))
Beta = theta * (1.0 - theta) / (Math.Pow(theta, 2) + thetaNew);
// (17) y_(k+1) = gamma_(k+1) + Beta_(k+1) * (gamma_(k+1) - gamma_k)
tmp.matrix.MatrSub(gammaNew.matrix, gamma.matrix); // Here tmp is equal to gammaNew - gamma;
tmp.matrix.MatrScale(Beta);
yNew.matrix.MatrAdd(gammaNew.matrix, tmp.matrix);
// (18) r = r(gamma_(k+1))
double res = Res4(ref sysd);
// (19) if r < epsilon_min
if (res < residual)
{
// (20) r_min = r
residual = res;
// (21) gamma_hat = gamma_(k+1)
gamma_hat.matrix.CopyFromMatrix(gammaNew.matrix);
// (22) endif
}
// (23) if r < Tau
if (residual < this.tolerance)
{
// (24) break
break;
// (25) endif
}
// (26) if g' * (gamma_(k+1) - gamma_k) > 0
tmp.matrix.MatrSub(gammaNew.matrix, gamma.matrix);
if (tmp.matrix.MatrDot(tmp.matrix, g.matrix) > 0)
{
// (27) y_(k+1) = gamma_(k+1)
yNew.matrix.CopyFromMatrix(gammaNew.matrix);
// (28) theta_(k+1) = 1
thetaNew = 1.0;
// (29) endif
}
// (30) L_k = 0.9 * L_k
L = 0.9 * L;
// (31) t_k = 1 / L_k
t = 1.0 / L;
// perform some tasks at the end of the iteration
/* if (this.record_violation_history)
* {
* tmp.MatrSub(gammaNew, gamma);
* AtIterationEnd(residual, tmp.NormInf(), tot_iterations);
* }*/
// Update iterates
theta = thetaNew;
gamma.matrix.CopyFromMatrix(gammaNew.matrix);
y.matrix.CopyFromMatrix(yNew.matrix);
// (32) endfor
}
// if (verbose)
// Debug.Log("Residual: " + residual + ", Iter: " + tot_iterations);
// (33) return Value at time step t_(l+1), gamma_(l+1) := gamma_hat
sysd.FromVectorToConstraints(gamma_hat.matrix);
// Resulting PRIMAL variables:
// compute the primal variables as v = (M^-1)(k + D*l)
// v = (M^-1)*k ... (by rewinding to the backup vector computed at the beginning)
sysd.FromVectorToVariables(Minvk.matrix); // PROBLEM slow!
// ... + (M^-1)*D*l (this increment and also stores 'qb' in the ChVariable items)
for (int ic = 0; ic < mconstraints.Count; ic++)
{
if (mconstraints[ic].IsActive())
{
mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i());
}
}
return(residual);
}
/// Performs the solution of the problem.
/// This function MUST be implemented in children classes, with specialized
/// methods such as iterative or direct solvers.
/// Returns true if it successfully solves the problem and false otherwise.
public abstract double Solve(ref ChSystemDescriptor sysd //< system description with constraints and variables
);
public override void InjectVariables(ref ChSystemDescriptor mdescriptor)
{
mdescriptor.InsertVariables(variable);
}
// Old...
public override void InjectConstraints(ref ChSystemDescriptor mdescriptor)
{
mdescriptor.InsertConstraint(constraint);
}
/// This function does the setup operations for the solver.
/// The purpose of this function is to prepare the solver for subsequent calls to the
/// solve function. This function is called only as frequently it is determined that
/// it is appropriate to perform the setup phase.
public virtual bool Setup(ref ChSystemDescriptor sysd //< system description with constraints and variables
)
{
return true;
}
/// Performs the solution of the problem.
/// \return the maximum constraint violation after termination.
public override double Solve(ref ChSystemDescriptor sysd //< system description with constraints and variables
)
{
List <ChConstraint> mconstraints = sysd.GetConstraintsList();
List <ChVariables> mvariables = sysd.GetVariablesList();
double maxviolation = 0.0;
double maxdeltalambda = 0.0;
int i_friction_comp = 0;
double[] old_lambda_friction = new double[3];
int nConstr = mconstraints.Count;
int nVars = mvariables.Count;
// 1) Update auxiliary data in all constraints before starting,
// that is: g_i=[Cq_i]*[invM_i]*[Cq_i]' and [Eq_i]=[invM_i]*[Cq_i]'
for (int ic = 0; ic < nConstr; ic++)
{
mconstraints[ic].Update_auxiliary();
}
// Average all g_i for the triplet of contact constraints n,u,v.
int j_friction_comp = 0;
double[] gi_values = new double[3];
for (int ic = 0; ic < nConstr; ic++)
{
if (mconstraints[ic].GetMode() == eChConstraintMode.CONSTRAINT_FRIC)
{
gi_values[j_friction_comp] = mconstraints[ic].Get_g_i();
j_friction_comp++;
if (j_friction_comp == 3)
{
double average_g_i = (gi_values[0] + gi_values[1] + gi_values[2]) / 3.0;
mconstraints[ic - 2].Set_g_i(average_g_i);
mconstraints[ic - 1].Set_g_i(average_g_i);
mconstraints[ic - 0].Set_g_i(average_g_i);
j_friction_comp = 0;
}
}
}
// 2) Compute, for all items with variables, the initial guess for
// still unconstrained system:
for (int iv = 0; iv < nVars; iv++)
{
if (mvariables[iv].IsActive())
{
mvariables[iv].Compute_invMb_v(mvariables[iv].Get_qb().matrix, mvariables[iv].Get_fb().matrix); // q = [M]'*fb
}
}
// 3) For all items with variables, add the effect of initial (guessed)
// lagrangian reactions of constraints, if a warm start is desired.
// Otherwise, if no warm start, simply resets initial lagrangians to zero.
if (warm_start)
{
for (int ic = 0; ic < nConstr; ic++)
{
if (mconstraints[ic].IsActive())
{
mconstraints[ic].Increment_q(mconstraints[ic].Get_l_i());
}
}
}
else
{
for (int ic = 0; ic < nConstr; ic++)
{
mconstraints[ic].Set_l_i(0.0);
}
}
// 4) Perform the iteration loops
for (int iter = 0; iter < max_iterations;)
{
//
// Forward sweep, for symmetric SOR
//
maxviolation = 0;
maxdeltalambda = 0;
i_friction_comp = 0;
int dummy = mconstraints.Count;
for (int ic = 0; ic < dummy; ic++)
{
// skip computations if constraint not active.
if (mconstraints[ic].IsActive())
{
// compute residual c_i = [Cq_i]*q + b_i + cfm_i*l_i
double mresidual = mconstraints[ic].Compute_Cq_q() + mconstraints[ic].Get_b_i() +
mconstraints[ic].Get_cfm_i() * mconstraints[ic].Get_l_i();
// true constraint violation may be different from 'mresidual' (ex:clamped if unilateral)
double candidate_violation = Math.Abs(mconstraints[ic].Violation(mresidual));
// compute: delta_lambda = -(omega/g_i) * ([Cq_i]*q + b_i + cfm_i*l_i )
double deltal = (omega / mconstraints[ic].Get_g_i()) * (-mresidual);
if (mconstraints[ic].GetMode() == eChConstraintMode.CONSTRAINT_FRIC)
{
candidate_violation = 0;
// update: lambda += delta_lambda;
old_lambda_friction[i_friction_comp] = mconstraints[ic].Get_l_i();
mconstraints[ic].Set_l_i(old_lambda_friction[i_friction_comp] + deltal);
i_friction_comp++;
if (i_friction_comp == 1)
{
candidate_violation = Math.Abs(ChMaths.ChMin(0.0, mresidual));
}
if (i_friction_comp == 3)
{
mconstraints[ic - 2].Project(); // the N normal component will take care of N,U,V
double new_lambda_0 = mconstraints[ic - 2].Get_l_i();
double new_lambda_1 = mconstraints[ic - 1].Get_l_i();
double new_lambda_2 = mconstraints[ic - 0].Get_l_i();
// Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old
if (this.shlambda != 1.0)
{
new_lambda_0 = shlambda * new_lambda_0 + (1.0 - shlambda) * old_lambda_friction[0];
new_lambda_1 = shlambda * new_lambda_1 + (1.0 - shlambda) * old_lambda_friction[1];
new_lambda_2 = shlambda * new_lambda_2 + (1.0 - shlambda) * old_lambda_friction[2];
mconstraints[ic - 2].Set_l_i(new_lambda_0);
mconstraints[ic - 1].Set_l_i(new_lambda_1);
mconstraints[ic - 0].Set_l_i(new_lambda_2);
}
double true_delta_0 = new_lambda_0 - old_lambda_friction[0];
double true_delta_1 = new_lambda_1 - old_lambda_friction[1];
double true_delta_2 = new_lambda_2 - old_lambda_friction[2];
mconstraints[ic - 2].Increment_q(true_delta_0);
mconstraints[ic - 1].Increment_q(true_delta_1);
mconstraints[ic - 0].Increment_q(true_delta_2);
if (this.record_violation_history)
{
maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_0));
maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_1));
maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_2));
}
i_friction_comp = 0;
}
}
else
{
// update: lambda += delta_lambda;
double old_lambda = mconstraints[ic].Get_l_i();
mconstraints[ic].Set_l_i(old_lambda + deltal);
// If new lagrangian multiplier does not satisfy inequalities, project
// it into an admissible orthant (or, in general, onto an admissible set)
mconstraints[ic].Project();
// After projection, the lambda may have changed a bit..
double new_lambda = mconstraints[ic].Get_l_i();
// Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old
if (this.shlambda != 1.0)
{
new_lambda = shlambda * new_lambda + (1.0 - shlambda) * old_lambda;
mconstraints[ic].Set_l_i(new_lambda);
}
double true_delta = new_lambda - old_lambda;
// For all items with variables, add the effect of incremented
// (and projected) lagrangian reactions:
mconstraints[ic].Increment_q(true_delta);
if (this.record_violation_history)
{
maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta));
}
}
maxviolation = ChMaths.ChMax(maxviolation, Math.Abs(candidate_violation));
} // end IsActive()
} // end constraint loop
// Terminate the loop if violation in constraints has been successfully limited.
// if (maxviolation < tolerance)
// break;
// For recording into violation history, if debugging
if (this.record_violation_history)
{
AtIterationEnd(maxviolation, maxdeltalambda, iter);
}
// Increment iter count (each sweep, either forward or backward, is considered
// as a complete iteration, to be fair when comparing to the non-symmetric SOR :)
iter++;
//
// Backward sweep, for symmetric SOR
//
maxviolation = 0.0;
maxdeltalambda = 0.0;
i_friction_comp = 0;
for (int ic = (nConstr - 1); ic >= 0; ic--)
{
// skip computations if constraint not active.
if (mconstraints[ic].IsActive())
{
// compute residual c_i = [Cq_i]*q + b_i + cfm_i*l_i
double mresidual = mconstraints[ic].Compute_Cq_q() + mconstraints[ic].Get_b_i() +
mconstraints[ic].Get_cfm_i() * mconstraints[ic].Get_l_i();
// true constraint violation may be different from 'mresidual' (ex:clamped if unilateral)
double candidate_violation = Math.Abs(mconstraints[ic].Violation(mresidual));
// compute: delta_lambda = -(omega/g_i) * ([Cq_i]*q + b_i + cfm_i*l_i )
double deltal = (omega / mconstraints[ic].Get_g_i()) * (-mresidual);
if (mconstraints[ic].GetMode() == eChConstraintMode.CONSTRAINT_FRIC)
{
candidate_violation = 0;
// update: lambda += delta_lambda;
old_lambda_friction[i_friction_comp] = mconstraints[ic].Get_l_i();
mconstraints[ic].Set_l_i(old_lambda_friction[i_friction_comp] + deltal);
i_friction_comp++;
if (i_friction_comp == 3)
{
mconstraints[ic].Project(); // the N normal component will take care of N,U,V
double new_lambda_0 = mconstraints[ic + 2].Get_l_i();
double new_lambda_1 = mconstraints[ic + 1].Get_l_i();
double new_lambda_2 = mconstraints[ic + 0].Get_l_i();
// Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old
if (this.shlambda != 1.0)
{
new_lambda_0 = shlambda * new_lambda_0 + (1.0 - shlambda) * old_lambda_friction[0];
new_lambda_1 = shlambda * new_lambda_1 + (1.0 - shlambda) * old_lambda_friction[1];
new_lambda_2 = shlambda * new_lambda_2 + (1.0 - shlambda) * old_lambda_friction[2];
mconstraints[ic + 2].Set_l_i(new_lambda_0);
mconstraints[ic + 1].Set_l_i(new_lambda_1);
mconstraints[ic + 0].Set_l_i(new_lambda_2);
}
double true_delta_0 = new_lambda_0 - old_lambda_friction[0];
double true_delta_1 = new_lambda_1 - old_lambda_friction[1];
double true_delta_2 = new_lambda_2 - old_lambda_friction[2];
mconstraints[ic + 2].Increment_q(true_delta_0);
mconstraints[ic + 1].Increment_q(true_delta_1);
mconstraints[ic + 0].Increment_q(true_delta_2);
candidate_violation = Math.Abs(ChMaths.ChMin(0.0, mresidual));
if (this.record_violation_history)
{
maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_0));
maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_1));
maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta_2));
}
i_friction_comp = 0;
}
}
else
{
// update: lambda += delta_lambda;
double old_lambda = mconstraints[ic].Get_l_i();
mconstraints[ic].Set_l_i(old_lambda + deltal);
// If new lagrangian multiplier does not satisfy inequalities, project
// it into an admissible orthant (or, in general, onto an admissible set)
mconstraints[ic].Project();
// After projection, the lambda may have changed a bit..
double new_lambda = mconstraints[ic].Get_l_i();
// Apply the smoothing: lambda= sharpness*lambda_new_projected + (1-sharpness)*lambda_old
if (this.shlambda != 1.0)
{
new_lambda = shlambda * new_lambda + (1.0 - shlambda) * old_lambda;
mconstraints[ic].Set_l_i(new_lambda);
}
double true_delta = new_lambda - old_lambda;
// For all items with variables, add the effect of incremented
// (and projected) lagrangian reactions:
mconstraints[ic].Increment_q(true_delta);
if (this.record_violation_history)
{
maxdeltalambda = ChMaths.ChMax(maxdeltalambda, Math.Abs(true_delta));
}
}
maxviolation = ChMaths.ChMax(maxviolation, Math.Abs(candidate_violation));
} // end IsActive()
} // end loop on constraints
// For recording into violation history, if debugging
if (this.record_violation_history)
{
AtIterationEnd(maxviolation, maxdeltalambda, iter);
}
// Terminate the loop if violation in constraints has been successfully limited.
if (maxviolation < tolerance)
{
break;
}
iter++;
}
return(maxviolation);
}
public virtual void ContInjectKRMmatrices(ref ChSystemDescriptor mdescriptor)
{
}
public virtual void InjectConstraints(ref ChSystemDescriptor mdescriptor)
{
}