//
// UPDATING FUNCTIONS
//
/// Perform the update of this joint at the specified time: compute jacobians
/// and constraint violations, cache in internal structures
public override void update(double time, bool update_assets = true)
{
// Inherit time changes of parent class
base.UpdateTime(time);
// Express the joint frames in absolute frame
ChFrame <double> frame1_abs = m_frame1.BitShiftRight(Body1);
ChFrame <double> frame2_abs = m_frame2.BitShiftRight(Body2);
// Calculate violations of the spherical constraints
m_C.matrix.SetElement(0, 0, frame2_abs.coord.pos.x - frame1_abs.coord.pos.x);
m_C.matrix.SetElement(1, 0, frame2_abs.coord.pos.y - frame1_abs.coord.pos.y);
m_C.matrix.SetElement(2, 0, frame2_abs.coord.pos.z - frame1_abs.coord.pos.z);
// Compute Jacobian of the spherical constraints
// pos2_abs - pos1_abs = 0
{
ChMatrix33 <double> tilde1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> tilde2 = new ChMatrix33 <double>(0);
tilde1.Set_X_matrix(m_frame1.coord.pos);
tilde2.Set_X_matrix(m_frame2.coord.pos);
ChMatrix33 <double> Phi_pi1 = Body1.GetA() * tilde1;
ChMatrix33 <double> Phi_pi2 = Body2.GetA() * tilde2;
m_cnstr_x.Get_Cq_a().ElementN(0) = -1;
m_cnstr_x.Get_Cq_b().ElementN(0) = +1;
m_cnstr_x.Get_Cq_a().ElementN(1) = 0;
m_cnstr_x.Get_Cq_b().ElementN(1) = 0;
m_cnstr_x.Get_Cq_a().ElementN(2) = 0;
m_cnstr_x.Get_Cq_b().ElementN(2) = 0;
m_cnstr_x.Get_Cq_a().ElementN(3) = Phi_pi1.nm.matrix[0, 0];
m_cnstr_x.Get_Cq_b().ElementN(3) = -Phi_pi2.nm.matrix[0, 0];
m_cnstr_x.Get_Cq_a().ElementN(4) = Phi_pi1.nm.matrix[0, 1];
m_cnstr_x.Get_Cq_b().ElementN(4) = -Phi_pi2.nm.matrix[0, 1];
m_cnstr_x.Get_Cq_a().ElementN(5) = Phi_pi1.nm.matrix[0, 2];
m_cnstr_x.Get_Cq_b().ElementN(5) = -Phi_pi2.nm.matrix[0, 2];
m_cnstr_y.Get_Cq_a().ElementN(0) = 0;
m_cnstr_y.Get_Cq_b().ElementN(0) = 0;
m_cnstr_y.Get_Cq_a().ElementN(1) = -1;
m_cnstr_y.Get_Cq_b().ElementN(1) = +1;
m_cnstr_y.Get_Cq_a().ElementN(2) = 0;
m_cnstr_y.Get_Cq_b().ElementN(2) = 0;
m_cnstr_y.Get_Cq_a().ElementN(3) = Phi_pi1.nm.matrix[1, 0];
m_cnstr_y.Get_Cq_b().ElementN(3) = -Phi_pi2.nm.matrix[1, 0];
m_cnstr_y.Get_Cq_a().ElementN(4) = Phi_pi1.nm.matrix[1, 1];
m_cnstr_y.Get_Cq_b().ElementN(4) = -Phi_pi2.nm.matrix[1, 1];
m_cnstr_y.Get_Cq_a().ElementN(5) = Phi_pi1.nm.matrix[1, 2];
m_cnstr_y.Get_Cq_b().ElementN(5) = -Phi_pi2.nm.matrix[1, 2];
m_cnstr_z.Get_Cq_a().ElementN(0) = 0;
m_cnstr_z.Get_Cq_b().ElementN(0) = 0;
m_cnstr_z.Get_Cq_a().ElementN(1) = 0;
m_cnstr_z.Get_Cq_b().ElementN(1) = 0;
m_cnstr_z.Get_Cq_a().ElementN(2) = -1;
m_cnstr_z.Get_Cq_b().ElementN(2) = +1;
m_cnstr_z.Get_Cq_a().ElementN(3) = Phi_pi1.nm.matrix[2, 0];
m_cnstr_z.Get_Cq_b().ElementN(3) = -Phi_pi2.nm.matrix[2, 0];
m_cnstr_z.Get_Cq_a().ElementN(4) = Phi_pi1.nm.matrix[2, 1];
m_cnstr_z.Get_Cq_b().ElementN(4) = -Phi_pi2.nm.matrix[2, 1];
m_cnstr_z.Get_Cq_a().ElementN(5) = Phi_pi1.nm.matrix[2, 2];
m_cnstr_z.Get_Cq_b().ElementN(5) = -Phi_pi2.nm.matrix[2, 2];
}
// Calculate violation of the dot constraint
ChVector u1 = frame1_abs.GetA().Get_A_Xaxis();
ChVector v2 = frame2_abs.GetA().Get_A_Yaxis();
m_C.matrix.SetElement(3, 0, ChVector.Vdot(u1, v2));
// Compute Jacobian of the dot constraint
// dot(u1_abs, v2_abs) = 0
{
ChMatrix33 <double> mat1 = Body1.GetA() * m_u1_tilde;
ChMatrix33 <double> mat2 = Body2.GetA() * m_v2_tilde;
ChVector Phi_pi1 = mat1.MatrT_x_Vect(v2);
ChVector Phi_pi2 = mat2.MatrT_x_Vect(u1);
m_cnstr_dot.Get_Cq_a().ElementN(0) = 0;
m_cnstr_dot.Get_Cq_a().ElementN(1) = 0;
m_cnstr_dot.Get_Cq_a().ElementN(2) = 0;
m_cnstr_dot.Get_Cq_a().ElementN(3) = -Phi_pi1.x;
m_cnstr_dot.Get_Cq_a().ElementN(4) = -Phi_pi1.y;
m_cnstr_dot.Get_Cq_a().ElementN(5) = -Phi_pi1.z;
m_cnstr_dot.Get_Cq_b().ElementN(0) = 0;
m_cnstr_dot.Get_Cq_b().ElementN(1) = 0;
m_cnstr_dot.Get_Cq_b().ElementN(2) = 0;
m_cnstr_dot.Get_Cq_b().ElementN(3) = -Phi_pi2.x;
m_cnstr_dot.Get_Cq_b().ElementN(4) = -Phi_pi2.y;
m_cnstr_dot.Get_Cq_b().ElementN(5) = -Phi_pi2.z;
}
}
//
// UPDATING FUNCTIONS
//
/// Override _all_ time, jacobian etc. updating.
public override void update(double mytime, bool update_assets = true)
{
// Inherit time changes of parent class (ChLink), basically doing nothing :)
base.update(mytime, update_assets);
if (this.Body1 != null && this.Body2 != null)
{
this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables());
ChFrame <double> aframe = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1));
ChVector p1_abs = aframe.GetPos();
ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2));
ChVector p2_abs = aframe2.GetPos();
ChFrame <double> bframe = new ChFrame <double>();
(this.Body2).TransformParentToLocal(aframe, bframe);
this.frame2.TransformParentToLocal(bframe, aframe);
// Now 'aframe' contains the position/rotation of frame 1 respect to frame 2, in frame 2 coords.
//***TODO*** check if it is faster to do aframe2.TransformParentToLocal(aframe, bframe); instead of two transforms above
ChMatrix33 <double> Jx1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jx2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jr1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jr2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jw1 = new ChMatrix33 <double>(0);
ChMatrix33 <double> Jw2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> mtempM = new ChMatrix33 <double>(0);
ChMatrix33 <double> mtempQ = new ChMatrix33 <double>(0);
ChMatrix33 <double> abs_plane = new ChMatrix33 <double>(0);
abs_plane.nm.matrix.MatrMultiply(Body2.GetA().nm.matrix, frame2.GetA().nm.matrix);
Jx1.nm.matrix.CopyFromMatrixT(abs_plane.nm.matrix);
Jx2.nm.matrix.CopyFromMatrixT(abs_plane.nm.matrix);
Jx2.nm.matrix.MatrNeg();
Jw1.nm.matrix.MatrTMultiply(abs_plane.nm.matrix, Body1.GetA().nm.matrix);
Jw2.nm.matrix.MatrTMultiply(abs_plane.nm.matrix, Body2.GetA().nm.matrix);
mtempM.Set_X_matrix(frame1.GetPos());
Jr1.nm.matrix.MatrMultiply(Jw1.nm.matrix, mtempM.nm.matrix);
Jr1.nm.matrix.MatrNeg();
mtempM.Set_X_matrix(frame2.GetPos());
Jr2.nm.matrix.MatrMultiply(Jw2.nm.matrix, mtempM.nm.matrix);
ChVector p2p1_base2 = (Body2.GetA()).MatrT_x_Vect(ChVector.Vsub(p1_abs, p2_abs));
mtempM.Set_X_matrix(p2p1_base2);
mtempQ.nm.matrix.MatrTMultiply(frame2.GetA().nm.matrix, mtempM.nm.matrix);
Jr2.nm.matrix.MatrInc(mtempQ.nm.matrix);
Jw2.nm.matrix.MatrNeg();
// Premultiply by Jw1 and Jw2 by 0.5*[Fp(q_resid)]' to get residual as imaginary part of a quaternion.
// For small misalignment this effect is almost insignificant cause [Fp(q_resid)]=[I],
// but otherwise it is needed (if you want to use the stabilization term - if not, you can live without).
mtempM.Set_X_matrix((aframe.GetRot().GetVector()) * 0.5);
mtempM.nm.matrix[0, 0] = 0.5 * aframe.GetRot().e0;
mtempM.nm.matrix[1, 1] = 0.5 * aframe.GetRot().e0;
mtempM.nm.matrix[2, 2] = 0.5 * aframe.GetRot().e0;
mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw1.nm.matrix);
Jw1 = mtempQ;
mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw2.nm.matrix);
Jw2 = mtempQ;
int nc = 0;
if (c_x)
{
this.C.matrix.ElementN(nc) = aframe.GetPos().x;
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 0, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 0, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 0, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 0, 0, 1, 3, 0, 3);
nc++;
}
if (c_y)
{
this.C.matrix.ElementN(nc) = aframe.GetPos().y;
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 1, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 1, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 1, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 1, 0, 1, 3, 0, 3);
nc++;
}
if (c_z)
{
this.C.matrix.ElementN(nc) = aframe.GetPos().z;
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jx1.nm.matrix, 2, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jr1.nm.matrix, 2, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jx2.nm.matrix, 2, 0, 1, 3, 0, 0);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jr2.nm.matrix, 2, 0, 1, 3, 0, 3);
nc++;
}
if (c_rx)
{
this.C.matrix.ElementN(nc) = aframe.GetRot().e1;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 0, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 0, 0, 1, 3, 0, 3);
nc++;
}
if (c_ry)
{
this.C.matrix.ElementN(nc) = aframe.GetRot().e2;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 1, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 1, 0, 1, 3, 0, 3);
nc++;
}
if (c_rz)
{
this.C.matrix.ElementN(nc) = aframe.GetRot().e3;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 2, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 2, 0, 1, 3, 0, 3);
nc++;
}
}
}
