MGADSMTrajectory::MGADSMTrajectory(const std::vector<TrajectoryNodePtr>& nodes) :
m_numNodes(0),
m_numStates(0),
m_legsInitialized(false),
m_initialEpoch(Epoch()),
m_finalEpoch(Epoch()),
m_initialStateVector(),
m_finalStateVector(),
m_planetStateVector()
{
Init();
SetNodes(0, nodes);
}
void HMMWV_ReissnerTire::CreateMesh(const ChFrameMoving<>& wheel_frame, VehicleSide side) {
// Create piece-wise cubic spline approximation of the tire profile.
// x - radial direction
// y - transversal direction
ChCubicSpline splineX(m_profile_t, m_profile_x);
ChCubicSpline splineY(m_profile_t, m_profile_y);
// Create the mesh nodes.
// The nodes are first created in the wheel local frame, assuming Y as the tire axis,
// and are then transformed to the global frame.
for (int i = 0; i < m_div_circumference; i++) {
double phi = (CH_C_2PI * i) / m_div_circumference;
ChVector<> nrm(-std::sin(phi), 0, std::cos(phi));
for (int j = 0; j <= m_div_width; j++) {
double t_prf = double(j) / m_div_width;
double x_prf, xp_prf, xpp_prf;
double y_prf, yp_prf, ypp_prf;
splineX.Evaluate(t_prf, x_prf, xp_prf, xpp_prf);
splineY.Evaluate(t_prf, y_prf, yp_prf, ypp_prf);
// Node position with respect to rim center
double x = (m_rim_radius + x_prf) * std::cos(phi);
double y = y_prf;
double z = (m_rim_radius + x_prf) * std::sin(phi);
// Node position in global frame (actual coordinate values)
ChVector<> loc = wheel_frame.TransformPointLocalToParent(ChVector<>(x, y, z));
// Node direction
ChVector<> tan_prf(std::cos(phi) * xp_prf, yp_prf, std::sin(phi) * xp_prf);
ChVector<> nrm_prf = Vcross(tan_prf, nrm).GetNormalized();
ChVector<> dir = wheel_frame.TransformDirectionLocalToParent(nrm_prf);
ChMatrix33<> mrot; mrot.Set_A_Xdir(tan_prf,nrm_prf);
auto node = std::make_shared<ChNodeFEAxyzrot>(ChFrame<>(loc, mrot));
// Node velocity
ChVector<> vel = wheel_frame.PointSpeedLocalToParent(ChVector<>(x, y, z));
node->SetPos_dt(vel);
node->SetMass(0);
m_mesh->AddNode(node);
}
}
// Create the Reissner shell elements
for (int i = 0; i < m_div_circumference; i++) {
for (int j = 0; j < m_div_width; j++) {
// Adjacent nodes
int inode0, inode1, inode2, inode3;
inode1 = j + i * (m_div_width + 1);
inode2 = j + 1 + i * (m_div_width + 1);
if (i == m_div_circumference - 1) {
inode0 = j;
inode3 = j + 1;
} else {
inode0 = j + (i + 1) * (m_div_width + 1);
inode3 = j + 1 + (i + 1) * (m_div_width + 1);
}
auto node0 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode0));
auto node1 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode1));
auto node2 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode2));
auto node3 = std::dynamic_pointer_cast<ChNodeFEAxyzrot>(m_mesh->GetNode(inode3));
// Create the element and set its nodes.
auto element = std::make_shared<ChElementShellReissner4>();
element->SetNodes(node0, node1, node2, node3);
// Element dimensions
double len_circumference =
0.5 * ((node1->GetPos() - node0->GetPos()).Length() + (node3->GetPos() - node2->GetPos()).Length());
double len_width = (node2->GetPos() - node0->GetPos()).Length();
// Figure out the section for this element
int b1 = m_num_elements_bead;
int b2 = m_div_width - m_num_elements_bead;
int s1 = b1 + m_num_elements_sidewall;
int s2 = b2 - m_num_elements_sidewall;
if (j < b1 || j >= b2) {
// Bead section
for (unsigned int im = 0; im < m_num_layers_bead; im++) {
element->AddLayer(m_layer_thickness_bead[im], CH_C_DEG_TO_RAD * m_ply_angle_bead[im],
m_materials[m_material_id_bead[im]]);
}
} else if (j < s1 || j >= s2) {
// Sidewall section
for (unsigned int im = 0; im < m_num_layers_sidewall; im++) {
element->AddLayer(m_layer_thickness_sidewall[im], CH_C_DEG_TO_RAD * m_ply_angle_sidewall[im],
m_materials[m_material_id_sidewall[im]]);
}
} else {
// Tread section
for (unsigned int im = 0; im < m_num_layers_tread; im++) {
element->AddLayer(m_layer_thickness_tread[im], CH_C_DEG_TO_RAD * m_ply_angle_tread[im],
m_materials[m_material_id_tread[im]]);
}
}
// Set other element properties
element->SetAlphaDamp(m_alpha);
// Add element to mesh
m_mesh->AddElement(element);
}
}
// Switch on automatic gravity
m_mesh->SetAutomaticGravity(true);
}