/// Initialize this joint by specifying the two bodies to be connected, a point
/// and a direction on body1 defining the revolute joint, and a point on the
/// second body defining the spherical joint. If local = true, it is assumed
/// that these quantities are specified in the local body frames. Otherwise,
/// it is assumed that they are specified in the absolute frame. The imposed
/// distance between the two points can be either inferred from the provided
/// configuration (auto_distance = true) or specified explicitly.
public void Initialize(ChBodyFrame body1, //< first frame (revolute side)
ChBodyFrame body2, //< second frame (spherical side)
bool local, //< true if data given in body local frames
ChVector pos1, //< point on first frame (center of revolute)
ChVector dir1, //< direction of revolute on first frame
ChVector pos2, //< point on second frame (center of spherical)
bool auto_distance = true, //< true if imposed distance equal to |pos1 - po2|
double distance = 0 //< imposed distance (used only if auto_distance = false)
)
{
Body1 = body1;
Body2 = body2;
m_cnstr_dist.SetVariables(Body1.Variables(), Body2.Variables());
m_cnstr_dot.SetVariables(Body1.Variables(), Body2.Variables());
ChVector pos1_abs;
ChVector pos2_abs;
ChVector dir1_abs;
if (local)
{
m_pos1 = pos1;
m_pos2 = pos2;
m_dir1 = ChVector.Vnorm(dir1);
pos1_abs = Body1.TransformPointLocalToParent(m_pos1);
pos2_abs = Body2.TransformPointLocalToParent(m_pos2);
dir1_abs = Body1.TransformDirectionLocalToParent(m_dir1);
}
else
{
pos1_abs = pos1;
pos2_abs = pos2;
dir1_abs = ChVector.Vnorm(dir1);
m_pos1 = Body1.TransformPointParentToLocal(pos1_abs);
m_pos2 = Body2.TransformPointParentToLocal(pos2_abs);
m_dir1 = Body1.TransformDirectionParentToLocal(dir1_abs);
}
ChVector d12_abs = pos2_abs - pos1_abs;
m_cur_dist = d12_abs.Length();
m_dist = auto_distance ? m_cur_dist : distance;
m_cur_dot = ChVector.Vdot(d12_abs, dir1_abs);
}
/// Initialize this constraint, given the two bodies to be connected, the
/// positions of the two anchor endpoints of the distance (each expressed
/// in body or abs. coordinates) and the imposed distance.
public virtual bool Initialize(
ChBodyFrame mbody1, //< first frame to link
ChBodyFrame mbody2, //< second frame to link
bool pos_are_relative, //< true: following pos. are relative to bodies
ChVector mpos1, //< pos. of distance endpoint, for 1st body (rel. or abs., see flag above)
ChVector mpos2, //< pos. of distance endpoint, for 2nd body (rel. or abs., see flag above)
bool auto_distance = true, //< if true, initializes the imposed distance as the distance between mpos1 and mpos2
double mdistance = 0 //< imposed distance (no need to define, if auto_distance=true.)
)
{
Body1 = mbody1;
Body2 = mbody2;
Cx.SetVariables(Body1.Variables(), Body2.Variables());
if (pos_are_relative)
{
pos1 = mpos1;
pos2 = mpos2;
}
else
{
pos1 = Body1.TransformPointParentToLocal(mpos1);
pos2 = Body2.TransformPointParentToLocal(mpos2);
}
ChVector AbsDist = Body1.TransformPointLocalToParent(pos1) - Body2.TransformPointLocalToParent(pos2);
curr_dist = AbsDist.Length();
if (auto_distance)
{
distance = curr_dist;
}
else
{
distance = mdistance;
}
return(true);
}