Update VehiclePhysicsControl fields for UE5-Chaos (#8141)
* Add VehiclePhysicsControl progress... * Update parameter names. * Minor fixes. * Update python initializer. * More VehiclePhysicsControl changes. * Fix minor UE errors. * Update Vehicle and WheelPhysicsControl. * Various fixes. * Add missing linebreak. * Fix indentation. * Make FVehiclePhysicsControl UPROPERTYs uniform. * Fix indentation again... * Fix indentation, yet again...
This commit is contained in:
parent
0db777202c
commit
e326742a67
|
@ -11,7 +11,7 @@
|
||||||
namespace carla {
|
namespace carla {
|
||||||
namespace geom {
|
namespace geom {
|
||||||
|
|
||||||
class Location;
|
struct Location;
|
||||||
|
|
||||||
class GeoLocation {
|
class GeoLocation {
|
||||||
public:
|
public:
|
||||||
|
|
|
@ -19,8 +19,7 @@
|
||||||
namespace carla {
|
namespace carla {
|
||||||
namespace geom {
|
namespace geom {
|
||||||
|
|
||||||
class Location : public Vector3D {
|
struct Location : Vector3D {
|
||||||
public:
|
|
||||||
|
|
||||||
// =========================================================================
|
// =========================================================================
|
||||||
// -- Constructors ---------------------------------------------------------
|
// -- Constructors ---------------------------------------------------------
|
||||||
|
|
|
@ -14,8 +14,9 @@
|
||||||
namespace carla {
|
namespace carla {
|
||||||
namespace geom {
|
namespace geom {
|
||||||
|
|
||||||
class Vector2D {
|
struct Vector2D {
|
||||||
public:
|
|
||||||
|
static constexpr auto Dim = 2;
|
||||||
|
|
||||||
// =========================================================================
|
// =========================================================================
|
||||||
// -- Public data members --------------------------------------------------
|
// -- Public data members --------------------------------------------------
|
||||||
|
|
|
@ -14,8 +14,9 @@
|
||||||
namespace carla {
|
namespace carla {
|
||||||
namespace geom {
|
namespace geom {
|
||||||
|
|
||||||
class Vector3D {
|
struct Vector3D {
|
||||||
public:
|
|
||||||
|
static constexpr auto Dim = 3;
|
||||||
|
|
||||||
// =========================================================================
|
// =========================================================================
|
||||||
// -- Public data members --------------------------------------------------
|
// -- Public data members --------------------------------------------------
|
||||||
|
|
|
@ -11,7 +11,7 @@
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
namespace carla {
|
namespace carla {
|
||||||
namespace geom { class Location; }
|
namespace geom { struct Location; }
|
||||||
namespace road {
|
namespace road {
|
||||||
|
|
||||||
class Map;
|
class Map;
|
||||||
|
|
|
@ -22,7 +22,7 @@ class AActor;
|
||||||
namespace carla {
|
namespace carla {
|
||||||
namespace geom {
|
namespace geom {
|
||||||
class GeoLocation;
|
class GeoLocation;
|
||||||
class Vector3D;
|
struct Vector3D;
|
||||||
}
|
}
|
||||||
namespace sensor {
|
namespace sensor {
|
||||||
namespace data {
|
namespace data {
|
||||||
|
|
|
@ -16,313 +16,230 @@
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
|
||||||
namespace carla {
|
namespace carla {
|
||||||
namespace rpc {
|
namespace rpc {
|
||||||
class VehiclePhysicsControl {
|
struct VehiclePhysicsControl
|
||||||
public:
|
{
|
||||||
|
// Engine Setup:
|
||||||
|
std::vector<geom::Vector2D> torque_curve = {
|
||||||
|
geom::Vector2D(0.0f, 500.0f),
|
||||||
|
geom::Vector2D(5000.0f, 500.0f)
|
||||||
|
};
|
||||||
|
|
||||||
VehiclePhysicsControl() = default;
|
float max_torque = 300.0f;
|
||||||
|
float max_rpm = 5000.0f;
|
||||||
|
float idle_rpm = 1.0f;
|
||||||
|
float brake_effect = 1.0f;
|
||||||
|
float rev_up_moi = 1.0f;
|
||||||
|
float rev_down_rate = 600.0f;
|
||||||
|
|
||||||
VehiclePhysicsControl(
|
// ToDo: Convert to an enum, see EVehicleDifferential.
|
||||||
const std::vector<carla::geom::Vector2D> &in_torque_curve,
|
uint8_t differential_type = 0;
|
||||||
float in_max_torque,
|
float front_rear_split = 0.5f;
|
||||||
float in_max_rpm,
|
|
||||||
float in_moi,
|
|
||||||
float in_rev_down_rate,
|
|
||||||
|
|
||||||
uint8_t in_differential_type,
|
bool use_automatic_gears = true;
|
||||||
float in_front_rear_split,
|
float gear_change_time = 0.5f;
|
||||||
|
float final_ratio = 4.0f;
|
||||||
|
std::vector<float> forward_gear_ratios = { 2.85, 2.02, 1.35, 1.0, 2.85, 2.02, 1.35, 1.0 };
|
||||||
|
std::vector<float> reverse_gear_ratios = { 2.86, 2.86 };
|
||||||
|
float change_up_rpm = 4500.0f;
|
||||||
|
float change_down_rpm = 2000.0f;
|
||||||
|
float transmission_efficiency = 0.9f;
|
||||||
|
|
||||||
bool in_use_gear_autobox,
|
float mass = 1000.0f;
|
||||||
float in_gear_switch_time,
|
float drag_coefficient = 0.3f;
|
||||||
float in_final_ratio,
|
geom::Location center_of_mass = geom::Location(0, 0, 0);
|
||||||
std::vector<float> &in_forward_gears,
|
float chassis_width = 180.f;
|
||||||
std::vector<float> &in_reverse_gears,
|
float chassis_height = 140.f;
|
||||||
float in_change_up_rpm,
|
float downforce_coefficient = 0.3f;
|
||||||
float in_change_down_rpm,
|
float drag_area = 0.0f;
|
||||||
float in_transmission_efficiency,
|
geom::Vector3D inertia_tensor_scale = geom::Vector3D(1, 1, 1);
|
||||||
|
float sleep_threshold = 10.0f;
|
||||||
|
float sleep_slope_limit = 0.866f;
|
||||||
|
|
||||||
float in_mass,
|
std::vector<geom::Vector2D> steering_curve = {
|
||||||
float in_drag_coefficient,
|
geom::Vector2D(0.0f, 1.0f),
|
||||||
geom::Location in_center_of_mass,
|
geom::Vector2D(10.0f, 0.5f)
|
||||||
const std::vector<carla::geom::Vector2D> &in_steering_curve,
|
};
|
||||||
std::vector<WheelPhysicsControl> &in_wheels,
|
std::vector<WheelPhysicsControl> wheels;
|
||||||
bool in_use_sweep_wheel_collision)
|
|
||||||
: torque_curve(in_torque_curve),
|
|
||||||
max_torque(in_max_torque),
|
|
||||||
max_rpm(in_max_rpm),
|
|
||||||
moi(in_moi),
|
|
||||||
rev_down_rate(in_rev_down_rate),
|
|
||||||
differential_type(in_differential_type),
|
|
||||||
front_rear_split(in_front_rear_split),
|
|
||||||
use_gear_autobox(in_use_gear_autobox),
|
|
||||||
gear_switch_time(in_gear_switch_time),
|
|
||||||
final_ratio(in_final_ratio),
|
|
||||||
forward_gears(in_forward_gears),
|
|
||||||
reverse_gears(in_reverse_gears),
|
|
||||||
change_up_rpm(in_change_up_rpm),
|
|
||||||
change_down_rpm(in_change_down_rpm),
|
|
||||||
transmission_efficiency(in_transmission_efficiency),
|
|
||||||
mass(in_mass),
|
|
||||||
drag_coefficient(in_drag_coefficient),
|
|
||||||
center_of_mass(in_center_of_mass),
|
|
||||||
steering_curve(in_steering_curve),
|
|
||||||
wheels(in_wheels),
|
|
||||||
use_sweep_wheel_collision(in_use_sweep_wheel_collision) {}
|
|
||||||
|
|
||||||
const std::vector<float> &GetForwardGears() const {
|
bool use_sweep_wheel_collision = false;
|
||||||
return forward_gears;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SetForwardGears(std::vector<float> &in_forward_gears) {
|
inline bool operator==(const VehiclePhysicsControl& rhs) const {
|
||||||
forward_gears = in_forward_gears;
|
const bool cmp[] = {
|
||||||
}
|
torque_curve == rhs.torque_curve,
|
||||||
|
max_torque == rhs.max_torque,
|
||||||
|
max_rpm == rhs.max_rpm,
|
||||||
|
idle_rpm == rhs.idle_rpm,
|
||||||
|
brake_effect == rhs.brake_effect,
|
||||||
|
rev_up_moi == rhs.rev_up_moi,
|
||||||
|
rev_down_rate == rhs.rev_down_rate,
|
||||||
|
differential_type == rhs.differential_type,
|
||||||
|
front_rear_split == rhs.front_rear_split,
|
||||||
|
use_automatic_gears == rhs.use_automatic_gears,
|
||||||
|
gear_change_time == rhs.gear_change_time,
|
||||||
|
final_ratio == rhs.final_ratio,
|
||||||
|
forward_gear_ratios == rhs.forward_gear_ratios,
|
||||||
|
reverse_gear_ratios == rhs.reverse_gear_ratios,
|
||||||
|
change_up_rpm == rhs.change_up_rpm,
|
||||||
|
change_down_rpm == rhs.change_down_rpm,
|
||||||
|
transmission_efficiency == rhs.transmission_efficiency,
|
||||||
|
mass == rhs.mass,
|
||||||
|
drag_coefficient == rhs.drag_coefficient,
|
||||||
|
center_of_mass == rhs.center_of_mass,
|
||||||
|
chassis_width == rhs.chassis_width,
|
||||||
|
chassis_height == rhs.chassis_height,
|
||||||
|
downforce_coefficient == rhs.downforce_coefficient,
|
||||||
|
drag_area == rhs.drag_area,
|
||||||
|
inertia_tensor_scale == rhs.inertia_tensor_scale,
|
||||||
|
sleep_threshold == rhs.sleep_threshold,
|
||||||
|
sleep_slope_limit == rhs.sleep_slope_limit,
|
||||||
|
steering_curve == rhs.steering_curve,
|
||||||
|
wheels == rhs.wheels,
|
||||||
|
use_sweep_wheel_collision == rhs.use_sweep_wheel_collision,
|
||||||
|
};
|
||||||
|
|
||||||
const std::vector<float> &GetReverseGears() const {
|
return std::all_of(
|
||||||
return reverse_gears;
|
std::begin(cmp),
|
||||||
}
|
std::end(cmp),
|
||||||
|
std::identity());
|
||||||
|
}
|
||||||
|
|
||||||
void SetReverseGears(std::vector<float> &in_reverse_gears) {
|
inline bool operator!=(const VehiclePhysicsControl& rhs) const {
|
||||||
reverse_gears = in_reverse_gears;
|
return !(*this == rhs);
|
||||||
}
|
}
|
||||||
|
|
||||||
const std::vector<WheelPhysicsControl> &GetWheels() const {
|
|
||||||
return wheels;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SetWheels(std::vector<WheelPhysicsControl> &in_wheels) {
|
|
||||||
wheels = in_wheels;
|
|
||||||
}
|
|
||||||
|
|
||||||
const std::vector<geom::Vector2D> &GetTorqueCurve() const {
|
|
||||||
return torque_curve;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SetTorqueCurve(std::vector<geom::Vector2D> &in_torque_curve) {
|
|
||||||
torque_curve = in_torque_curve;
|
|
||||||
}
|
|
||||||
|
|
||||||
const std::vector<geom::Vector2D> &GetSteeringCurve() const {
|
|
||||||
return steering_curve;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SetSteeringCurve(std::vector<geom::Vector2D> &in_steering_curve) {
|
|
||||||
steering_curve = in_steering_curve;
|
|
||||||
}
|
|
||||||
|
|
||||||
void SetUseSweepWheelCollision(bool in_sweep) {
|
|
||||||
use_sweep_wheel_collision = in_sweep;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool GetUseSweepWheelCollision() {
|
|
||||||
return use_sweep_wheel_collision;
|
|
||||||
}
|
|
||||||
|
|
||||||
std::vector<geom::Vector2D> torque_curve = {geom::Vector2D(0.0f, 500.0f), geom::Vector2D(5000.0f, 500.0f)};
|
|
||||||
float max_torque = 300.0f;
|
|
||||||
float max_rpm = 5000.0f;
|
|
||||||
float moi = 1.0f;
|
|
||||||
float rev_down_rate = 600.0f;
|
|
||||||
|
|
||||||
// ToDo: Convert to an enum, see EVehicleDifferential.
|
|
||||||
uint8_t differential_type = 0;
|
|
||||||
float front_rear_split = 0.5f;
|
|
||||||
|
|
||||||
bool use_gear_autobox = true;
|
|
||||||
float gear_switch_time = 0.5f;
|
|
||||||
float final_ratio = 4.0f;
|
|
||||||
std::vector<float> forward_gears = {2.85, 2.02, 1.35, 1.0, 2.85, 2.02, 1.35, 1.0};
|
|
||||||
std::vector<float> reverse_gears = {2.86, 2.86};
|
|
||||||
float change_up_rpm = 4500.0f;
|
|
||||||
float change_down_rpm = 2000.0f;
|
|
||||||
float transmission_efficiency = 0.9f;
|
|
||||||
|
|
||||||
float mass = 1000.0f;
|
|
||||||
float drag_coefficient = 0.3f;
|
|
||||||
geom::Location center_of_mass;
|
|
||||||
|
|
||||||
std::vector<geom::Vector2D> steering_curve = {geom::Vector2D(0.0f, 1.0f), geom::Vector2D(10.0f, 0.5f)};
|
|
||||||
std::vector<WheelPhysicsControl> wheels;
|
|
||||||
|
|
||||||
bool use_sweep_wheel_collision = false;
|
|
||||||
|
|
||||||
bool operator!=(const VehiclePhysicsControl &rhs) const {
|
|
||||||
return
|
|
||||||
max_torque != rhs.max_torque ||
|
|
||||||
max_rpm != rhs.max_rpm ||
|
|
||||||
moi != rhs.moi ||
|
|
||||||
rev_down_rate != rhs.rev_down_rate ||
|
|
||||||
|
|
||||||
differential_type != rhs.differential_type ||
|
|
||||||
front_rear_split != rhs.front_rear_split ||
|
|
||||||
|
|
||||||
use_gear_autobox != rhs.use_gear_autobox ||
|
|
||||||
gear_switch_time != rhs.gear_switch_time ||
|
|
||||||
final_ratio != rhs.final_ratio ||
|
|
||||||
forward_gears != rhs.forward_gears ||
|
|
||||||
reverse_gears != rhs.reverse_gears ||
|
|
||||||
change_up_rpm != rhs.change_up_rpm ||
|
|
||||||
change_down_rpm != rhs.change_down_rpm ||
|
|
||||||
transmission_efficiency != rhs.transmission_efficiency ||
|
|
||||||
|
|
||||||
mass != rhs.mass ||
|
|
||||||
drag_coefficient != rhs.drag_coefficient ||
|
|
||||||
steering_curve != rhs.steering_curve ||
|
|
||||||
center_of_mass != rhs.center_of_mass ||
|
|
||||||
wheels != rhs.wheels ||
|
|
||||||
use_sweep_wheel_collision != rhs.use_sweep_wheel_collision;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool operator==(const VehiclePhysicsControl &rhs) const {
|
|
||||||
return !(*this != rhs);
|
|
||||||
}
|
|
||||||
|
|
||||||
#ifdef LIBCARLA_INCLUDED_FROM_UE4
|
#ifdef LIBCARLA_INCLUDED_FROM_UE4
|
||||||
|
|
||||||
VehiclePhysicsControl(const FVehiclePhysicsControl &Control) {
|
static VehiclePhysicsControl FromFVehiclePhysicsControl(
|
||||||
// Engine Setup
|
const FVehiclePhysicsControl& Control) {
|
||||||
torque_curve = std::vector<carla::geom::Vector2D>();
|
VehiclePhysicsControl Out = { };
|
||||||
TArray<FRichCurveKey> TorqueCurveKeys = Control.TorqueCurve.GetCopyOfKeys();
|
Out.torque_curve.reserve(Control.TorqueCurve.GetNumKeys());
|
||||||
for (int32 KeyIdx = 0; KeyIdx < TorqueCurveKeys.Num(); KeyIdx++) {
|
for (auto& Key : Control.TorqueCurve.GetConstRefOfKeys())
|
||||||
geom::Vector2D point(TorqueCurveKeys[KeyIdx].Time, TorqueCurveKeys[KeyIdx].Value);
|
Out.torque_curve.push_back(geom::Vector2D(Key.Time, Key.Value));
|
||||||
torque_curve.push_back(point);
|
Out.max_torque = Control.MaxTorque;
|
||||||
}
|
Out.max_rpm = Control.MaxRPM;
|
||||||
max_torque = Control.MaxTorque;
|
Out.idle_rpm = Control.IdleRPM;
|
||||||
max_rpm = Control.MaxRPM;
|
Out.brake_effect = Control.BrakeEffect;
|
||||||
moi = Control.MOI;
|
Out.rev_up_moi = Control.RevUpMOI;
|
||||||
rev_down_rate = Control.RevDownRate;
|
Out.rev_down_rate = Control.RevDownRate;
|
||||||
|
Out.differential_type = Control.DifferentialType;
|
||||||
// Differential Setup
|
Out.front_rear_split = Control.FrontRearSplit;
|
||||||
differential_type = Control.DifferentialType;
|
Out.use_automatic_gears = Control.bUseAutomaticGears;
|
||||||
front_rear_split = Control.FrontRearSplit;
|
Out.gear_change_time = Control.GearChangeTime;
|
||||||
|
Out.final_ratio = Control.FinalRatio;
|
||||||
// Transmission Setup
|
Out.forward_gear_ratios.resize(Control.ForwardGearRatios.Num());
|
||||||
use_gear_autobox = Control.bUseGearAutoBox;
|
for (size_t i = 0; i != Out.forward_gear_ratios.size(); ++i)
|
||||||
gear_switch_time = Control.GearSwitchTime;
|
Out.forward_gear_ratios[i] = Control.ForwardGearRatios[i];
|
||||||
final_ratio = Control.FinalRatio;
|
Out.reverse_gear_ratios.resize(Control.ReverseGearRatios.Num());
|
||||||
change_up_rpm = Control.ChangeUpRPM;
|
for (size_t i = 0; i != Out.reverse_gear_ratios.size(); ++i)
|
||||||
change_down_rpm = Control.ChangeDownRPM;
|
Out.reverse_gear_ratios[i] = Control.ReverseGearRatios[i];
|
||||||
transmission_efficiency = Control.TransmissionEfficiency;
|
Out.change_up_rpm = Control.ChangeUpRPM;
|
||||||
forward_gears = std::vector<float>();
|
Out.change_down_rpm = Control.ChangeDownRPM;
|
||||||
forward_gears.reserve(Control.ForwardGears.Num());
|
Out.transmission_efficiency = Control.TransmissionEfficiency;
|
||||||
for (const auto &Gear : Control.ForwardGears) {
|
Out.mass = Control.Mass;
|
||||||
forward_gears.push_back(Gear);
|
Out.drag_coefficient = Control.DragCoefficient;
|
||||||
}
|
Out.center_of_mass = Control.CenterOfMass;
|
||||||
reverse_gears = std::vector<float>();
|
Out.chassis_width = Control.ChassisWidth;
|
||||||
reverse_gears.reserve(Control.ReverseGears.Num());
|
Out.chassis_height = Control.ChassisHeight;
|
||||||
for (const auto &Gear : Control.ReverseGears) {
|
Out.downforce_coefficient = Control.DownforceCoefficient;
|
||||||
reverse_gears.push_back(Gear);
|
Out.drag_area = Control.DragArea;
|
||||||
|
Out.inertia_tensor_scale = geom::Vector3D(
|
||||||
|
Control.InertiaTensorScale.X,
|
||||||
|
Control.InertiaTensorScale.Y,
|
||||||
|
Control.InertiaTensorScale.Z);
|
||||||
|
Out.sleep_threshold = Control.SleepThreshold;
|
||||||
|
Out.sleep_slope_limit = Control.SleepSlopeLimit;
|
||||||
|
Out.steering_curve.reserve(Control.SteeringCurve.GetNumKeys());
|
||||||
|
for (auto& Key : Control.SteeringCurve.GetConstRefOfKeys())
|
||||||
|
Out.steering_curve.push_back(geom::Vector2D(Key.Time, Key.Value));
|
||||||
|
Out.wheels.resize(Control.Wheels.Num());
|
||||||
|
for (size_t i = 0; i != Out.wheels.size(); ++i)
|
||||||
|
Out.wheels[i] = WheelPhysicsControl::FromFWheelPhysicsControl(Control.Wheels[i]);
|
||||||
|
Out.use_sweep_wheel_collision = Control.UseSweepWheelCollision;
|
||||||
|
return Out;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Vehicle Setup
|
operator FVehiclePhysicsControl() const {
|
||||||
mass = Control.Mass;
|
FVehiclePhysicsControl Control = { };
|
||||||
drag_coefficient = Control.DragCoefficient;
|
for (auto [x, y] : torque_curve)
|
||||||
|
Control.TorqueCurve.AddKey(x, y);
|
||||||
steering_curve = std::vector<carla::geom::Vector2D>();
|
Control.MaxTorque = max_torque;
|
||||||
TArray<FRichCurveKey> SteeringCurveKeys = Control.SteeringCurve.GetCopyOfKeys();
|
Control.MaxRPM = max_rpm;
|
||||||
for (int32 KeyIdx = 0; KeyIdx < SteeringCurveKeys.Num(); KeyIdx++) {
|
Control.IdleRPM = idle_rpm;
|
||||||
geom::Vector2D point(SteeringCurveKeys[KeyIdx].Time, SteeringCurveKeys[KeyIdx].Value);
|
Control.BrakeEffect = brake_effect;
|
||||||
steering_curve.push_back(point);
|
Control.RevUpMOI = rev_up_moi;
|
||||||
|
Control.RevDownRate = rev_down_rate;
|
||||||
|
Control.DifferentialType = differential_type;
|
||||||
|
Control.FrontRearSplit = front_rear_split;
|
||||||
|
Control.bUseAutomaticGears = use_automatic_gears;
|
||||||
|
Control.GearChangeTime = gear_change_time;
|
||||||
|
Control.FinalRatio = final_ratio;
|
||||||
|
Control.ForwardGearRatios.SetNum(forward_gear_ratios.size());
|
||||||
|
for (size_t i = 0; i != Control.ForwardGearRatios.Num(); ++i)
|
||||||
|
Control.ForwardGearRatios[i] = forward_gear_ratios[i];
|
||||||
|
Control.ReverseGearRatios.SetNum(reverse_gear_ratios.size());
|
||||||
|
for (size_t i = 0; i != Control.ReverseGearRatios.Num(); ++i)
|
||||||
|
Control.ReverseGearRatios[i] = reverse_gear_ratios[i];
|
||||||
|
Control.ChangeUpRPM = change_up_rpm;
|
||||||
|
Control.ChangeDownRPM = change_down_rpm;
|
||||||
|
Control.TransmissionEfficiency = transmission_efficiency;
|
||||||
|
Control.Mass = mass;
|
||||||
|
Control.DragCoefficient = drag_coefficient;
|
||||||
|
Control.CenterOfMass = center_of_mass;
|
||||||
|
Control.ChassisWidth = chassis_width;
|
||||||
|
Control.ChassisHeight = chassis_height;
|
||||||
|
Control.DownforceCoefficient = downforce_coefficient;
|
||||||
|
Control.DragArea = drag_area;
|
||||||
|
Control.InertiaTensorScale = FVector(
|
||||||
|
inertia_tensor_scale.x,
|
||||||
|
inertia_tensor_scale.y,
|
||||||
|
inertia_tensor_scale.z);
|
||||||
|
Control.SleepThreshold = sleep_threshold;
|
||||||
|
Control.SleepSlopeLimit = sleep_slope_limit;
|
||||||
|
for (auto [x, y] : steering_curve)
|
||||||
|
Control.SteeringCurve.AddKey(x, y);
|
||||||
|
Control.Wheels.SetNum(wheels.size());
|
||||||
|
for (size_t i = 0; i != Control.Wheels.Num(); ++i)
|
||||||
|
Control.Wheels[i] = wheels[i];
|
||||||
|
Control.UseSweepWheelCollision = use_sweep_wheel_collision;
|
||||||
|
return Control;
|
||||||
}
|
}
|
||||||
|
|
||||||
center_of_mass = Control.CenterOfMass;
|
|
||||||
|
|
||||||
// Wheels Setup
|
|
||||||
wheels = std::vector<WheelPhysicsControl>();
|
|
||||||
for (const auto &Wheel : Control.Wheels) {
|
|
||||||
wheels.push_back(WheelPhysicsControl(Wheel));
|
|
||||||
}
|
|
||||||
|
|
||||||
use_sweep_wheel_collision = Control.UseSweepWheelCollision;
|
|
||||||
}
|
|
||||||
|
|
||||||
operator FVehiclePhysicsControl() const {
|
|
||||||
FVehiclePhysicsControl Control;
|
|
||||||
|
|
||||||
// Engine Setup
|
|
||||||
FRichCurve TorqueCurve;
|
|
||||||
for (const auto &point : torque_curve) {
|
|
||||||
TorqueCurve.AddKey(point.x, point.y);
|
|
||||||
}
|
|
||||||
Control.TorqueCurve = TorqueCurve;
|
|
||||||
Control.MaxTorque = max_torque;
|
|
||||||
Control.MaxRPM = max_rpm;
|
|
||||||
Control.MOI = moi;
|
|
||||||
Control.RevDownRate = rev_down_rate;
|
|
||||||
|
|
||||||
// Differential Setup
|
|
||||||
Control.DifferentialType = differential_type;
|
|
||||||
Control.FrontRearSplit = front_rear_split;
|
|
||||||
|
|
||||||
// Transmission Setup
|
|
||||||
Control.bUseGearAutoBox = use_gear_autobox;
|
|
||||||
Control.GearSwitchTime = gear_switch_time;
|
|
||||||
Control.FinalRatio = final_ratio;
|
|
||||||
Control.ChangeUpRPM = change_up_rpm;
|
|
||||||
Control.ChangeDownRPM = change_down_rpm;
|
|
||||||
Control.TransmissionEfficiency = transmission_efficiency;
|
|
||||||
TArray<float> ForwardGears;
|
|
||||||
ForwardGears.Reserve(forward_gears.size());
|
|
||||||
for (const auto &gear : forward_gears) {
|
|
||||||
ForwardGears.Add(gear);
|
|
||||||
}
|
|
||||||
Control.ForwardGears = ForwardGears;
|
|
||||||
TArray<float> ReverseGears;
|
|
||||||
ReverseGears.Reserve(reverse_gears.size());
|
|
||||||
for (const auto &gear : reverse_gears) {
|
|
||||||
ReverseGears.Add(gear);
|
|
||||||
}
|
|
||||||
Control.ReverseGears = ReverseGears;
|
|
||||||
|
|
||||||
|
|
||||||
// Vehicle Setup
|
|
||||||
Control.Mass = mass;
|
|
||||||
Control.DragCoefficient = drag_coefficient;
|
|
||||||
|
|
||||||
// Transmission Setup
|
|
||||||
FRichCurve SteeringCurve;
|
|
||||||
for (const auto &point : steering_curve) {
|
|
||||||
SteeringCurve.AddKey(point.x, point.y);
|
|
||||||
}
|
|
||||||
Control.SteeringCurve = SteeringCurve;
|
|
||||||
|
|
||||||
Control.CenterOfMass = center_of_mass;
|
|
||||||
|
|
||||||
// Wheels Setup
|
|
||||||
TArray<FWheelPhysicsControl> Wheels;
|
|
||||||
for (const auto &wheel : wheels) {
|
|
||||||
Wheels.Add(FWheelPhysicsControl(wheel));
|
|
||||||
}
|
|
||||||
Control.Wheels = Wheels;
|
|
||||||
|
|
||||||
Control.UseSweepWheelCollision = use_sweep_wheel_collision;
|
|
||||||
|
|
||||||
return Control;
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
MSGPACK_DEFINE_ARRAY(torque_curve,
|
MSGPACK_DEFINE_ARRAY(
|
||||||
|
torque_curve,
|
||||||
max_torque,
|
max_torque,
|
||||||
max_rpm,
|
max_rpm,
|
||||||
moi,
|
idle_rpm,
|
||||||
|
brake_effect,
|
||||||
|
rev_up_moi,
|
||||||
rev_down_rate,
|
rev_down_rate,
|
||||||
differential_type,
|
differential_type,
|
||||||
front_rear_split,
|
front_rear_split,
|
||||||
use_gear_autobox,
|
use_automatic_gears,
|
||||||
gear_switch_time,
|
gear_change_time,
|
||||||
final_ratio,
|
final_ratio,
|
||||||
forward_gears,
|
forward_gear_ratios,
|
||||||
reverse_gears,
|
reverse_gear_ratios,
|
||||||
change_up_rpm,
|
change_up_rpm,
|
||||||
change_down_rpm,
|
change_down_rpm,
|
||||||
transmission_efficiency,
|
transmission_efficiency,
|
||||||
mass,
|
mass,
|
||||||
drag_coefficient,
|
drag_coefficient,
|
||||||
center_of_mass,
|
center_of_mass,
|
||||||
|
chassis_width,
|
||||||
|
chassis_height,
|
||||||
|
downforce_coefficient,
|
||||||
|
drag_area,
|
||||||
|
inertia_tensor_scale,
|
||||||
|
sleep_threshold,
|
||||||
|
sleep_slope_limit,
|
||||||
steering_curve,
|
steering_curve,
|
||||||
wheels,
|
wheels,
|
||||||
use_sweep_wheel_collision);
|
use_sweep_wheel_collision
|
||||||
};
|
);
|
||||||
|
};
|
||||||
|
|
||||||
} // namespace rpc
|
} // namespace rpc
|
||||||
} // namespace carla
|
} // namespace carla
|
||||||
|
|
|
@ -6,101 +6,255 @@
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include "carla/geom/Vector2D.h"
|
||||||
#include "carla/geom/Vector3D.h"
|
#include "carla/geom/Vector3D.h"
|
||||||
|
#include "carla/geom/Location.h"
|
||||||
#include "carla/MsgPack.h"
|
#include "carla/MsgPack.h"
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
namespace carla {
|
namespace carla {
|
||||||
namespace rpc {
|
namespace rpc {
|
||||||
|
|
||||||
class WheelPhysicsControl {
|
struct WheelPhysicsControl {
|
||||||
public:
|
|
||||||
|
|
||||||
WheelPhysicsControl() = default;
|
uint8_t axle_type = 0; // @TODO INTRODUCE ENUM
|
||||||
|
geom::Vector3D offset = geom::Vector3D(0, 0, 0);
|
||||||
|
float wheel_radius = 30.0f;
|
||||||
|
float wheel_width = 30.0f;
|
||||||
|
float wheel_mass = 30.0f;
|
||||||
|
float cornering_stiffness = 1000.0f;
|
||||||
|
float friction_force_multiplier = 3.0f;
|
||||||
|
float side_slip_modifier = 1.0f;
|
||||||
|
float slip_threshold = 20.0f;
|
||||||
|
float skid_threshold = 20.0f;
|
||||||
|
float max_steer_angle = 70.0f;
|
||||||
|
bool affected_by_steering = true;
|
||||||
|
bool affected_by_brake = true;
|
||||||
|
bool affected_by_handbrake = true;
|
||||||
|
bool affected_by_engine = true;
|
||||||
|
bool abs_enabled = false;
|
||||||
|
bool traction_control_enabled = false;
|
||||||
|
float max_wheelspin_rotation = 30;
|
||||||
|
uint8_t external_torque_combine_method = 0; // @TODO INTRODUCE ENUM
|
||||||
|
std::vector<geom::Vector2D> lateral_slip_graph;
|
||||||
|
geom::Vector3D suspension_axis = geom::Vector3D(0, 0, -1);
|
||||||
|
geom::Vector3D suspension_force_offset = geom::Vector3D(0, 0, 0);
|
||||||
|
float suspension_max_raise = 10.0f;
|
||||||
|
float suspension_max_drop = 10.0f;
|
||||||
|
float suspension_damping_ratio = 0.5f;
|
||||||
|
float wheel_load_ratio = 0.5f;
|
||||||
|
float spring_rate = 250.0f;
|
||||||
|
float spring_preload = 50.0f;
|
||||||
|
int suspension_smoothing = 0;
|
||||||
|
float rollbar_scaling = 0.15f;
|
||||||
|
uint8_t sweep_shape = 0; // @TODO INTRODUCE ENUM
|
||||||
|
uint8_t sweep_type = 0; // @TODO INTRODUCE ENUM
|
||||||
|
float max_brake_torque = 1500.0f;
|
||||||
|
float max_hand_brake_torque = 3000.0f;
|
||||||
|
int32_t wheel_index = -1;
|
||||||
|
geom::Location location = geom::Location(0, 0, 0);
|
||||||
|
geom::Location old_location = geom::Location(0, 0, 0);
|
||||||
|
geom::Location velocity = geom::Location(0, 0, 0);
|
||||||
|
|
||||||
WheelPhysicsControl(
|
inline bool operator==(const WheelPhysicsControl& rhs) const {
|
||||||
float in_tire_friction,
|
const bool cmp[] =
|
||||||
float in_max_steer_angle,
|
{
|
||||||
float in_radius,
|
axle_type == rhs.axle_type,
|
||||||
float in_cornering_stiffness,
|
offset == rhs.offset,
|
||||||
bool in_abs,
|
wheel_radius == rhs.wheel_radius,
|
||||||
bool in_traction_control,
|
wheel_width == rhs.wheel_width,
|
||||||
float in_max_brake_torque,
|
wheel_mass == rhs.wheel_mass,
|
||||||
float in_max_handbrake_torque,
|
cornering_stiffness == rhs.cornering_stiffness,
|
||||||
geom::Vector3D in_position)
|
friction_force_multiplier == rhs.friction_force_multiplier,
|
||||||
: tire_friction(in_tire_friction),
|
side_slip_modifier == rhs.side_slip_modifier,
|
||||||
max_steer_angle(in_max_steer_angle),
|
slip_threshold == rhs.slip_threshold,
|
||||||
radius(in_radius),
|
skid_threshold == rhs.skid_threshold,
|
||||||
cornering_stiffness(in_cornering_stiffness),
|
max_steer_angle == rhs.max_steer_angle,
|
||||||
abs(in_abs),
|
affected_by_steering == rhs.affected_by_steering,
|
||||||
traction_control(in_traction_control),
|
affected_by_brake == rhs.affected_by_brake,
|
||||||
max_brake_torque(in_max_brake_torque),
|
affected_by_handbrake == rhs.affected_by_handbrake,
|
||||||
max_handbrake_torque(in_max_handbrake_torque),
|
affected_by_engine == rhs.affected_by_engine,
|
||||||
position(in_position) {}
|
abs_enabled == rhs.abs_enabled,
|
||||||
|
traction_control_enabled == rhs.traction_control_enabled,
|
||||||
|
max_wheelspin_rotation == rhs.max_wheelspin_rotation,
|
||||||
|
external_torque_combine_method == rhs.external_torque_combine_method,
|
||||||
|
lateral_slip_graph == rhs.lateral_slip_graph,
|
||||||
|
suspension_axis == rhs.suspension_axis,
|
||||||
|
suspension_force_offset == rhs.suspension_force_offset,
|
||||||
|
suspension_max_raise == rhs.suspension_max_raise,
|
||||||
|
suspension_max_drop == rhs.suspension_max_drop,
|
||||||
|
suspension_damping_ratio == rhs.suspension_damping_ratio,
|
||||||
|
wheel_load_ratio == rhs.wheel_load_ratio,
|
||||||
|
spring_rate == rhs.spring_rate,
|
||||||
|
spring_preload == rhs.spring_preload,
|
||||||
|
suspension_smoothing == rhs.suspension_smoothing,
|
||||||
|
rollbar_scaling == rhs.rollbar_scaling,
|
||||||
|
sweep_shape == rhs.sweep_shape,
|
||||||
|
sweep_type == rhs.sweep_type,
|
||||||
|
max_brake_torque == rhs.max_brake_torque,
|
||||||
|
max_hand_brake_torque == rhs.max_hand_brake_torque,
|
||||||
|
wheel_index == rhs.wheel_index,
|
||||||
|
location == rhs.location,
|
||||||
|
old_location == rhs.old_location,
|
||||||
|
velocity == rhs.velocity
|
||||||
|
};
|
||||||
|
return std::all_of(std::begin(cmp), std::end(cmp), std::identity());
|
||||||
|
}
|
||||||
|
|
||||||
float tire_friction = 3.0f;
|
inline bool operator!=(const WheelPhysicsControl& rhs) const {
|
||||||
float max_steer_angle = 70.0f;
|
return !(*this == rhs);
|
||||||
float radius = 30.0f;
|
}
|
||||||
float cornering_stiffness = 1000.0f;
|
|
||||||
bool abs = false;
|
|
||||||
bool traction_control = false;
|
|
||||||
float max_brake_torque = 1500.0f;
|
|
||||||
float max_handbrake_torque = 3000.0f;
|
|
||||||
geom::Vector3D position = {0.0f, 0.0f, 0.0f};
|
|
||||||
|
|
||||||
bool operator!=(const WheelPhysicsControl &rhs) const {
|
|
||||||
return
|
|
||||||
tire_friction != rhs.tire_friction ||
|
|
||||||
max_steer_angle != rhs.max_steer_angle ||
|
|
||||||
radius != rhs.radius ||
|
|
||||||
cornering_stiffness != rhs.cornering_stiffness ||
|
|
||||||
abs != rhs.abs ||
|
|
||||||
traction_control != rhs.traction_control ||
|
|
||||||
max_brake_torque != rhs.max_brake_torque ||
|
|
||||||
max_handbrake_torque != rhs.max_handbrake_torque ||
|
|
||||||
position != rhs.position;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool operator==(const WheelPhysicsControl &rhs) const {
|
|
||||||
return !(*this != rhs);
|
|
||||||
}
|
|
||||||
#ifdef LIBCARLA_INCLUDED_FROM_UE4
|
#ifdef LIBCARLA_INCLUDED_FROM_UE4
|
||||||
|
|
||||||
WheelPhysicsControl(const FWheelPhysicsControl &Wheel)
|
static WheelPhysicsControl FromFWheelPhysicsControl(
|
||||||
: tire_friction(Wheel.FrictionForceMultiplier),
|
const FWheelPhysicsControl& Wheel)
|
||||||
max_steer_angle(Wheel.MaxSteerAngle),
|
{
|
||||||
radius(Wheel.Radius),
|
WheelPhysicsControl Out = { };
|
||||||
cornering_stiffness(Wheel.CorneringStiffness),
|
Out.axle_type = (uint8_t)Wheel.AxleType;
|
||||||
abs(Wheel.bABSEnabled),
|
Out.offset = geom::Vector3D(
|
||||||
traction_control(Wheel.bTractionControlEnabled),
|
Wheel.Offset.X,
|
||||||
max_brake_torque(Wheel.MaxBrakeTorque),
|
Wheel.Offset.Y,
|
||||||
max_handbrake_torque(Wheel.MaxHandBrakeTorque),
|
Wheel.Offset.Z);
|
||||||
position(Wheel.Position.X, Wheel.Position.Y, Wheel.Position.Z) {}
|
Out.wheel_radius = Wheel.WheelRadius;
|
||||||
|
Out.wheel_width = Wheel.WheelWidth;
|
||||||
|
Out.wheel_mass = Wheel.WheelMass;
|
||||||
|
Out.cornering_stiffness = Wheel.CorneringStiffness;
|
||||||
|
Out.friction_force_multiplier = Wheel.FrictionForceMultiplier;
|
||||||
|
Out.side_slip_modifier = Wheel.SideSlipModifier;
|
||||||
|
Out.slip_threshold = Wheel.SlipThreshold;
|
||||||
|
Out.skid_threshold = Wheel.SkidThreshold;
|
||||||
|
Out.max_steer_angle = Wheel.MaxSteerAngle;
|
||||||
|
Out.affected_by_steering = Wheel.bAffectedBySteering;
|
||||||
|
Out.affected_by_brake = Wheel.bAffectedByBrake;
|
||||||
|
Out.affected_by_handbrake = Wheel.bAffectedByHandbrake;
|
||||||
|
Out.affected_by_engine = Wheel.bAffectedByEngine;
|
||||||
|
Out.abs_enabled = Wheel.bABSEnabled;
|
||||||
|
Out.traction_control_enabled = Wheel.bTractionControlEnabled;
|
||||||
|
Out.max_wheelspin_rotation = Wheel.MaxWheelspinRotation;
|
||||||
|
Out.external_torque_combine_method = (uint8_t)Wheel.ExternalTorqueCombineMethod;
|
||||||
|
Out.lateral_slip_graph.reserve(Wheel.LateralSlipGraph.GetNumKeys());
|
||||||
|
for (auto& e : Wheel.LateralSlipGraph.GetConstRefOfKeys())
|
||||||
|
Out.lateral_slip_graph.push_back({ e.Time, e.Value });
|
||||||
|
Out.suspension_axis = geom::Vector3D(
|
||||||
|
Wheel.SuspensionAxis.X,
|
||||||
|
Wheel.SuspensionAxis.Y,
|
||||||
|
Wheel.SuspensionAxis.Z);
|
||||||
|
Out.suspension_force_offset = geom::Vector3D(
|
||||||
|
Wheel.SuspensionForceOffset.X,
|
||||||
|
Wheel.SuspensionForceOffset.Y,
|
||||||
|
Wheel.SuspensionForceOffset.Z);
|
||||||
|
Out.suspension_max_raise = Wheel.SuspensionMaxRaise;
|
||||||
|
Out.suspension_max_drop = Wheel.SuspensionMaxDrop;
|
||||||
|
Out.suspension_damping_ratio = Wheel.SuspensionDampingRatio;
|
||||||
|
Out.wheel_load_ratio = Wheel.WheelLoadRatio;
|
||||||
|
Out.spring_rate = Wheel.SpringRate;
|
||||||
|
Out.spring_preload = Wheel.SpringPreload;
|
||||||
|
Out.suspension_smoothing = Wheel.SuspensionSmoothing;
|
||||||
|
Out.rollbar_scaling = Wheel.RollbarScaling;
|
||||||
|
Out.sweep_shape = (uint8_t)Wheel.SweepShape;
|
||||||
|
Out.sweep_type = (uint8_t)Wheel.SweepType;
|
||||||
|
Out.max_brake_torque = Wheel.MaxBrakeTorque;
|
||||||
|
Out.max_hand_brake_torque = Wheel.MaxHandBrakeTorque;
|
||||||
|
Out.wheel_index = Wheel.WheelIndex;
|
||||||
|
Out.location = Wheel.Location;
|
||||||
|
Out.old_location = Wheel.OldLocation;
|
||||||
|
Out.velocity = Wheel.Velocity;
|
||||||
|
return Out;
|
||||||
|
}
|
||||||
|
|
||||||
operator FWheelPhysicsControl() const {
|
operator FWheelPhysicsControl() const {
|
||||||
FWheelPhysicsControl Wheel;
|
FWheelPhysicsControl Wheel;
|
||||||
Wheel.FrictionForceMultiplier = tire_friction;
|
Wheel.AxleType = (EAxleType)axle_type;
|
||||||
Wheel.MaxSteerAngle = max_steer_angle;
|
Wheel.Offset = FVector(offset.x, offset.y, offset.z);
|
||||||
Wheel.Radius = radius;
|
Wheel.WheelRadius = wheel_radius;
|
||||||
Wheel.CorneringStiffness = cornering_stiffness;
|
Wheel.WheelWidth = wheel_width;
|
||||||
Wheel.bABSEnabled = abs;
|
Wheel.WheelMass = wheel_mass;
|
||||||
Wheel.bTractionControlEnabled = traction_control;
|
Wheel.CorneringStiffness = cornering_stiffness;
|
||||||
Wheel.MaxBrakeTorque = max_brake_torque;
|
Wheel.FrictionForceMultiplier = friction_force_multiplier;
|
||||||
Wheel.MaxHandBrakeTorque = max_handbrake_torque;
|
Wheel.SideSlipModifier = side_slip_modifier;
|
||||||
Wheel.Position = {position.x, position.y, position.z};
|
Wheel.SlipThreshold = slip_threshold;
|
||||||
return Wheel;
|
Wheel.SkidThreshold = skid_threshold;
|
||||||
}
|
Wheel.MaxSteerAngle = max_steer_angle;
|
||||||
|
Wheel.bAffectedBySteering = affected_by_steering;
|
||||||
|
Wheel.bAffectedByBrake = affected_by_brake;
|
||||||
|
Wheel.bAffectedByHandbrake = affected_by_handbrake;
|
||||||
|
Wheel.bAffectedByEngine = affected_by_engine;
|
||||||
|
Wheel.bABSEnabled = abs_enabled;
|
||||||
|
Wheel.bTractionControlEnabled = traction_control_enabled;
|
||||||
|
Wheel.MaxWheelspinRotation = max_wheelspin_rotation;
|
||||||
|
Wheel.ExternalTorqueCombineMethod = (ETorqueCombineMethod)external_torque_combine_method;
|
||||||
|
for (auto [x, y] : lateral_slip_graph)
|
||||||
|
Wheel.LateralSlipGraph.AddKey(x, y);
|
||||||
|
Wheel.SuspensionAxis = FVector(
|
||||||
|
suspension_axis.x,
|
||||||
|
suspension_axis.y,
|
||||||
|
suspension_axis.z);
|
||||||
|
Wheel.SuspensionForceOffset = FVector(
|
||||||
|
suspension_force_offset.x,
|
||||||
|
suspension_force_offset.y,
|
||||||
|
suspension_force_offset.z);
|
||||||
|
Wheel.SuspensionMaxRaise = suspension_max_raise;
|
||||||
|
Wheel.SuspensionMaxDrop = suspension_max_drop;
|
||||||
|
Wheel.SuspensionDampingRatio = suspension_damping_ratio;
|
||||||
|
Wheel.WheelLoadRatio = wheel_load_ratio;
|
||||||
|
Wheel.SpringRate = spring_rate;
|
||||||
|
Wheel.SpringPreload = spring_preload;
|
||||||
|
Wheel.SuspensionSmoothing = suspension_smoothing;
|
||||||
|
Wheel.RollbarScaling = rollbar_scaling;
|
||||||
|
Wheel.SweepShape = (ESweepShape)sweep_shape;
|
||||||
|
Wheel.SweepType = (ESweepType)sweep_type;
|
||||||
|
Wheel.MaxBrakeTorque = max_brake_torque;
|
||||||
|
Wheel.MaxHandBrakeTorque = max_hand_brake_torque;
|
||||||
|
Wheel.WheelIndex = wheel_index;
|
||||||
|
Wheel.Location = location;
|
||||||
|
Wheel.OldLocation = old_location;
|
||||||
|
Wheel.Velocity = velocity;
|
||||||
|
return Wheel;
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
MSGPACK_DEFINE_ARRAY(tire_friction,
|
MSGPACK_DEFINE_ARRAY(
|
||||||
max_steer_angle,
|
axle_type,
|
||||||
radius,
|
offset,
|
||||||
|
wheel_radius,
|
||||||
|
wheel_width,
|
||||||
|
wheel_mass,
|
||||||
cornering_stiffness,
|
cornering_stiffness,
|
||||||
abs,
|
friction_force_multiplier,
|
||||||
traction_control,
|
side_slip_modifier,
|
||||||
|
slip_threshold,
|
||||||
|
skid_threshold,
|
||||||
|
max_steer_angle,
|
||||||
|
affected_by_steering,
|
||||||
|
affected_by_brake,
|
||||||
|
affected_by_handbrake,
|
||||||
|
affected_by_engine,
|
||||||
|
abs_enabled,
|
||||||
|
traction_control_enabled,
|
||||||
|
max_wheelspin_rotation,
|
||||||
|
external_torque_combine_method,
|
||||||
|
lateral_slip_graph,
|
||||||
|
suspension_axis,
|
||||||
|
suspension_force_offset,
|
||||||
|
suspension_max_raise,
|
||||||
|
suspension_max_drop,
|
||||||
|
suspension_damping_ratio,
|
||||||
|
wheel_load_ratio,
|
||||||
|
spring_rate,
|
||||||
|
spring_preload,
|
||||||
|
suspension_smoothing,
|
||||||
|
rollbar_scaling,
|
||||||
|
sweep_shape,
|
||||||
|
sweep_type,
|
||||||
max_brake_torque,
|
max_brake_torque,
|
||||||
max_handbrake_torque,
|
max_hand_brake_torque,
|
||||||
position)
|
wheel_index,
|
||||||
};
|
location,
|
||||||
|
old_location,
|
||||||
|
velocity)
|
||||||
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -480,26 +480,36 @@ namespace rpc {
|
||||||
|
|
||||||
inline std::ostream &operator<<(std::ostream &out, const VehiclePhysicsControl &control) {
|
inline std::ostream &operator<<(std::ostream &out, const VehiclePhysicsControl &control) {
|
||||||
out << "VehiclePhysicsControl(torque_curve=" << control.torque_curve
|
out << "VehiclePhysicsControl(torque_curve=" << control.torque_curve
|
||||||
<< ", max_torque=" << std::to_string(control.max_torque)
|
<< ", max_torque=" << control.max_torque
|
||||||
<< ", max_rpm=" << std::to_string(control.max_rpm)
|
<< ", max_rpm=" << control.max_rpm
|
||||||
<< ", moi=" << std::to_string(control.moi)
|
<< ", idle_rpm=" << control.idle_rpm
|
||||||
<< ", rev_down_rate=" << std::to_string(control.rev_down_rate)
|
<< ", brake_effect=" << control.brake_effect
|
||||||
<< ", differential_type=" << std::to_string(control.differential_type)
|
<< ", rev_up_moi=" << control.rev_up_moi
|
||||||
<< ", front_rear_split=" << std::to_string(control.front_rear_split)
|
<< ", rev_down_rate=" << control.rev_down_rate
|
||||||
<< ", use_gear_autobox=" << boolalpha(control.use_gear_autobox)
|
<< ", differential_type=" << control.differential_type
|
||||||
<< ", gear_switch_time=" << std::to_string(control.gear_switch_time)
|
<< ", front_rear_split=" << control.front_rear_split
|
||||||
<< ", final_ratio=" << std::to_string(control.final_ratio)
|
<< ", use_automatic_gears=" << control.use_automatic_gears
|
||||||
<< ", forward_gears=" << control.forward_gears
|
<< ", gear_change_time=" << control.gear_change_time
|
||||||
<< ", reverse_gears=" << control.reverse_gears
|
<< ", final_ratio=" << control.final_ratio
|
||||||
<< ", change_up_rpm=" << std::to_string(control.change_up_rpm)
|
<< ", forward_gear_ratios=" << control.forward_gear_ratios
|
||||||
<< ", change_down_rpm=" << std::to_string(control.change_down_rpm)
|
<< ", reverse_gear_ratios=" << control.reverse_gear_ratios
|
||||||
<< ", transmission_efficiency=" << std::to_string(control.transmission_efficiency)
|
<< ", change_up_rpm=" << control.change_up_rpm
|
||||||
<< ", mass=" << std::to_string(control.mass)
|
<< ", change_down_rpm=" << control.change_down_rpm
|
||||||
<< ", drag_coefficient=" << std::to_string(control.drag_coefficient)
|
<< ", transmission_efficiency=" << control.transmission_efficiency
|
||||||
<< ", center_of_mass=" << control.center_of_mass
|
<< ", mass=" << control.mass
|
||||||
<< ", steering_curve=" << control.steering_curve
|
<< ", drag_coefficient=" << control.drag_coefficient
|
||||||
<< ", wheels=" << control.wheels
|
<< ", center_of_mass=" << control.center_of_mass
|
||||||
<< ", use_sweep_wheel_collision=" << control.use_sweep_wheel_collision << ')';
|
<< ", chassis_width=" << control.chassis_width
|
||||||
|
<< ", chassis_height=" << control.chassis_height
|
||||||
|
<< ", downforce_coefficient=" << control.downforce_coefficient
|
||||||
|
<< ", drag_area=" << control.drag_area
|
||||||
|
<< ", inertia_tensor_scale=" << control.inertia_tensor_scale
|
||||||
|
<< ", sleep_threshold=" << control.sleep_threshold
|
||||||
|
<< ", sleep_slope_limit=" << control.sleep_slope_limit
|
||||||
|
<< ", steering_curv=" << control.steering_curve
|
||||||
|
<< ", wheels=" << control.wheels
|
||||||
|
<< ", use_sweep_wheel_collision=" << control.use_sweep_wheel_collision
|
||||||
|
<< ")";
|
||||||
return out;
|
return out;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -61,36 +61,36 @@ static void SetWheels(carla::rpc::VehiclePhysicsControl &self, const boost::pyth
|
||||||
self.wheels = wheels;
|
self.wheels = wheels;
|
||||||
}
|
}
|
||||||
|
|
||||||
static auto GetForwardGears(const carla::rpc::VehiclePhysicsControl &self) {
|
static auto GetForwardGearRatios(const carla::rpc::VehiclePhysicsControl &self) {
|
||||||
const auto &gears = self.GetForwardGears();
|
const auto &gears = self.GetForwardGearRatios();
|
||||||
boost::python::object get_iter = boost::python::iterator<std::vector<float>>();
|
boost::python::object get_iter = boost::python::iterator<std::vector<float>>();
|
||||||
boost::python::object iter = get_iter(gears);
|
boost::python::object iter = get_iter(gears);
|
||||||
return boost::python::list(iter);
|
return boost::python::list(iter);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void SetForwardGears(carla::rpc::VehiclePhysicsControl &self, const boost::python::list &list) {
|
static void SetForwardGearRatios(carla::rpc::VehiclePhysicsControl &self, const boost::python::list &list) {
|
||||||
std::vector<float> gears;
|
std::vector<float> gears;
|
||||||
auto length = boost::python::len(list);
|
auto length = boost::python::len(list);
|
||||||
for (auto i = 0u; i < length; ++i) {
|
for (auto i = 0u; i < length; ++i) {
|
||||||
gears.push_back(boost::python::extract<float &>(list[i]));
|
gears.push_back(boost::python::extract<float &>(list[i]));
|
||||||
}
|
}
|
||||||
self.SetForwardGears(gears);
|
self.SetForwardGearRatios(gears);
|
||||||
}
|
}
|
||||||
|
|
||||||
static auto GetReverseGears(const carla::rpc::VehiclePhysicsControl &self) {
|
static auto GetReverseGearRatios(const carla::rpc::VehiclePhysicsControl &self) {
|
||||||
const auto &gears = self.GetReverseGears();
|
const auto &gears = self.GetReverseGearRatios();
|
||||||
boost::python::object get_iter = boost::python::iterator<std::vector<float>>();
|
boost::python::object get_iter = boost::python::iterator<std::vector<float>>();
|
||||||
boost::python::object iter = get_iter(gears);
|
boost::python::object iter = get_iter(gears);
|
||||||
return boost::python::list(iter);
|
return boost::python::list(iter);
|
||||||
}
|
}
|
||||||
|
|
||||||
static void SetReverseGears(carla::rpc::VehiclePhysicsControl &self, const boost::python::list &list) {
|
static void SetReverseGearRatios(carla::rpc::VehiclePhysicsControl &self, const boost::python::list &list) {
|
||||||
std::vector<float> gears;
|
std::vector<float> gears;
|
||||||
auto length = boost::python::len(list);
|
auto length = boost::python::len(list);
|
||||||
for (auto i = 0u; i < length; ++i) {
|
for (auto i = 0u; i < length; ++i) {
|
||||||
gears.push_back(boost::python::extract<float &>(list[i]));
|
gears.push_back(boost::python::extract<float &>(list[i]));
|
||||||
}
|
}
|
||||||
self.SetReverseGears(gears);
|
self.SetReverseGearRatios(gears);
|
||||||
}
|
}
|
||||||
|
|
||||||
static auto GetTorqueCurve(const carla::rpc::VehiclePhysicsControl &self) {
|
static auto GetTorqueCurve(const carla::rpc::VehiclePhysicsControl &self) {
|
||||||
|
@ -117,34 +117,39 @@ static void SetSteeringCurve(carla::rpc::VehiclePhysicsControl &self, const boos
|
||||||
|
|
||||||
boost::python::object VehiclePhysicsControl_init(boost::python::tuple args, boost::python::dict kwargs) {
|
boost::python::object VehiclePhysicsControl_init(boost::python::tuple args, boost::python::dict kwargs) {
|
||||||
// Args names
|
// Args names
|
||||||
const uint32_t NUM_ARGUMENTS = 21;
|
const char* const args_names[] = {
|
||||||
const char *args_names[NUM_ARGUMENTS] = {
|
|
||||||
"torque_curve",
|
"torque_curve",
|
||||||
"max_torque",
|
"max_torque",
|
||||||
"max_rpm",
|
"max_rpm",
|
||||||
"moi",
|
"idle_rpm",
|
||||||
|
"brake_effect",
|
||||||
|
"rev_up_moi",
|
||||||
"rev_down_rate",
|
"rev_down_rate",
|
||||||
|
|
||||||
"differential_type",
|
"differential_type",
|
||||||
"front_rear_split",
|
"front_rear_split",
|
||||||
|
"use_automatic_gears",
|
||||||
"use_gear_autobox",
|
"gear_change_time",
|
||||||
"gear_switch_time",
|
|
||||||
"final_ratio",
|
"final_ratio",
|
||||||
"forward_gears",
|
"forward_gear_ratios",
|
||||||
"reverse_gears",
|
"reverse_gear_ratios",
|
||||||
"change_up_rpm",
|
"change_up_rpm",
|
||||||
"change_down_rpm",
|
"change_down_rpm",
|
||||||
"transmission_efficiency",
|
"transmission_efficiency",
|
||||||
|
|
||||||
"mass",
|
"mass",
|
||||||
"drag_coefficient",
|
"drag_coefficient",
|
||||||
|
|
||||||
"center_of_mass",
|
"center_of_mass",
|
||||||
|
"chassis_width",
|
||||||
|
"chassis_height",
|
||||||
|
"downforce_coefficient",
|
||||||
|
"drag_area",
|
||||||
|
"inertia_tensor_scale",
|
||||||
|
"sleep_threshold",
|
||||||
|
"sleep_slope_limit",
|
||||||
"steering_curve",
|
"steering_curve",
|
||||||
"wheels",
|
"wheels",
|
||||||
"use_sweep_wheel_collision",
|
"use_sweep_wheel_collision"
|
||||||
};
|
};
|
||||||
|
const auto NUM_ARGUMENTS = sizeof(args_names) / sizeof(const char*);
|
||||||
|
|
||||||
boost::python::object self = args[0];
|
boost::python::object self = args[0];
|
||||||
args = boost::python::tuple(args.slice(1, boost::python::_));
|
args = boost::python::tuple(args.slice(1, boost::python::_));
|
||||||
|
@ -365,26 +370,35 @@ void export_control() {
|
||||||
.def("__init__", raw_function(VehiclePhysicsControl_init))
|
.def("__init__", raw_function(VehiclePhysicsControl_init))
|
||||||
.def(init<>())
|
.def(init<>())
|
||||||
.add_property("torque_curve", &GetTorqueCurve, &SetTorqueCurve)
|
.add_property("torque_curve", &GetTorqueCurve, &SetTorqueCurve)
|
||||||
.def_readwrite("max_torque", &cr::VehiclePhysicsControl::max_torque)
|
.add_property("max_torque", &cr::VehiclePhysicsControl::max_torque)
|
||||||
.def_readwrite("max_rpm", &cr::VehiclePhysicsControl::max_rpm)
|
.add_property("max_rpm", &cr::VehiclePhysicsControl::max_rpm)
|
||||||
.def_readwrite("moi", &cr::VehiclePhysicsControl::moi)
|
.add_property("idle_rpm", &cr::VehiclePhysicsControl::idle_rpm)
|
||||||
.def_readwrite("rev_down_rate", &cr::VehiclePhysicsControl::rev_down_rate)
|
.add_property("brake_effect", &cr::VehiclePhysicsControl::brake_effect)
|
||||||
.def_readwrite("differential_type", &cr::VehiclePhysicsControl::differential_type)
|
.add_property("rev_up_moi", &cr::VehiclePhysicsControl::rev_up_moi)
|
||||||
.def_readwrite("front_rear_split", &cr::VehiclePhysicsControl::front_rear_split)
|
.add_property("rev_down_rate", &cr::VehiclePhysicsControl::rev_down_rate)
|
||||||
.def_readwrite("use_gear_autobox", &cr::VehiclePhysicsControl::use_gear_autobox)
|
.add_property("differential_type", &cr::VehiclePhysicsControl::differential_type)
|
||||||
.def_readwrite("gear_switch_time", &cr::VehiclePhysicsControl::gear_switch_time)
|
.add_property("front_rear_split", &cr::VehiclePhysicsControl::front_rear_split)
|
||||||
.def_readwrite("final_ratio", &cr::VehiclePhysicsControl::final_ratio)
|
.add_property("use_automatic_gears", &cr::VehiclePhysicsControl::use_automatic_gears)
|
||||||
.add_property("forward_gears", &GetForwardGears, &SetForwardGears)
|
.add_property("gear_change_time", &cr::VehiclePhysicsControl::gear_change_time)
|
||||||
.add_property("reverse_gears", &GetReverseGears, &SetReverseGears)
|
.add_property("final_ratio", &cr::VehiclePhysicsControl::final_ratio)
|
||||||
.def_readwrite("change_up_rpm", &cr::VehiclePhysicsControl::change_up_rpm)
|
.add_property("forward_gear_ratios", &GetForwardGearRatios, &SetForwardGearRatios)
|
||||||
.def_readwrite("change_down_rpm", &cr::VehiclePhysicsControl::change_down_rpm)
|
.add_property("reverse_gear_ratios", &GetReverseGearRatios, &SetReverseGearRatios)
|
||||||
.def_readwrite("transmission_efficiency", &cr::VehiclePhysicsControl::mass)
|
.add_property("change_up_rpm", &cr::VehiclePhysicsControl::change_up_rpm)
|
||||||
.def_readwrite("mass", &cr::VehiclePhysicsControl::mass)
|
.add_property("change_down_rpm", &cr::VehiclePhysicsControl::change_down_rpm)
|
||||||
.def_readwrite("drag_coefficient", &cr::VehiclePhysicsControl::drag_coefficient)
|
.add_property("transmission_efficiency", &cr::VehiclePhysicsControl::transmission_efficiency)
|
||||||
.def_readwrite("center_of_mass", &cr::VehiclePhysicsControl::center_of_mass)
|
.add_property("mass", &cr::VehiclePhysicsControl::mass)
|
||||||
|
.add_property("drag_coefficient", &cr::VehiclePhysicsControl::drag_coefficient)
|
||||||
|
.add_property("center_of_mass", &cr::VehiclePhysicsControl::center_of_mass)
|
||||||
|
.add_property("chassis_width", &cr::VehiclePhysicsControl::chassis_width)
|
||||||
|
.add_property("chassis_height", &cr::VehiclePhysicsControl::chassis_height)
|
||||||
|
.add_property("downforce_coefficient", &cr::VehiclePhysicsControl::downforce_coefficient)
|
||||||
|
.add_property("drag_area", &cr::VehiclePhysicsControl::drag_area)
|
||||||
|
.add_property("inertia_tensor_scale", &cr::VehiclePhysicsControl::inertia_tensor_scale)
|
||||||
|
.add_property("sleep_threshold", &cr::VehiclePhysicsControl::sleep_threshold)
|
||||||
|
.add_property("sleep_slope_limit", &cr::VehiclePhysicsControl::sleep_slope_limit)
|
||||||
.add_property("steering_curve", &GetSteeringCurve, &SetSteeringCurve)
|
.add_property("steering_curve", &GetSteeringCurve, &SetSteeringCurve)
|
||||||
.add_property("wheels", &GetWheels, &SetWheels)
|
.add_property("wheels", &GetWheels, &SetWheels)
|
||||||
.def_readwrite("use_sweep_wheel_collision", &cr::VehiclePhysicsControl::use_sweep_wheel_collision)
|
.add_property("use_sweep_wheel_collision", &cr::VehiclePhysicsControl::use_sweep_wheel_collision)
|
||||||
.def("__eq__", &cr::VehiclePhysicsControl::operator==)
|
.def("__eq__", &cr::VehiclePhysicsControl::operator==)
|
||||||
.def("__ne__", &cr::VehiclePhysicsControl::operator!=)
|
.def("__ne__", &cr::VehiclePhysicsControl::operator!=)
|
||||||
.def(self_ns::str(self_ns::self))
|
.def(self_ns::str(self_ns::self))
|
||||||
|
|
|
@ -13,17 +13,21 @@
|
||||||
#include <compiler/enable-ue4-macros.h>
|
#include <compiler/enable-ue4-macros.h>
|
||||||
|
|
||||||
|
|
||||||
void CarlaRecorderPhysicsControl::Write(std::ostream &OutFile)
|
void CarlaRecorderPhysicsControl::Write(std::ostream& OutFile)
|
||||||
{
|
{
|
||||||
carla::rpc::VehiclePhysicsControl RPCPhysicsControl(VehiclePhysicsControl);
|
auto RPCPhysicsControl = carla::rpc::VehiclePhysicsControl::FromFVehiclePhysicsControl(
|
||||||
|
VehiclePhysicsControl);
|
||||||
WriteValue<uint32_t>(OutFile, this->DatabaseId);
|
WriteValue<uint32_t>(OutFile, this->DatabaseId);
|
||||||
WriteValue(OutFile, RPCPhysicsControl.max_torque);
|
WriteValue(OutFile, RPCPhysicsControl.max_torque);
|
||||||
WriteValue(OutFile, RPCPhysicsControl.max_rpm);
|
WriteValue(OutFile, RPCPhysicsControl.max_rpm);
|
||||||
WriteValue(OutFile, RPCPhysicsControl.moi);
|
WriteValue(OutFile, RPCPhysicsControl.idle_rpm);
|
||||||
|
WriteValue(OutFile, RPCPhysicsControl.brake_effect);
|
||||||
|
WriteValue(OutFile, RPCPhysicsControl.rev_up_moi);
|
||||||
WriteValue(OutFile, RPCPhysicsControl.rev_down_rate);
|
WriteValue(OutFile, RPCPhysicsControl.rev_down_rate);
|
||||||
WriteValue(OutFile, RPCPhysicsControl.differential_type);
|
WriteValue(OutFile, RPCPhysicsControl.differential_type);
|
||||||
WriteValue(OutFile, RPCPhysicsControl.front_rear_split);
|
WriteValue(OutFile, RPCPhysicsControl.front_rear_split);
|
||||||
WriteValue(OutFile, RPCPhysicsControl.use_gear_autobox);
|
WriteValue(OutFile, RPCPhysicsControl.use_automatic_gears);
|
||||||
|
WriteValue(OutFile, RPCPhysicsControl.gear_change_time);
|
||||||
WriteValue(OutFile, RPCPhysicsControl.final_ratio);
|
WriteValue(OutFile, RPCPhysicsControl.final_ratio);
|
||||||
WriteValue(OutFile, RPCPhysicsControl.change_up_rpm);
|
WriteValue(OutFile, RPCPhysicsControl.change_up_rpm);
|
||||||
WriteValue(OutFile, RPCPhysicsControl.change_down_rpm);
|
WriteValue(OutFile, RPCPhysicsControl.change_down_rpm);
|
||||||
|
@ -31,57 +35,55 @@ void CarlaRecorderPhysicsControl::Write(std::ostream &OutFile)
|
||||||
WriteValue(OutFile, RPCPhysicsControl.mass);
|
WriteValue(OutFile, RPCPhysicsControl.mass);
|
||||||
WriteValue(OutFile, RPCPhysicsControl.drag_coefficient);
|
WriteValue(OutFile, RPCPhysicsControl.drag_coefficient);
|
||||||
WriteValue(OutFile, RPCPhysicsControl.center_of_mass);
|
WriteValue(OutFile, RPCPhysicsControl.center_of_mass);
|
||||||
|
WriteValue(OutFile, RPCPhysicsControl.chassis_width);
|
||||||
// torque curve
|
WriteValue(OutFile, RPCPhysicsControl.chassis_height);
|
||||||
|
WriteValue(OutFile, RPCPhysicsControl.downforce_coefficient);
|
||||||
|
WriteValue(OutFile, RPCPhysicsControl.drag_area);
|
||||||
|
WriteValue(OutFile, RPCPhysicsControl.inertia_tensor_scale);
|
||||||
|
WriteValue(OutFile, RPCPhysicsControl.sleep_threshold);
|
||||||
|
WriteValue(OutFile, RPCPhysicsControl.sleep_slope_limit);
|
||||||
|
WriteValue(OutFile, RPCPhysicsControl.use_sweep_wheel_collision);
|
||||||
WriteStdVector(OutFile, RPCPhysicsControl.torque_curve);
|
WriteStdVector(OutFile, RPCPhysicsControl.torque_curve);
|
||||||
|
WriteStdVector(OutFile, RPCPhysicsControl.forward_gear_ratios);
|
||||||
// forward gears
|
WriteStdVector(OutFile, RPCPhysicsControl.reverse_gear_ratios);
|
||||||
WriteStdVector(OutFile, RPCPhysicsControl.forward_gears);
|
|
||||||
|
|
||||||
// reverse gears
|
|
||||||
WriteStdVector(OutFile, RPCPhysicsControl.reverse_gears);
|
|
||||||
|
|
||||||
// steering curve
|
|
||||||
WriteStdVector(OutFile, RPCPhysicsControl.steering_curve);
|
WriteStdVector(OutFile, RPCPhysicsControl.steering_curve);
|
||||||
|
|
||||||
// wheels
|
|
||||||
WriteStdVector(OutFile, RPCPhysicsControl.wheels);
|
WriteStdVector(OutFile, RPCPhysicsControl.wheels);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CarlaRecorderPhysicsControl::Read(std::istream &InFile)
|
void CarlaRecorderPhysicsControl::Read(std::istream& InFile)
|
||||||
{
|
{
|
||||||
carla::rpc::VehiclePhysicsControl RPCPhysicsControl;
|
carla::rpc::VehiclePhysicsControl RPCPhysicsControl;
|
||||||
ReadValue<uint32_t>(InFile, this->DatabaseId);
|
ReadValue<uint32_t>(InFile, this->DatabaseId);
|
||||||
ReadValue(InFile, RPCPhysicsControl.max_torque);
|
ReadValue(InFile, RPCPhysicsControl.max_torque);
|
||||||
ReadValue(InFile, RPCPhysicsControl.max_rpm);
|
ReadValue(InFile, RPCPhysicsControl.max_rpm);
|
||||||
ReadValue(InFile, RPCPhysicsControl.moi);
|
ReadValue(InFile, RPCPhysicsControl.idle_rpm);
|
||||||
|
ReadValue(InFile, RPCPhysicsControl.brake_effect);
|
||||||
|
ReadValue(InFile, RPCPhysicsControl.rev_up_moi);
|
||||||
ReadValue(InFile, RPCPhysicsControl.rev_down_rate);
|
ReadValue(InFile, RPCPhysicsControl.rev_down_rate);
|
||||||
ReadValue(InFile, RPCPhysicsControl.differential_type);
|
ReadValue(InFile, RPCPhysicsControl.differential_type);
|
||||||
ReadValue(InFile, RPCPhysicsControl.front_rear_split);
|
ReadValue(InFile, RPCPhysicsControl.front_rear_split);
|
||||||
ReadValue(InFile, RPCPhysicsControl.use_gear_autobox);
|
ReadValue(InFile, RPCPhysicsControl.use_automatic_gears);
|
||||||
|
ReadValue(InFile, RPCPhysicsControl.gear_change_time);
|
||||||
ReadValue(InFile, RPCPhysicsControl.final_ratio);
|
ReadValue(InFile, RPCPhysicsControl.final_ratio);
|
||||||
ReadValue(InFile, RPCPhysicsControl.change_up_rpm);
|
ReadValue(InFile, RPCPhysicsControl.change_up_rpm);
|
||||||
|
ReadValue(InFile, RPCPhysicsControl.change_down_rpm);
|
||||||
ReadValue(InFile, RPCPhysicsControl.transmission_efficiency);
|
ReadValue(InFile, RPCPhysicsControl.transmission_efficiency);
|
||||||
ReadValue(InFile, RPCPhysicsControl.final_ratio);
|
|
||||||
ReadValue(InFile, RPCPhysicsControl.mass);
|
ReadValue(InFile, RPCPhysicsControl.mass);
|
||||||
ReadValue(InFile, RPCPhysicsControl.drag_coefficient);
|
ReadValue(InFile, RPCPhysicsControl.drag_coefficient);
|
||||||
ReadValue(InFile, RPCPhysicsControl.center_of_mass);
|
ReadValue(InFile, RPCPhysicsControl.center_of_mass);
|
||||||
|
ReadValue(InFile, RPCPhysicsControl.chassis_width);
|
||||||
// torque curve
|
ReadValue(InFile, RPCPhysicsControl.chassis_height);
|
||||||
|
ReadValue(InFile, RPCPhysicsControl.downforce_coefficient);
|
||||||
|
ReadValue(InFile, RPCPhysicsControl.drag_area);
|
||||||
|
ReadValue(InFile, RPCPhysicsControl.inertia_tensor_scale);
|
||||||
|
ReadValue(InFile, RPCPhysicsControl.sleep_threshold);
|
||||||
|
ReadValue(InFile, RPCPhysicsControl.sleep_slope_limit);
|
||||||
|
ReadValue(InFile, RPCPhysicsControl.use_sweep_wheel_collision);
|
||||||
ReadStdVector(InFile, RPCPhysicsControl.torque_curve);
|
ReadStdVector(InFile, RPCPhysicsControl.torque_curve);
|
||||||
|
ReadStdVector(InFile, RPCPhysicsControl.forward_gear_ratios);
|
||||||
// forward gears
|
ReadStdVector(InFile, RPCPhysicsControl.reverse_gear_ratios);
|
||||||
ReadStdVector(InFile, RPCPhysicsControl.forward_gears);
|
|
||||||
|
|
||||||
// reverse gears
|
|
||||||
ReadStdVector(InFile, RPCPhysicsControl.reverse_gears);
|
|
||||||
|
|
||||||
// steering curve
|
|
||||||
ReadStdVector(InFile, RPCPhysicsControl.steering_curve);
|
ReadStdVector(InFile, RPCPhysicsControl.steering_curve);
|
||||||
|
|
||||||
// wheels
|
|
||||||
ReadStdVector(InFile, RPCPhysicsControl.wheels);
|
ReadStdVector(InFile, RPCPhysicsControl.wheels);
|
||||||
|
|
||||||
VehiclePhysicsControl = FVehiclePhysicsControl(RPCPhysicsControl);
|
VehiclePhysicsControl = FVehiclePhysicsControl(RPCPhysicsControl);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -92,12 +94,12 @@ void CarlaRecorderPhysicsControls::Clear(void)
|
||||||
PhysicsControls.clear();
|
PhysicsControls.clear();
|
||||||
}
|
}
|
||||||
|
|
||||||
void CarlaRecorderPhysicsControls::Add(const CarlaRecorderPhysicsControl &InObj)
|
void CarlaRecorderPhysicsControls::Add(const CarlaRecorderPhysicsControl& InObj)
|
||||||
{
|
{
|
||||||
PhysicsControls.push_back(InObj);
|
PhysicsControls.push_back(InObj);
|
||||||
}
|
}
|
||||||
|
|
||||||
void CarlaRecorderPhysicsControls::Write(std::ostream &OutFile)
|
void CarlaRecorderPhysicsControls::Write(std::ostream& OutFile)
|
||||||
{
|
{
|
||||||
if (PhysicsControls.size() == 0)
|
if (PhysicsControls.size() == 0)
|
||||||
{
|
{
|
||||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -23,11 +23,11 @@ ACollisionSensor::ACollisionSensor(const FObjectInitializer& ObjectInitializer)
|
||||||
FActorDefinition ACollisionSensor::GetSensorDefinition()
|
FActorDefinition ACollisionSensor::GetSensorDefinition()
|
||||||
{
|
{
|
||||||
return UActorBlueprintFunctionLibrary::MakeGenericSensorDefinition(
|
return UActorBlueprintFunctionLibrary::MakeGenericSensorDefinition(
|
||||||
TEXT("other"),
|
TEXT("other"),
|
||||||
TEXT("collision"));
|
TEXT("collision"));
|
||||||
}
|
}
|
||||||
|
|
||||||
void ACollisionSensor::SetOwner(AActor *NewOwner)
|
void ACollisionSensor::SetOwner(AActor* NewOwner)
|
||||||
{
|
{
|
||||||
Super::SetOwner(NewOwner);
|
Super::SetOwner(NewOwner);
|
||||||
|
|
||||||
|
@ -40,10 +40,10 @@ void ACollisionSensor::SetOwner(AActor *NewOwner)
|
||||||
}
|
}
|
||||||
|
|
||||||
void ACollisionSensor::OnCollisionEvent(
|
void ACollisionSensor::OnCollisionEvent(
|
||||||
AActor *Actor,
|
AActor* Actor,
|
||||||
AActor *OtherActor,
|
AActor* OtherActor,
|
||||||
FVector NormalImpulse,
|
FVector NormalImpulse,
|
||||||
const FHitResult &Hit)
|
const FHitResult& Hit)
|
||||||
{
|
{
|
||||||
if (Actor == nullptr || OtherActor == nullptr)
|
if (Actor == nullptr || OtherActor == nullptr)
|
||||||
{
|
{
|
||||||
|
@ -54,21 +54,21 @@ void ACollisionSensor::OnCollisionEvent(
|
||||||
|
|
||||||
// remove all items from previous frames
|
// remove all items from previous frames
|
||||||
CollisionRegistry.erase(
|
CollisionRegistry.erase(
|
||||||
std::remove_if(
|
std::remove_if(
|
||||||
CollisionRegistry.begin(),
|
CollisionRegistry.begin(),
|
||||||
CollisionRegistry.end(),
|
CollisionRegistry.end(),
|
||||||
[CurrentFrame](std::tuple<uint64_t, AActor*, AActor*> Item)
|
[CurrentFrame](std::tuple<uint64_t, AActor*, AActor*> Item)
|
||||||
{
|
{
|
||||||
return std::get<0>(Item) < CurrentFrame;
|
return std::get<0>(Item) < CurrentFrame;
|
||||||
}),
|
}),
|
||||||
CollisionRegistry.end());
|
CollisionRegistry.end());
|
||||||
|
|
||||||
// check if this collision has been procesed already in this frame
|
// check if this collision has been procesed already in this frame
|
||||||
for (auto& Collision: CollisionRegistry)
|
for (auto& Collision : CollisionRegistry)
|
||||||
{
|
{
|
||||||
if (std::get<0>(Collision) == CurrentFrame &&
|
if (std::get<0>(Collision) == CurrentFrame &&
|
||||||
std::get<1>(Collision) == Actor &&
|
std::get<1>(Collision) == Actor &&
|
||||||
std::get<2>(Collision) == OtherActor)
|
std::get<2>(Collision) == OtherActor)
|
||||||
{
|
{
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -78,37 +78,51 @@ void ACollisionSensor::OnCollisionEvent(
|
||||||
constexpr float TO_METERS = 1e-2;
|
constexpr float TO_METERS = 1e-2;
|
||||||
NormalImpulse *= TO_METERS;
|
NormalImpulse *= TO_METERS;
|
||||||
GetDataStream(*this).SerializeAndSend(
|
GetDataStream(*this).SerializeAndSend(
|
||||||
*this,
|
*this,
|
||||||
CurrentEpisode.SerializeActor(Actor),
|
CurrentEpisode.SerializeActor(Actor),
|
||||||
CurrentEpisode.SerializeActor(OtherActor),
|
CurrentEpisode.SerializeActor(OtherActor),
|
||||||
carla::geom::Vector3D(
|
carla::geom::Vector3D(
|
||||||
(float)NormalImpulse.X,
|
(float)NormalImpulse.X,
|
||||||
(float)NormalImpulse.Y,
|
(float)NormalImpulse.Y,
|
||||||
(float)NormalImpulse.Z));
|
(float)NormalImpulse.Z));
|
||||||
// record the collision event
|
// record the collision event
|
||||||
if (CurrentEpisode.GetRecorder()->IsEnabled()){
|
if (CurrentEpisode.GetRecorder()->IsEnabled()) {
|
||||||
CurrentEpisode.GetRecorder()->AddCollision(Actor, OtherActor);
|
CurrentEpisode.GetRecorder()->AddCollision(Actor, OtherActor);
|
||||||
}
|
}
|
||||||
|
|
||||||
CollisionRegistry.emplace_back(CurrentFrame, Actor, OtherActor);
|
CollisionRegistry.emplace_back(CurrentFrame, Actor, OtherActor);
|
||||||
|
|
||||||
// ROS2
|
// ROS2
|
||||||
#if defined(WITH_ROS2)
|
#if defined(WITH_ROS2)
|
||||||
auto ROS2 = carla::ros2::ROS2::GetInstance();
|
auto ROS2 = carla::ros2::ROS2::GetInstance();
|
||||||
if (ROS2->IsEnabled())
|
if (ROS2->IsEnabled())
|
||||||
{
|
{
|
||||||
TRACE_CPUPROFILER_EVENT_SCOPE_STR("ROS2 Send");
|
TRACE_CPUPROFILER_EVENT_SCOPE_STR("ROS2 Send");
|
||||||
auto StreamId = carla::streaming::detail::token_type(GetToken()).get_stream_id();
|
auto StreamId = carla::streaming::detail::token_type(GetToken()).get_stream_id();
|
||||||
|
auto NormalImpulseVector = carla::geom::Vector3D(
|
||||||
|
(float)NormalImpulse.X,
|
||||||
|
(float)NormalImpulse.Y,
|
||||||
|
(float)NormalImpulse.Z);
|
||||||
AActor* ParentActor = GetAttachParentActor();
|
AActor* ParentActor = GetAttachParentActor();
|
||||||
if (ParentActor)
|
if (ParentActor)
|
||||||
{
|
{
|
||||||
FTransform LocalTransformRelativeToParent = GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform());
|
FTransform LocalTransformRelativeToParent = GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform());
|
||||||
ROS2->ProcessDataFromCollisionSensor(0, StreamId, LocalTransformRelativeToParent, OtherActor->GetUniqueID(), carla::geom::Vector3D{NormalImpulse.X, NormalImpulse.Y, NormalImpulse.Z}, this);
|
ROS2->ProcessDataFromCollisionSensor(
|
||||||
|
0, StreamId,
|
||||||
|
LocalTransformRelativeToParent,
|
||||||
|
OtherActor->GetUniqueID(),
|
||||||
|
NormalImpulseVector,
|
||||||
|
this);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
ROS2->ProcessDataFromCollisionSensor(0, StreamId, GetActorTransform(), OtherActor->GetUniqueID(), carla::geom::Vector3D{NormalImpulse.X, NormalImpulse.Y, NormalImpulse.Z}, this);
|
ROS2->ProcessDataFromCollisionSensor(
|
||||||
|
0, StreamId,
|
||||||
|
GetActorTransform(),
|
||||||
|
OtherActor->GetUniqueID(),
|
||||||
|
NormalImpulseVector,
|
||||||
|
this);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
|
@ -1349,7 +1349,7 @@ BIND_SYNC(is_sensor_enabled_for_ros) << [this](carla::streaming::detail::stream_
|
||||||
Response,
|
Response,
|
||||||
" Actor Id: " + FString::FromInt(ActorId));
|
" Actor Id: " + FString::FromInt(ActorId));
|
||||||
}
|
}
|
||||||
return cr::VehiclePhysicsControl(PhysicsControl);
|
return cr::VehiclePhysicsControl::FromFVehiclePhysicsControl(PhysicsControl);
|
||||||
};
|
};
|
||||||
|
|
||||||
BIND_SYNC(get_vehicle_light_state) << [this](
|
BIND_SYNC(get_vehicle_light_state) << [this](
|
||||||
|
|
|
@ -69,12 +69,12 @@ void ACarlaWheeledVehicle::BeginPlay()
|
||||||
for (FName& ComponentName : ConstraintComponentNames)
|
for (FName& ComponentName : ConstraintComponentNames)
|
||||||
{
|
{
|
||||||
UPhysicsConstraintComponent* ConstraintComponent =
|
UPhysicsConstraintComponent* ConstraintComponent =
|
||||||
Cast<UPhysicsConstraintComponent>(GetDefaultSubobjectByName(ComponentName));
|
Cast<UPhysicsConstraintComponent>(GetDefaultSubobjectByName(ComponentName));
|
||||||
if (ConstraintComponent)
|
if (ConstraintComponent)
|
||||||
{
|
{
|
||||||
UPrimitiveComponent* DoorComponent = Cast<UPrimitiveComponent>(
|
UPrimitiveComponent* DoorComponent = Cast<UPrimitiveComponent>(
|
||||||
GetDefaultSubobjectByName(ConstraintComponent->ComponentName1.ComponentName));
|
GetDefaultSubobjectByName(ConstraintComponent->ComponentName1.ComponentName));
|
||||||
if(DoorComponent)
|
if (DoorComponent)
|
||||||
{
|
{
|
||||||
UE_LOG(LogCarla, Log, TEXT("Door name: %s"), *(DoorComponent->GetName()));
|
UE_LOG(LogCarla, Log, TEXT("Door name: %s"), *(DoorComponent->GetName()));
|
||||||
FTransform ComponentWorldTransform = DoorComponent->GetComponentTransform();
|
FTransform ComponentWorldTransform = DoorComponent->GetComponentTransform();
|
||||||
|
@ -101,9 +101,9 @@ void ACarlaWheeledVehicle::BeginPlay()
|
||||||
if (!ConstraintsComponents.Contains(Constraint))
|
if (!ConstraintsComponents.Contains(Constraint))
|
||||||
{
|
{
|
||||||
UPrimitiveComponent* CollisionDisabledComponent1 = Cast<UPrimitiveComponent>(
|
UPrimitiveComponent* CollisionDisabledComponent1 = Cast<UPrimitiveComponent>(
|
||||||
GetDefaultSubobjectByName(Constraint->ComponentName1.ComponentName));
|
GetDefaultSubobjectByName(Constraint->ComponentName1.ComponentName));
|
||||||
UPrimitiveComponent* CollisionDisabledComponent2 = Cast<UPrimitiveComponent>(
|
UPrimitiveComponent* CollisionDisabledComponent2 = Cast<UPrimitiveComponent>(
|
||||||
GetDefaultSubobjectByName(Constraint->ComponentName2.ComponentName));
|
GetDefaultSubobjectByName(Constraint->ComponentName2.ComponentName));
|
||||||
if (CollisionDisabledComponent1)
|
if (CollisionDisabledComponent1)
|
||||||
{
|
{
|
||||||
CollisionDisableConstraints.Add(CollisionDisabledComponent1, Constraint);
|
CollisionDisableConstraints.Add(CollisionDisabledComponent1, Constraint);
|
||||||
|
@ -132,11 +132,11 @@ void ACarlaWheeledVehicle::BeginPlay()
|
||||||
|
|
||||||
// Check if it overlaps with a Friction trigger, if so, update the friction
|
// Check if it overlaps with a Friction trigger, if so, update the friction
|
||||||
// scale.
|
// scale.
|
||||||
TArray<AActor *> OverlapActors;
|
TArray<AActor*> OverlapActors;
|
||||||
GetOverlappingActors(OverlapActors, AFrictionTrigger::StaticClass());
|
GetOverlappingActors(OverlapActors, AFrictionTrigger::StaticClass());
|
||||||
for (const auto &Actor : OverlapActors)
|
for (const auto& Actor : OverlapActors)
|
||||||
{
|
{
|
||||||
AFrictionTrigger *FrictionTrigger = Cast<AFrictionTrigger>(Actor);
|
AFrictionTrigger* FrictionTrigger = Cast<AFrictionTrigger>(Actor);
|
||||||
if (FrictionTrigger)
|
if (FrictionTrigger)
|
||||||
{
|
{
|
||||||
FrictionScale = FrictionTrigger->Friction;
|
FrictionScale = FrictionTrigger->Friction;
|
||||||
|
@ -147,7 +147,7 @@ void ACarlaWheeledVehicle::BeginPlay()
|
||||||
TArray<FChaosWheelSetup> NewWheelSetups = MovementComponent->WheelSetups;
|
TArray<FChaosWheelSetup> NewWheelSetups = MovementComponent->WheelSetups;
|
||||||
for (const FChaosWheelSetup& WheelSetup : NewWheelSetups)
|
for (const FChaosWheelSetup& WheelSetup : NewWheelSetups)
|
||||||
{
|
{
|
||||||
UChaosVehicleWheel *Wheel = WheelSetup.WheelClass.GetDefaultObject();
|
UChaosVehicleWheel* Wheel = WheelSetup.WheelClass.GetDefaultObject();
|
||||||
check(Wheel != nullptr);
|
check(Wheel != nullptr);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -171,9 +171,9 @@ bool ACarlaWheeledVehicle::IsInVehicleRange(const FVector& Location) const
|
||||||
void ACarlaWheeledVehicle::UpdateDetectionBox()
|
void ACarlaWheeledVehicle::UpdateDetectionBox()
|
||||||
{
|
{
|
||||||
const FTransform GlobalTransform = GetActorTransform();
|
const FTransform GlobalTransform = GetActorTransform();
|
||||||
const FVector Vec { DetectionSize, DetectionSize, DetectionSize };
|
const FVector Vec{ DetectionSize, DetectionSize, DetectionSize };
|
||||||
FBox Box = FBox(-Vec, Vec);
|
FBox Box = FBox(-Vec, Vec);
|
||||||
const FTransform NonScaledTransform(GlobalTransform.GetRotation(), GlobalTransform.GetLocation(), {1.0f, 1.0f, 1.0f});
|
const FTransform NonScaledTransform(GlobalTransform.GetRotation(), GlobalTransform.GetLocation(), { 1.0f, 1.0f, 1.0f });
|
||||||
FoliageBoundingBox = Box.TransformBy(NonScaledTransform);
|
FoliageBoundingBox = Box.TransformBy(NonScaledTransform);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -269,7 +269,7 @@ float ACarlaWheeledVehicle::GetMaximumSteerAngle() const
|
||||||
const auto& Wheels = MovementComponent->WheelSetups;
|
const auto& Wheels = MovementComponent->WheelSetups;
|
||||||
check(Wheels.Num() > 0);
|
check(Wheels.Num() > 0);
|
||||||
const UChaosVehicleWheel* FrontWheel =
|
const UChaosVehicleWheel* FrontWheel =
|
||||||
Cast<UChaosVehicleWheel>(Wheels[0].WheelClass->GetDefaultObject());
|
Cast<UChaosVehicleWheel>(Wheels[0].WheelClass->GetDefaultObject());
|
||||||
check(FrontWheel != nullptr);
|
check(FrontWheel != nullptr);
|
||||||
return FrontWheel->MaxSteerAngle;
|
return FrontWheel->MaxSteerAngle;
|
||||||
}
|
}
|
||||||
|
@ -333,7 +333,7 @@ TArray<float> ACarlaWheeledVehicle::GetWheelsFrictionScale()
|
||||||
{
|
{
|
||||||
check(Movement != nullptr);
|
check(Movement != nullptr);
|
||||||
|
|
||||||
for (auto &Wheel : Movement->Wheels)
|
for (auto& Wheel : Movement->Wheels)
|
||||||
{
|
{
|
||||||
WheelsFrictionScale.Add(Wheel->FrictionForceMultiplier);
|
WheelsFrictionScale.Add(Wheel->FrictionForceMultiplier);
|
||||||
}
|
}
|
||||||
|
@ -342,7 +342,7 @@ TArray<float> ACarlaWheeledVehicle::GetWheelsFrictionScale()
|
||||||
return WheelsFrictionScale;
|
return WheelsFrictionScale;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ACarlaWheeledVehicle::SetWheelsFrictionScale(TArray<float> &WheelsFrictionScale)
|
void ACarlaWheeledVehicle::SetWheelsFrictionScale(TArray<float>& WheelsFrictionScale)
|
||||||
{
|
{
|
||||||
UChaosWheeledVehicleMovementComponent* Movement = GetChaosWheeledVehicleMovementComponent();
|
UChaosWheeledVehicleMovementComponent* Movement = GetChaosWheeledVehicleMovementComponent();
|
||||||
if (Movement)
|
if (Movement)
|
||||||
|
@ -359,72 +359,99 @@ void ACarlaWheeledVehicle::SetWheelsFrictionScale(TArray<float> &WheelsFrictionS
|
||||||
|
|
||||||
FVehiclePhysicsControl ACarlaWheeledVehicle::GetVehiclePhysicsControl() const
|
FVehiclePhysicsControl ACarlaWheeledVehicle::GetVehiclePhysicsControl() const
|
||||||
{
|
{
|
||||||
FVehiclePhysicsControl PhysicsControl;
|
FVehiclePhysicsControl PhysicsControl = { };
|
||||||
UChaosWheeledVehicleMovementComponent* VehicleMovComponent = GetChaosWheeledVehicleMovementComponent();
|
auto VehicleMovComponentPtr = GetChaosWheeledVehicleMovementComponent();
|
||||||
check(VehicleMovComponent != nullptr);
|
check(VehicleMovComponentPtr != nullptr);
|
||||||
|
auto& VehicleMovComponent = *VehicleMovComponentPtr;
|
||||||
// Engine Setup
|
auto& EngineSetup = VehicleMovComponent.EngineSetup;
|
||||||
PhysicsControl.TorqueCurve = VehicleMovComponent->EngineSetup.TorqueCurve.EditorCurveData;
|
auto RCurve = EngineSetup.TorqueCurve.GetRichCurve();
|
||||||
PhysicsControl.MaxTorque = VehicleMovComponent->EngineSetup.MaxTorque;
|
check(RCurve != nullptr);
|
||||||
PhysicsControl.MaxRPM = VehicleMovComponent->EngineSetup.MaxRPM;
|
PhysicsControl.TorqueCurve = *RCurve;
|
||||||
PhysicsControl.MOI = VehicleMovComponent->EngineSetup.EngineRevUpMOI;
|
PhysicsControl.MaxTorque = EngineSetup.MaxTorque;
|
||||||
PhysicsControl.RevDownRate = VehicleMovComponent->EngineSetup.EngineRevDownRate;
|
PhysicsControl.MaxRPM = EngineSetup.MaxRPM;
|
||||||
|
PhysicsControl.IdleRPM = EngineSetup.EngineIdleRPM;
|
||||||
// Differential Setup
|
PhysicsControl.BrakeEffect = EngineSetup.EngineBrakeEffect;
|
||||||
PhysicsControl.DifferentialType = static_cast<uint8>(VehicleMovComponent->DifferentialSetup.DifferentialType);
|
PhysicsControl.RevUpMOI = EngineSetup.EngineRevUpMOI;
|
||||||
PhysicsControl.FrontRearSplit = VehicleMovComponent->DifferentialSetup.FrontRearSplit;
|
PhysicsControl.RevDownRate = EngineSetup.EngineRevDownRate;
|
||||||
|
auto& DifferentialSetup = VehicleMovComponent.DifferentialSetup;
|
||||||
// Transmission Setup
|
PhysicsControl.DifferentialType = (uint8_t)DifferentialSetup.DifferentialType;
|
||||||
PhysicsControl.bUseGearAutoBox = VehicleMovComponent->TransmissionSetup.bUseAutomaticGears;
|
PhysicsControl.FrontRearSplit = DifferentialSetup.FrontRearSplit;
|
||||||
PhysicsControl.FinalRatio = VehicleMovComponent->TransmissionSetup.FinalRatio;
|
auto& TransmissionSetup = VehicleMovComponent.TransmissionSetup;
|
||||||
PhysicsControl.ForwardGears = VehicleMovComponent->TransmissionSetup.ForwardGearRatios;
|
PhysicsControl.bUseAutomaticGears = TransmissionSetup.bUseAutomaticGears;
|
||||||
PhysicsControl.ReverseGears = VehicleMovComponent->TransmissionSetup.ReverseGearRatios;
|
PhysicsControl.GearChangeTime = TransmissionSetup.GearChangeTime;
|
||||||
PhysicsControl.ChangeUpRPM = VehicleMovComponent->TransmissionSetup.ChangeUpRPM;
|
PhysicsControl.FinalRatio = TransmissionSetup.FinalRatio;
|
||||||
PhysicsControl.ChangeDownRPM = VehicleMovComponent->TransmissionSetup.ChangeDownRPM;
|
PhysicsControl.ForwardGearRatios = TransmissionSetup.ForwardGearRatios;
|
||||||
PhysicsControl.GearSwitchTime = VehicleMovComponent->TransmissionSetup.GearChangeTime;
|
PhysicsControl.ReverseGearRatios = TransmissionSetup.ReverseGearRatios;
|
||||||
PhysicsControl.TransmissionEfficiency = VehicleMovComponent->TransmissionSetup.TransmissionEfficiency;
|
PhysicsControl.ChangeUpRPM = TransmissionSetup.ChangeUpRPM;
|
||||||
|
PhysicsControl.ChangeDownRPM = TransmissionSetup.ChangeDownRPM;
|
||||||
// VehicleMovComponent Setup
|
PhysicsControl.TransmissionEfficiency = TransmissionSetup.TransmissionEfficiency;
|
||||||
PhysicsControl.Mass = VehicleMovComponent->Mass;
|
PhysicsControl.Mass = VehicleMovComponent.Mass;
|
||||||
PhysicsControl.DragCoefficient = VehicleMovComponent->DragCoefficient;
|
PhysicsControl.DragCoefficient = VehicleMovComponent.DragCoefficient;
|
||||||
|
auto PrimitiveComponentPtr = Cast<UPrimitiveComponent>(VehicleMovComponent.UpdatedComponent);
|
||||||
// Center of mass offset (Center of mass is always zero vector in local
|
check(PrimitiveComponentPtr != nullptr);
|
||||||
// position)
|
PhysicsControl.CenterOfMass = PrimitiveComponentPtr->GetCenterOfMass();
|
||||||
UPrimitiveComponent *UpdatedPrimitive = Cast<UPrimitiveComponent>(VehicleMovComponent->UpdatedComponent);
|
PhysicsControl.ChassisWidth = VehicleMovComponent.ChassisWidth;
|
||||||
check(UpdatedPrimitive != nullptr);
|
PhysicsControl.ChassisHeight = VehicleMovComponent.ChassisHeight;
|
||||||
|
PhysicsControl.DownforceCoefficient = VehicleMovComponent.DownforceCoefficient;
|
||||||
PhysicsControl.CenterOfMass = UpdatedPrimitive->GetCenterOfMass();
|
PhysicsControl.DragArea = VehicleMovComponent.DragArea;
|
||||||
|
PhysicsControl.InertiaTensorScale = VehicleMovComponent.InertiaTensorScale;
|
||||||
// Transmission Setup
|
PhysicsControl.SleepThreshold = VehicleMovComponent.SleepThreshold;
|
||||||
PhysicsControl.SteeringCurve = VehicleMovComponent->SteeringSetup.SteeringCurve.EditorCurveData;
|
PhysicsControl.SleepSlopeLimit = VehicleMovComponent.SleepSlopeLimit;
|
||||||
|
RCurve = VehicleMovComponent.SteeringSetup.SteeringCurve.GetRichCurve();
|
||||||
|
check(RCurve != nullptr);
|
||||||
|
PhysicsControl.SteeringCurve = *RCurve;
|
||||||
|
PhysicsControl.UseSweepWheelCollision = false;
|
||||||
// Wheels Setup
|
// Wheels Setup
|
||||||
TArray<FWheelPhysicsControl> Wheels;
|
PhysicsControl.Wheels.SetNum(VehicleMovComponent.WheelSetups.Num());
|
||||||
|
for (int32 i = 0; i < PhysicsControl.Wheels.Num(); ++i)
|
||||||
for (int32 i = 0; i < VehicleMovComponent->WheelSetups.Num(); ++i)
|
|
||||||
{
|
{
|
||||||
FWheelPhysicsControl PhysicsWheel;
|
auto& In = *VehicleMovComponent.Wheels[i];
|
||||||
PhysicsWheel.Radius = VehicleMovComponent->Wheels[i]->WheelRadius;
|
auto& Out = PhysicsControl.Wheels[i];
|
||||||
PhysicsWheel.FrictionForceMultiplier = VehicleMovComponent->Wheels[i]->FrictionForceMultiplier;
|
Out.AxleType = In.AxleType;
|
||||||
PhysicsWheel.MaxSteerAngle = VehicleMovComponent->Wheels[i]->MaxSteerAngle;
|
Out.Offset = In.Offset;
|
||||||
PhysicsWheel.MaxBrakeTorque = VehicleMovComponent->Wheels[i]->MaxBrakeTorque;
|
Out.WheelRadius = In.WheelRadius;
|
||||||
PhysicsWheel.MaxHandBrakeTorque = VehicleMovComponent->Wheels[i]->MaxHandBrakeTorque;
|
Out.WheelWidth = In.WheelWidth;
|
||||||
PhysicsWheel.Position = VehicleMovComponent->Wheels[i]->Location;
|
Out.WheelMass = In.WheelMass;
|
||||||
PhysicsWheel.CorneringStiffness = VehicleMovComponent->Wheels[i]->CorneringStiffness;
|
Out.CorneringStiffness = In.CorneringStiffness;
|
||||||
PhysicsWheel.bABSEnabled = VehicleMovComponent->Wheels[i]->bABSEnabled;
|
Out.FrictionForceMultiplier = In.FrictionForceMultiplier;
|
||||||
PhysicsWheel.bTractionControlEnabled = VehicleMovComponent->Wheels[i]->bTractionControlEnabled;
|
Out.SideSlipModifier = In.SideSlipModifier;
|
||||||
|
Out.SlipThreshold = In.SlipThreshold;
|
||||||
if(VehicleMovComponent->Wheels[i]->SweepShape == ESweepShape::Spherecast)
|
Out.SkidThreshold = In.SkidThreshold;
|
||||||
{
|
Out.MaxSteerAngle = In.MaxSteerAngle;
|
||||||
// If some of the wheels use Spherecast, it will be used for everyone if we call ApplyPhisicsControl() as it has no sense to have it only for one.
|
Out.bAffectedBySteering = In.bAffectedBySteering;
|
||||||
|
Out.bAffectedByBrake = In.bAffectedByBrake;
|
||||||
|
Out.bAffectedByHandbrake = In.bAffectedByHandbrake;
|
||||||
|
Out.bAffectedByEngine = In.bAffectedByEngine;
|
||||||
|
Out.bABSEnabled = In.bABSEnabled;
|
||||||
|
Out.bTractionControlEnabled = In.bTractionControlEnabled;
|
||||||
|
Out.MaxWheelspinRotation = In.MaxWheelspinRotation;
|
||||||
|
Out.ExternalTorqueCombineMethod = In.ExternalTorqueCombineMethod;
|
||||||
|
RCurve = In.LateralSlipGraph.GetRichCurve();
|
||||||
|
check(RCurve != nullptr);
|
||||||
|
Out.LateralSlipGraph = *RCurve;
|
||||||
|
Out.SuspensionAxis = In.SuspensionAxis;
|
||||||
|
Out.SuspensionForceOffset = In.SuspensionForceOffset;
|
||||||
|
Out.SuspensionMaxRaise = In.SuspensionMaxRaise;
|
||||||
|
Out.SuspensionMaxDrop = In.SuspensionMaxDrop;
|
||||||
|
Out.SuspensionDampingRatio = In.SuspensionDampingRatio;
|
||||||
|
Out.WheelLoadRatio = In.WheelLoadRatio;
|
||||||
|
Out.SpringRate = In.SpringRate;
|
||||||
|
Out.SpringPreload = In.SpringPreload;
|
||||||
|
Out.SuspensionSmoothing = In.SuspensionSmoothing;
|
||||||
|
Out.RollbarScaling = In.RollbarScaling;
|
||||||
|
Out.SweepShape = In.SweepShape;
|
||||||
|
Out.SweepType = In.SweepType;
|
||||||
|
Out.MaxBrakeTorque = In.MaxBrakeTorque;
|
||||||
|
Out.MaxHandBrakeTorque = In.MaxHandBrakeTorque;
|
||||||
|
Out.WheelIndex = In.WheelIndex;
|
||||||
|
Out.Location = In.Location;
|
||||||
|
Out.OldLocation = In.OldLocation;
|
||||||
|
Out.Velocity = In.Velocity;
|
||||||
|
// If some of the wheels use Spherecast, it will be used for everyone
|
||||||
|
// if we call ApplyPhisicsControl() as it has no sense to have it only for one.
|
||||||
|
if (In.SweepShape == ESweepShape::Spherecast)
|
||||||
PhysicsControl.UseSweepWheelCollision = true;
|
PhysicsControl.UseSweepWheelCollision = true;
|
||||||
}
|
|
||||||
|
|
||||||
Wheels.Add(PhysicsWheel);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
PhysicsControl.Wheels = Wheels;
|
|
||||||
|
|
||||||
return PhysicsControl;
|
return PhysicsControl;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -438,81 +465,117 @@ void ACarlaWheeledVehicle::RestoreVehiclePhysicsControl()
|
||||||
ApplyVehiclePhysicsControl(LastPhysicsControl);
|
ApplyVehiclePhysicsControl(LastPhysicsControl);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ACarlaWheeledVehicle::ApplyVehiclePhysicsControl(const FVehiclePhysicsControl &PhysicsControl)
|
void ACarlaWheeledVehicle::ApplyVehiclePhysicsControl(
|
||||||
|
const FVehiclePhysicsControl& PhysicsControl)
|
||||||
{
|
{
|
||||||
LastPhysicsControl = PhysicsControl;
|
LastPhysicsControl = PhysicsControl;
|
||||||
UChaosWheeledVehicleMovementComponent* VehicleMovComponent = GetChaosWheeledVehicleMovementComponent();
|
auto VehicleMovComponentPtr = GetChaosWheeledVehicleMovementComponent();
|
||||||
check(VehicleMovComponent != nullptr);
|
check(VehicleMovComponentPtr != nullptr);
|
||||||
|
auto& VehicleMovComponent = *VehicleMovComponentPtr;
|
||||||
// Engine Setup
|
auto& EngineSetup = VehicleMovComponent.EngineSetup;
|
||||||
VehicleMovComponent->EngineSetup.TorqueCurve.EditorCurveData = PhysicsControl.TorqueCurve;
|
EngineSetup.TorqueCurve.EditorCurveData = PhysicsControl.TorqueCurve;
|
||||||
VehicleMovComponent->EngineSetup.MaxTorque = PhysicsControl.MaxTorque;
|
EngineSetup.MaxTorque = PhysicsControl.MaxTorque;
|
||||||
VehicleMovComponent->EngineSetup.MaxRPM = PhysicsControl.MaxRPM;
|
EngineSetup.MaxRPM = PhysicsControl.MaxRPM;
|
||||||
VehicleMovComponent->EngineSetup.EngineRevUpMOI = PhysicsControl.MOI;
|
EngineSetup.EngineIdleRPM = PhysicsControl.IdleRPM;
|
||||||
VehicleMovComponent->EngineSetup.EngineRevDownRate = PhysicsControl.RevDownRate;
|
EngineSetup.EngineBrakeEffect = PhysicsControl.BrakeEffect;
|
||||||
|
EngineSetup.EngineRevUpMOI = PhysicsControl.RevUpMOI;
|
||||||
// Differential Setup
|
EngineSetup.EngineRevDownRate = PhysicsControl.RevDownRate;
|
||||||
VehicleMovComponent->DifferentialSetup.DifferentialType = static_cast<EVehicleDifferential>(PhysicsControl.DifferentialType);
|
auto& DifferentialSetup = VehicleMovComponent.DifferentialSetup;
|
||||||
VehicleMovComponent->DifferentialSetup.FrontRearSplit = PhysicsControl.FrontRearSplit;
|
DifferentialSetup.DifferentialType = (EVehicleDifferential)PhysicsControl.DifferentialType;
|
||||||
|
DifferentialSetup.FrontRearSplit = PhysicsControl.FrontRearSplit;
|
||||||
// Transmission Setup
|
auto& TransmissionSetup = VehicleMovComponent.TransmissionSetup;
|
||||||
VehicleMovComponent->TransmissionSetup.bUseAutomaticGears = PhysicsControl.bUseGearAutoBox;
|
TransmissionSetup.bUseAutomaticGears = PhysicsControl.bUseAutomaticGears;
|
||||||
VehicleMovComponent->TransmissionSetup.FinalRatio = PhysicsControl.FinalRatio;
|
TransmissionSetup.GearChangeTime = PhysicsControl.GearChangeTime;
|
||||||
VehicleMovComponent->TransmissionSetup.ForwardGearRatios = PhysicsControl.ForwardGears;
|
TransmissionSetup.FinalRatio = PhysicsControl.FinalRatio;
|
||||||
VehicleMovComponent->TransmissionSetup.ReverseGearRatios = PhysicsControl.ReverseGears;
|
TransmissionSetup.ForwardGearRatios = PhysicsControl.ForwardGearRatios;
|
||||||
VehicleMovComponent->TransmissionSetup.ChangeUpRPM = PhysicsControl.ChangeUpRPM;
|
TransmissionSetup.ReverseGearRatios = PhysicsControl.ReverseGearRatios;
|
||||||
VehicleMovComponent->TransmissionSetup.ChangeDownRPM = PhysicsControl.ChangeDownRPM;
|
TransmissionSetup.ChangeUpRPM = PhysicsControl.ChangeUpRPM;
|
||||||
VehicleMovComponent->TransmissionSetup.GearChangeTime = PhysicsControl.GearSwitchTime;
|
TransmissionSetup.ChangeDownRPM = PhysicsControl.ChangeDownRPM;
|
||||||
VehicleMovComponent->TransmissionSetup.TransmissionEfficiency = PhysicsControl.TransmissionEfficiency;
|
TransmissionSetup.TransmissionEfficiency = PhysicsControl.TransmissionEfficiency;
|
||||||
|
VehicleMovComponent.Mass = PhysicsControl.Mass;
|
||||||
// Vehicle Setup
|
VehicleMovComponent.DragCoefficient = PhysicsControl.DragCoefficient;
|
||||||
VehicleMovComponent->Mass = PhysicsControl.Mass;
|
auto PrimitiveComponentPtr = Cast<UPrimitiveComponent>(VehicleMovComponent.UpdatedComponent);
|
||||||
VehicleMovComponent->DragCoefficient = PhysicsControl.DragCoefficient;
|
check(PrimitiveComponentPtr != nullptr);
|
||||||
|
auto& PrimitiveComponent = *PrimitiveComponentPtr;
|
||||||
// Center of mass
|
PrimitiveComponent.SetCenterOfMass(PhysicsControl.CenterOfMass);
|
||||||
UPrimitiveComponent *UpdatedPrimitive = Cast<UPrimitiveComponent>(VehicleMovComponent->UpdatedComponent);
|
VehicleMovComponent.ChassisWidth = PhysicsControl.ChassisWidth;
|
||||||
check(UpdatedPrimitive != nullptr);
|
VehicleMovComponent.ChassisHeight = PhysicsControl.ChassisHeight;
|
||||||
|
VehicleMovComponent.DownforceCoefficient = PhysicsControl.DownforceCoefficient;
|
||||||
UpdatedPrimitive->BodyInstance.COMNudge = PhysicsControl.CenterOfMass;
|
VehicleMovComponent.DragArea = PhysicsControl.DragArea;
|
||||||
|
VehicleMovComponent.InertiaTensorScale = PhysicsControl.InertiaTensorScale;
|
||||||
// Transmission Setup
|
VehicleMovComponent.SleepThreshold = PhysicsControl.SleepThreshold;
|
||||||
VehicleMovComponent->SteeringSetup.SteeringCurve.EditorCurveData = PhysicsControl.SteeringCurve;
|
VehicleMovComponent.SleepSlopeLimit = PhysicsControl.SleepSlopeLimit;
|
||||||
|
auto& SteeringSetup = VehicleMovComponent.SteeringSetup;
|
||||||
|
SteeringSetup.SteeringCurve.EditorCurveData = PhysicsControl.SteeringCurve;
|
||||||
|
|
||||||
// Wheels Setup
|
// Wheels Setup
|
||||||
const int PhysicsWheelsNum = PhysicsControl.Wheels.Num();
|
auto WheelCount = PhysicsControl.Wheels.Num();
|
||||||
|
|
||||||
// TODO:: What about bikes or NW vehicles?
|
// @TODO: What about bikes or NW vehicles?
|
||||||
if (PhysicsWheelsNum != 4)
|
if (WheelCount != 4)
|
||||||
{
|
{
|
||||||
UE_LOG(LogCarla, Error, TEXT("Number of WheelPhysicsControl is not 4."));
|
UE_LOG(LogCarla, Error, TEXT("Number of WheelPhysicsControl is not 4."));
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Change, if required, the collision mode for wheels
|
for (auto& WheelSetup : VehicleMovComponent.WheelSetups)
|
||||||
TArray<FChaosWheelSetup> NewWheelSetups = VehicleMovComponent->WheelSetups;
|
check(WheelSetup.WheelClass != nullptr);
|
||||||
for (int32 i = 0; i < PhysicsWheelsNum; ++i)
|
|
||||||
{
|
|
||||||
UChaosVehicleWheel *Wheel = NewWheelSetups[i].WheelClass.GetDefaultObject();
|
|
||||||
check(Wheel != nullptr);
|
|
||||||
}
|
|
||||||
VehicleMovComponent->WheelSetups = NewWheelSetups;
|
|
||||||
|
|
||||||
for (int32 i = 0; i < PhysicsWheelsNum; ++i)
|
for (int32 i = 0; i < WheelCount; ++i)
|
||||||
{
|
{
|
||||||
VehicleMovComponent->SetWheelRadius(i, PhysicsControl.Wheels[i].Radius);
|
auto& In = PhysicsControl.Wheels[i];
|
||||||
VehicleMovComponent->SetWheelFrictionMultiplier(i, PhysicsControl.Wheels[i].FrictionForceMultiplier);
|
auto OutPtr = VehicleMovComponent.Wheels[i];
|
||||||
VehicleMovComponent->SetWheelMaxSteerAngle(i, PhysicsControl.Wheels[i].MaxSteerAngle);
|
check(OutPtr != nullptr);
|
||||||
VehicleMovComponent->SetWheelMaxBrakeTorque(i, PhysicsControl.Wheels[i].MaxBrakeTorque);
|
auto& Out = *OutPtr;
|
||||||
VehicleMovComponent->SetWheelHandbrakeTorque(i, PhysicsControl.Wheels[i].MaxHandBrakeTorque);
|
|
||||||
VehicleMovComponent->SetABSEnabled(i, PhysicsControl.Wheels[i].bABSEnabled);
|
Out.AxleType = In.AxleType;
|
||||||
VehicleMovComponent->SetTractionControlEnabled(i, PhysicsControl.Wheels[i].bTractionControlEnabled);
|
Out.Offset = In.Offset;
|
||||||
VehicleMovComponent->Wheels[i]->CorneringStiffness = PhysicsControl.Wheels[i].CorneringStiffness;
|
VehicleMovComponent.SetWheelRadius(i, In.WheelRadius);
|
||||||
VehicleMovComponent->Wheels[i]->SweepShape = PhysicsControl.UseSweepWheelCollision ? ESweepShape::Spherecast : ESweepShape::Raycast;
|
Out.WheelWidth = In.WheelWidth;
|
||||||
|
Out.WheelMass = In.WheelMass;
|
||||||
|
Out.CorneringStiffness = In.CorneringStiffness;
|
||||||
|
VehicleMovComponent.SetWheelFrictionMultiplier(i, In.FrictionForceMultiplier);
|
||||||
|
Out.SideSlipModifier = In.SideSlipModifier;
|
||||||
|
Out.SlipThreshold = In.SlipThreshold;
|
||||||
|
Out.SkidThreshold = In.SkidThreshold;
|
||||||
|
VehicleMovComponent.SetWheelMaxSteerAngle(i, In.MaxSteerAngle);
|
||||||
|
Out.bAffectedBySteering = In.bAffectedBySteering;
|
||||||
|
Out.bAffectedByBrake = In.bAffectedByBrake;
|
||||||
|
Out.bAffectedByHandbrake = In.bAffectedByHandbrake;
|
||||||
|
Out.bAffectedByEngine = In.bAffectedByEngine;
|
||||||
|
VehicleMovComponent.SetABSEnabled(i, In.bABSEnabled);
|
||||||
|
VehicleMovComponent.SetTractionControlEnabled(i, In.bTractionControlEnabled);
|
||||||
|
Out.MaxWheelspinRotation = In.MaxWheelspinRotation;
|
||||||
|
Out.ExternalTorqueCombineMethod = In.ExternalTorqueCombineMethod;
|
||||||
|
Out.LateralSlipGraph.EditorCurveData = In.LateralSlipGraph;
|
||||||
|
Out.SuspensionAxis = In.SuspensionAxis;
|
||||||
|
Out.SuspensionForceOffset = In.SuspensionForceOffset;
|
||||||
|
Out.SuspensionMaxRaise = In.SuspensionMaxRaise;
|
||||||
|
Out.SuspensionMaxDrop = In.SuspensionMaxDrop;
|
||||||
|
Out.SuspensionDampingRatio = In.SuspensionDampingRatio;
|
||||||
|
Out.WheelLoadRatio = In.WheelLoadRatio;
|
||||||
|
Out.SpringRate = In.SpringRate;
|
||||||
|
Out.SpringPreload = In.SpringPreload;
|
||||||
|
Out.SuspensionSmoothing = In.SuspensionSmoothing;
|
||||||
|
Out.RollbarScaling = In.RollbarScaling;
|
||||||
|
Out.SweepShape = In.SweepShape;
|
||||||
|
Out.SweepType = In.SweepType;
|
||||||
|
VehicleMovComponent.SetWheelMaxBrakeTorque(i, In.MaxBrakeTorque);
|
||||||
|
VehicleMovComponent.SetWheelHandbrakeTorque(i, In.MaxHandBrakeTorque);
|
||||||
|
Out.WheelIndex = In.WheelIndex;
|
||||||
|
Out.Location = In.Location;
|
||||||
|
Out.OldLocation = In.OldLocation;
|
||||||
|
Out.Velocity = In.Velocity;
|
||||||
|
|
||||||
|
Out.SweepShape =
|
||||||
|
PhysicsControl.UseSweepWheelCollision ?
|
||||||
|
ESweepShape::Spherecast :
|
||||||
|
ESweepShape::Raycast;
|
||||||
}
|
}
|
||||||
|
|
||||||
ResetConstraints();
|
ResetConstraints();
|
||||||
|
|
||||||
auto * Recorder = UCarlaStatics::GetRecorder(GetWorld());
|
auto* Recorder = UCarlaStatics::GetRecorder(GetWorld());
|
||||||
if (Recorder && Recorder->IsEnabled())
|
if (Recorder && Recorder->IsEnabled())
|
||||||
{
|
{
|
||||||
Recorder->AddPhysicsControl(*this);
|
Recorder->AddPhysicsControl(*this);
|
||||||
|
@ -522,7 +585,7 @@ void ACarlaWheeledVehicle::ApplyVehiclePhysicsControl(const FVehiclePhysicsContr
|
||||||
AckermannController.UpdateVehiclePhysics(this);
|
AckermannController.UpdateVehiclePhysics(this);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ACarlaWheeledVehicle::ApplyVehicleControl(const FVehicleControl &Control, EVehicleInputPriority Priority)
|
void ACarlaWheeledVehicle::ApplyVehicleControl(const FVehicleControl& Control, EVehicleInputPriority Priority)
|
||||||
{
|
{
|
||||||
if (bAckermannControlActive) {
|
if (bAckermannControlActive) {
|
||||||
AckermannController.Reset();
|
AckermannController.Reset();
|
||||||
|
@ -536,14 +599,14 @@ void ACarlaWheeledVehicle::ApplyVehicleControl(const FVehicleControl &Control, E
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ACarlaWheeledVehicle::ApplyVehicleAckermannControl(const FVehicleAckermannControl &AckermannControl, EVehicleInputPriority Priority)
|
void ACarlaWheeledVehicle::ApplyVehicleAckermannControl(const FVehicleAckermannControl& AckermannControl, EVehicleInputPriority Priority)
|
||||||
{
|
{
|
||||||
bAckermannControlActive = true;
|
bAckermannControlActive = true;
|
||||||
LastAppliedAckermannControl = AckermannControl;
|
LastAppliedAckermannControl = AckermannControl;
|
||||||
AckermannController.SetTargetPoint(AckermannControl);
|
AckermannController.SetTargetPoint(AckermannControl);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ACarlaWheeledVehicle::ActivateVelocityControl(const FVector &Velocity)
|
void ACarlaWheeledVehicle::ActivateVelocityControl(const FVector& Velocity)
|
||||||
{
|
{
|
||||||
VelocityControl->Activate(Velocity);
|
VelocityControl->Activate(Velocity);
|
||||||
}
|
}
|
||||||
|
@ -564,45 +627,45 @@ void ACarlaWheeledVehicle::ShowDebugTelemetry(bool Enabled)
|
||||||
ACarlaHUD* hud = Cast<ACarlaHUD>(GetWorld()->GetFirstPlayerController()->GetHUD());
|
ACarlaHUD* hud = Cast<ACarlaHUD>(GetWorld()->GetFirstPlayerController()->GetHUD());
|
||||||
if (hud) {
|
if (hud) {
|
||||||
|
|
||||||
// Set/Unset the car movement component in HUD to show the temetry
|
// Set/Unset the car movement component in HUD to show the temetry
|
||||||
if (Enabled) {
|
if (Enabled) {
|
||||||
hud->AddDebugVehicleForTelemetry(GetChaosWheeledVehicleMovementComponent());
|
hud->AddDebugVehicleForTelemetry(GetChaosWheeledVehicleMovementComponent());
|
||||||
}
|
}
|
||||||
else{
|
else{
|
||||||
if (hud->DebugVehicle == GetChaosWheeledVehicleMovementComponent()) {
|
if (hud->DebugVehicle == GetChaosWheeledVehicleMovementComponent()) {
|
||||||
hud->AddDebugVehicleForTelemetry(nullptr);
|
hud->AddDebugVehicleForTelemetry(nullptr);
|
||||||
//GetChaosWheeledVehicleMovementComponent()->StopTelemetry();
|
//GetChaosWheeledVehicleMovementComponent()->StopTelemetry();
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
UE_LOG(LogCarla, Warning, TEXT("ACarlaWheeledVehicle::ShowDebugTelemetry:: Cannot find HUD for debug info"));
|
UE_LOG(LogCarla, Warning, TEXT("ACarlaWheeledVehicle::ShowDebugTelemetry:: Cannot find HUD for debug info"));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
*/
|
*/
|
||||||
}
|
}
|
||||||
|
|
||||||
void ACarlaWheeledVehicle::SetVehicleLightState(const FVehicleLightState &LightState)
|
void ACarlaWheeledVehicle::SetVehicleLightState(const FVehicleLightState& LightState)
|
||||||
{
|
{
|
||||||
if (LightState.Position != InputControl.LightState.Position ||
|
if (LightState.Position != InputControl.LightState.Position ||
|
||||||
LightState.LowBeam != InputControl.LightState.LowBeam ||
|
LightState.LowBeam != InputControl.LightState.LowBeam ||
|
||||||
LightState.HighBeam != InputControl.LightState.HighBeam ||
|
LightState.HighBeam != InputControl.LightState.HighBeam ||
|
||||||
LightState.Brake != InputControl.LightState.Brake ||
|
LightState.Brake != InputControl.LightState.Brake ||
|
||||||
LightState.RightBlinker != InputControl.LightState.RightBlinker ||
|
LightState.RightBlinker != InputControl.LightState.RightBlinker ||
|
||||||
LightState.LeftBlinker != InputControl.LightState.LeftBlinker ||
|
LightState.LeftBlinker != InputControl.LightState.LeftBlinker ||
|
||||||
LightState.Reverse != InputControl.LightState.Reverse ||
|
LightState.Reverse != InputControl.LightState.Reverse ||
|
||||||
LightState.Fog != InputControl.LightState.Fog ||
|
LightState.Fog != InputControl.LightState.Fog ||
|
||||||
LightState.Interior != InputControl.LightState.Interior ||
|
LightState.Interior != InputControl.LightState.Interior ||
|
||||||
LightState.Special1 != InputControl.LightState.Special1 ||
|
LightState.Special1 != InputControl.LightState.Special1 ||
|
||||||
LightState.Special2 != InputControl.LightState.Special2)
|
LightState.Special2 != InputControl.LightState.Special2)
|
||||||
{
|
{
|
||||||
InputControl.LightState = LightState;
|
InputControl.LightState = LightState;
|
||||||
RefreshLightState(LightState);
|
RefreshLightState(LightState);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ACarlaWheeledVehicle::SetFailureState(const carla::rpc::VehicleFailureState &InFailureState)
|
void ACarlaWheeledVehicle::SetFailureState(const carla::rpc::VehicleFailureState& InFailureState)
|
||||||
{
|
{
|
||||||
FailureState = InFailureState;
|
FailureState = InFailureState;
|
||||||
}
|
}
|
||||||
|
@ -621,10 +684,10 @@ void ACarlaWheeledVehicle::SetWheelSteerDirection(EVehicleWheelLocation WheelLoc
|
||||||
if (bPhysicsEnabled == false)
|
if (bPhysicsEnabled == false)
|
||||||
{
|
{
|
||||||
check((uint8)WheelLocation >= 0)
|
check((uint8)WheelLocation >= 0)
|
||||||
UVehicleAnimationInstance *VehicleAnim = Cast<UVehicleAnimationInstance>(GetMesh()->GetAnimInstance());
|
UVehicleAnimationInstance* VehicleAnim = Cast<UVehicleAnimationInstance>(GetMesh()->GetAnimInstance());
|
||||||
check(VehicleAnim != nullptr)
|
check(VehicleAnim != nullptr)
|
||||||
// ToDo We need to investigate about this
|
// ToDo We need to investigate about this
|
||||||
//VehicleAnim->GetWheelAnimData()SetWheelRotYaw((uint8)WheelLocation, AngleInDeg);
|
//VehicleAnim->GetWheelAnimData()SetWheelRotYaw((uint8)WheelLocation, AngleInDeg);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
@ -636,25 +699,25 @@ float ACarlaWheeledVehicle::GetWheelSteerAngle(EVehicleWheelLocation WheelLocati
|
||||||
|
|
||||||
#if 0 // @CARLAUE5 // ToDo We need to investigate about this
|
#if 0 // @CARLAUE5 // ToDo We need to investigate about this
|
||||||
check((uint8)WheelLocation >= 0)
|
check((uint8)WheelLocation >= 0)
|
||||||
UVehicleAnimationInstance *VehicleAnim = Cast<UVehicleAnimationInstance>(GetMesh()->GetAnimInstance());
|
UVehicleAnimationInstance* VehicleAnim = Cast<UVehicleAnimationInstance>(GetMesh()->GetAnimInstance());
|
||||||
check(VehicleAnim != nullptr)
|
check(VehicleAnim != nullptr)
|
||||||
check(VehicleAnim->GetWheeledVehicleMovementComponent() != nullptr)
|
check(VehicleAnim->GetWheeledVehicleMovementComponent() != nullptr)
|
||||||
|
|
||||||
if (bPhysicsEnabled == true)
|
if (bPhysicsEnabled == true)
|
||||||
{
|
{
|
||||||
return VehicleAnim->GetWheeledVehicleMovementComponent()->Wheels[(uint8)WheelLocation]->GetSteerAngle();
|
return VehicleAnim->GetWheeledVehicleMovementComponent()->Wheels[(uint8)WheelLocation]->GetSteerAngle();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
return VehicleAnim->GetWheelRotAngle((uint8)WheelLocation);
|
return VehicleAnim->GetWheelRotAngle((uint8)WheelLocation);
|
||||||
}
|
}
|
||||||
#else
|
#else
|
||||||
return 0.0F;
|
return 0.0F;
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
void ACarlaWheeledVehicle::SetSimulatePhysics(bool enabled) {
|
void ACarlaWheeledVehicle::SetSimulatePhysics(bool enabled) {
|
||||||
if(!GetCarlaMovementComponent<UDefaultMovementComponent>())
|
if (!GetCarlaMovementComponent<UDefaultMovementComponent>())
|
||||||
{
|
{
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
@ -664,27 +727,27 @@ void ACarlaWheeledVehicle::SetSimulatePhysics(bool enabled) {
|
||||||
{
|
{
|
||||||
check(Movement != nullptr);
|
check(Movement != nullptr);
|
||||||
|
|
||||||
if(bPhysicsEnabled == enabled)
|
if (bPhysicsEnabled == enabled)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
SetActorEnableCollision(true);
|
SetActorEnableCollision(true);
|
||||||
UPrimitiveComponent* RootPrimitive =
|
UPrimitiveComponent* RootPrimitive =
|
||||||
Cast<UPrimitiveComponent>(GetRootComponent());
|
Cast<UPrimitiveComponent>(GetRootComponent());
|
||||||
RootPrimitive->SetSimulatePhysics(enabled);
|
RootPrimitive->SetSimulatePhysics(enabled);
|
||||||
RootPrimitive->SetCollisionEnabled(ECollisionEnabled::QueryAndPhysics);
|
RootPrimitive->SetCollisionEnabled(ECollisionEnabled::QueryAndPhysics);
|
||||||
|
|
||||||
UVehicleAnimationInstance *VehicleAnim = Cast<UVehicleAnimationInstance>(GetMesh()->GetAnimInstance());
|
UVehicleAnimationInstance* VehicleAnim = Cast<UVehicleAnimationInstance>(GetMesh()->GetAnimInstance());
|
||||||
check(VehicleAnim != nullptr)
|
check(VehicleAnim != nullptr)
|
||||||
|
|
||||||
if (enabled)
|
if (enabled)
|
||||||
{
|
{
|
||||||
Movement->RecreatePhysicsState();
|
Movement->RecreatePhysicsState();
|
||||||
//VehicleAnim->ResetWheelCustomRotations();
|
//VehicleAnim->ResetWheelCustomRotations();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
Movement->DestroyPhysicsState();
|
Movement->DestroyPhysicsState();
|
||||||
}
|
}
|
||||||
bPhysicsEnabled = enabled;
|
bPhysicsEnabled = enabled;
|
||||||
|
|
||||||
ResetConstraints();
|
ResetConstraints();
|
||||||
|
@ -754,9 +817,9 @@ void ACarlaWheeledVehicle::OpenDoorPhys(const EVehicleDoor DoorIdx)
|
||||||
UPhysicsConstraintComponent* Constraint = ConstraintsComponents[static_cast<int>(DoorIdx)];
|
UPhysicsConstraintComponent* Constraint = ConstraintsComponents[static_cast<int>(DoorIdx)];
|
||||||
UPrimitiveComponent* DoorComponent = ConstraintDoor[Constraint];
|
UPrimitiveComponent* DoorComponent = ConstraintDoor[Constraint];
|
||||||
DoorComponent->DetachFromComponent(
|
DoorComponent->DetachFromComponent(
|
||||||
FDetachmentTransformRules(EDetachmentRule::KeepWorld, false));
|
FDetachmentTransformRules(EDetachmentRule::KeepWorld, false));
|
||||||
FTransform DoorInitialTransform =
|
FTransform DoorInitialTransform =
|
||||||
DoorComponentsTransform[DoorComponent] * GetActorTransform();
|
DoorComponentsTransform[DoorComponent] * GetActorTransform();
|
||||||
DoorComponent->SetWorldTransform(DoorInitialTransform);
|
DoorComponent->SetWorldTransform(DoorInitialTransform);
|
||||||
DoorComponent->SetSimulatePhysics(true);
|
DoorComponent->SetSimulatePhysics(true);
|
||||||
DoorComponent->SetCollisionProfileName(TEXT("BlockAll"));
|
DoorComponent->SetCollisionProfileName(TEXT("BlockAll"));
|
||||||
|
@ -773,7 +836,7 @@ void ACarlaWheeledVehicle::OpenDoorPhys(const EVehicleDoor DoorIdx)
|
||||||
Constraint->InitComponentConstraint();
|
Constraint->InitComponentConstraint();
|
||||||
|
|
||||||
UPhysicsConstraintComponent** CollisionDisable =
|
UPhysicsConstraintComponent** CollisionDisable =
|
||||||
CollisionDisableConstraints.Find(DoorComponent);
|
CollisionDisableConstraints.Find(DoorComponent);
|
||||||
if (CollisionDisable)
|
if (CollisionDisable)
|
||||||
{
|
{
|
||||||
(*CollisionDisable)->InitComponentConstraint();
|
(*CollisionDisable)->InitComponentConstraint();
|
||||||
|
@ -787,21 +850,21 @@ void ACarlaWheeledVehicle::CloseDoorPhys(const EVehicleDoor DoorIdx)
|
||||||
UPhysicsConstraintComponent* Constraint = ConstraintsComponents[static_cast<int>(DoorIdx)];
|
UPhysicsConstraintComponent* Constraint = ConstraintsComponents[static_cast<int>(DoorIdx)];
|
||||||
UPrimitiveComponent* DoorComponent = ConstraintDoor[Constraint];
|
UPrimitiveComponent* DoorComponent = ConstraintDoor[Constraint];
|
||||||
FTransform DoorInitialTransform =
|
FTransform DoorInitialTransform =
|
||||||
DoorComponentsTransform[DoorComponent] * GetActorTransform();
|
DoorComponentsTransform[DoorComponent] * GetActorTransform();
|
||||||
DoorComponent->SetSimulatePhysics(false);
|
DoorComponent->SetSimulatePhysics(false);
|
||||||
DoorComponent->SetCollisionProfileName(TEXT("NoCollision"));
|
DoorComponent->SetCollisionProfileName(TEXT("NoCollision"));
|
||||||
DoorComponent->SetWorldTransform(DoorInitialTransform);
|
DoorComponent->SetWorldTransform(DoorInitialTransform);
|
||||||
DoorComponent->AttachToComponent(
|
DoorComponent->AttachToComponent(
|
||||||
GetMesh(), FAttachmentTransformRules(EAttachmentRule::KeepWorld, true));
|
GetMesh(), FAttachmentTransformRules(EAttachmentRule::KeepWorld, true));
|
||||||
RecordDoorChange(DoorIdx, false);
|
RecordDoorChange(DoorIdx, false);
|
||||||
}
|
}
|
||||||
|
|
||||||
void ACarlaWheeledVehicle::RecordDoorChange(const EVehicleDoor DoorIdx, bool bIsOpen)
|
void ACarlaWheeledVehicle::RecordDoorChange(const EVehicleDoor DoorIdx, bool bIsOpen)
|
||||||
{
|
{
|
||||||
auto * Recorder = UCarlaStatics::GetRecorder(GetWorld());
|
auto* Recorder = UCarlaStatics::GetRecorder(GetWorld());
|
||||||
if (Recorder && Recorder->IsEnabled())
|
if (Recorder && Recorder->IsEnabled())
|
||||||
{
|
{
|
||||||
Recorder->AddVehicleDoor(*this, DoorIdx, bIsOpen);
|
Recorder->AddVehicleDoor(*this, DoorIdx, bIsOpen);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -812,28 +875,28 @@ void ACarlaWheeledVehicle::ApplyRolloverBehavior()
|
||||||
// The angular velocity reduction is applied in 4 stages, to improve its smoothness.
|
// The angular velocity reduction is applied in 4 stages, to improve its smoothness.
|
||||||
// Case 4 starts the timer to set the rollover flag, so users are notified.
|
// Case 4 starts the timer to set the rollover flag, so users are notified.
|
||||||
switch (RolloverBehaviorTracker) {
|
switch (RolloverBehaviorTracker) {
|
||||||
case 0: CheckRollover(roll, std::make_pair(130.0, 230.0)); break;
|
case 0: CheckRollover(roll, std::make_pair(130.0, 230.0)); break;
|
||||||
case 1: CheckRollover(roll, std::make_pair(140.0, 220.0)); break;
|
case 1: CheckRollover(roll, std::make_pair(140.0, 220.0)); break;
|
||||||
case 2: CheckRollover(roll, std::make_pair(150.0, 210.0)); break;
|
case 2: CheckRollover(roll, std::make_pair(150.0, 210.0)); break;
|
||||||
case 3: CheckRollover(roll, std::make_pair(160.0, 200.0)); break;
|
case 3: CheckRollover(roll, std::make_pair(160.0, 200.0)); break;
|
||||||
case 4:
|
case 4:
|
||||||
GetWorld()->GetTimerManager().SetTimer(TimerHandler, this, &ACarlaWheeledVehicle::SetRolloverFlag, RolloverFlagTime);
|
GetWorld()->GetTimerManager().SetTimer(TimerHandler, this, &ACarlaWheeledVehicle::SetRolloverFlag, RolloverFlagTime);
|
||||||
RolloverBehaviorTracker += 1;
|
RolloverBehaviorTracker += 1;
|
||||||
break;
|
break;
|
||||||
case 5: break;
|
case 5: break;
|
||||||
default:
|
default:
|
||||||
RolloverBehaviorTracker = 5;
|
RolloverBehaviorTracker = 5;
|
||||||
}
|
}
|
||||||
|
|
||||||
// In case the vehicle recovers, reset the rollover tracker
|
// In case the vehicle recovers, reset the rollover tracker
|
||||||
if (RolloverBehaviorTracker > 0 && -30 < roll && roll < 30){
|
if (RolloverBehaviorTracker > 0 && -30 < roll && roll < 30) {
|
||||||
RolloverBehaviorTracker = 0;
|
RolloverBehaviorTracker = 0;
|
||||||
FailureState = carla::rpc::VehicleFailureState::None;
|
FailureState = carla::rpc::VehicleFailureState::None;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ACarlaWheeledVehicle::CheckRollover(const float roll, const std::pair<float, float> threshold_roll){
|
void ACarlaWheeledVehicle::CheckRollover(const float roll, const std::pair<float, float> threshold_roll) {
|
||||||
if (threshold_roll.first < roll && roll < threshold_roll.second){
|
if (threshold_roll.first < roll && roll < threshold_roll.second) {
|
||||||
auto Root = Cast<UPrimitiveComponent>(GetRootComponent());
|
auto Root = Cast<UPrimitiveComponent>(GetRootComponent());
|
||||||
auto AngularVelocity = Root->GetPhysicsAngularVelocityInDegrees();
|
auto AngularVelocity = Root->GetPhysicsAngularVelocityInDegrees();
|
||||||
Root->SetPhysicsAngularVelocityInDegrees((1 - RolloverBehaviorForce) * AngularVelocity);
|
Root->SetPhysicsAngularVelocityInDegrees((1 - RolloverBehaviorForce) * AngularVelocity);
|
||||||
|
@ -841,14 +904,14 @@ void ACarlaWheeledVehicle::CheckRollover(const float roll, const std::pair<float
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void ACarlaWheeledVehicle::SetRolloverFlag(){
|
void ACarlaWheeledVehicle::SetRolloverFlag() {
|
||||||
// Make sure the vehicle hasn't recovered since the timer started
|
// Make sure the vehicle hasn't recovered since the timer started
|
||||||
if (RolloverBehaviorTracker >= 4) {
|
if (RolloverBehaviorTracker >= 4) {
|
||||||
FailureState = carla::rpc::VehicleFailureState::Rollover;
|
FailureState = carla::rpc::VehicleFailureState::Rollover;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
carla::rpc::VehicleFailureState ACarlaWheeledVehicle::GetFailureState() const{
|
carla::rpc::VehicleFailureState ACarlaWheeledVehicle::GetFailureState() const {
|
||||||
return FailureState;
|
return FailureState;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -883,13 +946,13 @@ void ACarlaWheeledVehicle::RemoveReferenceToManager()
|
||||||
}
|
}
|
||||||
|
|
||||||
FRotator ACarlaWheeledVehicle::GetPhysicsConstraintAngle(
|
FRotator ACarlaWheeledVehicle::GetPhysicsConstraintAngle(
|
||||||
UPhysicsConstraintComponent* Component)
|
UPhysicsConstraintComponent* Component)
|
||||||
{
|
{
|
||||||
return Component->ConstraintInstance.AngularRotationOffset;
|
return Component->ConstraintInstance.AngularRotationOffset;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ACarlaWheeledVehicle::SetPhysicsConstraintAngle(
|
void ACarlaWheeledVehicle::SetPhysicsConstraintAngle(
|
||||||
UPhysicsConstraintComponent* Component, const FRotator &NewAngle)
|
UPhysicsConstraintComponent* Component, const FRotator& NewAngle)
|
||||||
{
|
{
|
||||||
Component->ConstraintInstance.AngularRotationOffset = NewAngle;
|
Component->ConstraintInstance.AngularRotationOffset = NewAngle;
|
||||||
}
|
}
|
||||||
|
|
|
@ -23,13 +23,19 @@ struct CARLA_API FVehiclePhysicsControl
|
||||||
FRichCurve TorqueCurve;
|
FRichCurve TorqueCurve;
|
||||||
|
|
||||||
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
float MaxTorque = 300.0f;;
|
float MaxTorque = 300.0f;
|
||||||
|
|
||||||
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
float MaxRPM = 5000.0f;
|
float MaxRPM = 5000.0f;
|
||||||
|
|
||||||
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
float MOI = 1.0f;
|
float IdleRPM = 1.0f;
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
float BrakeEffect = 1.0f;
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
float RevUpMOI = 1.0f;
|
||||||
|
|
||||||
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
float RevDownRate = 600.0f;
|
float RevDownRate = 600.0f;
|
||||||
|
@ -44,19 +50,19 @@ struct CARLA_API FVehiclePhysicsControl
|
||||||
|
|
||||||
// Transmission Setup
|
// Transmission Setup
|
||||||
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
bool bUseGearAutoBox = true;
|
bool bUseAutomaticGears = true;
|
||||||
|
|
||||||
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
float GearSwitchTime = 0.5f;
|
float GearChangeTime = 0.5f;
|
||||||
|
|
||||||
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
float FinalRatio = 4.0f;
|
float FinalRatio = 4.0f;
|
||||||
|
|
||||||
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
TArray<float> ForwardGears = {2.85, 2.02, 1.35, 1.0, 2.85, 2.02, 1.35, 1.0};
|
TArray<float> ForwardGearRatios = { 2.85, 2.02, 1.35, 1.0, 2.85, 2.02, 1.35, 1.0 };
|
||||||
|
|
||||||
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
TArray<float> ReverseGears = {2.86, 2.86};
|
TArray<float> ReverseGearRatios = { 2.86, 2.86 };
|
||||||
|
|
||||||
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
float ChangeUpRPM = 4500.0f;
|
float ChangeUpRPM = 4500.0f;
|
||||||
|
@ -78,12 +84,55 @@ struct CARLA_API FVehiclePhysicsControl
|
||||||
FRichCurve SteeringCurve;
|
FRichCurve SteeringCurve;
|
||||||
|
|
||||||
// Center Of Mass
|
// Center Of Mass
|
||||||
UPROPERTY(Category = "Vehicle Center Of Mass", EditAnywhere, BlueprintReadWrite)
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
FVector CenterOfMass = FVector::ZeroVector;
|
FVector CenterOfMass = FVector::ZeroVector;
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
float ChassisWidth = 180.f;
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
float ChassisHeight = 140.f;
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
float DownforceCoefficient = 0.3f;
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
float DragArea = 0.0f;
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
FVector InertiaTensorScale = FVector(1.0f, 1.0f, 1.0f);
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
float SleepThreshold = 10.0f;
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
float SleepSlopeLimit = 0.866f; // 30º
|
||||||
|
|
||||||
|
/*
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
TArray<FVehicleAerofoilConfig> Aerofoils;
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
TArray<FVehicleThrustConfig> Thrusters;
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
FVehicleTorqueControlConfig TorqueControl;
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
FVehicleTargetRotationControlConfig TargetRotationControl;
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
FVehicleStabilizeControlConfig StabilizeControl;
|
||||||
|
|
||||||
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
|
uint32 VehicleSetupTag;
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
// Wheels Setup
|
// Wheels Setup
|
||||||
TArray<FWheelPhysicsControl> Wheels;
|
TArray<FWheelPhysicsControl> Wheels;
|
||||||
|
|
||||||
UPROPERTY(Category = "Vehicle Wheels Configuration", EditAnywhere, BlueprintReadWrite)
|
UPROPERTY(Category = "Vehicle Engine Physics Control", EditAnywhere, BlueprintReadWrite)
|
||||||
bool UseSweepWheelCollision = false;
|
bool UseSweepWheelCollision = false;
|
||||||
};
|
};
|
||||||
|
|
|
@ -5,7 +5,7 @@
|
||||||
// For a copy, see <https://opensource.org/licenses/MIT>.
|
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||||||
|
|
||||||
#pragma once
|
#pragma once
|
||||||
|
#include "ChaosVehicleWheel.h"
|
||||||
#include "WheelPhysicsControl.generated.h"
|
#include "WheelPhysicsControl.generated.h"
|
||||||
|
|
||||||
USTRUCT(BlueprintType)
|
USTRUCT(BlueprintType)
|
||||||
|
@ -13,30 +13,166 @@ struct CARLA_API FWheelPhysicsControl
|
||||||
{
|
{
|
||||||
GENERATED_BODY()
|
GENERATED_BODY()
|
||||||
|
|
||||||
UPROPERTY(Category = "Wheel Tire Friction", EditAnywhere, BlueprintReadWrite)
|
/** If left undefined then the bAffectedByEngine value is used, if defined then bAffectedByEngine is ignored and the differential setup on the vehicle defines which wheels get power from the engine */
|
||||||
float FrictionForceMultiplier = 3.5f;
|
UPROPERTY(EditAnywhere, Category = Wheel)
|
||||||
|
EAxleType AxleType;
|
||||||
|
|
||||||
UPROPERTY(Category = "Wheel Max Steer Angle", EditAnywhere, BlueprintReadWrite)
|
/**
|
||||||
float MaxSteerAngle = 70.0f;
|
* If BoneName is specified, offset the wheel from the bone's location.
|
||||||
|
* Otherwise this offsets the wheel from the vehicle's origin.
|
||||||
|
*/
|
||||||
|
UPROPERTY(EditAnywhere, Category = Wheel)
|
||||||
|
FVector Offset;
|
||||||
|
|
||||||
UPROPERTY(Category = "Wheel Shape Radius", EditAnywhere, BlueprintReadWrite)
|
/** Radius of the wheel */
|
||||||
float Radius = 30.0f;
|
UPROPERTY(EditAnywhere, Category = Wheel, meta = (ClampMin = "0.01", UIMin = "0.01"))
|
||||||
|
float WheelRadius;
|
||||||
|
|
||||||
UPROPERTY(Category = "Tyre Cornering Ability", EditAnywhere, BlueprintReadWrite)
|
/** Width of the wheel */
|
||||||
float CorneringStiffness = 1000.0f;
|
UPROPERTY(EditAnywhere, Category = Wheel, meta = (ClampMin = "0.01", UIMin = "0.01"))
|
||||||
|
float WheelWidth;
|
||||||
|
|
||||||
UPROPERTY(Category = "Advanced Braking System Enabled", EditAnywhere, BlueprintReadWrite)
|
/** Mass of the wheel Kg */
|
||||||
bool bABSEnabled = false;
|
UPROPERTY(EditAnywhere, Category = Wheel, meta = (ClampMin = "0.01", UIMin = "0.01"))
|
||||||
|
float WheelMass;
|
||||||
|
|
||||||
UPROPERTY(Category = "Straight Line Traction Control Enabled", EditAnywhere, BlueprintReadWrite)
|
/** Tyre Cornering Ability */
|
||||||
bool bTractionControlEnabled = false;
|
UPROPERTY(EditAnywhere, Category = Wheel)
|
||||||
|
float CorneringStiffness;
|
||||||
|
|
||||||
UPROPERTY(Category = "Wheel Max Brake Torque (Nm)", EditAnywhere, BlueprintReadWrite)
|
/** Friction Force Multiplier */
|
||||||
float MaxBrakeTorque = 1500.0f;
|
UPROPERTY(EditAnywhere, Category = Wheel)
|
||||||
|
float FrictionForceMultiplier;
|
||||||
|
|
||||||
UPROPERTY(Category = "Wheel Max Handbrake Torque (Nm)", EditAnywhere, BlueprintReadWrite)
|
/** Wheel Lateral Skid Grip Loss, lower number less grip on skid */
|
||||||
float MaxHandBrakeTorque = 3000.0f;
|
UPROPERTY(EditAnywhere, Category = Wheel, meta = (ClampMin = "0.0", UIMin = "0.0", ClampMax = "1.0", UIMax = "1.0"))
|
||||||
|
float SideSlipModifier;
|
||||||
|
|
||||||
|
/** Wheel Longitudinal Slip Threshold */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Wheel, meta = (ClampMin = "0.0", UIMin = "0.0"))
|
||||||
|
float SlipThreshold;
|
||||||
|
|
||||||
|
/** Wheel Lateral Skid Threshold */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Wheel, meta = (ClampMin = "0.0", UIMin = "0.0"))
|
||||||
|
float SkidThreshold;
|
||||||
|
|
||||||
|
// steer angle in degrees for this wheel
|
||||||
|
UPROPERTY(EditAnywhere, Category = WheelsSetup)
|
||||||
|
float MaxSteerAngle;
|
||||||
|
|
||||||
|
/** Whether steering should affect this wheel */
|
||||||
|
UPROPERTY(EditAnywhere, Category = WheelsSetup)
|
||||||
|
bool bAffectedBySteering;
|
||||||
|
|
||||||
|
/** Whether brake should affect this wheel */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Wheel)
|
||||||
|
bool bAffectedByBrake;
|
||||||
|
|
||||||
|
/** Whether handbrake should affect this wheel */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Wheel)
|
||||||
|
bool bAffectedByHandbrake;
|
||||||
|
|
||||||
|
/** Whether engine should power this wheel */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Wheel)
|
||||||
|
bool bAffectedByEngine;
|
||||||
|
|
||||||
|
/** Advanced Braking System Enabled */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Wheel)
|
||||||
|
bool bABSEnabled;
|
||||||
|
|
||||||
|
/** Straight Line Traction Control Enabled */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Wheel)
|
||||||
|
bool bTractionControlEnabled;
|
||||||
|
|
||||||
|
/** Max Wheelspin rotation rad/sec */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Wheel)
|
||||||
|
float MaxWheelspinRotation;
|
||||||
|
|
||||||
|
/** Determines how the SetDriveTorque/SetBrakeTorque inputs are combined with the internal torques */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Wheel)
|
||||||
|
ETorqueCombineMethod ExternalTorqueCombineMethod;
|
||||||
|
|
||||||
|
UPROPERTY(EditAnywhere, Category = Setup)
|
||||||
|
FRichCurve LateralSlipGraph;
|
||||||
|
|
||||||
|
/** Local body direction in which where suspension forces are applied (typically along -Z-axis) */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Suspension)
|
||||||
|
FVector SuspensionAxis;
|
||||||
|
|
||||||
|
/** Vertical offset from where suspension forces are applied (along Z-axis) */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Suspension)
|
||||||
|
FVector SuspensionForceOffset;
|
||||||
|
|
||||||
|
/** How far the wheel can go above the resting position */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Suspension)
|
||||||
|
float SuspensionMaxRaise;
|
||||||
|
|
||||||
|
/** How far the wheel can drop below the resting position */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Suspension)
|
||||||
|
float SuspensionMaxDrop;
|
||||||
|
|
||||||
|
/** Suspension damping, larger value causes the suspension to come to rest faster [range 0 to 1] */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Suspension)
|
||||||
|
float SuspensionDampingRatio;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Amount wheel load effects wheel friction.
|
||||||
|
At 0 wheel friction is completely independent of the loading on the wheel (This is artificial as it always assumes even balance between all wheels)
|
||||||
|
At 1 wheel friction is based on the force pressing wheel into the ground. This is more realistic.
|
||||||
|
Lower value cures lift off over-steer, generally makes vehicle easier to handle under extreme motions.
|
||||||
|
*/
|
||||||
|
UPROPERTY(EditAnywhere, Category = Suspension, meta = (ClampMin = "0.0", UIMin = "0.0", ClampMax = "1.0", UIMax = "1.0"))
|
||||||
|
float WheelLoadRatio;
|
||||||
|
|
||||||
|
/** Spring Force (N/m) */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Suspension)
|
||||||
|
float SpringRate;
|
||||||
|
|
||||||
|
/** Spring Preload (N/m) */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Suspension)
|
||||||
|
float SpringPreload;
|
||||||
|
|
||||||
|
/** Smooth suspension [0-off, 10-max] - Warning might cause momentary visual inter-penetration of the wheel against objects/terrain */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Suspension, meta = (ClampMin = "0.0", UIMin = "0", ClampMax = "10.0", UIMax = "10"))
|
||||||
|
int SuspensionSmoothing;
|
||||||
|
|
||||||
|
/** Anti-roll effect */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Suspension, meta = (ClampMin = "0.0", UIMin = "0", ClampMax = "1.0", UIMax = "1"))
|
||||||
|
float RollbarScaling;
|
||||||
|
|
||||||
|
/** Wheel suspension trace type, defaults to ray trace */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Suspension)
|
||||||
|
ESweepShape SweepShape;
|
||||||
|
|
||||||
|
/** Whether wheel suspension considers simple, complex */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Suspension)
|
||||||
|
ESweepType SweepType;
|
||||||
|
|
||||||
|
/** max brake torque for this wheel (Nm) */
|
||||||
|
UPROPERTY(EditAnywhere, Category = Brakes)
|
||||||
|
float MaxBrakeTorque;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Max handbrake brake torque for this wheel (Nm). A handbrake should have a stronger brake torque
|
||||||
|
* than the brake. This will be ignored for wheels that are not affected by the handbrake.
|
||||||
|
*/
|
||||||
|
UPROPERTY(EditAnywhere, Category = Brakes)
|
||||||
|
float MaxHandBrakeTorque;
|
||||||
|
|
||||||
|
// Our index in the vehicle's (and setup's) wheels array
|
||||||
|
UPROPERTY(transient)
|
||||||
|
int32 WheelIndex;
|
||||||
|
|
||||||
|
// Worldspace location of this wheel
|
||||||
|
UPROPERTY(transient)
|
||||||
|
FVector Location;
|
||||||
|
|
||||||
|
// Worldspace location of this wheel last frame
|
||||||
|
UPROPERTY(transient)
|
||||||
|
FVector OldLocation;
|
||||||
|
|
||||||
|
// Current velocity of the wheel center (change in location over time)
|
||||||
UPROPERTY(Category = "Wheel Position", EditAnywhere, BlueprintReadWrite)
|
UPROPERTY(Category = "Wheel Position", EditAnywhere, BlueprintReadWrite)
|
||||||
FVector Position = FVector::ZeroVector;
|
FVector Velocity;
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
|
@ -10,112 +10,112 @@
|
||||||
#include "Logging/MessageLog.h"
|
#include "Logging/MessageLog.h"
|
||||||
|
|
||||||
UWheeledVehicleMovementComponentNW::UWheeledVehicleMovementComponentNW(const FObjectInitializer& ObjectInitializer) :
|
UWheeledVehicleMovementComponentNW::UWheeledVehicleMovementComponentNW(const FObjectInitializer& ObjectInitializer) :
|
||||||
Super(ObjectInitializer)
|
Super(ObjectInitializer)
|
||||||
{
|
{
|
||||||
/*
|
/*
|
||||||
FVehicleEngineConfig DefEngineData;
|
FVehicleEngineConfig DefEngineData;
|
||||||
DefEngineData.InitDefaults();
|
DefEngineData.InitDefaults();
|
||||||
EngineSetup.MOI = DefEngineData.EngineRevUpMOI;
|
EngineSetup.MOI = DefEngineData.EngineRevUpMOI;
|
||||||
EngineSetup.MaxRPM = DefEngineData.MaxRPM;
|
EngineSetup.MaxRPM = DefEngineData.MaxRPM;
|
||||||
|
|
||||||
//
|
//
|
||||||
-- We need to investigate about this --
|
-- We need to investigate about this --
|
||||||
EngineSetup.DampingRateFullThrottle = DefEngineData.mDampingRateFullThrottle;
|
EngineSetup.DampingRateFullThrottle = DefEngineData.mDampingRateFullThrottle;
|
||||||
EngineSetup.DampingRateZeroThrottleClutchEngaged = DefEngineData.mDampingRateZeroThrottleClutchEngaged;
|
EngineSetup.DampingRateZeroThrottleClutchEngaged = DefEngineData.mDampingRateZeroThrottleClutchEngaged;
|
||||||
EngineSetup.DampingRateZeroThrottleClutchDisengaged = DefEngineData.mDampingRateZeroThrottleClutchDisengaged;
|
EngineSetup.DampingRateZeroThrottleClutchDisengaged = DefEngineData.mDampingRateZeroThrottleClutchDisengaged;
|
||||||
|
|
||||||
// Convert from Chaos curve to ours
|
// Convert from Chaos curve to ours
|
||||||
FRichCurve* TorqueCurveData = EngineSetup.TorqueCurve.GetRichCurve();
|
FRichCurve* TorqueCurveData = EngineSetup.TorqueCurve.GetRichCurve();
|
||||||
for (uint32 KeyIdx = 0; KeyIdx < DefEngineData.TorqueCurve.getNbDataPairs(); ++KeyIdx)
|
for (uint32 KeyIdx = 0; KeyIdx < DefEngineData.TorqueCurve.getNbDataPairs(); ++KeyIdx)
|
||||||
{
|
{
|
||||||
float Input = DefEngineData.TorqueCurve.getX(KeyIdx) * EngineSetup.MaxRPM;
|
float Input = DefEngineData.TorqueCurve.getX(KeyIdx) * EngineSetup.MaxRPM;
|
||||||
float Output = DefEngineData.TorqueCurve.getY(KeyIdx) * DefEngineData.MaxTorque;
|
float Output = DefEngineData.TorqueCurve.getY(KeyIdx) * DefEngineData.MaxTorque;
|
||||||
TorqueCurveData->AddKey(Input, Output);
|
TorqueCurveData->AddKey(Input, Output);
|
||||||
}
|
}
|
||||||
|
|
||||||
FVehicleTransmissionConfig TransmissionData;
|
FVehicleTransmissionConfig TransmissionData;
|
||||||
TransmissionSetup.GearSwitchTime = DefGearSetup.mSwitchTime;
|
TransmissionSetup.GearChangeTime = DefGearSetup.mSwitchTime;
|
||||||
TransmissionSetup.ReverseGearRatio = DefGearSetup.mRatios[PxVehicleGearsData::eREVERSE];
|
TransmissionSetup.ReverseGearRatio = DefGearSetup.mRatios[PxVehicleGearsData::eREVERSE];
|
||||||
TransmissionSetup.FinalRatio = DefGearSetup.mFinalRatio;
|
TransmissionSetup.FinalRatio = DefGearSetup.mFinalRatio;
|
||||||
|
|
||||||
PxVehicleAutoBoxData DefAutoBoxSetup;
|
PxVehicleAutoBoxData DefAutoBoxSetup;
|
||||||
TransmissionSetup.NeutralGearUpRatio = DefAutoBoxSetup.mUpRatios[PxVehicleGearsData::eNEUTRAL];
|
TransmissionSetup.NeutralGearUpRatio = DefAutoBoxSetup.mUpRatios[PxVehicleGearsData::eNEUTRAL];
|
||||||
TransmissionSetup.GearAutoBoxLatency = DefAutoBoxSetup.getLatency();
|
TransmissionSetup.GearAutoBoxLatency = DefAutoBoxSetup.getLatency();
|
||||||
TransmissionSetup.bUseGearAutoBox = true;
|
TransmissionSetup.bUseAutomaticGears = true;
|
||||||
|
|
||||||
for (uint32 i = PxVehicleGearsData::eFIRST; i < DefGearSetup.mNbRatios; ++i)
|
for (uint32 i = PxVehicleGearsData::eFIRST; i < DefGearSetup.mNbRatios; ++i)
|
||||||
{
|
{
|
||||||
FVehicleNWGearData GearData;
|
FVehicleNWGearData GearData;
|
||||||
GearData.DownRatio = DefAutoBoxSetup.mDownRatios[i];
|
GearData.DownRatio = DefAutoBoxSetup.mDownRatios[i];
|
||||||
GearData.UpRatio = DefAutoBoxSetup.mUpRatios[i];
|
GearData.UpRatio = DefAutoBoxSetup.mUpRatios[i];
|
||||||
GearData.Ratio = DefGearSetup.mRatios[i];
|
GearData.Ratio = DefGearSetup.mRatios[i];
|
||||||
TransmissionSetup.ForwardGears.Add(GearData);
|
TransmissionSetup.ForwardGearRatios.Add(GearData);
|
||||||
}
|
}
|
||||||
|
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// Init steering speed curve
|
// Init steering speed curve
|
||||||
//FRichCurve* SteeringCurveData = SteeringCurve.GetRichCurve();
|
//FRichCurve* SteeringCurveData = SteeringCurve.GetRichCurve();
|
||||||
//SteeringCurveData->AddKey(0.0f, 1.0f);
|
//SteeringCurveData->AddKey(0.0f, 1.0f);
|
||||||
//SteeringCurveData->AddKey(20.0f, 0.9f);
|
//SteeringCurveData->AddKey(20.0f, 0.9f);
|
||||||
//SteeringCurveData->AddKey(60.0f, 0.8f);
|
//SteeringCurveData->AddKey(60.0f, 0.8f);
|
||||||
//SteeringCurveData->AddKey(120.0f, 0.7f);
|
//SteeringCurveData->AddKey(120.0f, 0.7f);
|
||||||
|
|
||||||
// Initialize WheelSetups array with 4 wheels, this can be modified via editor later
|
// Initialize WheelSetups array with 4 wheels, this can be modified via editor later
|
||||||
const int32 NbrWheels = 4;
|
const int32 NbrWheels = 4;
|
||||||
WheelSetups.SetNum(NbrWheels);
|
WheelSetups.SetNum(NbrWheels);
|
||||||
//DifferentialSetup.SetNum(NbrWheels);
|
//DifferentialSetup.SetNum(NbrWheels);
|
||||||
|
|
||||||
IdleBrakeInput = 10;
|
IdleBrakeInput = 10;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if WITH_EDITOR
|
#if WITH_EDITOR
|
||||||
void UWheeledVehicleMovementComponentNW::PostEditChangeProperty(struct FPropertyChangedEvent& PropertyChangedEvent)
|
void UWheeledVehicleMovementComponentNW::PostEditChangeProperty(struct FPropertyChangedEvent& PropertyChangedEvent)
|
||||||
{
|
{
|
||||||
Super::PostEditChangeProperty(PropertyChangedEvent);
|
Super::PostEditChangeProperty(PropertyChangedEvent);
|
||||||
/*
|
/*
|
||||||
const FName PropertyName = PropertyChangedEvent.Property ? PropertyChangedEvent.Property->GetFName() : NAME_None;
|
const FName PropertyName = PropertyChangedEvent.Property ? PropertyChangedEvent.Property->GetFName() : NAME_None;
|
||||||
|
|
||||||
if (PropertyName == TEXT("DownRatio"))
|
if (PropertyName == TEXT("DownRatio"))
|
||||||
{
|
{
|
||||||
for (int32 GearIdx = 0; GearIdx < TransmissionSetup.ForwardGears.Num(); ++GearIdx)
|
for (int32 GearIdx = 0; GearIdx < TransmissionSetup.ForwardGearRatios.Num(); ++GearIdx)
|
||||||
{
|
{
|
||||||
FVehicleNWGearData& GearData = TransmissionSetup.ForwardGears[GearIdx];
|
FVehicleNWGearData& GearData = TransmissionSetup.ForwardGearRatios[GearIdx];
|
||||||
GearData.DownRatio = FMath::Min(GearData.DownRatio, GearData.UpRatio);
|
GearData.DownRatio = FMath::Min(GearData.DownRatio, GearData.UpRatio);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else if (PropertyName == TEXT("UpRatio"))
|
else if (PropertyName == TEXT("UpRatio"))
|
||||||
{
|
{
|
||||||
for (int32 GearIdx = 0; GearIdx < TransmissionSetup.ForwardGears.Num(); ++GearIdx)
|
for (int32 GearIdx = 0; GearIdx < TransmissionSetup.ForwardGearRatios.Num(); ++GearIdx)
|
||||||
{
|
{
|
||||||
FVehicleNWGearData& GearData = TransmissionSetup.ForwardGears[GearIdx];
|
FVehicleNWGearData& GearData = TransmissionSetup.ForwardGearRatios[GearIdx];
|
||||||
GearData.UpRatio = FMath::Max(GearData.DownRatio, GearData.UpRatio);
|
GearData.UpRatio = FMath::Max(GearData.DownRatio, GearData.UpRatio);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else if (PropertyName == TEXT("SteeringCurve"))
|
else if (PropertyName == TEXT("SteeringCurve"))
|
||||||
{
|
{
|
||||||
//make sure values are capped between 0 and 1
|
//make sure values are capped between 0 and 1
|
||||||
TArray<FRichCurveKey> SteerKeys = SteeringCurve.GetRichCurve()->GetCopyOfKeys();
|
TArray<FRichCurveKey> SteerKeys = SteeringCurve.GetRichCurve()->GetCopyOfKeys();
|
||||||
for (int32 KeyIdx = 0; KeyIdx < SteerKeys.Num(); ++KeyIdx)
|
for (int32 KeyIdx = 0; KeyIdx < SteerKeys.Num(); ++KeyIdx)
|
||||||
{
|
{
|
||||||
float NewValue = FMath::Clamp(SteerKeys[KeyIdx].Value, 0.0f, 1.0f);
|
float NewValue = FMath::Clamp(SteerKeys[KeyIdx].Value, 0.0f, 1.0f);
|
||||||
SteeringCurve.GetRichCurve()->UpdateOrAddKey(SteerKeys[KeyIdx].Time, NewValue);
|
SteeringCurve.GetRichCurve()->UpdateOrAddKey(SteerKeys[KeyIdx].Time, NewValue);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
*/
|
*/
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
float FVehicleNWEngineData::FindPeakTorque() const
|
float FVehicleNWEngineData::FindPeakTorque() const
|
||||||
{
|
{
|
||||||
// Find max torque
|
// Find max torque
|
||||||
float PeakTorque = 0.0f;
|
float PeakTorque = 0.0f;
|
||||||
TArray<FRichCurveKey> TorqueKeys = TorqueCurve.GetRichCurveConst()->GetCopyOfKeys();
|
TArray<FRichCurveKey> TorqueKeys = TorqueCurve.GetRichCurveConst()->GetCopyOfKeys();
|
||||||
for (int32 KeyIdx = 0; KeyIdx < TorqueKeys.Num(); ++KeyIdx)
|
for (int32 KeyIdx = 0; KeyIdx < TorqueKeys.Num(); ++KeyIdx)
|
||||||
{
|
{
|
||||||
FRichCurveKey& Key = TorqueKeys[KeyIdx];
|
FRichCurveKey& Key = TorqueKeys[KeyIdx];
|
||||||
PeakTorque = FMath::Max(PeakTorque, Key.Value);
|
PeakTorque = FMath::Max(PeakTorque, Key.Value);
|
||||||
}
|
}
|
||||||
|
|
||||||
return PeakTorque;
|
return PeakTorque;
|
||||||
}
|
}
|
||||||
|
|
|
@ -12,129 +12,129 @@
|
||||||
USTRUCT()
|
USTRUCT()
|
||||||
struct FVehicleNWWheelDifferentialData
|
struct FVehicleNWWheelDifferentialData
|
||||||
{
|
{
|
||||||
GENERATED_USTRUCT_BODY()
|
GENERATED_USTRUCT_BODY()
|
||||||
|
|
||||||
/** If True, torque is applied to this wheel */
|
/** If True, torque is applied to this wheel */
|
||||||
UPROPERTY(EditAnywhere, Category = Setup)
|
UPROPERTY(EditAnywhere, Category = Setup)
|
||||||
bool bDriven;
|
bool bDriven;
|
||||||
|
|
||||||
FVehicleNWWheelDifferentialData()
|
FVehicleNWWheelDifferentialData()
|
||||||
: bDriven(true)
|
: bDriven(true)
|
||||||
{ }
|
{ }
|
||||||
};
|
};
|
||||||
|
|
||||||
USTRUCT()
|
USTRUCT()
|
||||||
struct FVehicleNWEngineData
|
struct FVehicleNWEngineData
|
||||||
{
|
{
|
||||||
GENERATED_USTRUCT_BODY()
|
GENERATED_USTRUCT_BODY()
|
||||||
|
|
||||||
/** Torque (Nm) at a given RPM*/
|
/** Torque (Nm) at a given RPM*/
|
||||||
UPROPERTY(EditAnywhere, Category = Setup)
|
UPROPERTY(EditAnywhere, Category = Setup)
|
||||||
FRuntimeFloatCurve TorqueCurve;
|
FRuntimeFloatCurve TorqueCurve;
|
||||||
|
|
||||||
/** Maximum revolutions per minute of the engine */
|
/** Maximum revolutions per minute of the engine */
|
||||||
UPROPERTY(EditAnywhere, Category = Setup, meta = (ClampMin = "0.01", UIMin = "0.01"))
|
UPROPERTY(EditAnywhere, Category = Setup, meta = (ClampMin = "0.01", UIMin = "0.01"))
|
||||||
float MaxRPM = 0.0F;
|
float MaxRPM = 0.0F;
|
||||||
|
|
||||||
/** Moment of inertia of the engine around the axis of rotation (Kgm^2). */
|
/** Moment of inertia of the engine around the axis of rotation (Kgm^2). */
|
||||||
UPROPERTY(EditAnywhere, Category = Setup, meta = (ClampMin = "0.01", UIMin = "0.01"))
|
UPROPERTY(EditAnywhere, Category = Setup, meta = (ClampMin = "0.01", UIMin = "0.01"))
|
||||||
float MOI = 0.0F;
|
float MOI = 0.0F;
|
||||||
|
|
||||||
/** Damping rate of engine when full throttle is applied (Kgm^2/s) */
|
/** Damping rate of engine when full throttle is applied (Kgm^2/s) */
|
||||||
UPROPERTY(EditAnywhere, Category = Setup, AdvancedDisplay, meta = (ClampMin = "0.0", UIMin = "0.0"))
|
UPROPERTY(EditAnywhere, Category = Setup, AdvancedDisplay, meta = (ClampMin = "0.0", UIMin = "0.0"))
|
||||||
float DampingRateFullThrottle = 0.0F;
|
float DampingRateFullThrottle = 0.0F;
|
||||||
|
|
||||||
/** Damping rate of engine in at zero throttle when the clutch is engaged (Kgm^2/s)*/
|
/** Damping rate of engine in at zero throttle when the clutch is engaged (Kgm^2/s)*/
|
||||||
UPROPERTY(EditAnywhere, Category = Setup, AdvancedDisplay, meta = (ClampMin = "0.0", UIMin = "0.0"))
|
UPROPERTY(EditAnywhere, Category = Setup, AdvancedDisplay, meta = (ClampMin = "0.0", UIMin = "0.0"))
|
||||||
float DampingRateZeroThrottleClutchEngaged = 0.0F;
|
float DampingRateZeroThrottleClutchEngaged = 0.0F;
|
||||||
|
|
||||||
/** Damping rate of engine in at zero throttle when the clutch is disengaged (in neutral gear) (Kgm^2/s)*/
|
/** Damping rate of engine in at zero throttle when the clutch is disengaged (in neutral gear) (Kgm^2/s)*/
|
||||||
UPROPERTY(EditAnywhere, Category = Setup, AdvancedDisplay, meta = (ClampMin = "0.0", UIMin = "0.0"))
|
UPROPERTY(EditAnywhere, Category = Setup, AdvancedDisplay, meta = (ClampMin = "0.0", UIMin = "0.0"))
|
||||||
float DampingRateZeroThrottleClutchDisengaged = 0.0F;
|
float DampingRateZeroThrottleClutchDisengaged = 0.0F;
|
||||||
|
|
||||||
/** Find the peak torque produced by the TorqueCurve */
|
/** Find the peak torque produced by the TorqueCurve */
|
||||||
float FindPeakTorque() const;
|
float FindPeakTorque() const;
|
||||||
};
|
};
|
||||||
|
|
||||||
USTRUCT()
|
USTRUCT()
|
||||||
struct FVehicleNWGearData
|
struct FVehicleNWGearData
|
||||||
{
|
{
|
||||||
GENERATED_USTRUCT_BODY()
|
GENERATED_USTRUCT_BODY()
|
||||||
|
|
||||||
/** Determines the amount of torque multiplication*/
|
/** Determines the amount of torque multiplication*/
|
||||||
UPROPERTY(EditAnywhere, Category = Setup)
|
UPROPERTY(EditAnywhere, Category = Setup)
|
||||||
float Ratio = 0.0F;
|
float Ratio = 0.0F;
|
||||||
|
|
||||||
/** Value of engineRevs/maxEngineRevs that is low enough to gear down*/
|
/** Value of engineRevs/maxEngineRevs that is low enough to gear down*/
|
||||||
UPROPERTY(EditAnywhere, meta = (ClampMin = "0.0", UIMin = "0.0", ClampMax = "1.0", UIMax = "1.0"), Category = Setup)
|
UPROPERTY(EditAnywhere, meta = (ClampMin = "0.0", UIMin = "0.0", ClampMax = "1.0", UIMax = "1.0"), Category = Setup)
|
||||||
float DownRatio = 0.0F;
|
float DownRatio = 0.0F;
|
||||||
|
|
||||||
/** Value of engineRevs/maxEngineRevs that is high enough to gear up*/
|
/** Value of engineRevs/maxEngineRevs that is high enough to gear up*/
|
||||||
UPROPERTY(EditAnywhere, meta = (ClampMin = "0.0", UIMin = "0.0", ClampMax = "1.0", UIMax = "1.0"), Category = Setup)
|
UPROPERTY(EditAnywhere, meta = (ClampMin = "0.0", UIMin = "0.0", ClampMax = "1.0", UIMax = "1.0"), Category = Setup)
|
||||||
float UpRatio = 0.0F;
|
float UpRatio = 0.0F;
|
||||||
};
|
};
|
||||||
|
|
||||||
USTRUCT()
|
USTRUCT()
|
||||||
struct FVehicleNWTransmissionData
|
struct FVehicleNWTransmissionData
|
||||||
{
|
{
|
||||||
GENERATED_USTRUCT_BODY()
|
GENERATED_USTRUCT_BODY()
|
||||||
/** Whether to use automatic transmission */
|
/** Whether to use automatic transmission */
|
||||||
UPROPERTY(EditAnywhere, Category = VehicleSetup, meta = (DisplayName = "Automatic Transmission"))
|
UPROPERTY(EditAnywhere, Category = VehicleSetup, meta = (DisplayName = "Automatic Transmission"))
|
||||||
bool bUseGearAutoBox = false;
|
bool bUseAutomaticGears = false;
|
||||||
|
|
||||||
/** Time it takes to switch gears (seconds) */
|
/** Time it takes to switch gears (seconds) */
|
||||||
UPROPERTY(EditAnywhere, Category = Setup, meta = (ClampMin = "0.0", UIMin = "0.0"))
|
UPROPERTY(EditAnywhere, Category = Setup, meta = (ClampMin = "0.0", UIMin = "0.0"))
|
||||||
float GearSwitchTime = 0.0F;
|
float GearChangeTime = 0.0F;
|
||||||
|
|
||||||
/** Minimum time it takes the automatic transmission to initiate a gear change (seconds)*/
|
/** Minimum time it takes the automatic transmission to initiate a gear change (seconds)*/
|
||||||
UPROPERTY(EditAnywhere, Category = Setup, meta = (editcondition = "bUseGearAutoBox", ClampMin = "0.0", UIMin = "0.0"))
|
UPROPERTY(EditAnywhere, Category = Setup, meta = (editcondition = "bUseAutomaticGears", ClampMin = "0.0", UIMin = "0.0"))
|
||||||
float GearAutoBoxLatency = 0.0F;
|
float GearAutoBoxLatency = 0.0F;
|
||||||
|
|
||||||
/** The final gear ratio multiplies the transmission gear ratios.*/
|
/** The final gear ratio multiplies the transmission gear ratios.*/
|
||||||
UPROPERTY(EditAnywhere, AdvancedDisplay, Category = Setup)
|
UPROPERTY(EditAnywhere, AdvancedDisplay, Category = Setup)
|
||||||
float FinalRatio = 0.0F;
|
float FinalRatio = 0.0F;
|
||||||
|
|
||||||
/** Forward gear ratios (up to 30) */
|
/** Forward gear ratios (up to 30) */
|
||||||
UPROPERTY(EditAnywhere, Category = Setup, AdvancedDisplay)
|
UPROPERTY(EditAnywhere, Category = Setup, AdvancedDisplay)
|
||||||
TArray<FVehicleNWGearData> ForwardGears;
|
TArray<FVehicleNWGearData> ForwardGearRatios;
|
||||||
|
|
||||||
/** Reverse gear ratio */
|
/** Reverse gear ratio */
|
||||||
UPROPERTY(EditAnywhere, AdvancedDisplay, Category = Setup)
|
UPROPERTY(EditAnywhere, AdvancedDisplay, Category = Setup)
|
||||||
float ReverseGearRatio = 0.0F;
|
float ReverseGearRatio = 0.0F;
|
||||||
|
|
||||||
/** Value of engineRevs/maxEngineRevs that is high enough to increment gear*/
|
/** Value of engineRevs/maxEngineRevs that is high enough to increment gear*/
|
||||||
UPROPERTY(EditAnywhere, AdvancedDisplay, Category = Setup, meta = (ClampMin = "0.0", UIMin = "0.0", ClampMax = "1.0", UIMax = "1.0"))
|
UPROPERTY(EditAnywhere, AdvancedDisplay, Category = Setup, meta = (ClampMin = "0.0", UIMin = "0.0", ClampMax = "1.0", UIMax = "1.0"))
|
||||||
float NeutralGearUpRatio = 0.0F;
|
float NeutralGearUpRatio = 0.0F;
|
||||||
|
|
||||||
/** Strength of clutch (Kgm^2/s)*/
|
/** Strength of clutch (Kgm^2/s)*/
|
||||||
UPROPERTY(EditAnywhere, Category = Setup, AdvancedDisplay, meta = (ClampMin = "0.0", UIMin = "0.0"))
|
UPROPERTY(EditAnywhere, Category = Setup, AdvancedDisplay, meta = (ClampMin = "0.0", UIMin = "0.0"))
|
||||||
float ClutchStrength = 0.0F;
|
float ClutchStrength = 0.0F;
|
||||||
};
|
};
|
||||||
|
|
||||||
UCLASS(ClassGroup = (Physics), meta = (BlueprintSpawnableComponent), hidecategories = (PlanarMovement, "Components|Movement|Planar", Activation, "Components|Activation"))
|
UCLASS(ClassGroup = (Physics), meta = (BlueprintSpawnableComponent), hidecategories = (PlanarMovement, "Components|Movement|Planar", Activation, "Components|Activation"))
|
||||||
class CARLA_API UWheeledVehicleMovementComponentNW :
|
class CARLA_API UWheeledVehicleMovementComponentNW :
|
||||||
public UChaosWheeledVehicleMovementComponent
|
public UChaosWheeledVehicleMovementComponent
|
||||||
{
|
{
|
||||||
GENERATED_UCLASS_BODY()
|
GENERATED_UCLASS_BODY()
|
||||||
|
|
||||||
/** Engine */
|
/** Engine */
|
||||||
//UPROPERTY(EditAnywhere, Category = MechanicalSetup)
|
//UPROPERTY(EditAnywhere, Category = MechanicalSetup)
|
||||||
//FVehicleNWEngineData EngineSetupEXT;
|
//FVehicleNWEngineData EngineSetupEXT;
|
||||||
|
|
||||||
/** Differential */
|
/** Differential */
|
||||||
//UPROPERTY(EditAnywhere, Category = MechanicalSetup)
|
//UPROPERTY(EditAnywhere, Category = MechanicalSetup)
|
||||||
//TArray<FVehicleNWWheelDifferentialData> DifferentialSetupEXT;
|
//TArray<FVehicleNWWheelDifferentialData> DifferentialSetupEXT;
|
||||||
|
|
||||||
/** Transmission data */
|
/** Transmission data */
|
||||||
//UPROPERTY(EditAnywhere, Category = MechanicalSetup)
|
//UPROPERTY(EditAnywhere, Category = MechanicalSetup)
|
||||||
//FVehicleNWTransmissionData TransmissionSetupEXT;
|
//FVehicleNWTransmissionData TransmissionSetupEXT;
|
||||||
|
|
||||||
/** Maximum steering versus forward speed (km/h) */
|
/** Maximum steering versus forward speed (km/h) */
|
||||||
//UPROPERTY(EditAnywhere, Category = SteeringSetup)
|
//UPROPERTY(EditAnywhere, Category = SteeringSetup)
|
||||||
//FRuntimeFloatCurve SteeringCurve;
|
//FRuntimeFloatCurve SteeringCurve;
|
||||||
|
|
||||||
#if WITH_EDITOR
|
#if WITH_EDITOR
|
||||||
virtual void PostEditChangeProperty(struct FPropertyChangedEvent& PropertyChangedEvent) override;
|
virtual void PostEditChangeProperty(struct FPropertyChangedEvent& PropertyChangedEvent) override;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|
Loading…
Reference in New Issue