public override void IntFromDescriptor(int off_v, ref ChStateDelta v, int off_L, ref ChVectorDynamic <double> L) { int displ_L = off_L - this.offset_L; int displ_v = off_v - this.offset_w; for (int i = 0; i < bodylist.Count; i++) { if (bodylist[i].IsActive()) { bodylist[i].IntFromDescriptor(displ_v + bodylist[i].GetOffset_w(), ref v, displ_L + bodylist[i].GetOffset_L(), ref L); } } for (int i = 0; i < linklist.Count; i++) { if (linklist[i].IsActive()) { linklist[i].IntFromDescriptor(displ_v + linklist[i].GetOffset_w(), ref v, displ_L + linklist[i].GetOffset_L(), ref L); } } for (int i = 0; i < otherphysicslist.Count; i++) { otherphysicslist[i].IntFromDescriptor(displ_v + otherphysicslist[i].GetOffset_w(), ref v, displ_L + otherphysicslist[i].GetOffset_L(), ref L); } }
public override void IntFromDescriptor(int off_v, ref ChStateDelta v, int off_L, ref ChVectorDynamic <double> L) { v.matrix[off_v] = variables.Get_qb().matrix[0, 0]; }
public override void IntLoadConstraint_C(int off_L, ref ChVectorDynamic <double> Qc, double c, bool do_clamp, double recovery_clamp) { int cnt = 0; for (int i = 0; i < mask.nconstr; i++) { if (mask.Constr_N(i).IsActive()) { if (do_clamp) { if (mask.Constr_N(i).IsUnilateral()) { Qc.matrix[off_L + cnt] += ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp); } else { Qc.matrix[off_L + cnt] += ChMaths.ChMin(ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp), recovery_clamp); } } else { Qc.matrix[off_L + cnt] += c * C.matrix.ElementN(cnt); } cnt++; // The Gremlin in the works was here! I had accidently locked this variable in the above else, causing joint jittering } } }
public override void IntFromDescriptor(int off_v, ref ChStateDelta v, int off_L, ref ChVectorDynamic <double> L) { L.matrix[off_L] = constraint.Get_l_i(); }
public override void IntLoadResidual_Mv(int off, ref ChVectorDynamic <double> R, ChVectorDynamic <double> w, double c) { R.matrix[off] += c * 1.0 * w.matrix[off]; }
public override void IntLoadResidual_Mv(int off, ref ChVectorDynamic <double> R, ChVectorDynamic <double> w, double c) { int displ_v = off - this.offset_w; for (int i = 0; i < bodylist.Count; i++) { if (bodylist[i].IsActive()) { bodylist[i].IntLoadResidual_Mv(displ_v + bodylist[i].GetOffset_w(), ref R, w, c); } } for (int i = 0; i < linklist.Count; i++) { if (linklist[i].IsActive()) { linklist[i].IntLoadResidual_Mv(displ_v + linklist[i].GetOffset_w(), ref R, w, c); } } for (int i = 0; i < otherphysicslist.Count; i++) { otherphysicslist[i].IntLoadResidual_Mv(displ_v + otherphysicslist[i].GetOffset_w(), ref R, w, c); } }
/// Takes the M*v term, multiplying mass by a vector, scale and adds to R at given offset: /// R += c*M*w public virtual void IntLoadResidual_Mv(int off, //< offset in R residual ref ChVectorDynamic <double> R, //< result: the R residual, R += c*M*v ChVectorDynamic <double> w, //< the w vector double c //< a scaling factor ) { }
public override void IntLoadResidual_CqL(int off_L, ref ChVectorDynamic <double> R, ChVectorDynamic <double> L, double c) { int displ_L = off_L - this.offset_L; for (int i = 0; i < bodylist.Count; i++) { if (bodylist[i].IsActive()) { bodylist[i].IntLoadResidual_CqL(displ_L + bodylist[i].GetOffset_L(), ref R, L, c); } } for (int i = 0; i < linklist.Count; i++) { if (linklist[i].IsActive()) { linklist[i].IntLoadResidual_CqL(displ_L + linklist[i].GetOffset_L(), ref R, L, c); } } for (int i = 0; i < otherphysicslist.Count; i++) { otherphysicslist[i].IntLoadResidual_CqL(displ_L + otherphysicslist[i].GetOffset_L(), ref R, L, c); } }
public override void IntLoadConstraint_C(int off_L, ref ChVectorDynamic <double> Qc, double c, bool do_clamp, double recovery_clamp) { int displ_L = off_L - this.offset_L; for (int i = 0; i < bodylist.Count; i++) { if (bodylist[i].IsActive()) { bodylist[i].IntLoadConstraint_C(displ_L + bodylist[i].GetOffset_L(), ref Qc, c, do_clamp, recovery_clamp); } } for (int i = 0; i < linklist.Count; i++) { if (linklist[i].IsActive()) { linklist[i].IntLoadConstraint_C(displ_L + linklist[i].GetOffset_L(), ref Qc, c, do_clamp, recovery_clamp); } } for (int i = 0; i < otherphysicslist.Count; i++) { otherphysicslist[i].IntLoadConstraint_C(displ_L + otherphysicslist[i].GetOffset_L(), ref Qc, c, do_clamp, recovery_clamp); } }
public override void IntLoadConstraint_C(int off_L, ref ChVectorDynamic <double> Qc, double c, bool do_clamp, double recovery_clamp) { /* if (!IsActive()) * return; * * double cnstr_x_violation = c * m_C.matrix.GetElement(0, 0); * double cnstr_y_violation = c * m_C.matrix.GetElement(1, 0); * double cnstr_z_violation = c * m_C.matrix.GetElement(2, 0); * double cnstr_dot_violation = c * m_C.matrix.GetElement(3, 0); * * if (do_clamp) * { * cnstr_x_violation = ChMaths.ChMin(ChMaths.ChMax(cnstr_x_violation, -recovery_clamp), recovery_clamp); * cnstr_y_violation = ChMaths.ChMin(ChMaths.ChMax(cnstr_y_violation, -recovery_clamp), recovery_clamp); * cnstr_z_violation = ChMaths.ChMin(ChMaths.ChMax(cnstr_z_violation, -recovery_clamp), recovery_clamp); * cnstr_dot_violation = ChMaths.ChMin(ChMaths.ChMax(cnstr_dot_violation, -recovery_clamp), recovery_clamp); * } * Qc.matrix[off_L + 0] += cnstr_x_violation; * Qc.matrix[off_L + 1] += cnstr_y_violation; * Qc.matrix[off_L + 2] += cnstr_z_violation; * Qc.matrix[off_L + 3] += cnstr_dot_violation;*/ }
public override void IntStateScatterReactions(int off_L, ChVectorDynamic <double> L) { if (!this.IsActive()) { return; } m_multipliers[0] = L.matrix[off_L + 0]; m_multipliers[1] = L.matrix[off_L + 1]; // Also compute 'intuitive' reactions: double lam_dist = m_multipliers[0]; // ||pos2_abs - pos1_abs|| - dist = 0 double lam_dot = m_multipliers[1]; // dot(dir1_abs, pos2_abs - pos1_abs) = 0 // Calculate the reaction torques and forces on Body 2 in the joint frame // (Note: origin of the joint frame is at the center of the revolute joint // which is defined on body 1, the x-axis is along the vector from the // point on body 1 to the point on body 2. The z axis is along the revolute // axis defined for the joint) react_force.x = lam_dist; react_force.y = 0; react_force.z = lam_dot; react_torque.x = 0; react_torque.y = -m_cur_dist * lam_dot; react_torque.z = 0; }
public override void IntLoadResidual_CqL(int off_L, ref ChVectorDynamic <double> R, ChVectorDynamic <double> L, double c) { constraint.MultiplyTandAdd(R.matrix, L.matrix[off_L] * c); }
public override void IntLoadConstraint_C(int off_L, ref ChVectorDynamic <double> Qc, double c, bool do_clamp, double recovery_clamp) { int cnt = 0; for (int i = 0; i < mask.nconstr; i++) { if (mask.Constr_N(i).IsActive()) { if (do_clamp) { if (mask.Constr_N(i).IsUnilateral()) { Qc.matrix[off_L + cnt] += ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp); } else { Qc.matrix[off_L + cnt] += ChMaths.ChMin(ChMaths.ChMax(c * C.matrix.ElementN(cnt), -recovery_clamp), recovery_clamp); } } else { Qc.matrix[off_L + cnt] += c * C.matrix.ElementN(cnt); } cnt++; } } }
public virtual void ContIntLoadResidual_CqL(int off_L, //< offset in L multipliers ref ChVectorDynamic <double> R, //< result: the R residual, R += c*Cq'*L ChVectorDynamic <double> L, //< the L vector double c //< a scaling factor ) { }
public override void ContIntFromDescriptor(int off_L, ref ChVectorDynamic <double> L ) { L.matrix[off_L] = Nx.Get_l_i(); L.matrix[off_L + 1] = Tu.Get_l_i(); L.matrix[off_L + 2] = Tv.Get_l_i(); }
public override void IntStateScatterReactions(int off_L, ChVectorDynamic <double> L) { react_force = new ChVector(0, 0, 0); // Do not update 'intuitive' react force and torque here: just set as 0. react_torque = new ChVector(0, 0, 0); // Child classes implementations should compute them. // if (react != null) react.matrix.PasteClippedMatrix(L.matrix, off_L, 0, react.matrix.GetRows(), 1, 0, 0); }
public override void IntLoadResidual_CqL(int off_L, ref ChVectorDynamic <double> R, ChVectorDynamic <double> L, double c) { m_cnstr_dist.MultiplyTandAdd(R.matrix, L.matrix[off_L + 0] * c); m_cnstr_dot.MultiplyTandAdd(R.matrix, L.matrix[off_L + 1] * c); }
public override void IntToDescriptor(int off_v, ChStateDelta v, ChVectorDynamic <double> R, int off_L, ChVectorDynamic <double> L, ChVectorDynamic <double> Qc) { constraint.Set_l_i(L.matrix[off_L]); constraint.Set_b_i(Qc.matrix[off_L]); this.variable.Get_qb().matrix[0, 0] = v.matrix[off_v]; this.variable.Get_fb().matrix[0, 0] = R.matrix[off_v]; }
/// After a solver solution, fetch values from variables and constraints into vectors: public virtual void IntFromDescriptor( int off_v, //< offset for \e v ref ChStateDelta v, //< vector to where the \e q 'unknowns' term of the variables will be copied int off_L, //< offset for \e L ref ChVectorDynamic <double> L //< vector to where \e L 'lagrangian ' term of the constraints will be copied ) { }
public override void IntStateScatterReactions(int off_L, ChVectorDynamic <double> L) { react_force.x = -L.matrix[off_L]; react_force.y = 0; react_force.z = 0; react_torque = ChVector.VNULL; }
public virtual void ContIntLoadConstraint_C(int off_L, //< offset in Qc residual ref ChVectorDynamic <double> Qc, //< result: the Qc residual, Qc += c*C double c, //< a scaling factor bool do_clamp, //< apply clamping to c*C? double recovery_clamp ///< value for min/max clamping of c*C ) { }
// // STATE FUNCTIONS // // (override/implement interfaces for global state vectors, see ChPhysicsItem for comments.) public override void IntStateGatherReactions(int off_L, ref ChVectorDynamic <double> L) { if (!this.IsActive()) { return; } int nc = 0; if (c_x) { if (mask.Constr_N(nc).IsActive()) { L.matrix[off_L + nc] = -react_force.x; } nc++; } if (c_y) { if (mask.Constr_N(nc).IsActive()) { L.matrix[off_L + nc] = -react_force.y; } nc++; } if (c_z) { if (mask.Constr_N(nc).IsActive()) { L.matrix[off_L + nc] = -react_force.z; } nc++; } if (c_rx) { if (mask.Constr_N(nc).IsActive()) { L.matrix[off_L + nc] = -2.0 * react_torque.x; } nc++; } if (c_ry) { if (mask.Constr_N(nc).IsActive()) { L.matrix[off_L + nc] = -2.0 * react_torque.y; } nc++; } if (c_rz) { if (mask.Constr_N(nc).IsActive()) { L.matrix[off_L + nc] = -2.0 * react_torque.z; } nc++; } }
// UPDATING FUNCTIONS public override void ContIntStateGatherReactions(int off_L, ref ChVectorDynamic <double> L) { // base behaviour too base.ContIntStateGatherReactions(off_L, ref L); L.matrix[off_L + 3] = react_torque.x; L.matrix[off_L + 4] = react_torque.y; L.matrix[off_L + 5] = react_torque.z; }
public override void IntLoadConstraint_Ct(int off_L, ref ChVectorDynamic <double> Qc, double c) { double mCt = -m_func.Get_y(this.GetChTime()); if (mask.Constr_N(0).IsActive()) { Qc.matrix[off_L + 0] += c * mCt; } }
public override void ContIntStateScatterReactions(int off_L, ChVectorDynamic <double> L) { // base behaviour too base.ContIntStateScatterReactions(off_L, L); react_torque.x = L.matrix[off_L + 3]; react_torque.y = L.matrix[off_L + 4]; react_torque.z = L.matrix[off_L + 5]; }
// // STATE FUNCTIONS // public override void IntStateGatherReactions(int off_L, ref ChVectorDynamic <double> L) { if (!this.IsActive()) { return; } L.matrix[off_L + 0] = m_multipliers[0]; L.matrix[off_L + 1] = m_multipliers[1]; }
// // STATE FUNCTIONS // public override void IntLoadConstraint_Ct(int off_L, ref ChVectorDynamic <double> Qc, double c) { double mCt = -0.5 * m_func.Get_y_dx(this.GetChTime()); int ncrz = mask.nconstr - 1; if (mask.Constr_N(ncrz).IsActive()) { Qc.matrix[off_L + ncrz] += c * mCt; } }
// // STATE FUNCTIONS // // (override/implement interfaces for global state vectors, see ChPhysicsItem for comments.) public override void IntStateGatherReactions(int off_L, ref ChVectorDynamic <double> L) { /* if (!this.IsActive()) * return; * * L.matrix[off_L + 0] = m_multipliers[0]; * L.matrix[off_L + 1] = m_multipliers[1]; * L.matrix[off_L + 2] = m_multipliers[2]; * L.matrix[off_L + 3] = m_multipliers[3];*/ }
public override void IntFromDescriptor(int off_v, ref ChStateDelta v, int off_L, ref ChVectorDynamic <double> L) { // inherit parent base.IntFromDescriptor(off_v, ref v, off_L, ref L); v.matrix[off_v] = this.variable.Get_qb().matrix[0, 0]; }
public override void ContIntLoadResidual_CqL(int off_L, ref ChVectorDynamic <double> R, ChVectorDynamic <double> L, double c ) { this.Nx.MultiplyTandAdd(R.matrix, L.matrix[off_L] * c); this.Tu.MultiplyTandAdd(R.matrix, L.matrix[off_L + 1] * c); this.Tv.MultiplyTandAdd(R.matrix, L.matrix[off_L + 2] * c); }