/// 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..)
}
//
// STATE FUNCTIONS
//
/// Adds force to residual R, as R*= F*c
/// NOTE: here the off offset in R is NOT used because add F at the TWO offsets of the two connected bodies,
/// so it is assumed that offsets for Body1 and Body2 variables have been already set properly!
public override void IntLoadResidual_F(int off, ref ChVectorDynamic <double> R, double c)
{
if (Body1 == null || Body2 == null)
{
return;
}
ChVector mbody_force = new ChVector(0, 0, 0);
ChVector mbody_torque = new ChVector(0, 0, 0);
if (ChVector.Vnotnull(C_force))
{
ChVector m_abs_force = Body2.GetA().Matr_x_Vect(marker2.FrameMoving.GetA().Matr_x_Vect(C_force));
if (Body2.Variables().IsActive())
{
Body2.To_abs_forcetorque(m_abs_force,
marker1.GetAbsCoord().pos, // absolute application point is always marker1
false, // from abs. space
ref mbody_force, ref mbody_torque); // resulting force-torque, both in abs coords
R.matrix.PasteSumVector(mbody_force * -c, Body2.Variables().GetOffset(), 0);
R.matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(mbody_torque) * -c,
Body2.Variables().GetOffset() + 3, 0);
}
if (Body1.Variables().IsActive())
{
Body1.To_abs_forcetorque(m_abs_force,
marker1.GetAbsCoord().pos, // absolute application point is always marker1
false, // from abs. space
ref mbody_force, ref mbody_torque); // resulting force-torque, both in abs coords
R.matrix.PasteSumVector(mbody_force * c, Body1.Variables().GetOffset(), 0);
R.matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(mbody_torque) * c,
Body1.Variables().GetOffset() + 3, 0);
}
}
if (ChVector.Vnotnull(C_torque))
{
ChVector m_abs_torque = Body2.GetA().Matr_x_Vect(marker2.FrameMoving.GetA().Matr_x_Vect(C_torque));
// load torques in 'fb' vector accumulator of body variables (torques in local coords)
if (Body1.Variables().IsActive())
{
R.matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(m_abs_torque) * c,
Body1.Variables().GetOffset() + 3, 0);
}
if (Body2.Variables().IsActive())
{
R.matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(m_abs_torque) * -c,
Body2.Variables().GetOffset() + 3, 0);
}
}
}
/// 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()));
}
//
// SOLVER INTERFACE (OLD)
//
public override void ConstraintsFbLoadForces(double factor = 1)
{
// compute instant torque
double mT = m_func.Get_y(this.GetChTime());
ChFrame <double> aframe1 = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1));
ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2));
ChVector m_abs_torque = aframe2.GetA().Matr_x_Vect(new ChVector(0, 0, mT));
Body2.Variables().Get_fb().matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(m_abs_torque) * -factor, 3, 0);
Body1.Variables().Get_fb().matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(m_abs_torque) * factor, 3, 0);
}
/// 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));
}
//
// STATE FUNCTIONS
//
public override void IntLoadResidual_F(int off, ref ChVectorDynamic <double> R, double c)
{
// compute instant torque
double mT = m_func.Get_y(this.GetChTime());
ChFrame <double> aframe1 = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1));
ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2));
ChVector m_abs_torque = aframe2.GetA().Matr_x_Vect(new ChVector(0, 0, mT));
if (Body2.Variables().IsActive())
{
R.matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(m_abs_torque) * -c, Body2.Variables().GetOffset() + 3,
0);
}
if (Body1.Variables().IsActive())
{
R.matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(m_abs_torque) * c, Body1.Variables().GetOffset() + 3,
0);
}
}
//
// SOLVER INTERFACE
//
/// Overrides the empty behaviour of the parent ChLink implementation, which
/// does not consider any user-imposed force between the two bodies.
/// It adds the current link-forces, if any, (caused by springs, etc.) to the 'fb' vectors
/// of the ChVariables referenced by encapsulated ChConstraints.
/// In details, it adds the effect caused by C_force and C_torque.
/// Both C_force and C_torque these forces are considered expressed in the
/// reference coordsystem of marker2 (the MAIN marker),
/// and their application point is the origin of marker1 (the SLAVE marker).
public override void ConstraintsFbLoadForces(double factor = 1)
{
if (Body1 == null || Body2 == null)
{
return;
}
ChVector mbody_force = new ChVector(0, 0, 0);
ChVector mbody_torque = new ChVector(0, 0, 0);
if (ChVector.Vnotnull(C_force))
{
ChVector m_abs_force = Body2.GetA().Matr_x_Vect(marker2.FrameMoving.GetA().Matr_x_Vect(C_force));
Body2.To_abs_forcetorque(m_abs_force,
marker1.GetAbsCoord().pos, // absolute application point is always marker1
false, // from abs. space
ref mbody_force, ref mbody_torque); // resulting force-torque, both in abs coords
Body2.Variables().Get_fb().matrix.PasteSumVector(mbody_force * -factor, 0, 0);
Body2.Variables().Get_fb().matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(mbody_torque) * -factor, 3,
0);
Body1.To_abs_forcetorque(m_abs_force,
marker1.GetAbsCoord().pos, // absolute application point is always marker1
false, // from abs. space
ref mbody_force, ref mbody_torque); // resulting force-torque, both in abs coords
Body1.Variables().Get_fb().matrix.PasteSumVector(mbody_force * factor, 0, 0);
Body1.Variables().Get_fb().matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(mbody_torque) * factor, 3, 0);
}
if (ChVector.Vnotnull(C_torque))
{
ChVector m_abs_torque = Body2.GetA().Matr_x_Vect(marker2.FrameMoving.GetA().Matr_x_Vect(C_torque));
// load torques in 'fb' vector accumulator of body variables (torques in local coords)
Body1.Variables().Get_fb().matrix.PasteSumVector(Body1.TransformDirectionParentToLocal(m_abs_torque) * factor, 3, 0);
Body2.Variables().Get_fb().matrix.PasteSumVector(Body2.TransformDirectionParentToLocal(m_abs_torque) * -factor, 3,
0);
}
}
//
// 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;
}
}
