// 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 ChPac89Tire::Advance(double step) { // Return now if no contact. Tire force and moment are already set to 0 in Synchronize(). if (!m_data.in_contact) return; if (m_states.vx != 0) { m_states.cp_long_slip = -m_states.vsx / m_states.vx; } else { m_states.cp_long_slip = 0; } if (m_states.omega != 0) { m_states.cp_side_slip = std::atan(m_states.vsy / std::abs(m_states.omega * (m_unloaded_radius - m_data.depth))); } else { m_states.cp_side_slip = 0; } // Ensure that cp_lon_slip stays between -1 & 1 ChClampValue(m_states.cp_long_slip, -1.0, 1.0); // Ensure that cp_side_slip stays between -pi()/2 & pi()/2 (a little less to prevent tan from going to infinity) ChClampValue(m_states.cp_side_slip, -CH_C_PI_2 + 0.001, CH_C_PI_2 - 0.001); // Calculate the new force and moment values (normal force and moment have already been accounted for in // Synchronize()). // Express Fz in kN (note that all other forces and moments are in N and Nm). // See reference for details on the calculations. double Fx = 0; double Fy = 0; double Fz = m_data.normal_force / 1000; double Mx = 0; double My = 0; double Mz = 0; // Express alpha and gamma in degrees. Express kappa as percentage. // Flip sign of alpha to convert to PAC89 modified SAE coordinates. m_gamma = 90.0 - std::acos(m_states.disc_normal.z()) * CH_C_RAD_TO_DEG; m_alpha = -m_states.cp_side_slip * CH_C_RAD_TO_DEG; m_kappa = m_states.cp_long_slip * 100.0; // Clamp |gamma| to specified value: Limit due to tire testing, avoids erratic extrapolation. double gamma = ChClamp(m_gamma, -m_gamma_limit, m_gamma_limit); // Longitudinal Force { double C = m_PacCoeff.B0; double D = (m_PacCoeff.B1 * std::pow(Fz, 2) + m_PacCoeff.B2 * Fz); double BCD = (m_PacCoeff.B3 * std::pow(Fz, 2) + m_PacCoeff.B4 * Fz) * std::exp(-m_PacCoeff.B5 * Fz); double B = BCD / (C * D); double Sh = m_PacCoeff.B9 * Fz + m_PacCoeff.B10; double Sv = 0.0; double X1 = (m_kappa + Sh); double E = (m_PacCoeff.B6 * std::pow(Fz, 2) + m_PacCoeff.B7 * Fz + m_PacCoeff.B8); Fx = (D * std::sin(C * std::atan(B * X1 - E * (B * X1 - std::atan(B * X1))))) + Sv; } // Lateral Force { double C = m_PacCoeff.A0; double D = (m_PacCoeff.A1 * std::pow(Fz, 2) + m_PacCoeff.A2 * Fz); double BCD = m_PacCoeff.A3 * std::sin(std::atan(Fz / m_PacCoeff.A4) * 2.0) * (1.0 - m_PacCoeff.A5 * std::abs(gamma)); double B = BCD / (C * D); double Sh = m_PacCoeff.A9 * Fz + m_PacCoeff.A10 + m_PacCoeff.A8 * gamma; double Sv = m_PacCoeff.A11 * Fz * gamma + m_PacCoeff.A12 * Fz + m_PacCoeff.A13; double X1 = m_alpha + Sh; double E = m_PacCoeff.A6 * Fz + m_PacCoeff.A7; // Ensure that X1 stays within +/-90 deg minus a little bit ChClampValue(X1, -89.5, 89.5); Fy = (D * std::sin(C * std::atan(B * X1 - E * (B * X1 - std::atan(B * X1))))) + Sv; } // Self-Aligning Torque { double C = m_PacCoeff.C0; double D = (m_PacCoeff.C1 * std::pow(Fz, 2) + m_PacCoeff.C2 * Fz); double BCD = (m_PacCoeff.C3 * std::pow(Fz, 2) + m_PacCoeff.C4 * Fz) * (1 - m_PacCoeff.C6 * std::abs(gamma)) * std::exp(-m_PacCoeff.C5 * Fz); double B = BCD / (C * D); double Sh = m_PacCoeff.C11 * gamma + m_PacCoeff.C12 * Fz + m_PacCoeff.C13; double Sv = (m_PacCoeff.C14 * std::pow(Fz, 2) + m_PacCoeff.C15 * Fz) * gamma + m_PacCoeff.C16 * Fz + m_PacCoeff.C17; double X1 = m_alpha + Sh; double E = (m_PacCoeff.C7 * std::pow(Fz, 2) + m_PacCoeff.C8 * Fz + m_PacCoeff.C9) * (1.0 - m_PacCoeff.C10 * std::abs(gamma)); // Ensure that X1 stays within +/-90 deg minus a little bit ChClampValue(X1, -89.5, 89.5); Mz = (D * std::sin(C * std::atan(B * X1 - E * (B * X1 - std::atan(B * X1))))) + Sv; } // Overturning Moment { double deflection = Fy / m_lateral_stiffness; Mx = -(Fz * 1000) * deflection; Mz = Mz + Fx * deflection; } // Rolling Resistance { double Lrad = (m_unloaded_radius - m_data.depth); // Smoothing interval for My const double vx_min = 0.125; const double vx_max = 0.5; // Smoothing factor dependend on m_state.abs_vx, allows soft switching of My double myStartUp = ChSineStep(std::abs(m_states.vx), vx_min, 0.0, vx_max, 1.0); My = myStartUp * m_rolling_resistance * m_data.normal_force * Lrad * ChSignum(m_states.omega); } // std::cout << "Fx:" << Fx // << " Fy:" << Fy // << " Fz:" << Fz // << " Mx:" << Mx // << " My:" << My // << " Mz:" << Mz // << std::endl // << " G:" << gamma // << " A:" << alpha // << " K:" << kappa // << " O:" << m_states.omega // << std::endl; // Compile the force and moment vectors so that they can be // transformed into the global coordinate system. // Convert from SAE to ISO Coordinates at the contact patch. m_tireforce.force = ChVector<>(Fx, -Fy, m_data.normal_force); m_tireforce.moment = ChVector<>(Mx, -My, -Mz); // Rotate into global coordinates m_tireforce.force = m_data.frame.TransformDirectionLocalToParent(m_tireforce.force); m_tireforce.moment = m_data.frame.TransformDirectionLocalToParent(m_tireforce.moment); // Move the tire forces from the contact patch to the wheel center m_tireforce.moment += Vcross((m_data.frame.pos + m_data.depth * m_data.frame.rot.GetZaxis()) - m_tireforce.point, m_tireforce.force); }