/// Override _all_ time, jacobian etc. updating.
/// In detail, it computes jacobians, violations, etc. and stores
/// results in inner structures.
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);
// compute jacobians
ChVector AbsDist = Body1.TransformPointLocalToParent(pos1) - Body2.TransformPointLocalToParent(pos2);
curr_dist = AbsDist.Length();
ChVector D2abs = ChVector.Vnorm(AbsDist);
ChVector D2relB = Body2.TransformDirectionParentToLocal(D2abs);
ChVector D2relA = Body1.TransformDirectionParentToLocal(D2abs);
ChVector CqAx = D2abs;
ChVector CqBx = -D2abs;
ChVector CqAr = -ChVector.Vcross(D2relA, pos1);
ChVector CqBr = ChVector.Vcross(D2relB, pos2);
Cx.Get_Cq_a().ElementN(0) = CqAx.x;
Cx.Get_Cq_a().ElementN(1) = CqAx.y;
Cx.Get_Cq_a().ElementN(2) = CqAx.z;
Cx.Get_Cq_a().ElementN(3) = CqAr.x;
Cx.Get_Cq_a().ElementN(4) = CqAr.y;
Cx.Get_Cq_a().ElementN(5) = CqAr.z;
Cx.Get_Cq_b().ElementN(0) = CqBx.x;
Cx.Get_Cq_b().ElementN(1) = CqBx.y;
Cx.Get_Cq_b().ElementN(2) = CqBx.z;
Cx.Get_Cq_b().ElementN(3) = CqBr.x;
Cx.Get_Cq_b().ElementN(4) = CqBr.y;
Cx.Get_Cq_b().ElementN(5) = CqBr.z;
//***TO DO*** C_dt? C_dtdt? (may be never used..)
}
//
// UPDATING FUNCTIONS
//
/// Perform the update of this joint at the specified time: compute jacobians,
/// raint violations, etc. and cache in internal structures
public override void update(double time, bool update_assets = true)
{
// Inherit time changes of parent class (ChLink)
base.update(time, update_assets);
// Express the body locations and direction in absolute frame
ChVector pos1_abs = Body1.TransformPointLocalToParent(m_pos1);
ChVector pos2_abs = Body2.TransformPointLocalToParent(m_pos2);
ChVector dir1_abs = Body1.TransformDirectionLocalToParent(m_dir1);
ChVector d12_abs = pos2_abs - pos1_abs;
// Update current distance and dot product
m_cur_dist = d12_abs.Length();
m_cur_dot = ChVector.Vdot(d12_abs, dir1_abs);
// Calculate a unit vector in the direction d12, expressed in absolute frame
// Then express it in the two body frames
ChVector u12_abs = d12_abs / m_cur_dist;
ChVector u12_loc1 = Body1.TransformDirectionParentToLocal(u12_abs);
ChVector u12_loc2 = Body2.TransformDirectionParentToLocal(u12_abs);
// Express the direction vector in the frame of body 2
ChVector dir1_loc2 = Body2.TransformDirectionParentToLocal(dir1_abs);
// Cache violation of the distance constraint
m_C.matrix.SetElement(0, 0, m_cur_dist - m_dist);
// Compute Jacobian of the distance constraint
// ||pos2_abs - pos1_abs|| - dist = 0
{
ChVector Phi_r1 = -u12_abs;
ChVector Phi_pi1 = ChVector.Vcross(u12_loc1, m_pos1);
m_cnstr_dist.Get_Cq_a().ElementN(0) = Phi_r1.x;
m_cnstr_dist.Get_Cq_a().ElementN(1) = Phi_r1.y;
m_cnstr_dist.Get_Cq_a().ElementN(2) = Phi_r1.z;
m_cnstr_dist.Get_Cq_a().ElementN(3) = Phi_pi1.x;
m_cnstr_dist.Get_Cq_a().ElementN(4) = Phi_pi1.y;
m_cnstr_dist.Get_Cq_a().ElementN(5) = Phi_pi1.z;
ChVector Phi_r2 = u12_abs;
ChVector Phi_pi2 = -ChVector.Vcross(u12_loc2, m_pos2);
m_cnstr_dist.Get_Cq_b().ElementN(0) = Phi_r2.x;
m_cnstr_dist.Get_Cq_b().ElementN(1) = Phi_r2.y;
m_cnstr_dist.Get_Cq_b().ElementN(2) = Phi_r2.z;
m_cnstr_dist.Get_Cq_b().ElementN(3) = Phi_pi2.x;
m_cnstr_dist.Get_Cq_b().ElementN(4) = Phi_pi2.y;
m_cnstr_dist.Get_Cq_b().ElementN(5) = Phi_pi2.z;
}
// Cache violation of the dot constraint
m_C.matrix.SetElement(1, 0, m_cur_dot);
// Compute Jacobian of the dot constraint
// dot(dir1_abs, pos2_abs - pos1_abs) = 0
{
ChVector Phi_r1 = -dir1_abs;
ChVector Phi_pi1 = ChVector.Vcross(m_dir1, m_pos1) - ChVector.Vcross(u12_loc1, m_pos1);
m_cnstr_dot.Get_Cq_a().ElementN(0) = Phi_r1.x;
m_cnstr_dot.Get_Cq_a().ElementN(1) = Phi_r1.y;
m_cnstr_dot.Get_Cq_a().ElementN(2) = Phi_r1.z;
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;
ChVector Phi_r2 = dir1_abs;
ChVector Phi_pi2 = -ChVector.Vcross(dir1_loc2, m_pos2);
m_cnstr_dot.Get_Cq_b().ElementN(0) = Phi_r2.x;
m_cnstr_dot.Get_Cq_b().ElementN(1) = Phi_r2.y;
m_cnstr_dot.Get_Cq_b().ElementN(2) = Phi_r2.z;
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;
}
}
/// Get the link coordinate system, expressed relative to Body2 (the 'master'
/// body). This represents the 'main' reference of the link: reaction forces
/// are expressed in this coordinate system.
/// (It is the coordinate system of the contact plane relative to Body2)
public override ChCoordsys GetLinkRelativeCoords()
{
//ChVector D2local;
ChVector D2temp = (ChVector.Vnorm(Body1.TransformPointLocalToParent(pos1) - Body2.TransformPointLocalToParent(pos2)));
ChVector D2rel = Body2.TransformDirectionParentToLocal(D2temp);
ChVector Vx = new ChVector(0, 0, 0), Vy = new ChVector(0, 0, 0), Vz = new ChVector(0, 0, 0);
//ChMatrix33<double> rel_matrix = new ChMatrix33<double>();
ChVector.XdirToDxDyDz(D2rel, ChVector.VECT_Y, ref Vx, ref Vy, ref Vz);
rel_matrix.Set_A_axis(Vx, Vy, Vz);
ChQuaternion Ql2 = rel_matrix.Get_A_quaternion();
return(new ChCoordsys(pos2, Ql2));
}
/// Get the point on Body2 (spherical side), expressed in absolute coordinate system.
public ChVector GetPoint2Abs()
{
return(Body2.TransformPointLocalToParent(m_pos2));
}