// [mostly internal]. as before, but uses the current (just changed) mask
protected void BuildLink()
{
// set ndoc by counting non-dofs
ndoc = mask.GetMaskDoc();
ndoc_c = mask.GetMaskDoc_c();
ndoc_d = mask.GetMaskDoc_d();
// create matrices
if (ndoc > 0)
{
C = new ChMatrixDynamic <double>(ndoc, 1);
C_dt = new ChMatrixDynamic <double>(ndoc, 1);
C_dtdt = new ChMatrixDynamic <double>(ndoc, 1);
react = new ChMatrixDynamic <double>(ndoc, 1);
Qc = new ChMatrixDynamic <double>(ndoc, 1);
Ct = new ChMatrixDynamic <double>(ndoc, 1);
Cq1 = new ChMatrixDynamic <double>(ndoc, ChBody.BODY_QDOF);
Cq2 = new ChMatrixDynamic <double>(ndoc, ChBody.BODY_QDOF);
Cqw1 = new ChMatrixDynamic <double>(ndoc, ChBody.BODY_DOF);
Cqw2 = new ChMatrixDynamic <double>(ndoc, ChBody.BODY_DOF);
}
else
{
/* C = null;
* C_dt = null;
* C_dtdt = null;
* react = null;
* Qc = null;
* Ct = null;
* Cq1 = null;
* Cq2 = null;
* Cqw1 = null;
* Cqw2 = null;*/
}
}
public static ChMatrixDynamic <Real> operator *(ChMatrixDynamic <Real> mat, ChMatrix matbis)
{
ChMatrixDynamic <Real> result = new ChMatrixDynamic <Real>(mat.matrix.rows, matbis.GetColumns());
result.matrix.MatrMultiply(mat.matrix, matbis);
return(result);
}
public ChVariablesGeneric(int m_ndof = 1) : base(m_ndof)
{
ndof = m_ndof;
Mmass = new ChMatrixDynamic <double>(ndof, ndof);
Mmass.matrix.SetIdentity();
inv_Mmass = new ChMatrixDynamic <double>(ndof, ndof);
inv_Mmass.matrix.SetIdentity();
}
public ChLinkUniversal()
{
m_C = new ChMatrixDynamic <double>(4, 1);
m_multipliers[0] = 0;
m_multipliers[1] = 0;
m_multipliers[2] = 0;
m_multipliers[3] = 0;
}
/// Copy constructor
public ChConstraintTwoGeneric(ChConstraintTwoGeneric other)
{
// Cq_a = Cq_b = Eq_a = Eq_b = null;
// if (other.Cq_a != null)
Cq_a = new ChMatrixDynamic <double>(other.Cq_a);
// if (other.Cq_b != null)
Cq_b = new ChMatrixDynamic <double>(other.Cq_b);
// if (other.Eq_a != null)
Eq_a = new ChMatrixDynamic <double>(other.Eq_a);
// if (other.Eq_b != null)
Eq_b = new ChMatrixDynamic <double>(other.Eq_b);
}
public ChLinkRevoluteSpherical()
{
m_pos1 = new ChVector(0, 0, 0);
m_pos2 = new ChVector(0, 0, 0);
m_dir1 = new ChVector(0, 0, 1);
m_dist = 0;
m_cur_dist = 0;
m_cur_dot = 0;
m_C = new ChMatrixDynamic <double>(2, 1);
m_multipliers[0] = 0;
m_multipliers[1] = 0;
}
public ChVariables(int m_ndof)
{
disabled = false;
ndof = m_ndof;
offset = 0;
// int dof = Get_ndof();
if (Get_ndof() > 0)
{
qb = new ChMatrixDynamic <double>(Get_ndof(), 1);
fb = new ChMatrixDynamic <double>(Get_ndof(), 1);
}
else
{
// qb = fb = null;
}
}
/// Copy constructor
public ChMatrixDynamic(ChMatrixDynamic <Real> msource)//:this()
{
matrix = new ChMatrix();
this.matrix.rows = msource.matrix.GetRows();
this.matrix.columns = msource.matrix.GetColumns();
this.matrix.address = new double[this.matrix.rows * this.matrix.columns];
/*this.address = (double*)Marshal.AllocHGlobal(this.rows * this.columns * sizeof(double));
* double[] array = new double[this.rows * this.columns];
* for (int i = 0; i < array.Length; i++)
* {
* this.address[i] = array[i];
* }*/
for (int i = 0; i < this.matrix.rows * this.matrix.columns; ++i)
{
this.matrix.address[i] = msource.matrix.GetAddress()[i];
}
}
/// Set references to the constrained objects, each of ChVariables type,
/// automatically creating/resizing K matrix if needed.
public void SetVariables(List <ChVariables> mvariables)
{
// Debug.Assert(mvariables.Count > 0);
variables = mvariables;
// destroy the K matrix if needed
// if (K != null)
// K = null;
int msize = 0;
for (int iv = 0; iv < variables.Count; iv++)
{
msize += variables[iv].Get_ndof();
}
// reallocate the K matrix
K = new ChMatrixDynamic <double>(msize, msize);
}
protected void SetupLinkMask()
{
int nc = 0;
if (c_x)
{
nc++;
}
if (c_y)
{
nc++;
}
if (c_z)
{
nc++;
}
if (c_rx)
{
nc++;
}
if (c_ry)
{
nc++;
}
if (c_rz)
{
nc++;
}
mask.ResetNconstr(nc);
// if (C != null)
// C = null;
C = new ChMatrixDynamic <double>(nc, 1);
ChangedLinkMask();
}
private ChMatrixDynamic <double> Qf = new ChMatrixDynamic <double>(); //< Lagrangian force
public ChForce()
{
Body = null;
vpoint = new ChVector(0, 0, 0);
vrelpoint = new ChVector(0, 0, 0);
force = new ChVector(0, 0, 0);
relforce = new ChVector(0, 0, 0);
vdir = ChVector.VECT_X;
vreldir = ChVector.VECT_X;
restpos = new ChVector(0, 0, 0);
mforce = 0;
align = AlignmentFrame.BODY_DIR;
frame = ReferenceFrame.BODY;
mode = ForceType.FORCE;
Qf = new ChMatrixDynamic <double>(7, 1);
modula = new ChFunction_Const(1);
move_x = new ChFunction_Const(0);
move_y = new ChFunction_Const(0);
move_z = new ChFunction_Const(0);
f_x = new ChFunction_Const(0);
f_y = new ChFunction_Const(0);
f_z = new ChFunction_Const(0);
}
/// 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);
}
/// This function updates the following auxiliary data:
/// - the Eq_a and Eq_b and Eq_c matrices
/// - the g_i product
/// This is often called by solvers at the beginning
/// of the solution process.
/// Most often, inherited classes won't need to override this.
public override void Update_auxiliary()
{
// 1- Assuming jacobians are already computed, now compute
// the matrices [Eq_a]=[invM_a]*[Cq_a]' and [Eq_b]
if (variables_a.IsActive())
{
if (variables_a.Get_ndof() != 0)
{
ChMatrixDynamic <double> mtemp1 = new ChMatrixDynamic <double>(variables_a.Get_ndof(), 1);
mtemp1.matrix.CopyFromMatrixT(Cq_a.matrix);
variables_a.Compute_invMb_v(Eq_a.matrix, mtemp1.matrix);
}
}
if (variables_b.IsActive())
{
if (variables_b.Get_ndof() != 0)
{
ChMatrixDynamic <double> mtemp1 = new ChMatrixDynamic <double>(variables_b.Get_ndof(), 1);
mtemp1.matrix.CopyFromMatrixT(Cq_b.matrix);
variables_b.Compute_invMb_v(Eq_b.matrix, mtemp1.matrix);
}
}
if (variables_c.IsActive())
{
if (variables_c.Get_ndof() != 0)
{
ChMatrixDynamic <double> mtemp1 = new ChMatrixDynamic <double>(variables_c.Get_ndof(), 1);
mtemp1.matrix.CopyFromMatrixT(Cq_c.matrix);
variables_c.Compute_invMb_v(Eq_c.matrix, mtemp1.matrix);
}
}
// 2- Compute g_i = [Cq_i]*[invM_i]*[Cq_i]' + cfm_i
ChMatrixDynamic <double> res = new ChMatrixDynamic <double>(1, 1);
g_i = 0;
if (variables_a.IsActive())
{
if (variables_a.Get_ndof() != 0)
{
res.matrix.MatrMultiply(Cq_a.matrix, Eq_a.matrix);
g_i = res.matrix[0, 0];
}
}
if (variables_b.IsActive())
{
if (variables_b.Get_ndof() != 0)
{
res.matrix.MatrMultiply(Cq_b.matrix, Eq_b.matrix);
g_i += res.matrix[0, 0];
}
}
if (variables_c.IsActive())
{
if (variables_c.Get_ndof() != 0)
{
res.matrix.MatrMultiply(Cq_c.matrix, Eq_c.matrix);
g_i += res.matrix[0, 0];
}
}
// 3- adds the constraint force mixing term (usually zero):
if (cfm_i != 0)
{
g_i += cfm_i;
}
}
public ChStateDelta(ChMatrixDynamic <double> matr, ChIntegrable mint) : base(matr.matrix)
{
integrable = mint;
}
