<commit_before>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All right reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: <NAME>
// =============================================================================
//
// Tracked vehicle model built from subsystems.
//  Location of subsystems hard-coded for M113 vehicle
//  TODO: specify this w/ JSON input data file
//
// =============================================================================

#include <cstdio>

#include "assets/ChCylinderShape.h"
#include "assets/ChTriangleMeshShape.h"
#include "assets/ChTexture.h"
#include "assets/ChColorAsset.h"
#include "physics/ChGlobal.h"

#include "TrackVehicle.h"

#include "subsys/trackSystem/TrackSystem.h"
#include "subsys/driveline/TrackDriveline.h"

#include "utils/ChUtilsInputOutput.h"
#include "utils/ChUtilsData.h"

// collision mesh
#include "geometry/ChCTriangleMeshSoup.h"

namespace chrono {

// -----------------------------------------------------------------------------
// Static variables
const size_t TrackVehicle::m_num_tracks = 2;    // number of trackSystems to create
const size_t TrackVehicle::m_num_engines = 1;   // number of powertrains (and drivelines) to create

const ChVector<> TrackVehicle::m_trackPos_Right(0.23644, -0.4780, 0.83475); // relative to chassis c-sys
const ChVector<> TrackVehicle::m_trackPos_Left(0.23644, -0.4780, -0.83475); // relative to chassis c-sys

const double     TrackVehicle::m_mass = 5489.2;   // chassis sprung mass
const ChVector<> TrackVehicle::m_COM = ChVector<>(0., 0., 0.);  // COM location, relative to body Csys REF frame
const ChVector<> TrackVehicle::m_inertia(1786.9, 10449.7, 10721.2);  // chassis inertia (roll,yaw,pitch)

const std::string TrackVehicle::m_meshName("M113_chassis");
const std::string TrackVehicle::m_meshFile = utils::GetModelDataFile("M113/Chassis_XforwardYup.obj");
const ChCoordsys<> TrackVehicle::m_driverCsys(ChVector<>(0.0, 0.5, 1.2), ChQuaternion<>(1, 0, 0, 0));
const ChVector<> TrackVehicle::m_chassisBoxSize(4.0 *0.5, 2.0 *0.5, 3.0 *0.5);  // length, height, width HALF DIMS

/// constructor sets the basic integrator settings for this ChSystem, as well as the usual stuff
TrackVehicle::TrackVehicle(const std::string& name, VisualizationType chassisVis, CollisionType chassisCollide)
  : m_vis(chassisVis),
  m_collide(chassisCollide),
  m_ownsSystem(true),
  m_stepsize(1e-4)
{
  // Integration and Solver settings
  SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
  SetIterLCPmaxItersSpeed(150);
  SetIterLCPmaxItersStab(150);
  SetMaxPenetrationRecoverySpeed(1.0);

  // create the chassis body    
  m_chassis = ChSharedPtr<ChBodyAuxRef>(new ChBodyAuxRef);
  m_chassis->SetIdentifier(0);
  m_chassis->SetNameString(name);
  m_chassis->SetFrame_COG_to_REF(ChFrame<>(m_COM, ChQuaternion<>(1, 0, 0, 0)));
  // basic body info
  m_chassis->SetMass(m_mass);
  m_chassis->SetInertiaXX(m_inertia);

  // add visualization assets to the chassis
  AddVisualization();

  m_chassis->SetBodyFixed(false);
  // add the chassis body to the system
  Add(m_chassis);

  // resize all vectors for the number of track systems
  m_TrackSystems.resize(m_num_tracks);
  m_TrackSystem_locs.resize(m_num_tracks);
  // Right and Left track System relative locations, respectively
  m_TrackSystem_locs[0] = m_trackPos_Right;
  m_TrackSystem_locs[1] = m_trackPos_Left;

  // Each trackSystem has its own driveline and powertrain, so the left and right
  // sides can have power applied independently
  m_drivelines.resize(m_num_engines);
  m_ptrains.resize(m_num_engines);

  // create track systems
  for (int i = 0; i < m_num_tracks; i++) {
    m_TrackSystems[i] = ChSharedPtr<TrackSystem>(new TrackSystem("track chain "+std::to_string(i), i) );
  }
  
  // create the powertrain and drivelines
  for (int j = 0; j < m_num_engines; j++)
  {
    m_drivelines[j] = ChSharedPtr<TrackDriveline>(new TrackDriveline("driveline "+std::to_string(j)) );
    m_ptrains[j] = ChSharedPtr<TrackPowertrain>(new TrackPowertrain("powertrain "+std::to_string(j)) );
  
  }

  // TODO: add brakes. Perhaps they are a part of the suspension subsystem?

}


TrackVehicle::~TrackVehicle()
{
  if(m_ownsSystem)
    delete m_system;
}

// Set any collision geometry on the hull, then Initialize() all subsystems
void TrackVehicle::Initialize(const ChCoordsys<>& chassis_Csys)
{
  // move the chassis REF frame to the specified initial position/orientation
  m_chassis->SetFrame_REF_to_abs(ChFrame<>(chassis_Csys));

  // add collision geometry to the chassis
  AddCollisionGeometry();

  // initialize the subsystems with the initial c-sys and specified offsets
  for (int i = 0; i < m_num_tracks; i++)
  {
    m_TrackSystems[i]->Initialize(m_chassis, m_TrackSystem_locs[i]);
  }

  // initialize the powertrain, drivelines
  for (int j = 0; j < m_num_engines; j++)
  {
    size_t driveGear_R_idx = 2*j;
    size_t driveGear_L_idx = 2*j + 1;
    m_drivelines[j]->Initialize(m_chassis,
      m_TrackSystems[driveGear_R_idx]->GetDriveGear(),
      m_TrackSystems[driveGear_L_idx]->GetDriveGear());
    m_ptrains[j]->Initialize(m_chassis, m_drivelines[j]->GetDriveshaft());
  }

}


void TrackVehicle::Update(double	time,
                          const std::vector<double>&  throttle,
                          const std::vector<double>&  braking)
{
  assert( throttle.size() == m_num_tracks);
  assert( braking.size() == m_num_tracks );
  // update left and right powertrains, with the new left and right throttle/shaftspeed
  for(int i = 0; i < m_num_engines; i++)
  {
    m_ptrains[i]->Update(time, throttle[0], throttle[1], m_drivelines[0]->GetDriveshaftSpeed() );
  }

}

void TrackVehicle::AddVisualization()
{
  // add visual geometry asset to the chassis, if enabled
  switch (m_vis) {
  case VisualizationType::PRIMITIVES:
  {
    // put a sphere at the chassis COM and at the REF point
    ChSharedPtr<ChSphereShape> COMsphere(new ChSphereShape);
    COMsphere->GetSphereGeometry().rad = 0.1;
    COMsphere->Pos = m_COM;
    // make the COM sphere blue
    COMsphere->SetColor(ChColor(0.1f, 0.2f, 0.8f));
    m_chassis->AddAsset(COMsphere);
    
   
    ChSharedPtr<ChSphereShape> REFsphere(new ChSphereShape);
    REFsphere->GetSphereGeometry().rad = 0.1;
    REFsphere->Pos = ChVector<>(0,0,0); // REF should be at the body c-sys origin
    // make the REF sphere red
    REFsphere->SetColor(ChColor(0.8f, 0.2f, 0.1f));
    m_chassis->AddAsset(REFsphere);

    break;
  }
  case VisualizationType::MESH:
  {
    geometry::ChTriangleMeshConnected trimesh;
    trimesh.LoadWavefrontMesh(m_meshFile, true, true);

    ChSharedPtr<ChTriangleMeshShape> trimesh_shape(new ChTriangleMeshShape);
    trimesh_shape->SetMesh(trimesh);
    trimesh_shape->SetName("chassis triMesh");
    m_chassis->AddAsset(trimesh_shape);

    break;
  }
  } // end switch


}

void TrackVehicle::AddCollisionGeometry()
{
  // add collision geometrey to the chassis, if enabled
  if( m_collide == CollisionType::NONE)
  {
    m_chassis->SetCollide(false);
    return;
  }
  m_chassis->SetCollide(true);<commit_msg>change chassis primitives visualization to match the collision shape<commit_after>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All right reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: <NAME>
// =============================================================================
//
// Tracked vehicle model built from subsystems.
//  Location of subsystems hard-coded for M113 vehicle
//  TODO: specify this w/ JSON input data file
//
// =============================================================================

#include <cstdio>

#include "assets/ChCylinderShape.h"
#include "assets/ChTriangleMeshShape.h"
#include "assets/ChTexture.h"
#include "assets/ChColorAsset.h"
#include "physics/ChGlobal.h"

#include "TrackVehicle.h"

#include "subsys/trackSystem/TrackSystem.h"
#include "subsys/driveline/TrackDriveline.h"

#include "utils/ChUtilsInputOutput.h"
#include "utils/ChUtilsData.h"

// collision mesh
#include "geometry/ChCTriangleMeshSoup.h"

namespace chrono {

// -----------------------------------------------------------------------------
// Static variables
const size_t TrackVehicle::m_num_tracks = 2;    // number of trackSystems to create
const size_t TrackVehicle::m_num_engines = 1;   // number of powertrains (and drivelines) to create

const ChVector<> TrackVehicle::m_trackPos_Right(0.23644, -0.4780, 0.83475); // relative to chassis c-sys
const ChVector<> TrackVehicle::m_trackPos_Left(0.23644, -0.4780, -0.83475); // relative to chassis c-sys

const double     TrackVehicle::m_mass = 5489.2;   // chassis sprung mass
const ChVector<> TrackVehicle::m_COM = ChVector<>(0., 0., 0.);  // COM location, relative to body Csys REF frame
const ChVector<> TrackVehicle::m_inertia(1786.9, 10449.7, 10721.2);  // chassis inertia (roll,yaw,pitch)

const std::string TrackVehicle::m_meshName("M113_chassis");
const std::string TrackVehicle::m_meshFile = utils::GetModelDataFile("M113/Chassis_XforwardYup.obj");
const ChCoordsys<> TrackVehicle::m_driverCsys(ChVector<>(0.0, 0.5, 1.2), ChQuaternion<>(1, 0, 0, 0));
const ChVector<> TrackVehicle::m_chassisBoxSize(4.0 *0.5, 2.0 *0.5, 3.0 *0.5);  // length, height, width HALF DIMS

/// constructor sets the basic integrator settings for this ChSystem, as well as the usual stuff
TrackVehicle::TrackVehicle(const std::string& name, VisualizationType chassisVis, CollisionType chassisCollide)
  : m_vis(chassisVis),
  m_collide(chassisCollide),
  m_ownsSystem(true),
  m_stepsize(1e-4)
{
  // Integration and Solver settings
  SetLcpSolverType(ChSystem::LCP_ITERATIVE_SOR);
  SetIterLCPmaxItersSpeed(150);
  SetIterLCPmaxItersStab(150);
  SetMaxPenetrationRecoverySpeed(1.0);

  // create the chassis body    
  m_chassis = ChSharedPtr<ChBodyAuxRef>(new ChBodyAuxRef);
  m_chassis->SetIdentifier(0);
  m_chassis->SetNameString(name);
  m_chassis->SetFrame_COG_to_REF(ChFrame<>(m_COM, ChQuaternion<>(1, 0, 0, 0)));
  // basic body info
  m_chassis->SetMass(m_mass);
  m_chassis->SetInertiaXX(m_inertia);

  // add visualization assets to the chassis
  AddVisualization();

  m_chassis->SetBodyFixed(false);
  // add the chassis body to the system
  Add(m_chassis);

  // resize all vectors for the number of track systems
  m_TrackSystems.resize(m_num_tracks);
  m_TrackSystem_locs.resize(m_num_tracks);
  // Right and Left track System relative locations, respectively
  m_TrackSystem_locs[0] = m_trackPos_Right;
  m_TrackSystem_locs[1] = m_trackPos_Left;

  // Each trackSystem has its own driveline and powertrain, so the left and right
  // sides can have power applied independently
  m_drivelines.resize(m_num_engines);
  m_ptrains.resize(m_num_engines);

  // create track systems
  for (int i = 0; i < m_num_tracks; i++) {
    m_TrackSystems[i] = ChSharedPtr<TrackSystem>(new TrackSystem("track chain "+std::to_string(i), i) );
  }
  
  // create the powertrain and drivelines
  for (int j = 0; j < m_num_engines; j++)
  {
    m_drivelines[j] = ChSharedPtr<TrackDriveline>(new TrackDriveline("driveline "+std::to_string(j)) );
    m_ptrains[j] = ChSharedPtr<TrackPowertrain>(new TrackPowertrain("powertrain "+std::to_string(j)) );
  
  }

  // TODO: add brakes. Perhaps they are a part of the suspension subsystem?

}


TrackVehicle::~TrackVehicle()
{
  if(m_ownsSystem)
    delete m_system;
}

// Set any collision geometry on the hull, then Initialize() all subsystems
void TrackVehicle::Initialize(const ChCoordsys<>& chassis_Csys)
{
  // move the chassis REF frame to the specified initial position/orientation
  m_chassis->SetFrame_REF_to_abs(ChFrame<>(chassis_Csys));

  // add collision geometry to the chassis
  AddCollisionGeometry();

  // initialize the subsystems with the initial c-sys and specified offsets
  for (int i = 0; i < m_num_tracks; i++)
  {
    m_TrackSystems[i]->Initialize(m_chassis, m_TrackSystem_locs[i]);
  }

  // initialize the powertrain, drivelines
  for (int j = 0; j < m_num_engines; j++)
  {
    size_t driveGear_R_idx = 2*j;
    size_t driveGear_L_idx = 2*j + 1;
    m_drivelines[j]->Initialize(m_chassis,
      m_TrackSystems[driveGear_R_idx]->GetDriveGear(),
      m_TrackSystems[driveGear_L_idx]->GetDriveGear());
    m_ptrains[j]->Initialize(m_chassis, m_drivelines[j]->GetDriveshaft());
  }

}


void TrackVehicle::Update(double	time,
                          const std::vector<double>&  throttle,
                          const std::vector<double>&  braking)
{
  assert( throttle.size() == m_num_tracks);
  assert( braking.size() == m_num_tracks );
  // update left and right powertrains, with the new left and right throttle/shaftspeed
  for(int i = 0; i < m_num_engines; i++)
  {
    m_ptrains[i]->Update(time, throttle[0], throttle[1], m_drivelines[0]->GetDriveshaftSpeed() );
  }

}

void TrackVehicle::AddVisualization()
{
  // add visual geometry asset to the chassis, if enabled
  switch (m_vis) {
  case VisualizationType::NONE:
  {
    // put a sphere at the chassis COM and at the REF point
    ChSharedPtr<ChSphereShape> COMsphere(new ChSphereShape);
    COMsphere->GetSphereGeometry().rad = 0.1;
    COMsphere->Pos = m_COM;
    // make the COM sphere blue
    COMsphere->SetColor(ChColor(0.1f, 0.2f, 0.8f));
    m_chassis->AddAsset(COMsphere);
    
   
    ChSharedPtr<ChSphereShape> REFsphere(new ChSphereShape);
    REFsphere->GetSphereGeometry().rad = 0.1;
    REFsphere->Pos = ChVector<>(0,0,0); // REF should be at the body c-sys origin
    // make the REF sphere red
    REFsphere->SetColor(ChColor(0.8f, 0.2f, 0.1f));
    m_chassis->AddAsset(REFsphere);

    break;
  }
  case VisualizationType::PRIMITIVES:
  {
    ChSharedPtr<ChBoxShape> box(new ChBoxShape);
    box->GetBoxGeometry().SetLengths(m_chassisBoxSize );
    m_chassis->AddAsset(box);
    break;
  }
  case VisualizationType::MESH:
  {
    geometry::ChTriangleMeshConnected trimesh;
    trimesh.LoadWavefrontMesh(m_meshFile, true, true);

    ChSharedPtr<ChTriangleMeshShape> trimesh_shape(new ChTriangleMeshShape);
    trimesh_shape->SetMesh(trimesh);
    trimesh_shape->SetName("chassis triMesh");
    m_chassis->AddAsset(trimesh_shape);

    break;
  }
  } // end switch


}

void TrackVehicle::AddCollisionGeometry()
{
  // add collision geometrey to the chassis, if enabled
  if( m_collide == CollisionType::NONE)
  {
    m_chassis->SetCollide(false);
    return;
  }
  m_chassis->SetCollide(true);
{'fraction_non_alphanumeric': 0.1674831192903482, 'fraction_numerical': 0.0189328743545611, 'mean_word_length': 4.232767578801524, 'pattern_counts': {'":': 0, '<': 64, '<?xml version=': 0, '>': 112, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '415165', 'n_tokens_mistral': 5423, 'n_tokens_neox': 5106, 'n_words': 1149}