// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
const ChVector<> M113a_Suspension::GetLocation(PointId which) {
ChVector<> point;
switch (which) {
case ARM:
point = ChVector<>(0.213, -0.12, 0.199);
break;
case ARM_WHEEL:
point = ChVector<>(0, -0.12, 0);
break;
case ARM_CHASSIS:
point = ChVector<>(0.232, -0.12, 0.217);
break;
case SHOCK_A:
point = ChVector<>(0, -0.12, 0);
break;
case SHOCK_C:
point = ChVector<>(0, -0.12, 1);
break;
default:
point = ChVector<>(0, 0, 0);
break;
}
if (m_side == RIGHT)
point.y() *= -1;
return point;
}
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 X axis of a coordsystem, given the quaternion which
// represents the alignment of the coordsystem.
ChVector<double> VaxisXfromQuat(const ChQuaternion<double>& quat) {
ChVector<double> res;
res.x() = (pow(quat.e0(), 2) + pow(quat.e1(), 2)) * 2 - 1;
res.y() = ((quat.e1() * quat.e2()) + (quat.e0() * quat.e3())) * 2;
res.z() = ((quat.e1() * quat.e3()) - (quat.e0() * quat.e2())) * 2;
return res;
}
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..)
}
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
const ChVector<> M113_Idler::GetLocation(PointId which) {
ChVector<> point;
switch (which) {
case WHEEL:
point = ChVector<>(0, 0, 0);
break;
case CARRIER:
point = ChVector<>(0, -0.1, 0);
break;
case CARRIER_CHASSIS:
point = ChVector<>(0, -0.2, 0);
break;
case TSDA_CARRIER:
point = ChVector<>(0, -0.2, 0);
break;
case TSDA_CHASSIS:
point = ChVector<>(0.5, -0.2, 0);
break;
default:
point = ChVector<>(0, 0, 0);
break;
}
if (GetVehicleSide() == RIGHT)
point.y() *= -1;
return point;
}
// Given the imaginary (vectorial) {e1 e2 e3} part of a quaternion time derivative,
// find the entire quaternion q = {e0, e1, e2, e3}.
// Note: singularities are possible.
ChQuaternion<double> ImmQ_dt_complete(const ChQuaternion<double>& mq, const ChVector<double>& qimm_dt) {
ChQuaternion<double> mqdt;
mqdt.e1() = qimm_dt.x();
mqdt.e2() = qimm_dt.y();
mqdt.e3() = qimm_dt.z();
mqdt.e0() = (-mq.e1() * mqdt.e1() - mq.e2() * mqdt.e2() - mq.e3() * mqdt.e3()) / mq.e0();
return mqdt;
}
// Given the imaginary (vectorial) {e1 e2 e3} part of a quaternion,
// find the entire quaternion q = {e0, e1, e2, e3}.
// Note: singularities are possible.
ChQuaternion<double> ImmQ_complete(const ChVector<double>& qimm) {
ChQuaternion<double> mq;
mq.e1() = qimm.x();
mq.e2() = qimm.y();
mq.e3() = qimm.z();
mq.e0() = sqrt(1 - mq.e1() * mq.e1() - mq.e2() * mq.e2() - mq.e3() * mq.e3());
return mq;
}
void ChModelBullet::GetAABB(ChVector<>& bbmin, ChVector<>& bbmax) const
{
btVector3 btmin;
btVector3 btmax;
if (bt_collision_object->getCollisionShape())
bt_collision_object->getCollisionShape()->getAabb( bt_collision_object->getWorldTransform(), btmin, btmax);
bbmin.Set(btmin.x(), btmin.y(), btmin.z());
bbmax.Set(btmax.x(), btmax.y(), btmax.z());
}
void AddWall(std::shared_ptr<ChBody> body, const ChVector<>& dim, const ChVector<>& loc) {
body->GetCollisionModel()->AddBox(dim.x(), dim.y(), dim.z(), loc);
auto box = std::make_shared<ChBoxShape>();
box->GetBoxGeometry().Size = dim;
box->GetBoxGeometry().Pos = loc;
box->SetColor(ChColor(1, 0, 0));
box->SetFading(0.6f);
body->AddAsset(box);
}
// Get the quaternion time derivative from the vector of angular speed, with w specified in _local_ coords.
ChQuaternion<double> Qdt_from_Wrel(const ChVector<double>& w, const ChQuaternion<double>& q) {
ChQuaternion<double> qw;
double half = 0.5;
qw.e0() = 0;
qw.e1() = w.x();
qw.e2() = w.y();
qw.e3() = w.z();
return Qscale(Qcross(q, qw), half); // {q_dt} = 1/2 {q}*{0,w_rel}
}
void WrdStyleSheet::GetStyleList(ChUINT2 in_unParagraphStyleIndex, ChVector<ChUINT2>& vecStyles) const
{
ChUINT2 unStyleIndex = in_unParagraphStyleIndex;
while (unStyleIndex != 0x0FFF)
{
vecStyles.push_back(unStyleIndex);
const WrdStyle& style = getStyle(unStyleIndex);
unStyleIndex = style.getBaseStyleIndex();
}
if (vecStyles.size() == 0 || vecStyles[vecStyles.size() - 1] != 0)
vecStyles.push_back(0);
}
// Get the quaternion from an angle of rotation and an axis, defined in _abs_ coords.
// The axis is supposed to be fixed, i.e. it is constant during rotation.
// The 'axis' vector must be normalized.
ChQuaternion<double> Q_from_AngAxis(double angle, const ChVector<double>& axis) {
ChQuaternion<double> quat;
double halfang;
double sinhalf;
halfang = (angle * 0.5);
sinhalf = sin(halfang);
quat.e0() = cos(halfang);
quat.e1() = axis.x() * sinhalf;
quat.e2() = axis.y() * sinhalf;
quat.e3() = axis.z() * sinhalf;
return (quat);
}
ChVector<double> Q_to_NasaAngles(const ChQuaternion<double>& q1) {
ChVector<double> mnasa;
double sqw = q1.e0() * q1.e0();
double sqx = q1.e1() * q1.e1();
double sqy = q1.e2() * q1.e2();
double sqz = q1.e3() * q1.e3();
// heading
mnasa.z() = atan2(2.0 * (q1.e1() * q1.e2() + q1.e3() * q1.e0()), (sqx - sqy - sqz + sqw));
// bank
mnasa.y() = atan2(2.0 * (q1.e2() * q1.e3() + q1.e1() * q1.e0()), (-sqx - sqy + sqz + sqw));
// attitude
mnasa.x() = asin(-2.0 * (q1.e1() * q1.e3() - q1.e2() * q1.e0()));
return mnasa;
}
ChQuaternion<double> Q_from_NasaAngles(const ChVector<double>& mang) {
ChQuaternion<double> mq;
double c1 = cos(mang.z() / 2);
double s1 = sin(mang.z() / 2);
double c2 = cos(mang.x() / 2);
double s2 = sin(mang.x() / 2);
double c3 = cos(mang.y() / 2);
double s3 = sin(mang.y() / 2);
double c1c2 = c1 * c2;
double s1s2 = s1 * s2;
mq.e0() = c1c2 * c3 + s1s2 * s3;
mq.e1() = c1c2 * s3 - s1s2 * c3;
mq.e2() = c1 * s2 * c3 + s1 * c2 * s3;
mq.e3() = s1 * c2 * c3 - c1 * s2 * s3;
return mq;
}
void ChMatterSPH::FillBox(const ChVector<> size,
const double spacing,
const double initial_density,
const ChCoordsys<> boxcoords,
const bool do_centeredcube,
const double kernel_sfactor,
const double randomness) {
int samples_x = (int)(size.x() / spacing);
int samples_y = (int)(size.y() / spacing);
int samples_z = (int)(size.z() / spacing);
int totsamples = 0;
double mrandomness = randomness;
if (do_centeredcube)
mrandomness = randomness * 0.5;
for (int ix = 0; ix < samples_x; ix++)
for (int iy = 0; iy < samples_y; iy++)
for (int iz = 0; iz < samples_z; iz++) {
ChVector<> pos(ix * spacing - 0.5 * size.x(), iy * spacing - 0.5 * size.y(),
iz * spacing - 0.5 * size.z());
pos += ChVector<>(mrandomness * ChRandom() * spacing, mrandomness * ChRandom() * spacing,
mrandomness * ChRandom() * spacing);
AddNode(boxcoords.TransformLocalToParent(pos));
totsamples++;
if (do_centeredcube) {
ChVector<> pos2 = pos + 0.5 * ChVector<>(spacing, spacing, spacing);
pos2 += ChVector<>(mrandomness * ChRandom() * spacing, mrandomness * ChRandom() * spacing,
mrandomness * ChRandom() * spacing);
AddNode(boxcoords.TransformLocalToParent(pos2));
totsamples++;
}
}
double mtotvol = size.x() * size.y() * size.z();
double mtotmass = mtotvol * initial_density;
double nodemass = mtotmass / (double)totsamples;
double kernelrad = kernel_sfactor * spacing;
for (unsigned int ip = 0; ip < GetNnodes(); ip++) {
// downcasting
std::shared_ptr<ChNodeSPH> mnode(nodes[ip]);
assert(mnode);
mnode->SetKernelRadius(kernelrad);
mnode->SetCollisionRadius(spacing * 0.05);
mnode->SetMass(nodemass);
}
GetMaterial().Set_density(initial_density);
}
void AddContainerWall(std::shared_ptr<ChBody> body,
const ChVector<>& pos,
const ChVector<>& size,
bool visible = true) {
ChVector<> hsize = 0.5 * size;
body->GetCollisionModel()->AddBox(hsize.x(), hsize.y(), hsize.z(), pos);
if (visible) {
auto box = std::make_shared<ChBoxShape>();
box->GetBoxGeometry().Pos = pos;
box->GetBoxGeometry().Size = hsize;
box->SetColor(ChColor(1, 0, 0));
box->SetFading(0.6f);
body->AddAsset(box);
}
}
// -----------------------------------------------------------------------------
// Calculate kinematics quantities (slip angle, longitudinal slip, camber angle,
// and toe-in angle using the current state of the associated wheel body.
// -----------------------------------------------------------------------------
void ChTire::CalculateKinematics(double time, const WheelState& state, const ChTerrain& terrain) {
// Wheel normal (expressed in global frame)
ChVector<> wheel_normal = state.rot.GetYaxis();
// Terrain normal at wheel location (expressed in global frame)
ChVector<> Z_dir = terrain.GetNormal(state.pos.x(), state.pos.y());
// Longitudinal (heading) and lateral directions, in the terrain plane
ChVector<> X_dir = Vcross(wheel_normal, Z_dir);
X_dir.Normalize();
ChVector<> Y_dir = Vcross(Z_dir, X_dir);
// Tire reference coordinate system
ChMatrix33<> rot;
rot.Set_A_axis(X_dir, Y_dir, Z_dir);
ChCoordsys<> tire_csys(state.pos, rot.Get_A_quaternion());
// Express wheel linear velocity in tire frame
ChVector<> V = tire_csys.TransformDirectionParentToLocal(state.lin_vel);
// Express wheel normal in tire frame
ChVector<> n = tire_csys.TransformDirectionParentToLocal(wheel_normal);
// Slip angle
double abs_Vx = std::abs(V.x());
double zero_Vx = 1e-4;
m_slip_angle = (abs_Vx > zero_Vx) ? std::atan(V.y() / abs_Vx) : 0;
// Longitudinal slip
m_longitudinal_slip = (abs_Vx > zero_Vx) ? -(V.x() - state.omega * GetRadius()) / abs_Vx : 0;
// Camber angle
m_camber_angle = std::atan2(n.z(), n.y());
}
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
}
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;
}
bool ChCascadeDoc::GetVolumeProperties(const TopoDS_Shape& mshape, ///< pass the shape here
const double density, ///< pass the density here
ChVector<>& center_position, ///< get the position center, respect to shape pos.
ChVector<>& inertiaXX, ///< get the inertia diagonal terms
ChVector<>& inertiaXY, ///< get the inertia extradiagonal terms
double& volume, ///< get the volume
double& mass ///< get the mass
) {
if (mshape.IsNull())
return false;
GProp_GProps mprops;
GProp_GProps vprops;
BRepGProp::VolumeProperties(mshape, mprops);
BRepGProp::VolumeProperties(mshape, vprops);
mprops.Add(mprops, density);
mass = mprops.Mass();
volume = vprops.Mass();
gp_Pnt G = mprops.CentreOfMass();
gp_Mat I = mprops.MatrixOfInertia();
center_position.x() = G.X();
center_position.y() = G.Y();
center_position.z() = G.Z();
inertiaXX.x() = I(1, 1);
inertiaXX.y() = I(2, 2);
inertiaXX.z() = I(3, 3);
inertiaXY.x() = I(1, 2);
inertiaXY.y() = I(1, 3);
inertiaXY.z() = I(2, 3);
return true;
}
void Q_to_AngAxis(const ChQuaternion<double>& quat, double& angle, ChVector<double>& axis) {
if (fabs(quat.e0()) < 0.99999999) {
double arg = acos(quat.e0());
double invsine = 1 / sin(arg);
ChVector<double> vtemp;
vtemp.x() = invsine * quat.e1();
vtemp.y() = invsine * quat.e2();
vtemp.z() = invsine * quat.e3();
angle = 2 * arg;
axis = Vnorm(vtemp);
} else {
axis.x() = 1;
axis.y() = 0;
axis.z() = 0;
angle = 0;
}
}
// 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);
}
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..)
}
// -----------------------------------------------------------------------------
// Utility function for characterizing the geometric contact between a disc with
// specified center location, normal direction, and radius and the terrain,
// assumed to be specified as a height field (over the x-y domain).
// This function returns false if no contact occurs. Otherwise, it sets the
// contact points on the disc (ptD) and on the terrain (ptT), the normal contact
// direction, and the resulting penetration depth (a positive value).
// -----------------------------------------------------------------------------
bool ChTire::disc_terrain_contact(const ChTerrain& terrain,
const ChVector<>& disc_center,
const ChVector<>& disc_normal,
double disc_radius,
ChCoordsys<>& contact,
double& depth) {
// Find terrain height below disc center. There is no contact if the disc
// center is below the terrain or farther away by more than its radius.
double hc = terrain.GetHeight(disc_center.x(), disc_center.y());
if (disc_center.z() <= hc || disc_center.z() >= hc + disc_radius)
return false;
// Find the lowest point on the disc. There is no contact if the disc is
// (almost) horizontal.
ChVector<> dir1 = Vcross(disc_normal, ChVector<>(0, 0, 1));
double sinTilt2 = dir1.Length2();
if (sinTilt2 < 1e-3)
return false;
// Contact point (lowest point on disc).
ChVector<> ptD = disc_center + disc_radius * Vcross(disc_normal, dir1 / sqrt(sinTilt2));
// Find terrain height at lowest point. No contact if lowest point is above
// the terrain.
double hp = terrain.GetHeight(ptD.x(), ptD.y());
if (ptD.z() > hp)
return false;
// Approximate the terrain with a plane. Define the projection of the lowest
// point onto this plane as the contact point on the terrain.
ChVector<> normal = terrain.GetNormal(ptD.x(), ptD.y());
ChVector<> longitudinal = Vcross(disc_normal, normal);
longitudinal.Normalize();
ChVector<> lateral = Vcross(normal, longitudinal);
ChMatrix33<> rot;
rot.Set_A_axis(longitudinal, lateral, normal);
contact.pos = ptD;
contact.rot = rot.Get_A_quaternion();
depth = Vdot(ChVector<>(0, 0, hp - ptD.z()), normal);
assert(depth > 0);
return true;
}
void XlChartPlotSeries::classifySeries (XlChartBinaryReader& in_reader)
{
//NOTE! modifies m_uCurrentIndex.
//
enum {undef = -2, aux, std, trend, xPos, xNeg, yPos, yNeg} type = undef;
ChAutoPtr<XlHeader> pHeader(ChNEW XlHeader());
ChSINT4 lPos = in_reader.getStream()->getPos();
if ( in_reader.nextSibling(*pHeader) )
{
in_reader.getStream()->seek( pHeader->getLength() + XlRecord::HDR_SIZE, SsrwOO_CUR );
// Open container and parse subordinates.
// Find out about series type,
// also get main series group index or pick up aux series parent while parsing.
in_reader.m_pParser->getHeader( *pHeader, in_reader.getSheetVersion() );
if ( pHeader->getId() == XlChartBegin::ID )
{
ChSINT2 unAuxParentInd = -1;
ChVector<SeriesDescription>* pList = NULL; // tells about group type
// when completed
while ( in_reader.nextSibling(*pHeader) )
{
switch ( pHeader->getId() )
{
case XlChartSerToCrt::ID: //order 1
{
ChAutoPtr<XlChartSerToCrt> pSerToCrt( ChNEW XlChartSerToCrt(*pHeader) );
in_reader.getStream()->seek(XlRecord::HDR_SIZE, SsrwOO_CUR);
in_reader.m_pParser->visit(*pSerToCrt);
if (type == undef)
{
type = std;
pList = ( pSerToCrt->getChartGroup() == 1 )? &m_secondaryGroupSeries :
&m_mainGroupSeries;
}
break;
}
case XlChartSerParent::ID: //order 1
{
ChAutoPtr<XlChartSerParent> pSerParent( ChNEW XlChartSerParent(*pHeader) );
in_reader.getStream()->seek(XlRecord::HDR_SIZE, SsrwOO_CUR);
in_reader.m_pParser->visit(*pSerParent);
if (type == undef)
{
type = aux;
// this record seems to use 1 based indexes so subtract 1 to get 0 based
unAuxParentInd = pSerParent->getSeriesNumber() - 1;
}
break;
}
case XlChartSerAuxErrBar::ID: //order 2
{
ChAutoPtr<XlChartSerAuxErrBar> pSerErr( ChNEW XlChartSerAuxErrBar(*pHeader) );
in_reader.getStream()->seek(XlRecord::HDR_SIZE, SsrwOO_CUR);
in_reader.m_pParser->visit(*pSerErr);
if (type == aux)
{
switch ( pSerErr->getErrorBarType() )
{
case EB_XPLUS:
{
type = xPos;
break;
}
case EB_XMINUS:
{
type = xNeg;
break;
}
case EB_YPLUS:
{
type = yPos;
break;
}
case EB_YMINUS:
{
type = yNeg;
break;
}
}
}
break;
}
case XlChartSerAuxTrend::ID: //order 2
// no need to parse yet...
if (type == aux)
{
type = trend;
}
// no break!
// step over the header as we didn't read anything.
default:
in_reader.getStream()->seek( pHeader->getLength() + XlRecord::HDR_SIZE, SsrwOO_CUR );
break;
}
}
switch (type)
{
default: //
case undef: // incorrect file data
case aux: //
{
break;
}
case std: // primary series
{
// standard series are simply appended to their list
ChAutoPtr<SeriesDescription> pSeries( ChNEW SeriesDescription() );
pSeries->m_primary.m_lLoc = lPos;
pSeries->m_primary.m_unIndex = m_uCurrentIndex;
pList->push_back(*pSeries);
break;
}
case trend: //
case xPos: //
case xNeg: // aux series
case yPos: //
case yNeg: //
{
// need to find the parent
// ASSUMPTION: parent already exists. If BIFF files define an auxillary
// series before the parent series, this assumption is invalid and
// the result will be missing auxillary information.
//Look in the main axis group first, then in the secondary axis group
if (unAuxParentInd >= 0)
{
ChUINT2 iGroup;
for (iGroup = 0; iGroup < 2; iGroup ++)
{
ChVector<SeriesDescription>* pList = iGroup == 0? &m_mainGroupSeries :
&m_secondaryGroupSeries;
ChUINT2 iListItem;
for (iListItem = 0; iListItem < pList->size(); iListItem ++)
{
if (pList->at(iListItem).m_primary.m_unIndex == unAuxParentInd)
{
switch (type)
{
case trend:
{
SeriesDesc desc;
desc.m_lLoc = lPos;
desc.m_unIndex = m_uCurrentIndex;
pList->at(iListItem).m_trendlines.push_back(desc);
break;
}
case xPos:
pList->at(iListItem).m_ErrorXPos.m_lLoc = lPos;
pList->at(iListItem).m_ErrorXPos.m_unIndex = m_uCurrentIndex;
break;
case xNeg:
pList->at(iListItem).m_ErrorXNeg.m_lLoc = lPos;
pList->at(iListItem).m_ErrorXNeg.m_unIndex = m_uCurrentIndex;
break;
case yPos:
pList->at(iListItem).m_ErrorYPos.m_lLoc = lPos;
pList->at(iListItem).m_ErrorYPos.m_unIndex = m_uCurrentIndex;
break;
case yNeg:
pList->at(iListItem).m_ErrorYNeg.m_lLoc = lPos;
pList->at(iListItem).m_ErrorYNeg.m_unIndex = m_uCurrentIndex;
break;
}
iGroup = 2; // break the group loop as we don't need to look in
// secondary series if we succeeded.
break; // break this list search
}
}
}
}
break;
}
}
}
}
m_uCurrentIndex ++;
}
void ChMeshExporter::writeFrame(std::shared_ptr<ChMesh> my_mesh, char SaveAsBuffer[256], std::string MeshFileBuffer) {
std::ofstream output;
std::string SaveAsBuffer_string(SaveAsBuffer);
SaveAsBuffer_string.erase(SaveAsBuffer_string.length() - 4, 4);
std::cout << SaveAsBuffer_string << std::endl;
snprintf(SaveAsBuffer, sizeof(char) * 256, ("%s"), (SaveAsBuffer_string + ".vtk").c_str());
output.open(SaveAsBuffer, std::ios::app);
output << "# vtk DataFile Version 2.0" << std::endl;
output << "Unstructured Grid Example" << std::endl;
output << "ASCII" << std::endl;
output << "DATASET UNSTRUCTURED_GRID" << std::endl;
output << "POINTS " << my_mesh->GetNnodes() << " float\n";
for (unsigned int i = 0; i < my_mesh->GetNnodes(); i++) {
auto node = std::dynamic_pointer_cast<ChNodeFEAxyz>(my_mesh->GetNode(i));
output << node->GetPos().x() << " " << node->GetPos().y() << " " << node->GetPos().z() << "\n";
}
std::ifstream CopyFrom(MeshFileBuffer);
output << CopyFrom.rdbuf();
int numCell = 0;
for (unsigned int iele = 0; iele < my_mesh->GetNelements(); iele++) {
if (auto element = std::dynamic_pointer_cast<ChElementCableANCF>(my_mesh->GetElement(iele)))
numCell++;
else if (auto element = std::dynamic_pointer_cast<ChElementShellANCF>(my_mesh->GetElement(iele)))
numCell++;
}
output << "\nCELL_DATA " << numCell << "\n";
output << "SCALARS Deflection float 1\n";
output << "LOOKUP_TABLE default\n";
double scalar = 0;
for (unsigned int iele = 0; iele < my_mesh->GetNelements(); iele++) {
if (auto element = std::dynamic_pointer_cast<ChElementCableANCF>(my_mesh->GetElement(iele)))
scalar = element->GetCurrLength() - element->GetRestLength();
else if (auto element = std::dynamic_pointer_cast<ChElementShellANCF>(my_mesh->GetElement(iele)))
element->EvaluateDeflection(scalar);
output << scalar + 1e-20 << "\n";
}
output << "VECTORS Strain float\n";
ChVector<> StrainV;
for (unsigned int iele = 0; iele < my_mesh->GetNelements(); iele++) {
if (auto element = std::dynamic_pointer_cast<ChElementCableANCF>(my_mesh->GetElement(iele)))
element->EvaluateSectionStrain(0.0, StrainV);
else if (auto element = std::dynamic_pointer_cast<ChElementShellANCF>(my_mesh->GetElement(iele)))
StrainV = element->EvaluateSectionStrains();
StrainV += ChVector<>(1e-20);
output << StrainV.x() << " " << StrainV.y() << " " << StrainV.z() << "\n";
}
output << "\nPOINT_DATA " << my_mesh->GetNnodes() << "\n";
output << "VECTORS Velocity float\n";
for (unsigned int i = 0; i < my_mesh->GetNnodes(); i++) {
ChVector<> vel = std::dynamic_pointer_cast<ChNodeFEAxyz>(my_mesh->GetNode(i))->GetPos_dt();
vel += ChVector<>(1e-20);
output << (double)vel.x() << " " << (double)vel.y() << " " << (double)vel.z() << "\n";
}
output << "VECTORS Acceleration float\n";
for (unsigned int i = 0; i < my_mesh->GetNnodes(); i++) {
ChVector<> acc = std::dynamic_pointer_cast<ChNodeFEAxyz>(my_mesh->GetNode(i))->GetPos_dtdt();
acc += ChVector<>(1e-20);
output << (double)acc.x() << " " << (double)acc.y() << " " << (double)acc.z() << "\n";
}
output.close();
} // namespace fea
// -----------------------------------------------------------------------------
// -----------------------------------------------------------------------------
void ChLinearDamperRWAssembly::Initialize(std::shared_ptr<ChBodyAuxRef> chassis, const ChVector<>& location) {
// Express the suspension reference frame in the absolute coordinate system.
ChFrame<> susp_to_abs(location);
susp_to_abs.ConcatenatePreTransformation(chassis->GetFrame_REF_to_abs());
// Transform all points and directions to absolute frame.
std::vector<ChVector<> > points(NUM_POINTS);
for (int i = 0; i < NUM_POINTS; i++) {
ChVector<> rel_pos = GetLocation(static_cast<PointId>(i));
points[i] = susp_to_abs.TransformPointLocalToParent(rel_pos);
}
// Create the trailing arm body. The reference frame of the arm body has its
// x-axis aligned with the line between the arm-chassis connection point and
// the arm-wheel connection point.
ChVector<> y_dir = susp_to_abs.GetA().Get_A_Yaxis();
ChVector<> u = susp_to_abs.GetPos() - points[ARM_CHASSIS];
u.Normalize();
ChVector<> w = Vcross(u, y_dir);
w.Normalize();
ChVector<> v = Vcross(w, u);
ChMatrix33<> rot;
rot.Set_A_axis(u, v, w);
m_arm = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
m_arm->SetNameString(m_name + "_arm");
m_arm->SetPos(points[ARM]);
m_arm->SetRot(rot);
m_arm->SetMass(GetArmMass());
m_arm->SetInertiaXX(GetArmInertia());
chassis->GetSystem()->AddBody(m_arm);
// Cache points and directions for arm visualization (expressed in the arm frame)
m_pO = m_arm->TransformPointParentToLocal(susp_to_abs.GetPos());
m_pA = m_arm->TransformPointParentToLocal(points[ARM]);
m_pAW = m_arm->TransformPointParentToLocal(points[ARM_WHEEL]);
m_pAC = m_arm->TransformPointParentToLocal(points[ARM_CHASSIS]);
m_pAS = m_arm->TransformPointParentToLocal(points[SHOCK_A]);
m_dY = m_arm->TransformDirectionParentToLocal(y_dir);
// Create and initialize the revolute joint between arm and chassis.
// The axis of rotation is the y axis of the suspension reference frame.
m_revolute = std::make_shared<ChLinkLockRevolute>();
m_revolute->SetNameString(m_name + "_revolute");
m_revolute->Initialize(chassis, m_arm,
ChCoordsys<>(points[ARM_CHASSIS], susp_to_abs.GetRot() * Q_from_AngX(CH_C_PI_2)));
chassis->GetSystem()->AddLink(m_revolute);
// Create and initialize the rotational spring torque element.
m_spring = std::make_shared<ChLinkRotSpringCB>();
m_spring->SetNameString(m_name + "_spring");
m_spring->Initialize(chassis, m_arm, ChCoordsys<>(points[ARM_CHASSIS], susp_to_abs.GetRot() * Q_from_AngX(CH_C_PI_2)));
m_spring->RegisterTorqueFunctor(GetSpringTorqueFunctor());
chassis->GetSystem()->AddLink(m_spring);
// Create and initialize the translational shock force element.
if (m_has_shock) {
m_shock = std::make_shared<ChLinkSpringCB>();
m_shock->SetNameString(m_name + "_shock");
m_shock->Initialize(chassis, m_arm, false, points[SHOCK_C], points[SHOCK_A]);
m_shock->RegisterForceFunctor(GetShockForceFunctor());
chassis->GetSystem()->AddLink(m_shock);
}
// Invoke the base class implementation. This initializes the associated road wheel.
// Note: we must call this here, after the m_arm body is created.
ChRoadWheelAssembly::Initialize(chassis, location);
}
bool ChOpenGLMesh::Initialize(chrono::ChTriangleMeshShape* tri_mesh, ChOpenGLMaterial mat) {
if (GLReturnedError("Mesh::Initialize - on entry")) {
return false;
}
if (!super::Initialize()) {
return false;
}
int num_triangles = tri_mesh->GetMesh().getNumTriangles();
for (unsigned int i = 0; i < (unsigned)num_triangles; i++) {
chrono::geometry::ChTriangle tri = tri_mesh->GetMesh().getTriangle(i);
ChVector<> norm = tri.GetNormal();
ChVector<> v1 = tri.p1;
ChVector<> v2 = tri.p2;
ChVector<> v3 = tri.p3;
this->vertex_indices.push_back(i * 3 + 0);
this->vertex_indices.push_back(i * 3 + 1);
this->vertex_indices.push_back(i * 3 + 2);
glm::vec3 v, n;
v = glm::vec3(v1.x(), v1.y(), v1.z());
n = glm::vec3(norm.x(), norm.y(), norm.z());
this->data.push_back(ChOpenGLVertexAttributesPN(v, n));
v = glm::vec3(v2.x(), v2.y(), v2.z());
this->data.push_back(ChOpenGLVertexAttributesPN(v, n));
v = glm::vec3(v3.x(), v3.y(), v3.z());
this->data.push_back(ChOpenGLVertexAttributesPN(v, n));
}
ambient = mat.ambient_color;
diffuse = mat.diffuse_color;
specular = mat.specular_color;
//
// std::vector<ChVector<int> >& indices =
// tri_mesh->GetMesh().getIndicesVertexes();
// std::vector<ChVector<double> >& vertices =
// tri_mesh->GetMesh().getCoordsVertices();
// std::vector<ChVector<double> >& normals =
// tri_mesh->GetMesh().getCoordsNormals();
//
// this->vertex_indices.resize(indices.size() * 3);
// for (unsigned int i = 0; i < indices.size(); i++) {
//
// this->vertex_indices[i * 3 + 0] = indices[i].x;
// this->vertex_indices[i * 3 + 1] = indices[i].y;
// this->vertex_indices[i * 3 + 2] = indices[i].z;
// }
// for (unsigned int i = 0; i < vertices.size(); i++) {
// glm::vec3 n;
// glm::vec3 v(vertices[i].x, vertices[i].y, vertices[i].z);
// if (normals.size() > 0) {
// n = glm::vec3(normals[i].x, normals[i].y, normals[i].z);
// } else {
// n = glm::vec3(1, 0, 0);
// }
//
// this->data.push_back(ChOpenGLVertexAttributesPN(v, n));
// }
PostInitialize();
if (GLReturnedError("ChOpenGLMesh::Initialize - on exit")) {
return false;
}
return true;
}
