/// Get the link coordinate system, expressed relative to Body2 (spherical side).
/// This represents the 'main' reference of the link: reaction forces
/// and reaction torques are reported in this coordinate system.
public override ChCoordsys GetLinkRelativeCoords()
{
ChVector pos1 = Body2.TransformPointParentToLocal(Body1.TransformPointLocalToParent(m_pos1));
ChMatrix33 <double> A = new ChMatrix33 <double>(0);
ChVector u = (m_pos2 - pos1).GetNormalized();
ChVector w = Body2.TransformDirectionParentToLocal(Body1.TransformDirectionLocalToParent(m_dir1));
ChVector v = ChVector.Vcross(w, u);
A.Set_A_axis(u, v, w);
return(new ChCoordsys(pos1, A.Get_A_quaternion()));
}
//
// 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 direction of the revolute joint, expressed in absolute coordinate system.
public ChVector GetDir1Abs()
{
return(Body1.TransformDirectionLocalToParent(m_dir1));
}