text: // =============================================================================
  // PROJECT CHRONO - http://projectchrono.org
  //
  // Copyright (c) 2014 projectchrono.org
  // All rights 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: Radu Serban
  // =============================================================================
  //
  // Demonstration program for M113 vehicle on rigid terrain.
  //
  // =============================================================================
  
  #include "chrono/utils/ChUtilsInputOutput.h"
  
  #include "chrono_vehicle/ChVehicleModelData.h"
  #include "chrono_vehicle/driver/ChIrrGuiDriver.h"
  #include "chrono_vehicle/terrain/RigidTerrain.h"
  #include "chrono_vehicle/output/ChVehicleOutputASCII.h"
  
  #include "chrono_vehicle/tracked_vehicle/utils/ChTrackedVehicleIrrApp.h"
  
  #include "chrono_models/vehicle/m113/M113_SimplePowertrain.h"
  #include "chrono_models/vehicle/m113/M113_Vehicle.h"
  
  #ifdef CHRONO_MKL
  #include "chrono_mkl/ChSolverMKL.h"
  #endif
  
  #include "chrono_thirdparty/filesystem/path.h"
  
  using namespace chrono;
  using namespace chrono::vehicle;
  using namespace chrono::vehicle::m113;
  
  using std::cout;
  using std::endl;
  
  // =============================================================================
  // USER SETTINGS
  // =============================================================================
  // Initial vehicle position
  ChVector<> initLoc(0, 0, 1.1);
  
  // Initial vehicle orientation
  ChQuaternion<> initRot(1, 0, 0, 0);
  // ChQuaternion<> initRot(0.866025, 0, 0, 0.5);
  // ChQuaternion<> initRot(0.7071068, 0, 0, 0.7071068);
  // ChQuaternion<> initRot(0.25882, 0, 0, 0.965926);
  // ChQuaternion<> initRot(0, 0, 0, 1);
  
  // Rigid terrain dimensions
  double terrainHeight = 0;
  double terrainLength = 100.0;  // size in X direction
  double terrainWidth = 100.0;   // size in Y direction
  
  // Simulation step size
  double step_size = 1e-3;
  
  // Use HHT + MKL
  bool use_mkl = false;
  
  // Time interval between two render frames
  double render_step_size = 1.0 / 50;  // FPS = 50
  
  // Point on chassis tracked by the camera
  ChVector<> trackPoint(0.0, 0.0, 0.0);
  
  // Output directories
  const std::string out_dir = GetChronoOutputPath() + "M113";
  const std::string pov_dir = out_dir + "/POVRAY";
  const std::string img_dir = out_dir + "/IMG";
  
  // Output
  bool povray_output = false;
  bool img_output = false;
  bool dbg_output = false;
  
  // =============================================================================
  
  // Simple powertrain model
  std::string simplepowertrain_file("generic/powertrain/SimplePowertrain.json");
  
  // =============================================================================
  
  // Forward declarations
  void AddFixedObstacles(ChSystem* system);
  void AddFallingObjects(ChSystem* system);
  
  // =============================================================================
  int main(int argc, char* argv[]) {
      GetLog() << "Copyright (c) 2017 projectchrono.org\nChrono version: " << CHRONO_VERSION << "\n\n";
  
      // --------------------------
      // Construct the M113 vehicle
      // --------------------------
  
      ChMaterialSurface::ContactMethod contact_method = ChMaterialSurface::SMC;
      ChassisCollisionType chassis_collision_type = ChassisCollisionType::NONE;
      TrackShoeType shoe_type = TrackShoeType::SINGLE_PIN;
  
      //// TODO
      //// When using SMC, a double-pin shoe type requires MKL or MUMPS.  
      //// However, there appear to still be redundant constraints in the double-pin assembly
      //// resulting in solver failures with MKL and MUMPS (rank-deficient matrix).
      if (shoe_type == TrackShoeType::DOUBLE_PIN)
          contact_method = ChMaterialSurface::NSC;
  
      M113_Vehicle vehicle(false, shoe_type, contact_method, chassis_collision_type);
  
      // ------------------------------
      // Solver and integrator settings
      // ------------------------------
  
      // Cannot use HHT + MKL with NSC contact
      if (contact_method == ChMaterialSurface::NSC) {
          use_mkl = false;
      }
  
  #ifndef CHRONO_MKL
      // Cannot use HHT + MKL if Chrono::MKL not available
      use_mkl = false;
  #endif
  
      if (use_mkl) {
  #ifdef CHRONO_MKL
          auto mkl_solver = std::make_shared<ChSolverMKL<>>();
          mkl_solver->SetSparsityPatternLock(true);
          vehicle.GetSystem()->SetSolver(mkl_solver);
  
          vehicle.GetSystem()->SetTimestepperType(ChTimestepper::Type::HHT);
          auto integrator = std::static_pointer_cast<ChTimestepperHHT>(vehicle.GetSystem()->GetTimestepper());
          integrator->SetAlpha(-0.2);
          integrator->SetMaxiters(50);
          integrator->SetAbsTolerances(1e-4, 1e2);
          integrator->SetMode(ChTimestepperHHT::ACCELERATION);
          integrator->SetStepControl(false);
          integrator->SetModifiedNewton(false);
          integrator->SetScaling(true);
          integrator->SetVerbose(true);
  #endif
      } else {
          vehicle.GetSystem()->SetSolverType(ChSolver::Type::SOR);
          vehicle.GetSystem()->SetMaxItersSolverSpeed(50);
          vehicle.GetSystem()->SetMaxItersSolverStab(50);
          vehicle.GetSystem()->SetTol(0);
          vehicle.GetSystem()->SetMaxPenetrationRecoverySpeed(1.5);
          vehicle.GetSystem()->SetMinBounceSpeed(2.0);
          vehicle.GetSystem()->SetSolverOverrelaxationParam(0.8);
          vehicle.GetSystem()->SetSolverSharpnessParam(1.0);
      }
  
      // Disable gravity in this simulation
      ////vehicle.GetSystem()->Set_G_acc(ChVector<>(0, 0, 0));
  
      // Control steering type (enable crossdrive capability)
      ////vehicle.GetDriveline()->SetGyrationMode(true);
  
      // ------------------------------------------------
      // Initialize the vehicle at the specified position
      // ------------------------------------------------
  
      vehicle.Initialize(ChCoordsys<>(initLoc, initRot));
  
      // Set visualization type for vehicle components.
      VisualizationType track_vis =
          (shoe_type == TrackShoeType::SINGLE_PIN) ? VisualizationType::MESH : VisualizationType::PRIMITIVES;
      vehicle.SetChassisVisualizationType(VisualizationType::PRIMITIVES);
      vehicle.SetSprocketVisualizationType(track_vis);
      vehicle.SetIdlerVisualizationType(track_vis);
      vehicle.SetRoadWheelAssemblyVisualizationType(track_vis);
      vehicle.SetRoadWheelVisualizationType(track_vis);
      vehicle.SetTrackShoeVisualizationType(track_vis);
  
      // --------------------------------------------------
      // Control internal collisions and contact monitoring
      // --------------------------------------------------
  
      // Enable contact on all tracked vehicle parts, except the left sprocket
      ////vehicle.SetCollide(TrackedCollisionFlag::ALL & (~TrackedCollisionFlag::SPROCKET_LEFT));
  
      // Disable contact for all tracked vehicle parts
      ////vehicle.SetCollide(TrackedCollisionFlag::NONE);
  
      // Disable all contacts for vehicle chassis (if chassis collision was defined)
      ////vehicle.SetChassisCollide(false);
  
      // Disable only contact between chassis and track shoes (if chassis collision was defined)
      ////vehicle.SetChassisVehicleCollide(false);
  
      // Monitor internal contacts for the chassis, left sprocket, left idler, and first shoe on the left track.
      ////vehicle.MonitorContacts(TrackedCollisionFlag::CHASSIS | TrackedCollisionFlag::SPROCKET_LEFT |
      ////                        TrackedCollisionFlag::SHOES_LEFT | TrackedCollisionFlag::IDLER_LEFT);
  
      // Monitor only contacts involving the chassis.
      vehicle.MonitorContacts(TrackedCollisionFlag::CHASSIS);
  
      // Collect contact information.
      // If enabled, number of contacts and local contact point locations are collected for all
      // monitored parts.  Data can be written to a file by invoking ChTrackedVehicle::WriteContacts().
      ////vehicle.SetContactCollection(true);
  
      // ------------------
      // Create the terrain
      // ------------------
  
      RigidTerrain terrain(vehicle.GetSystem());
      auto patch = terrain.AddPatch(ChCoordsys<>(ChVector<>(0, 0, terrainHeight - 5), QUNIT),
                                    ChVector<>(terrainLength, terrainWidth, 10));
      patch->SetContactFrictionCoefficient(0.9f);
      patch->SetContactRestitutionCoefficient(0.01f);
      patch->SetContactMaterialProperties(2e7f, 0.3f);
      patch->SetColor(ChColor(0.5f, 0.8f, 0.5f));
      patch->SetTexture(vehicle::GetDataFile("terrain/textures/tile4.jpg"), 200, 200);
      terrain.Initialize();
  
      // --------------------------------
      // Add fixed and/or falling objects
      // --------------------------------
  
      AddFixedObstacles(vehicle.GetSystem());
      ////AddFallingObjects(vehicle.GetSystem());
  
      // ----------------------------
      // Create the powertrain system
      // ----------------------------
  
      M113_SimplePowertrain powertrain("Powertrain");
      powertrain.Initialize(vehicle.GetChassisBody(), vehicle.GetDriveshaft());
  
      // ---------------------------------------
      // Create the vehicle Irrlicht application
      // ---------------------------------------
  
      ChTrackedVehicleIrrApp app(&vehicle, &powertrain, L"M113 Vehicle Demo");
      app.SetSkyBox();
      app.AddTypicalLights(irr::core::vector3df(30.f, -30.f, 100.f), irr::core::vector3df(30.f, 50.f, 100.f), 250, 130);
      app.SetChaseCamera(trackPoint, 6.0, 0.5);
      ////app.SetChaseCameraPosition(vehicle.GetVehiclePos() + ChVector<>(0, 2, 0));
      app.SetChaseCameraMultipliers(1e-4, 10);
      app.SetTimestep(step_size);
      app.AssetBindAll();
      app.AssetUpdateAll();
  
      // ------------------------
      // Create the driver system
      // ------------------------
  
      ChIrrGuiDriver driver(app);
  
      // Set the time response for keyboard inputs.
      double steering_time = 0.5;  // time to go from 0 to +1 (or from 0 to -1)
      double throttle_time = 1.0;  // time to go from 0 to +1
      double braking_time = 0.3;   // time to go from 0 to +1
      driver.SetSteeringDelta(render_step_size / steering_time);
      driver.SetThrottleDelta(render_step_size / throttle_time);
      driver.SetBrakingDelta(render_step_size / braking_time);
  
      driver.Initialize();
  
      // -----------------
      // Initialize output
      // -----------------
  
      if (!filesystem::create_directory(filesystem::path(out_dir))) {
          std::cout << "Error creating directory " << out_dir << std::endl;
          return 1;
      }
  
      if (povray_output) {
          if (!filesystem::create_directory(filesystem::path(pov_dir))) {
              std::cout << "Error creating directory " << pov_dir << std::endl;
              return 1;
          }
          terrain.ExportMeshPovray(out_dir);
      }
  
      if (img_output) {
          if (!filesystem::create_directory(filesystem::path(img_dir))) {
              std::cout << "Error creating directory " << img_dir << std::endl;
              return 1;
          }
      }
  
      // Set up vehicle output
      vehicle.SetChassisOutput(true);
      vehicle.SetTrackAssemblyOutput(VehicleSide::LEFT, true);
      vehicle.SetOutput(ChVehicleOutput::ASCII, out_dir, "output", 0.1);
  
      // Generate JSON information with available output channels
      vehicle.ExportComponentList(out_dir + "/component_list.json");
  
      // ---------------
      // Simulation loop
      // ---------------
  
      // Inter-module communication data
      BodyStates shoe_states_left(vehicle.GetNumTrackShoes(LEFT));
      BodyStates shoe_states_right(vehicle.GetNumTrackShoes(RIGHT));
      TerrainForces shoe_forces_left(vehicle.GetNumTrackShoes(LEFT));
      TerrainForces shoe_forces_right(vehicle.GetNumTrackShoes(RIGHT));
  
      // Number of simulation steps between two 3D view render frames
      int render_steps = (int)std::ceil(render_step_size / step_size);
  
      // Initialize simulation frame counter
      int step_number = 0;
      int render_frame = 0;
  
      while (app.GetDevice()->run()) {
          // Debugging output
          if (dbg_output) {
              cout << "Time: " << vehicle.GetSystem()->GetChTime() << endl;
              const ChFrameMoving<>& c_ref = vehicle.GetChassisBody()->GetFrame_REF_to_abs();
              const ChVector<>& c_pos = vehicle.GetVehiclePos();
              cout << "      chassis:    " << c_pos.x() << "  " << c_pos.y() << "  " << c_pos.z() << endl;
              {
                  const ChVector<>& i_pos_abs = vehicle.GetTrackAssembly(LEFT)->GetIdler()->GetWheelBody()->GetPos();
                  const ChVector<>& s_pos_abs = vehicle.GetTrackAssembly(LEFT)->GetSprocket()->GetGearBody()->GetPos();
                  ChVector<> i_pos_rel = c_ref.TransformPointParentToLocal(i_pos_abs);
                  ChVector<> s_pos_rel = c_ref.TransformPointParentToLocal(s_pos_abs);
                  cout << "      L idler:    " << i_pos_rel.x() << "  " << i_pos_rel.y() << "  " << i_pos_rel.z() << endl;
                  cout << "      L sprocket: " << s_pos_rel.x() << "  " << s_pos_rel.y() << "  " << s_pos_rel.z() << endl;
              }
              {
                  const ChVector<>& i_pos_abs = vehicle.GetTrackAssembly(RIGHT)->GetIdler()->GetWheelBody()->GetPos();
                  const ChVector<>& s_pos_abs = vehicle.GetTrackAssembly(RIGHT)->GetSprocket()->GetGearBody()->GetPos();
                  ChVector<> i_pos_rel = c_ref.TransformPointParentToLocal(i_pos_abs);
                  ChVector<> s_pos_rel = c_ref.TransformPointParentToLocal(s_pos_abs);
                  cout << "      R idler:    " << i_pos_rel.x() << "  " << i_pos_rel.y() << "  " << i_pos_rel.z() << endl;
                  cout << "      R sprocket: " << s_pos_rel.x() << "  " << s_pos_rel.y() << "  " << s_pos_rel.z() << endl;
              }
              cout << "      L suspensions (arm angles):" << endl;
              for (size_t i = 0; i < vehicle.GetTrackAssembly(LEFT)->GetNumRoadWheelAssemblies(); i++) {
                  cout << " " << vehicle.GetTrackAssembly(VehicleSide::LEFT)->GetRoadWheelAssembly(i)->GetCarrierAngle();
              }
              cout << endl;
              cout << "      R suspensions (arm angles):" << endl;
              for (size_t i = 0; i < vehicle.GetTrackAssembly(RIGHT)->GetNumRoadWheelAssemblies(); i++) {
                  cout << " " << vehicle.GetTrackAssembly(VehicleSide::RIGHT)->GetRoadWheelAssembly(i)->GetCarrierAngle();
              }
              cout << endl;
          }
  
          // Render scene
          app.BeginScene(true, true, irr::video::SColor(255, 140, 161, 192));
          app.DrawAll();
  
          if (step_number % render_steps == 0) {
              if (povray_output) {
                  char filename[100];
                  sprintf(filename, "%s/data_%03d.dat", pov_dir.c_str(), render_frame + 1);
                  utils::WriteShapesPovray(vehicle.GetSystem(), filename);
              }
              if (img_output && step_number > 200) {
                  char filename[100];
                  sprintf(filename, "%s/img_%03d.jpg", img_dir.c_str(), render_frame + 1);
                  app.WriteImageToFile(filename);
              }
              render_frame++;
          }
  
          // Collect output data from modules
          double throttle_input = driver.GetThrottle();
          double steering_input = driver.GetSteering();
          double braking_input = driver.GetBraking();
          double powertrain_torque = powertrain.GetOutputTorque();
          double driveshaft_speed = vehicle.GetDriveshaftSpeed();
          vehicle.GetTrackShoeStates(LEFT, shoe_states_left);
          vehicle.GetTrackShoeStates(RIGHT, shoe_states_right);
  
          // Update modules (process inputs from other modules)
          double time = vehicle.GetChTime();
          driver.Synchronize(time);
          terrain.Synchronize(time);
          powertrain.Synchronize(time, throttle_input, driveshaft_speed);
          vehicle.Synchronize(time, steering_input, braking_input, powertrain_torque, shoe_forces_left, shoe_forces_right);
          app.Synchronize("", steering_input, throttle_input, braking_input);
  
          // Advance simulation for one timestep for all modules
          driver.Advance(step_size);
          terrain.Advance(step_size);
          powertrain.Advance(step_size);
          vehicle.Advance(step_size);
          app.Advance(step_size);
  
          // Report if the chassis experienced a collision
          if (vehicle.IsPartInContact(TrackedCollisionFlag::CHASSIS)) {
              std::cout << time << "  chassis contact" << std::endl;
          }
  
          // Increment frame number
          step_number++;
  
          app.EndScene();
      }
  
      vehicle.WriteContacts("M113_contacts.out");
  
      return 0;
  }
  
  // =============================================================================
  void AddFixedObstacles(ChSystem* system) {
      double radius = 2.2;
      double length = 6;
  
      float friction_coefficient = 0.9f;
      float restitution_coefficient = 0.01f;
      float young_modulus = 2e7f;
      float poisson_ratio = 0.3f;
  
      auto obstacle = std::shared_ptr<ChBody>(system->NewBody());
      obstacle->SetPos(ChVector<>(10, 0, -1.8));
      obstacle->SetBodyFixed(true);
      obstacle->SetCollide(true);
  
      // Visualization
      auto shape = std::make_shared<ChCylinderShape>();
      shape->GetCylinderGeometry().p1 = ChVector<>(0, -length * 0.5, 0);
      shape->GetCylinderGeometry().p2 = ChVector<>(0, length * 0.5, 0);
      shape->GetCylinderGeometry().rad = radius;
      obstacle->AddAsset(shape);
  
      auto color = std::make_shared<ChColorAsset>();
      color->SetColor(ChColor(1, 1, 1));
      obstacle->AddAsset(color);
  
      auto texture = std::make_shared<ChTexture>();
      texture->SetTextureFilename(vehicle::GetDataFile("terrain/textures/tile4.jpg"));
      texture->SetTextureScale(10, 10);
      obstacle->AddAsset(texture);
  
      // Contact
      obstacle->GetCollisionModel()->ClearModel();
      obstacle->GetCollisionModel()->AddCylinder(radius, radius, length * 0.5);
      obstacle->GetCollisionModel()->BuildModel();
  
      switch (obstacle->GetContactMethod()) {
          case ChMaterialSurface::NSC:
              obstacle->GetMaterialSurfaceNSC()->SetFriction(friction_coefficient);
              obstacle->GetMaterialSurfaceNSC()->SetRestitution(restitution_coefficient);
              break;
          case ChMaterialSurface::SMC:
              obstacle->GetMaterialSurfaceSMC()->SetFriction(friction_coefficient);
              obstacle->GetMaterialSurfaceSMC()->SetRestitution(restitution_coefficient);
              obstacle->GetMaterialSurfaceSMC()->SetYoungModulus(young_modulus);
              obstacle->GetMaterialSurfaceSMC()->SetPoissonRatio(poisson_ratio);
              break;
      }
  
      system->AddBody(obstacle);
  }
  
  // =============================================================================
  void AddFallingObjects(ChSystem* system) {
      double radius = 0.1;
      double mass = 10;
  
      auto ball = std::shared_ptr<ChBody>(system->NewBody());
      ball->SetMass(mass);
      ball->SetInertiaXX(0.4 * mass * radius * radius * ChVector<>(1, 1, 1));
      ball->SetPos(initLoc + ChVector<>(-3, 0, 2));
      ball->SetRot(ChQuaternion<>(1, 0, 0, 0));
      ball->SetPos_dt(ChVector<>(3, 0, 0));
      ball->SetBodyFixed(false);
  
      ball->SetCollide(true);
      ball->GetCollisionModel()->ClearModel();
      ball->GetCollisionModel()->AddSphere(radius);
      ball->GetCollisionModel()->BuildModel();
  
      auto sphere = std::make_shared<ChSphereShape>();
      sphere->GetSphereGeometry().rad = radius;
      ball->AddAsset(sphere);
  
      auto mtexture = std::make_shared<ChTexture>();
      mtexture->SetTextureFilename(GetChronoDataFile("bluwhite.png"));
      ball->AddAsset(mtexture);
  
      system->AddBody(ball);
  }