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: Rainer Gericke, Radu Serban
  // =============================================================================
  //
  // Base class for a leaf-spring solid axle suspension.
  //
  // This class is meant for modelling a very simple nonsteerable solid leafspring
  // axle. The guiding function of leafspring is modelled by a ChLinkLockRevolutePrismatic
  // joint, it allows vertical movement and tilting of the axle tube but no elasticity.
  // The spring function of the leafspring is modelled by a vertical spring element.
  // Tie up of the leafspring is not possible with this approach, as well as the
  // characteristic kinematics along wheel travel. The roll center and roll stability
  // is met well, if spring track is defined correctly. The class has been designed
  // for maximum easyness and numerical efficiency.
  //
  // The suspension subsystem is modeled with respect to a right-handed frame,
  // with X pointing towards the front, Y to the left, and Z up (ISO standard).
  // The suspension reference frame is assumed to be always aligned with that of
  // the vehicle.  When attached to a chassis, only an offset is provided.
  //
  // All point locations are assumed to be given for the left half of the
  // suspension and will be mirrored (reflecting the y coordinates) to construct
  // the right side.
  //
  // =============================================================================
  
  #include "chrono/assets/ChColorAsset.h"
  #include "chrono/assets/ChCylinderShape.h"
  #include "chrono/assets/ChPointPointDrawing.h"
  
  #include "chrono_vehicle/wheeled_vehicle/suspension/ChLeafspringAxle.h"
  
  namespace chrono {
  namespace vehicle {
  
  // -----------------------------------------------------------------------------
  // Static variables
  // -----------------------------------------------------------------------------
  const std::string ChLeafspringAxle::m_pointNames[] = {"SHOCK_A    ", "SHOCK_C    ", "SPRING_A   ", "SPRING_C   ",
                                                        "SPINDLE    "};
  
  // -----------------------------------------------------------------------------
  // -----------------------------------------------------------------------------
  ChLeafspringAxle::ChLeafspringAxle(const std::string& name) : ChSuspension(name) {}
  
  // -----------------------------------------------------------------------------
  // -----------------------------------------------------------------------------
  void ChLeafspringAxle::Initialize(std::shared_ptr<ChBodyAuxRef> chassis,
                                    const ChVector<>& location,
                                    std::shared_ptr<ChBody> tierod_body,
                                    int steering_index,
                                    double left_ang_vel,
                                    double right_ang_vel) {
      m_location = location;
      m_steering_index = steering_index;
  
      // Unit vectors for orientation matrices.
      ChVector<> u;
      ChVector<> v;
      ChVector<> w;
      ChMatrix33<> rot;
  
      // Express the suspension reference frame in the absolute coordinate system.
      ChFrame<> suspension_to_abs(location);
      suspension_to_abs.ConcatenatePreTransformation(chassis->GetFrame_REF_to_abs());
  
      // Transform the location of the axle body COM to absolute frame.
      ChVector<> axleCOM_local = getAxleTubeCOM();
      ChVector<> axleCOM = suspension_to_abs.TransformLocalToParent(axleCOM_local);
  
      // Calculate end points on the axle body, expressed in the absolute frame
      // (for visualization)
      ChVector<> midpoint_local = 0.0;
      // ChVector<> outer_local(axleCOM_local.x(), midpoint_local.y(), axleCOM_local.z());
      ChVector<> outer_local(getLocation(SPINDLE));
      m_axleOuterL = suspension_to_abs.TransformPointLocalToParent(outer_local);
      outer_local.y() = -outer_local.y();
      m_axleOuterR = suspension_to_abs.TransformPointLocalToParent(outer_local);
  
      // Create and initialize the axle body.
      m_axleTube = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
      m_axleTube->SetNameString(m_name + "_axleTube");
      m_axleTube->SetPos(axleCOM);
      m_axleTube->SetRot(chassis->GetFrame_REF_to_abs().GetRot());
      m_axleTube->SetMass(getAxleTubeMass());
      m_axleTube->SetInertiaXX(getAxleTubeInertia());
      chassis->GetSystem()->AddBody(m_axleTube);
  
      // Fix the axle body to the chassis
      m_axleTubeGuide = chrono_types::make_shared<ChLinkLockRevolutePrismatic>();
      m_axleTubeGuide->SetNameString(m_name + "_revolutePrismaticAxleTube");
      const ChQuaternion<>& guideRot = chassis->GetFrame_REF_to_abs().GetRot();
      m_axleTubeGuide->Initialize(chassis, m_axleTube, ChCoordsys<>(axleCOM, guideRot * Q_from_AngY(CH_C_PI_2)));
  
      chassis->GetSystem()->AddLink(m_axleTubeGuide);
  
      // Transform all hardpoints to absolute frame.
      m_pointsL.resize(NUM_POINTS);
      m_pointsR.resize(NUM_POINTS);
      for (int i = 0; i < NUM_POINTS; i++) {
          ChVector<> rel_pos = getLocation(static_cast<PointId>(i));
          m_pointsL[i] = suspension_to_abs.TransformLocalToParent(rel_pos);
          rel_pos.y() = -rel_pos.y();
          m_pointsR[i] = suspension_to_abs.TransformLocalToParent(rel_pos);
      }
  
      // Initialize left and right sides.
      InitializeSide(LEFT, chassis, tierod_body, m_pointsL, left_ang_vel);
      InitializeSide(RIGHT, chassis, tierod_body, m_pointsR, right_ang_vel);
  }
  
  void ChLeafspringAxle::InitializeSide(VehicleSide side,
                                        std::shared_ptr<ChBodyAuxRef> chassis,
                                        std::shared_ptr<ChBody> tierod_body,
                                        const std::vector<ChVector<>>& points,
                                        double ang_vel) {
      std::string suffix = (side == LEFT) ? "_L" : "_R";
  
      // Unit vectors for orientation matrices.
      ChVector<> u;
      ChVector<> v;
      ChVector<> w;
      ChMatrix33<> rot;
  
      // Chassis orientation (expressed in absolute frame)
      // Recall that the suspension reference frame is aligned with the chassis.
      ChQuaternion<> chassisRot = chassis->GetFrame_REF_to_abs().GetRot();
  
      // Create and initialize spindle body (same orientation as the chassis)
      m_spindle[side] = std::shared_ptr<ChBody>(chassis->GetSystem()->NewBody());
      m_spindle[side]->SetNameString(m_name + "_spindle" + suffix);
      m_spindle[side]->SetPos(points[SPINDLE]);
      m_spindle[side]->SetRot(chassisRot);
      m_spindle[side]->SetWvel_loc(ChVector<>(0, ang_vel, 0));
      m_spindle[side]->SetMass(getSpindleMass());
      m_spindle[side]->SetInertiaXX(getSpindleInertia());
      chassis->GetSystem()->AddBody(m_spindle[side]);
  
      // Create and initialize the revolute joint between axle tube and spindle.
      ChCoordsys<> rev_csys(points[SPINDLE], chassisRot * Q_from_AngAxis(CH_C_PI / 2.0, VECT_X));
      m_revolute[side] = chrono_types::make_shared<ChLinkLockRevolute>();
      m_revolute[side]->SetNameString(m_name + "_revolute" + suffix);
      m_revolute[side]->Initialize(m_spindle[side], m_axleTube, rev_csys);
      chassis->GetSystem()->AddLink(m_revolute[side]);
  
      // Create and initialize the spring/damper
      m_shock[side] = chrono_types::make_shared<ChLinkTSDA>();
      m_shock[side]->SetNameString(m_name + "_shock" + suffix);
      m_shock[side]->Initialize(chassis, m_axleTube, false, points[SHOCK_C], points[SHOCK_A]);
      m_shock[side]->RegisterForceFunctor(getShockForceFunctor());
      chassis->GetSystem()->AddLink(m_shock[side]);
  
      m_spring[side] = chrono_types::make_shared<ChLinkTSDA>();
      m_spring[side]->SetNameString(m_name + "_spring" + suffix);
      m_spring[side]->Initialize(chassis, m_axleTube, false, points[SPRING_C], points[SPRING_A], false,
                                 getSpringRestLength());
      m_spring[side]->RegisterForceFunctor(getSpringForceFunctor());
      chassis->GetSystem()->AddLink(m_spring[side]);
  
      // Create and initialize the axle shaft and its connection to the spindle. Note that the
      // spindle rotates about the Y axis.
      m_axle[side] = chrono_types::make_shared<ChShaft>();
      m_axle[side]->SetNameString(m_name + "_axle" + suffix);
      m_axle[side]->SetInertia(getAxleInertia());
      m_axle[side]->SetPos_dt(-ang_vel);
      chassis->GetSystem()->Add(m_axle[side]);
  
      m_axle_to_spindle[side] = chrono_types::make_shared<ChShaftsBody>();
      m_axle_to_spindle[side]->SetNameString(m_name + "_axle_to_spindle" + suffix);
      m_axle_to_spindle[side]->Initialize(m_axle[side], m_spindle[side], ChVector<>(0, -1, 0));
      chassis->GetSystem()->Add(m_axle_to_spindle[side]);
  }
  
  // -----------------------------------------------------------------------------
  // Get the total mass of the suspension subsystem.
  // -----------------------------------------------------------------------------
  double ChLeafspringAxle::GetMass() const {
      return getAxleTubeMass() + 2 * (getSpindleMass());
  }
  
  // -----------------------------------------------------------------------------
  // Get the current COM location of the suspension subsystem.
  // -----------------------------------------------------------------------------
  ChVector<> ChLeafspringAxle::GetCOMPos() const {
      ChVector<> com(0, 0, 0);
  
      com += getAxleTubeMass() * m_axleTube->GetPos();
  
      com += getSpindleMass() * m_spindle[LEFT]->GetPos();
      com += getSpindleMass() * m_spindle[RIGHT]->GetPos();
  
      return com / GetMass();
  }
  
  // -----------------------------------------------------------------------------
  // Get the wheel track using the spindle local position.
  // -----------------------------------------------------------------------------
  double ChLeafspringAxle::GetTrack() {
      return 2 * getLocation(SPINDLE).y();
  }
  
  // -----------------------------------------------------------------------------
  // Return current suspension forces
  // -----------------------------------------------------------------------------
  ChSuspension::Force ChLeafspringAxle::ReportSuspensionForce(VehicleSide side) const {
      ChSuspension::Force force;
  
      force.spring_force = m_spring[side]->GetForce();
      force.spring_length = m_spring[side]->GetLength();
      force.spring_velocity = m_spring[side]->GetVelocity();
  
      force.shock_force = m_shock[side]->GetForce();
      force.shock_length = m_shock[side]->GetLength();
      force.shock_velocity = m_shock[side]->GetVelocity();
  
      return force;
  }
  
  // -----------------------------------------------------------------------------
  // -----------------------------------------------------------------------------
  void ChLeafspringAxle::LogHardpointLocations(const ChVector<>& ref, bool inches) {
      double unit = inches ? 1 / 0.0254 : 1.0;
  
      for (int i = 0; i < NUM_POINTS; i++) {
          ChVector<> pos = ref + unit * getLocation(static_cast<PointId>(i));
  
          GetLog() << "   " << m_pointNames[i].c_str() << "  " << pos.x() << "  " << pos.y() << "  " << pos.z() << "\n";
      }
  }
  
  // -----------------------------------------------------------------------------
  // -----------------------------------------------------------------------------
  void ChLeafspringAxle::LogConstraintViolations(VehicleSide side) {
      // TODO: Update this to reflect new suspension joints
      // Revolute joints
  
      {
          ChVectorDynamic<> C = m_sphericalTierod->GetC();
          GetLog() << "Tierod spherical          ";
          GetLog() << "  " << C(0) << "  ";
          GetLog() << "  " << C(1) << "  ";
          GetLog() << "  " << C(2) << "\n";
      }
  }
  
  // -----------------------------------------------------------------------------
  // -----------------------------------------------------------------------------
  void ChLeafspringAxle::AddVisualizationAssets(VisualizationType vis) {
      ChSuspension::AddVisualizationAssets(vis);
  
      if (vis == VisualizationType::NONE)
          return;
  
      AddVisualizationLink(m_axleTube, m_axleOuterL, m_axleOuterR, getAxleTubeRadius(), ChColor(0.7f, 0.7f, 0.7f));
  
      // Add visualization for the springs and shocks
      m_spring[LEFT]->AddAsset(chrono_types::make_shared<ChPointPointSpring>(0.06, 150, 15));
      m_spring[RIGHT]->AddAsset(chrono_types::make_shared<ChPointPointSpring>(0.06, 150, 15));
  
      m_shock[LEFT]->AddAsset(chrono_types::make_shared<ChPointPointSegment>());
      m_shock[RIGHT]->AddAsset(chrono_types::make_shared<ChPointPointSegment>());
  }
  
  void ChLeafspringAxle::RemoveVisualizationAssets() {
      ChSuspension::RemoveVisualizationAssets();
  
      m_axleTube->GetAssets().clear();
  
      m_spring[LEFT]->GetAssets().clear();
      m_spring[RIGHT]->GetAssets().clear();
  
      m_shock[LEFT]->GetAssets().clear();
      m_shock[RIGHT]->GetAssets().clear();
  }
  
  // -----------------------------------------------------------------------------
  // -----------------------------------------------------------------------------
  void ChLeafspringAxle::AddVisualizationLink(std::shared_ptr<ChBody> body,
                                              const ChVector<> pt_1,
                                              const ChVector<> pt_2,
                                              double radius,
                                              const ChColor& color) {
      // Express hardpoint locations in body frame.
      ChVector<> p_1 = body->TransformPointParentToLocal(pt_1);
      ChVector<> p_2 = body->TransformPointParentToLocal(pt_2);
  
      auto cyl = chrono_types::make_shared<ChCylinderShape>();
      cyl->GetCylinderGeometry().p1 = p_1;
      cyl->GetCylinderGeometry().p2 = p_2;
      cyl->GetCylinderGeometry().rad = radius;
      body->AddAsset(cyl);
  
      auto col = chrono_types::make_shared<ChColorAsset>();
      col->SetColor(color);
      body->AddAsset(col);
  }
  
  // -----------------------------------------------------------------------------
  // -----------------------------------------------------------------------------
  void ChLeafspringAxle::ExportComponentList(rapidjson::Document& jsonDocument) const {
      ChPart::ExportComponentList(jsonDocument);
  
      std::vector<std::shared_ptr<ChBody>> bodies;
      bodies.push_back(m_spindle[0]);
      bodies.push_back(m_spindle[1]);
      bodies.push_back(m_axleTube);
      ChPart::ExportBodyList(jsonDocument, bodies);
  
      std::vector<std::shared_ptr<ChShaft>> shafts;
      shafts.push_back(m_axle[0]);
      shafts.push_back(m_axle[1]);
      ChPart::ExportShaftList(jsonDocument, shafts);
  
      std::vector<std::shared_ptr<ChLink>> joints;
      joints.push_back(m_revolute[0]);
      joints.push_back(m_revolute[1]);
      ChPart::ExportJointList(jsonDocument, joints);
  
      std::vector<std::shared_ptr<ChLinkTSDA>> springs;
      springs.push_back(m_spring[0]);
      springs.push_back(m_spring[1]);
      springs.push_back(m_shock[0]);
      springs.push_back(m_shock[1]);
      ChPart::ExportLinSpringList(jsonDocument, springs);
  }
  
  void ChLeafspringAxle::Output(ChVehicleOutput& database) const {
      if (!m_output)
          return;
  
      std::vector<std::shared_ptr<ChBody>> bodies;
      bodies.push_back(m_spindle[0]);
      bodies.push_back(m_spindle[1]);
      bodies.push_back(m_axleTube);
      database.WriteBodies(bodies);
  
      std::vector<std::shared_ptr<ChShaft>> shafts;
      shafts.push_back(m_axle[0]);
      shafts.push_back(m_axle[1]);
      database.WriteShafts(shafts);
  
      std::vector<std::shared_ptr<ChLink>> joints;
      joints.push_back(m_revolute[0]);
      joints.push_back(m_revolute[1]);
      database.WriteJoints(joints);
  
      std::vector<std::shared_ptr<ChLinkTSDA>> springs;
      springs.push_back(m_spring[0]);
      springs.push_back(m_spring[1]);
      springs.push_back(m_shock[0]);
      springs.push_back(m_shock[1]);
      database.WriteLinSprings(springs);
  }
  
  }  // end namespace vehicle
  }  // end namespace chrono
  