/// 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())); }