void ChLinkDistance::Update (double mytime)
{
// Inherit time changes of parent class (ChLink), basically doing nothing :)
ChLink::UpdateTime(mytime);
// compute jacobians
ChVector<> AbsDist = Body1->Point_Body2World(&pos1)-Body2->Point_Body2World(&pos2);
curr_dist = AbsDist.Length();
ChVector<> D2abs = Vnorm(AbsDist);
ChVector<> D2relB = Body2->Dir_World2Body(&D2abs);
ChVector<> D2relA = Body1->Dir_World2Body(&D2abs);
ChVector<> CqAx = D2abs;
ChVector<> CqBx = -D2abs;
ChVector<> CqAr = -Vcross(D2relA,pos1);
ChVector<> CqBr = Vcross(D2relB,pos2);
Cx.Get_Cq_a()->ElementN(0)=(float)CqAx.x;
Cx.Get_Cq_a()->ElementN(1)=(float)CqAx.y;
Cx.Get_Cq_a()->ElementN(2)=(float)CqAx.z;
Cx.Get_Cq_a()->ElementN(3)=(float)CqAr.x;
Cx.Get_Cq_a()->ElementN(4)=(float)CqAr.y;
Cx.Get_Cq_a()->ElementN(5)=(float)CqAr.z;
Cx.Get_Cq_b()->ElementN(0)=(float)CqBx.x;
Cx.Get_Cq_b()->ElementN(1)=(float)CqBx.y;
Cx.Get_Cq_b()->ElementN(2)=(float)CqBx.z;
Cx.Get_Cq_b()->ElementN(3)=(float)CqBr.x;
Cx.Get_Cq_b()->ElementN(4)=(float)CqBr.y;
Cx.Get_Cq_b()->ElementN(5)=(float)CqBr.z;
//***TO DO*** C_dt? C_dtdt? (may be never used..)
}
int ChPathSteeringControllerXT::CalcCurvatureCode(ChVector<>& a, ChVector<>& b) {
// a[] is a unit vector pointing to the left vehicle side
// b[] is a unit vector pointing to the instantanous curve center
a.z() = 0;
a.Normalize();
b.z() = 0;
b.Normalize();
// In a left turn the distance between the two points will be nearly zero
// in a right turn the distance will be around 2, at least > 1
ChVector<> ab = a - b;
double ltest = ab.Length();
// What is a straight line? We define a threshold radius R_threshold
// if the actual curvature is greater than 1/R_treshold, we are in a curve
// otherwise we take this point as part of a straight line
// m_pcurvature is always >= 0
if(m_pcurvature <= 1.0/m_R_threshold) {
return 0; // -> straight line
}
if(ltest < 1.0) {
return 1; // left bending curve
}
return -1; // right bending curve
}
void ChLinkDistance::Update(double mytime, bool update_assets) {
// Inherit time changes of parent class (ChLink), basically doing nothing :)
ChLink::Update(mytime, update_assets);
// compute jacobians
ChVector<> AbsDist = Body1->TransformPointLocalToParent(pos1) - Body2->TransformPointLocalToParent(pos2);
curr_dist = AbsDist.Length();
ChVector<> D2abs = Vnorm(AbsDist);
ChVector<> D2relB = Body2->TransformDirectionParentToLocal(D2abs);
ChVector<> D2relA = Body1->TransformDirectionParentToLocal(D2abs);
ChVector<> CqAx = D2abs;
ChVector<> CqBx = -D2abs;
ChVector<> CqAr = -Vcross(D2relA, pos1);
ChVector<> CqBr = 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..)
}
int ChLinkDistance::Initialize(std::shared_ptr<ChBodyFrame> mbody1,
std::shared_ptr<ChBodyFrame> mbody2,
bool pos_are_relative,
ChVector<> mpos1,
ChVector<> mpos2,
bool auto_distance,
double mdistance) {
Body1 = mbody1.get();
Body2 = mbody2.get();
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;
}
// Get the quaternion from a source vector and a destination vector which specifies
// the rotation from one to the other. The vectors do not need to be normalized.
ChQuaternion<double> Q_from_Vect_to_Vect(const ChVector<double>& fr_vect, const ChVector<double>& to_vect) {
const double ANGLE_TOLERANCE = 1e-6;
ChQuaternion<double> quat;
double halfang;
double sinhalf;
ChVector<double> axis;
double lenXlen = fr_vect.Length() * to_vect.Length();
axis = fr_vect % to_vect;
double sinangle = ChClamp(axis.Length() / lenXlen, -1.0, +1.0);
double cosangle = ChClamp(fr_vect ^ to_vect / lenXlen, -1.0, +1.0);
// Consider three cases: Parallel, Opposite, non-collinear
if (std::abs(sinangle) == 0.0 && cosangle > 0) {
// fr_vect & to_vect are parallel
quat.e0() = 1.0;
quat.e1() = 0.0;
quat.e2() = 0.0;
quat.e3() = 0.0;
} else if (std::abs(sinangle) < ANGLE_TOLERANCE && cosangle < 0) {
// fr_vect & to_vect are opposite, i.e. ~180 deg apart
axis = fr_vect.GetOrthogonalVector() + (-to_vect).GetOrthogonalVector();
axis.Normalize();
quat.e0() = 0.0;
quat.e1() = ChClamp(axis.x(), -1.0, +1.0);
quat.e2() = ChClamp(axis.y(), -1.0, +1.0);
quat.e3() = ChClamp(axis.z(), -1.0, +1.0);
} else {
// fr_vect & to_vect are not co-linear case
axis.Normalize();
halfang = 0.5 * ChAtan2(sinangle, cosangle);
sinhalf = sin(halfang);
quat.e0() = cos(halfang);
quat.e1() = ChClamp(axis.x(), -1.0, +1.0);
quat.e2() = ChClamp(axis.y(), -1.0, +1.0);
quat.e3() = ChClamp(axis.z(), -1.0, +1.0);
}
return (quat);
}
int ChLinkDistance::Initialize(ChSharedPtr<ChBody>& mbody1, ///< first body to link
ChSharedPtr<ChBody>& mbody2, ///< second body to link
bool pos_are_relative,///< true: following posit. are considered relative to bodies. false: pos.are absolute
ChVector<> mpos1, ///< position of distance endpoint, for 1st body (rel. or abs., see flag above)
ChVector<> mpos2, ///< position of distance endpoint, for 2nd body (rel. or abs., see flag above)
bool auto_distance,///< if true, initializes the imposed distance as the distance between mpos1 and mpos2
double mdistance ///< imposed distance (no need to define, if auto_distance=true.)
)
{
this->Body1 = mbody1.get_ptr();
this->Body2 = mbody2.get_ptr();
this->Cx.SetVariables(&this->Body1->Variables(),&this->Body2->Variables());
if (pos_are_relative)
{
this->pos1 = mpos1;
this->pos2 = mpos2;
}
else
{
this->pos1 = this->Body1->Point_World2Body(&mpos1);
this->pos2 = this->Body2->Point_World2Body(&mpos2);
}
ChVector<> AbsDist = Body1->Point_Body2World(&pos1)-Body2->Point_Body2World(&pos2);
this->curr_dist = AbsDist.Length();
if (auto_distance)
{
this->distance = this->curr_dist;
}
else
{
this->distance = mdistance;
}
return true;
}
double ChPathSteeringControllerXT::CalcHeadingError(ChVector<>& a, ChVector<>& b) {
double ang = 0.0;
// test for velocity > 0
m_vel.z() = 0;
m_vel.Normalize();
double speed = m_vel.Length();
if(speed < 1) {
// vehicle is standing still, we take the chassis orientation
a.z() = 0;
b.z() = 0;
a.Normalize();
b.Normalize();
} else {
// vehicle is running, we take the {x,y} velocity vector
a = m_vel;
b.z() = 0;
b.Normalize();
}
// it might happen to cruise against the path definition (end->begin instead of begin->end),
// then the the tangent points backwards to driving direction
// the distance |ab| is > 1 then
ChVector<> ab = a - b;
double ltest = ab.Length();
ChVector<> vpc;
if(ltest < 1) {
vpc = Vcross(a,b);
} else {
vpc = Vcross(a,-b);
}
ang = std::asin(vpc.z());
return ang;
}
void ChLinkSpring::Initialize(std::shared_ptr<ChBody> mbody1,
std::shared_ptr<ChBody> mbody2,
bool pos_are_relative,
ChVector<> mpos1,
ChVector<> mpos2,
bool auto_rest_length,
double mrest_length) {
// First, initialize as all constraint with markers.
// In this case, create the two markers also!.
ChLinkMarkers::Initialize(mbody1, mbody2, CSYSNORM);
if (pos_are_relative) {
marker1->Impose_Rel_Coord(ChCoordsys<>(mpos1, QUNIT));
marker2->Impose_Rel_Coord(ChCoordsys<>(mpos2, QUNIT));
} else {
marker1->Impose_Abs_Coord(ChCoordsys<>(mpos1, QUNIT));
marker2->Impose_Abs_Coord(ChCoordsys<>(mpos2, QUNIT));
}
ChVector<> AbsDist = marker1->GetAbsCoord().pos - marker2->GetAbsCoord().pos;
dist = AbsDist.Length();
spr_restlength = auto_rest_length ? dist : mrest_length;
}
