/// Given the position of a point in parent frame coords, and
/// assuming it has an absolute speed parentspeed,
/// return the speed in local coords.
public ChVector PointSpeedParentToLocal(ChVector parentpos, ChVector parentspeed)
{
ChFrame <Real> f = new ChFrame <Real>();
ChVector localpos = f.TransformParentToLocal(parentpos);
return(this.Amatrix.MatrT_x_Vect(
parentspeed - coord_dt.pos -
((coord_dt.rot % new ChQuaternion(0, localpos) % this.coord.rot.GetConjugate()).GetVector() * 2)));
}
/// Given the position of a point in parent frame coords, and
/// assuming it has an absolute speed parentspeed and absolute
/// acceleration parentacc, return the acceleration in local coords.
public ChVector PointAccelerationParentToLocal(ChVector parentpos,
ChVector parentspeed,
ChVector parentacc)
{
ChFrame <Real> f = this;// new ChFrame<Real>();
ChVector localpos = f.TransformParentToLocal(parentpos);
ChVector localspeed = PointSpeedParentToLocal(parentpos, parentspeed);
return(this.Amatrix.MatrT_x_Vect(
parentacc - coord_dtdt.pos -
(coord_dtdt.rot % new ChQuaternion(0, localpos) % this.coord.rot.GetConjugate()).GetVector() * 2 -
(coord_dt.rot % new ChQuaternion(0, localpos) % coord_dt.rot.GetConjugate()).GetVector() * 2 -
(coord_dt.rot % new ChQuaternion(0, localspeed) % this.coord.rot.GetConjugate()).GetVector() * 4));
}
public override void update(double mytime, bool update_assets)
{
// Inherit parent class:
base.update(mytime, update_assets);
// Override the rotational jacobian [Cq] and the rotational residual C,
// by assuming an additional hidden frame that rotates about frame2:
if (this.Body1 != null && this.Body2 != null)
{
ChFrame <double> aframe1 = ChFrame <double> .BitShiftRight(this.frame1, (this.Body1));
ChFrame <double> aframe2 = ChFrame <double> .BitShiftRight(this.frame2, (this.Body2));
ChFrame <double> aframe12 = new ChFrame <double>();
aframe2.TransformParentToLocal(aframe1, aframe12);
ChFrame <double> aframe2rotating = new ChFrame <double>();
double aux_rotation;
if (this.avoid_angle_drift)
{
aux_rotation = this.aux_dt + this.rot_offset;
}
else
{
// to have it aligned to current rot, to allow C=0.
aux_rotation = aframe12.GetRot().Q_to_Rotv().z;
}
aframe2rotating.SetRot(aframe2.GetRot() * ChQuaternion.Q_from_AngAxis2(aux_rotation, ChVector.VECT_Z));
// TODO this needs to be addressed, with it it causes rotation problems, seems to work fine without the TransformParentToLocal?
ChFrame <double> aframe12rotating = new ChFrame <double>();
// aframe2rotating.TransformParentToLocal(aframe1, aframe12rotating);
ChMatrix33 <double> Jw1 = new ChMatrix33 <double>();
ChMatrix33 <double> Jw2 = new ChMatrix33 <double>();
ChMatrix33 <double> mtempM = new ChMatrix33 <double>();
ChMatrix33 <double> mtempQ = new ChMatrix33 <double>();
ChMatrix33 <double> abs_plane_rotating = aframe2rotating.GetA();
Jw1.nm.matrix.MatrTMultiply(abs_plane_rotating.nm.matrix, Body1.GetA().nm.matrix);
Jw2.nm.matrix.MatrTMultiply(abs_plane_rotating.nm.matrix, Body2.GetA().nm.matrix);
Jw2.nm.matrix.MatrNeg();
// TODO this also needs to be addressed, with it it causes rotation problems/
// Premultiply by Jw1 and Jw2 by 0.5*[Fp(q_resid)]' to get residual as imaginary part of a quaternion.
/* mtempM.Set_X_matrix((aframe12rotating.GetRot().GetVector()) * 0.5);
* mtempM[0, 0] = 0.5 * aframe12rotating.GetRot().e0;
* mtempM[1, 1] = 0.5 * aframe12rotating.GetRot().e0;
* mtempM[2, 2] = 0.5 * aframe12rotating.GetRot().e0;
* mtempQ.MatrTMultiply(mtempM, Jw1);
* Jw1 = mtempQ;
* mtempQ.MatrTMultiply(mtempM, Jw2);
* Jw2 = mtempQ;*/
int nc = 0;
if (c_x)
{
nc++;
}
if (c_y)
{
nc++;
}
if (c_z)
{
nc++;
}
if (c_rx)
{
this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e1;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 0, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 0, 0, 1, 3, 0, 3);
nc++;
}
if (c_ry)
{
this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e2;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 1, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 1, 0, 1, 3, 0, 3);
nc++;
}
if (c_rz)
{
this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e3;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 2, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 2, 0, 1, 3, 0, 3);
nc++;
}
}
}
public override void update(double mytime, bool update_assets)
{
// Inherit parent class:
base.update(mytime, update_assets);
// Override the rotational jacobian [Cq] and the rotational residual C,
// by assuming an additional hidden frame that rotates about frame2:
if (this.Body1 != null && this.Body2 != null)
{
ChFrame <double> aframe1 = this.frame1.BitShiftRight(this.Body1);
ChFrame <double> aframe2 = this.frame2.BitShiftRight(this.Body2);
ChFrame <double> aframe12 = new ChFrame <double>();// ChFrame<double>.FNULL;
aframe2.TransformParentToLocal(aframe1, aframe12);
ChFrame <double> aframe2rotating = new ChFrame <double>();// ChFrame<double>.FNULL;
double aux_rotation;
aux_rotation = m_func.Get_y(mytime) + rot_offset;
aframe2rotating.SetRot(aframe2.GetRot() * ChQuaternion.Q_from_AngAxis2(aux_rotation, ChVector.VECT_Z));
ChFrame <double> aframe12rotating = new ChFrame <double>();
// aframe2rotating.TransformParentToLocal(aframe1, aframe12rotating);
ChMatrix33 <double> Jw1 = new ChMatrix33 <double>(0), Jw2 = new ChMatrix33 <double>(0);
ChMatrix33 <double> mtempM = new ChMatrix33 <double>(0), mtempQ = new ChMatrix33 <double>(0);
ChMatrix33 <double> abs_plane_rotating = aframe2rotating.GetA();
Jw1.nm.matrix.MatrTMultiply(abs_plane_rotating.nm.matrix, Body1.GetA().nm.matrix);
Jw2.nm.matrix.MatrTMultiply(abs_plane_rotating.nm.matrix, Body2.GetA().nm.matrix);
Jw2.nm.matrix.MatrNeg();
// Premultiply by Jw1 and Jw2 by 0.5*[Fp(q_resid)]' to get residual as imaginary part of a quaternion.
/* mtempM.Set_X_matrix((aframe12rotating.GetRot().GetVector()) * 0.5);
* mtempM.nm.matrix[0, 0] = 0.5 * aframe12rotating.GetRot().e0;
* mtempM.nm.matrix[1, 1] = 0.5 * aframe12rotating.GetRot().e0;
* mtempM.nm.matrix[2, 2] = 0.5 * aframe12rotating.GetRot().e0;
* mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw1.nm.matrix);
* Jw1 = mtempQ;
* mtempQ.nm.matrix.MatrTMultiply(mtempM.nm.matrix, Jw2.nm.matrix);
* Jw2 = mtempQ;*/
int nc = 0;
if (c_x)
{
nc++;
}
if (c_y)
{
nc++;
}
if (c_z)
{
nc++;
}
if (c_rx)
{
this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e1;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 0, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 0, 0, 1, 3, 0, 3);
nc++;
}
if (c_ry)
{
this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e2;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 1, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 1, 0, 1, 3, 0, 3);
nc++;
}
if (c_rz)
{
this.C.matrix.ElementN(nc) = aframe12rotating.GetRot().e3;
this.mask.Constr_N(nc).Get_Cq_a().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_b().FillElem(0);
this.mask.Constr_N(nc).Get_Cq_a().PasteClippedMatrix(Jw1.nm.matrix, 2, 0, 1, 3, 0, 3);
this.mask.Constr_N(nc).Get_Cq_b().PasteClippedMatrix(Jw2.nm.matrix, 2, 0, 1, 3, 0, 3);
nc++;
}
}
}
