////////////////////////////////////
///
/// UPDATING PROCEDURES
///////// 4.5- UPDATE Cqw1 and Cqw2
/////////
public void Transform_Cq_to_Cqw(ChMatrix mCq, ChMatrix mCqw, ChBodyFrame mbody)
{
// if (mCq == null)
// return;
// translational part - not changed
mCqw.PasteClippedMatrix(mCq, 0, 0, mCq.GetRows(), 3, 0, 0);
// rotational part [Cq_w] = [Cq_q]*[Gl]'*1/4
int col, row, colres;
double sum;
// ChMatrixNM<IntInterface.Three, IntInterface.Four> mGl = new ChMatrixNM<IntInterface.Three, IntInterface.Four>(0);
ChFrame <double> .SetMatrix_Gl(ref mGl, mbody.GetCoord().rot);
for (colres = 0; colres < 3; colres++)
{
for (row = 0; row < (mCq.GetRows()); row++)
{
sum = 0;
for (col = 0; col < 4; col++)
{
sum += ((mCq.GetElement(row, col + 3)) * (mGl.matrix.GetElement(colres, col)));
}
mCqw.SetElement(row, colres + 3, sum * 0.25);
}
}
}
/// Initialization based on passing two vectors (point + dir) on the
/// two bodies, they will represent the X axes of the two frames (Y and Z will
/// be built from the X vector via Gram Schmidt orthonormalization).
/// Use the other ChLinkMateGeneric::Initialize() if you want to set the two frames directly.
public virtual void Initialize(ChBodyFrame mbody1, //< first body to link
ChBodyFrame mbody2, //< second body to link
bool pos_are_relative, //< true: following pos. are relative to bodies.
ChVector mpt1, //< origin of slave frame 1, for 1st body (rel. or abs., see flag above)
ChVector mpt2, //< origin of master frame 2, for 2nd body (rel. or abs., see flag above)
ChVector mnorm1, //< X axis of slave plane, for 1st body (rel. or abs., see flag above)
ChVector mnorm2 //< X axis of master plane, for 2nd body (rel. or abs., see flag above)
)
{
Debug.Assert(mbody1 != mbody2);
this.Body1 = mbody1;
this.Body2 = mbody2;
// this.SetSystem(mbody1.GetSystem());
this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables());
ChVector mx = new ChVector(0, 0, 0);
ChVector my = new ChVector(0, 0, 0);
ChVector mz = new ChVector(0, 0, 0);
ChVector mN = new ChVector(0, 0, 0);
ChMatrix33 <double> mrot = new ChMatrix33 <double>();
ChFrame <double> mfr1 = new ChFrame <double>();
ChFrame <double> mfr2 = new ChFrame <double>();
if (pos_are_relative)
{
mN = mnorm1;
mN.DirToDxDyDz(ref mx, ref my, ref mz, new ChVector(0, 1, 0));
mrot.Set_A_axis(mx, my, mz);
mfr1.SetRot(mrot);
mfr1.SetPos(mpt1);
mN = mnorm2;
mN.DirToDxDyDz(ref mx, ref my, ref mz, new ChVector(0, 1, 0));
mrot.Set_A_axis(mx, my, mz);
mfr2.SetRot(mrot);
mfr2.SetPos(mpt2);
}
else
{
ChVector temp = ChVector.VECT_Z;
// from abs to body-rel
mN = this.Body1.TransformDirectionParentToLocal(mnorm1);
mN.DirToDxDyDz(ref mx, ref my, ref mz, temp);
mrot.Set_A_axis(mx, my, mz);
mfr1.SetRot(mrot);
mfr1.SetPos(this.Body1.TransformPointParentToLocal(mpt1));
mN = this.Body2.TransformDirectionParentToLocal(mnorm2);
mN.DirToDxDyDz(ref mx, ref my, ref mz, temp);
mrot.Set_A_axis(mx, my, mz);
mfr2.SetRot(mrot);
mfr2.SetPos(this.Body2.TransformPointParentToLocal(mpt2));
}
this.frame1 = mfr1;
this.frame2 = mfr2;
}
/// Specialized initialization for generic mate, given the two bodies to be connected, and
/// the absolute position of the mate (the two frames to connect on the bodies will be initially cohincindent to that frame)
public virtual void Initialize(ChBodyFrame mbody1, //< first body to link
ChBodyFrame mbody2, //< second body to link
ChFrame <double> mabsframe //< mate frame (both for slave and master), in abs. coordinate
)
{
this.Initialize(mbody1, mbody2, false, mabsframe, mabsframe);
}
/// Specialized initialization for generic mate, given the two bodies to be connected, the
/// positions of the two frames to connect on the bodies (each expressed
/// in body or abs. coordinates).
public virtual void Initialize(ChBodyFrame mbody1, //< first body to link
ChBodyFrame mbody2, /// second body to link
bool pos_are_relative, //< true: following pos. are relative to bodies.
ChFrame <double> mpos1, //< mate frame (slave), for 1st body (rel. or abs., see flag above)
ChFrame <double> mpos2 //< mate frame (master), for 2nd body (rel. or abs., see flag above)
)
{
Debug.Assert(mbody1 != mbody2);
this.Body1 = mbody1;
this.Body2 = mbody2;
// this.SetSystem(mbody1.GetSystem());
this.mask.SetTwoBodiesVariables(Body1.Variables(), Body2.Variables());
if (pos_are_relative)
{
this.frame1 = mpos1;
this.frame2 = mpos2;
}
else
{
// from abs to body-rel
(this.Body1).TransformParentToLocal(mpos1, this.frame1);
this.Body2.TransformParentToLocal(mpos2, this.frame2);
}
}
public ChLink(ChLink other) : base(other)
{
Body1 = null;
Body2 = null;
react_force = other.react_force;
react_torque = other.react_torque;
}
/// 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);
}
/// Use this function after object creation, to initialize it, given
/// the 1D shaft and 3D body to join.
/// Each item must belong to the same ChSystem.
/// Direction is expressed in the local coordinates of the body.
public bool Initialize(ChShaft mshaft, //< shaft to join
ChBodyFrame mbody, //< body to join
ChVector mdir //< the direction of the shaft on 3D body (applied on COG: pure torque)
)
{
ChShaft mm1 = mshaft;
ChBodyFrame mm2 = mbody;
//Debug.Assert(mm1 == null && mm2 == null);
shaft = mm1;
body.BodyFrame = mm2;
shaft_dir = ChVector.Vnorm(mdir);
constraint.SetVariables(mm1.Variables(), mm2.Variables());
SetSystem(shaft.GetSystem());
return(true);
}
/// Initialize this joint by specifying the two bodies to be connected and the
/// joint frames on each body. 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.
public void Initialize(ChBodyFrame body1, //< first body frame
ChBodyFrame body2, //< second body frame
bool local, //< true if data given in body local frames
ChFrame <double> frame1, //< joint frame on body 1
ChFrame <double> frame2 //< joint frame on body 2
)
{
Body1 = body1;
Body2 = body2;
m_cnstr_x.SetVariables(Body1.Variables(), Body2.Variables());
m_cnstr_y.SetVariables(Body1.Variables(), Body2.Variables());
m_cnstr_z.SetVariables(Body1.Variables(), Body2.Variables());
m_cnstr_dot.SetVariables(Body1.Variables(), Body2.Variables());
ChFrame <double> frame1_abs;
ChFrame <double> frame2_abs;
if (local)
{
m_frame1 = frame1;
m_frame2 = frame2;
frame1_abs = ChFrame <double> .BitShiftRight(frame1, Body1);
frame2_abs = ChFrame <double> .BitShiftRight(frame2, Body2);
}
else
{
((ChFrame <double>)Body1).TransformParentToLocal(frame1, m_frame1);
((ChFrame <double>)Body2).TransformParentToLocal(frame2, m_frame2);
frame1_abs = frame1;
frame2_abs = frame2;
}
m_u1_tilde.Set_X_matrix(m_frame1.GetA().Get_A_Xaxis());
m_v2_tilde.Set_X_matrix(m_frame2.GetA().Get_A_Yaxis());
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);
m_C.matrix.SetElement(3, 0, ChVector.Vdot(frame1_abs.GetA().Get_A_Xaxis(), frame2_abs.GetA().Get_A_Yaxis()));
}
/// Sets up the markers associated with the engine link
public override void SetUpMarkers(ChMarker mark1, ChMarker mark2)
{
base.SetUpMarkers(mark1, mark2);
if (Body1 != null && Body2 != null)
{
// Note: we wrap Body1 and Body2 in shared_ptr with custom no-op destructors
// so that the two objects are not destroyed when these shared_ptr go out of
// scope (since Body1 and Body2 are still managed through other shared_ptr).
ChBodyFrame b1 = new ChBodyFrame(Body1);
ChBodyFrame b2 = new ChBodyFrame(Body2);
if (innerconstraint1)
{
innerconstraint1.Initialize(innershaft1, b1, ChVector.VECT_Z);
}
if (innerconstraint2)
{
innerconstraint2.Initialize(innershaft2, b2, ChVector.VECT_Z);
}
}
}
/// 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);
}
/// Initialize this joint by specifying the two bodies to be connected, a
/// coordinate system specified in the absolute frame, and the distance of
/// the massless connector. The composite joint is ructed such that the
/// direction of the revolute joint is aligned with the z axis of the specified
/// coordinate system and the spherical joint is at the specified distance
/// along the x axis.
public void Initialize(ChBodyFrame body1, //< first frame (revolute side)
ChBodyFrame body2, //< second frame (spherical side)
ChCoordsys csys, //< joint coordinate system (in absolute frame)
double distance //< imposed distance
)
{
Body1 = body1;
Body2 = body2;
m_cnstr_dist.SetVariables(Body1.Variables(), Body2.Variables());
m_cnstr_dot.SetVariables(Body1.Variables(), Body2.Variables());
ChVector x_Axis = csys.rot.GetXaxis();
ChVector z_axis = csys.rot.GetZaxis();
m_pos1 = Body1.TransformPointParentToLocal(csys.pos);
m_dir1 = Body1.TransformDirectionParentToLocal(z_axis);
m_pos2 = Body2.TransformPointParentToLocal(csys.pos + distance * x_Axis);
m_dist = distance;
m_cur_dist = distance;
m_cur_dot = 0;
}
/// Initialize this joint by specifying the two bodies to be connected and a
/// joint frame specified in the absolute frame. Two local joint frames, one
/// on each body, are constructed so that they coincide with the specified
/// global joint frame at the current configuration. The kinematics of the
/// universal joint are obtained by imposing that the origins of these two
/// frames are the same and that the X axis of the joint frame on body 1 and
/// the Y axis of the joint frame on body 2 are perpendicular.
public void Initialize(ChBodyFrame body1, //< first body frame
ChBodyFrame body2, //< second body frame
ChFrame <double> frame //< joint frame (in absolute frame)
)
{
Body1 = body1;
Body2 = body2;
m_cnstr_x.SetVariables(Body1.Variables(), Body2.Variables());
m_cnstr_y.SetVariables(Body1.Variables(), Body2.Variables());
m_cnstr_z.SetVariables(Body1.Variables(), Body2.Variables());
m_cnstr_dot.SetVariables(Body1.Variables(), Body2.Variables());
((ChFrame <double>)Body1).TransformParentToLocal(frame, m_frame1);
((ChFrame <double>)Body2).TransformParentToLocal(frame, m_frame2);
m_u1_tilde.Set_X_matrix(m_frame1.GetA().Get_A_Xaxis());
m_v2_tilde.Set_X_matrix(m_frame2.GetA().Get_A_Yaxis());
m_C.matrix.SetElement(0, 0, 0.0);
m_C.matrix.SetElement(1, 0, 0.0);
m_C.matrix.SetElement(2, 0, 0.0);
m_C.matrix.SetElement(3, 0, 0.0);
}
public ChBodyFrame(ChBodyFrame other) : base(other)
{
}