Vector ChLinkPulley::Get_shaft_pos2()
{
if (this->Body1)
{
ChFrame<double> absframe;
((ChFrame<double>*)Body2)->TrasformLocalToParent(local_shaft2, absframe);
return absframe.GetPos();
} else return VNULL;
}
Vector ChLinkPulley::Get_shaft_dir2()
{
if (this->Body1)
{
ChFrame<double> absframe;
((ChFrame<double>*)Body2)->TrasformLocalToParent(local_shaft2, absframe);
return absframe.GetA()->Get_A_Zaxis();
} else return VECT_Z;
}
void ChCascadeDoc::FromChronoToCascade(const ChFrame<>& from_coord, TopLoc_Location& to_coord) {
const ChVector<>& mpos = from_coord.GetPos();
gp_Vec mtr(mpos.x(), mpos.y(), mpos.z());
const ChMatrix33<>& from_mat = from_coord.GetA();
((gp_Trsf)(to_coord.Transformation()))
.SetValues(from_mat(0, 0), from_mat(0, 1), from_mat(0, 2), mpos.x(), from_mat(1, 0), from_mat(1, 1),
from_mat(1, 2), mpos.y(), from_mat(2, 0), from_mat(2, 1), from_mat(2, 2), mpos.z()); //0, 0);
}
void ChPathSteeringControllerXT::CalcTargetLocation() {
// Let the underlying tracker do its magic.
// we need more information about the path properties here:
ChFrame<> tnb;
m_tracker->calcClosestPoint(m_sentinel, tnb, m_pcurvature);
m_target = tnb.GetPos();
m_ptangent = tnb.GetRot().GetXaxis();
m_pnormal = tnb.GetRot().GetYaxis();
m_pbinormal = tnb.GetRot().GetZaxis();
}
void ChCascadeDoc::FromCascadeToChrono(const TopLoc_Location& from_coord, ChFrame<>& to_coord) {
gp_XYZ mtr = from_coord.Transformation().TranslationPart();
to_coord.SetPos(ChVector<>(mtr.X(), mtr.Y(), mtr.Z()));
gp_Mat mro = from_coord.Transformation().VectorialPart();
ChMatrix33<> to_mat;
to_mat(0, 0) = mro(1, 1);
to_mat(0, 1) = mro(1, 2);
to_mat(0, 2) = mro(1, 3);
to_mat(1, 0) = mro(2, 1);
to_mat(1, 1) = mro(2, 2);
to_mat(1, 2) = mro(2, 3);
to_mat(2, 0) = mro(3, 1);
to_mat(2, 1) = mro(3, 2);
to_mat(2, 2) = mro(3, 3);
to_coord.SetRot(to_mat);
}
