From d016305690c71268f3a48689ec68a1b427d853cc Mon Sep 17 00:00:00 2001 From: "Mr. Capybara" Date: Sun, 31 Dec 2023 15:00:31 -0400 Subject: [PATCH 01/15] Add tank vehicle physics --- .../Engine/Physics/Actors/WheeledVehicle.cpp | 91 ++- Source/Engine/Physics/Actors/WheeledVehicle.h | 66 +- .../Physics/PhysX/PhysicsBackendPhysX.cpp | 591 ++++++++++++------ 3 files changed, 518 insertions(+), 230 deletions(-) diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.cpp b/Source/Engine/Physics/Actors/WheeledVehicle.cpp index afb5309ea..9051c7eed 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.cpp +++ b/Source/Engine/Physics/Actors/WheeledVehicle.cpp @@ -14,7 +14,7 @@ #include "Engine/Core/Log.h" #endif -WheeledVehicle::WheeledVehicle(const SpawnParams& params) +WheeledVehicle::WheeledVehicle(const SpawnParams ¶ms) : RigidBody(params) { _useCCD = 1; @@ -33,12 +33,22 @@ void WheeledVehicle::SetDriveType(DriveTypes value) Setup(); } -const Array& WheeledVehicle::GetWheels() const +void WheeledVehicle::SetDriveMode(DriveModes value) +{ + _driveMode = value; +} + +WheeledVehicle::DriveModes WheeledVehicle::GetDriveMode() const +{ + return _driveMode; +} + +const Array &WheeledVehicle::GetWheels() const { return _wheels; } -void WheeledVehicle::SetWheels(const Array& value) +void WheeledVehicle::SetWheels(const Array &value) { #if WITH_VEHICLE // Don't recreate whole vehicle when some wheel properties are only changed (eg. suspension) @@ -47,8 +57,8 @@ void WheeledVehicle::SetWheels(const Array& value) bool softUpdate = true; for (int32 wheelIndex = 0; wheelIndex < value.Count(); wheelIndex++) { - auto& oldWheel = _wheels.Get()[wheelIndex]; - auto& newWheel = value.Get()[wheelIndex]; + auto &oldWheel = _wheels.Get()[wheelIndex]; + auto &newWheel = value.Get()[wheelIndex]; if (oldWheel.Type != newWheel.Type || Math::NotNearEqual(oldWheel.SuspensionForceOffset, newWheel.SuspensionForceOffset) || oldWheel.Collider != newWheel.Collider) @@ -74,7 +84,7 @@ WheeledVehicle::EngineSettings WheeledVehicle::GetEngine() const return _engine; } -void WheeledVehicle::SetEngine(const EngineSettings& value) +void WheeledVehicle::SetEngine(const EngineSettings &value) { #if WITH_VEHICLE if (_vehicle) @@ -88,7 +98,7 @@ WheeledVehicle::DifferentialSettings WheeledVehicle::GetDifferential() const return _differential; } -void WheeledVehicle::SetDifferential(const DifferentialSettings& value) +void WheeledVehicle::SetDifferential(const DifferentialSettings &value) { #if WITH_VEHICLE if (_vehicle) @@ -102,7 +112,7 @@ WheeledVehicle::GearboxSettings WheeledVehicle::GetGearbox() const return _gearbox; } -void WheeledVehicle::SetGearbox(const GearboxSettings& value) +void WheeledVehicle::SetGearbox(const GearboxSettings &value) { #if WITH_VEHICLE if (_vehicle) @@ -131,11 +141,35 @@ void WheeledVehicle::SetHandbrake(float value) _handBrake = Math::Saturate(value); } +void WheeledVehicle::SetTankLeftThrottle(float value) +{ + _tankLeftThrottle = Math::Clamp(value, -1.0f, 1.0f); +} + +void WheeledVehicle::SetTankRightThrottle(float value) +{ + _tankRightThrottle = Math::Clamp(value, -1.0f, 1.0f); +} + +void WheeledVehicle::SetTankLeftBrake(float value) +{ + _tankLeftBrake = Math::Saturate(value); +} + +void WheeledVehicle::SetTankRightBrake(float value) +{ + _tankRightBrake = Math::Saturate(value); +} + void WheeledVehicle::ClearInput() { _throttle = 0; _steering = 0; _brake = 0; + _tankLeftThrottle = 0; + _tankRightThrottle = 0; + _tankLeftBrake = 0; + _tankRightBrake = 0; _handBrake = 0; } @@ -200,12 +234,12 @@ void WheeledVehicle::SetTargetGear(int32 value) #endif } -void WheeledVehicle::GetWheelState(int32 index, WheelState& result) +void WheeledVehicle::GetWheelState(int32 index, WheelState &result) { if (index >= 0 && index < _wheels.Count()) { const auto collider = _wheels[index].Collider.Get(); - for (auto& wheelData : _wheelsData) + for (auto &wheelData : _wheelsData) { if (wheelData.Collider == collider) { @@ -223,7 +257,7 @@ void WheeledVehicle::Setup() return; // Release previous - void* scene = GetPhysicsScene()->GetPhysicsScene(); + void *scene = GetPhysicsScene()->GetPhysicsScene(); if (_vehicle) { PhysicsBackend::RemoveVehicle(scene, this); @@ -246,10 +280,10 @@ void WheeledVehicle::Setup() #if USE_EDITOR -void WheeledVehicle::DrawPhysicsDebug(RenderView& view) +void WheeledVehicle::DrawPhysicsDebug(RenderView &view) { // Wheels shapes - for (const auto& data : _wheelsData) + for (const auto &data : _wheelsData) { int32 wheelIndex = 0; for (; wheelIndex < _wheels.Count(); wheelIndex++) @@ -259,7 +293,7 @@ void WheeledVehicle::DrawPhysicsDebug(RenderView& view) } if (wheelIndex == _wheels.Count()) break; - const auto& wheel = _wheels[wheelIndex]; + const auto &wheel = _wheels[wheelIndex]; if (wheel.Collider && wheel.Collider->GetParent() == this && !wheel.Collider->GetIsTrigger()) { const Vector3 currentPos = wheel.Collider->GetPosition(); @@ -280,7 +314,7 @@ void WheeledVehicle::DrawPhysicsDebug(RenderView& view) void WheeledVehicle::OnDebugDrawSelected() { // Wheels shapes - for (const auto& data : _wheelsData) + for (const auto &data : _wheelsData) { int32 wheelIndex = 0; for (; wheelIndex < _wheels.Count(); wheelIndex++) @@ -290,7 +324,7 @@ void WheeledVehicle::OnDebugDrawSelected() } if (wheelIndex == _wheels.Count()) break; - const auto& wheel = _wheels[wheelIndex]; + const auto &wheel = _wheels[wheelIndex]; if (wheel.Collider && wheel.Collider->GetParent() == this && !wheel.Collider->GetIsTrigger()) { const Vector3 currentPos = wheel.Collider->GetPosition(); @@ -314,6 +348,14 @@ void WheeledVehicle::OnDebugDrawSelected() DEBUG_DRAW_WIRE_SPHERE(BoundingSphere(data.State.TireContactPoint, 5.0f), Color::Green, 0, false); } } + + // Draw wheels axes + if (wheelIndex % 2 == 0 && wheelIndex + 1 < _wheels.Count()) + { + const Vector3 wheelPos = _wheels[wheelIndex].Collider->GetPosition(); + const Vector3 nextWheelPos = _wheels[wheelIndex + 1].Collider->GetPosition(); + DEBUG_DRAW_LINE(wheelPos, nextWheelPos, Color::Yellow, 0, false); + } } // Center of mass @@ -324,13 +366,14 @@ void WheeledVehicle::OnDebugDrawSelected() #endif -void WheeledVehicle::Serialize(SerializeStream& stream, const void* otherObj) +void WheeledVehicle::Serialize(SerializeStream &stream, const void *otherObj) { RigidBody::Serialize(stream, otherObj); SERIALIZE_GET_OTHER_OBJ(WheeledVehicle); SERIALIZE_MEMBER(DriveType, _driveType); + SERIALIZE_MEMBER(DriveModes, _driveMode); SERIALIZE_MEMBER(Wheels, _wheels); SERIALIZE(UseReverseAsBrake); SERIALIZE(UseAnalogSteering); @@ -339,11 +382,12 @@ void WheeledVehicle::Serialize(SerializeStream& stream, const void* otherObj) SERIALIZE_MEMBER(Gearbox, _gearbox); } -void WheeledVehicle::Deserialize(DeserializeStream& stream, ISerializeModifier* modifier) +void WheeledVehicle::Deserialize(DeserializeStream &stream, ISerializeModifier *modifier) { RigidBody::Deserialize(stream, modifier); DESERIALIZE_MEMBER(DriveType, _driveType); + DESERIALIZE_MEMBER(DriveModes, _driveMode); DESERIALIZE_MEMBER(Wheels, _wheels); DESERIALIZE(UseReverseAsBrake); DESERIALIZE(UseAnalogSteering); @@ -355,7 +399,7 @@ void WheeledVehicle::Deserialize(DeserializeStream& stream, ISerializeModifier* _fixInvalidForwardDir |= modifier->EngineBuild < 6341; } -void WheeledVehicle::OnColliderChanged(Collider* c) +void WheeledVehicle::OnColliderChanged(Collider *c) { RigidBody::OnColliderChanged(c); @@ -378,7 +422,7 @@ void WheeledVehicle::OnActiveInTreeChanged() Setup(); } -void WheeledVehicle::OnPhysicsSceneChanged(PhysicsScene* previous) +void WheeledVehicle::OnPhysicsSceneChanged(PhysicsScene *previous) { RigidBody::OnPhysicsSceneChanged(previous); @@ -401,9 +445,10 @@ void WheeledVehicle::OnTransformChanged() // Transform all vehicle children around the vehicle origin to fix the vehicle facing direction const Quaternion rotationDelta(0.0f, -0.7071068f, 0.0f, 0.7071068f); const Vector3 origin = GetPosition(); - for (Actor* child : Children) + for (Actor *child : Children) { - Transform trans = child->GetTransform();; + Transform trans = child->GetTransform(); + ; const Vector3 pivotOffset = trans.Translation - origin; if (pivotOffset.IsZero()) { @@ -423,7 +468,7 @@ void WheeledVehicle::OnTransformChanged() } } -void WheeledVehicle::BeginPlay(SceneBeginData* data) +void WheeledVehicle::BeginPlay(SceneBeginData *data) { RigidBody::BeginPlay(data); diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.h b/Source/Engine/Physics/Actors/WheeledVehicle.h index 3a329edc4..49c00b17a 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.h +++ b/Source/Engine/Physics/Actors/WheeledVehicle.h @@ -26,6 +26,18 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo DriveNW, // Non-drivable vehicle. NoDrive, + Tank, + }; + /// + /// Vehicle driving types. + /// Used only on tanks to specify the drive mode. + /// + API_ENUM() enum class DriveModes + { + // Drive turning the vehicle using only one track + Standard, + // Drive turning the vehicle using all tracks inverse direction. + Special }; /// @@ -128,6 +140,11 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo /// API_FIELD() bool AutoGear = true; + /// + /// Number of gears to move to forward + /// + API_FIELD(Attributes = "Limit(1, 30)") int ForwardGearsRatios = 5; + /// /// Time it takes to switch gear. Specified in seconds (s). /// @@ -322,8 +339,9 @@ private: void* _vehicle = nullptr; DriveTypes _driveType = DriveTypes::Drive4W, _driveTypeCurrent; + DriveModes _driveMode = DriveModes::Standard; Array> _wheelsData; - float _throttle = 0.0f, _steering = 0.0f, _brake = 0.0f, _handBrake = 0.0f; + float _throttle = 0.0f, _steering = 0.0f, _brake = 0.0f, _handBrake = 0.0f, _tankLeftThrottle, _tankRightThrottle, _tankLeftBrake, _tankRightBrake; Array _wheels; EngineSettings _engine; DifferentialSettings _differential; @@ -347,17 +365,27 @@ public: /// /// Gets the vehicle driving model type. /// - API_PROPERTY(Attributes="EditorOrder(1), EditorDisplay(\"Vehicle\")") DriveTypes GetDriveType() const; + API_PROPERTY(Attributes="EditorOrder(2), EditorDisplay(\"Vehicle\")") DriveTypes GetDriveType() const; /// /// Sets the vehicle driving model type. /// API_PROPERTY() void SetDriveType(DriveTypes value); + /// + /// Used only for tanks, set the drive mode. + /// + API_PROPERTY() void SetDriveMode(DriveModes value); + + /// + /// Gets the vehicle driving mode. Used only on tanks + /// + API_PROPERTY(Attributes="EditorOrder(3), EditorDisplay(\"Vehicle\")") DriveModes GetDriveMode() const; + /// /// Gets the vehicle wheels settings. /// - API_PROPERTY(Attributes="EditorOrder(2), EditorDisplay(\"Vehicle\")") const Array& GetWheels() const; + API_PROPERTY(Attributes="EditorOrder(4), EditorDisplay(\"Vehicle\")") const Array& GetWheels() const; /// /// Sets the vehicle wheels settings. @@ -367,7 +395,7 @@ public: /// /// Gets the vehicle engine settings. /// - API_PROPERTY(Attributes="EditorOrder(3), EditorDisplay(\"Vehicle\")") EngineSettings GetEngine() const; + API_PROPERTY(Attributes="EditorOrder(5), EditorDisplay(\"Vehicle\")") EngineSettings GetEngine() const; /// /// Sets the vehicle engine settings. @@ -377,7 +405,7 @@ public: /// /// Gets the vehicle differential settings. /// - API_PROPERTY(Attributes="EditorOrder(4), EditorDisplay(\"Vehicle\")") DifferentialSettings GetDifferential() const; + API_PROPERTY(Attributes="EditorOrder(6), EditorDisplay(\"Vehicle\")") DifferentialSettings GetDifferential() const; /// /// Sets the vehicle differential settings. @@ -387,7 +415,7 @@ public: /// /// Gets the vehicle gearbox settings. /// - API_PROPERTY(Attributes="EditorOrder(5), EditorDisplay(\"Vehicle\")") GearboxSettings GetGearbox() const; + API_PROPERTY(Attributes="EditorOrder(7), EditorDisplay(\"Vehicle\")") GearboxSettings GetGearbox() const; /// /// Sets the vehicle gearbox settings. @@ -419,6 +447,32 @@ public: /// The value (0,1 range). API_FUNCTION() void SetHandbrake(float value); + /// + /// Sets the input for tank left track throttle. It is the analog accelerator pedal value in range (-1,1) where 1 represents the pedal fully pressed to move to forward, 0 to represents the + /// pedal in its rest state and -1 represents the pedal fully pressed to move to backward. The track direction will be inverted if the vehicle current gear is rear. + /// + /// The value (-1,1 range). + API_FUNCTION() void SetTankLeftThrottle(float value); + + /// + /// Sets the input for tank right track throttle. It is the analog accelerator pedal value in range (-1,1) where 1 represents the pedal fully pressed to move to forward, 0 to represents the + /// pedal in its rest state and -1 represents the pedal fully pressed to move to backward. The track direction will be inverted if the vehicle current gear is rear. + /// + /// The value (-1,1 range). + API_FUNCTION() void SetTankRightThrottle(float value); + + /// + /// Sets the input for tank brakes the left track. Brake is the analog brake pedal value in range (0,1) where 1 represents the pedal fully pressed and 0 represents the pedal in its rest state. + /// + /// The value (0,1 range). + API_FUNCTION() void SetTankLeftBrake(float value); + + /// + /// Sets the input for tank brakes the right track. Brake is the analog brake pedal value in range (0,1) where 1 represents the pedal fully pressed and 0 represents the pedal in its rest state. + /// + /// The value (0,1 range). + API_FUNCTION() void SetTankRightBrake(float value); + /// /// Clears all the vehicle control inputs to the default values (throttle, steering, breaks). /// diff --git a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp index cbc381142..99bc093fc 100644 --- a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp +++ b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp @@ -37,6 +37,7 @@ #include #include #include +#include #include #include #endif @@ -961,7 +962,6 @@ void PhysicalMaterial::UpdatePhysicsMaterial() bool CollisionCooking::CookConvexMesh(CookingInput& input, BytesContainer& output) { - PROFILE_CPU(); ENSURE_CAN_COOK; if (input.VertexCount == 0) LOG(Warning, "Empty mesh data for collision cooking."); @@ -1005,7 +1005,6 @@ bool CollisionCooking::CookConvexMesh(CookingInput& input, BytesContainer& outpu bool CollisionCooking::CookTriangleMesh(CookingInput& input, BytesContainer& output) { - PROFILE_CPU(); ENSURE_CAN_COOK; if (input.VertexCount == 0 || input.IndexCount == 0) LOG(Warning, "Empty mesh data for collision cooking."); @@ -1040,7 +1039,6 @@ bool CollisionCooking::CookTriangleMesh(CookingInput& input, BytesContainer& out bool CollisionCooking::CookHeightField(int32 cols, int32 rows, const PhysicsBackend::HeightFieldSample* data, WriteStream& stream) { - PROFILE_CPU(); ENSURE_CAN_COOK; PxHeightFieldDesc heightFieldDesc; @@ -1386,62 +1384,172 @@ void PhysicsBackend::EndSimulateScene(void* scene) WheelVehiclesCache.Add(drive); wheelsCount += drive->mWheelsSimData.getNbWheels(); + const float deadZone = 0.1f; + bool isTank = wheelVehicle->_driveType == WheeledVehicle::DriveTypes::Tank; float throttle = wheelVehicle->_throttle; + float steering = wheelVehicle->_steering; float brake = wheelVehicle->_brake; + float leftThrottle = wheelVehicle->_tankLeftThrottle; + float rightThrottle = wheelVehicle->_tankRightThrottle; + float leftBrake = Math::Max(wheelVehicle->_tankLeftBrake, wheelVehicle->_handBrake); + float rightBrake = Math::Max(wheelVehicle->_tankRightBrake, wheelVehicle->_handBrake); + + WheeledVehicle::DriveModes vehicleDriveMode = wheelVehicle->_driveMode; + + if (isTank) + { + // Converting default vehicle controls to tank controls. + if (throttle != 0 || steering != 0) + { + leftThrottle = throttle + steering; + rightThrottle = throttle - steering; + } + } + + // Converting special tank drive mode to standard tank mode when is turning. + if (isTank && vehicleDriveMode == WheeledVehicle::DriveModes::Standard) + { + // Special inputs when turning vehicle -1 1 to left or 1 -1 to turn right + // to: + // Standard inputs when turning vehicle 0 1 to left or 1 0 to turn right + + if (leftThrottle < -deadZone && rightThrottle > deadZone) + { + leftThrottle = 0; + } + else if (leftThrottle > deadZone && rightThrottle < -deadZone) + { + rightThrottle = 0; + } + } + if (wheelVehicle->UseReverseAsBrake) { const float invalidDirectionThreshold = 80.0f; const float breakThreshold = 8.0f; const float forwardSpeed = wheelVehicle->GetForwardSpeed(); + int currentGear = wheelVehicle->GetCurrentGear(); + // Tank tracks direction: 1 forward -1 backward 0 neutral + bool toForward = false; + toForward |= throttle > deadZone; + toForward |= (leftThrottle > deadZone) && (rightThrottle > deadZone); // 1 1 + bool toBackward = false; + toBackward |= throttle < -deadZone; + toBackward |= (leftThrottle < -deadZone) && (rightThrottle < -deadZone); // -1 -1 + toBackward |= (leftThrottle < -deadZone) && (rightThrottle < deadZone); // -1 0 + toBackward |= (leftThrottle < deadZone) && (rightThrottle < -deadZone); // 0 -1 + + bool isTankTurning = false; + + if (isTank) + { + isTankTurning |= leftThrottle > deadZone && rightThrottle < -deadZone; // 1 -1 + isTankTurning |= leftThrottle < -deadZone && rightThrottle > deadZone; // -1 1 + isTankTurning |= leftThrottle < deadZone && rightThrottle > deadZone; // 0 1 + isTankTurning |= leftThrottle > deadZone && rightThrottle < deadZone; // 1 0 + isTankTurning |= leftThrottle < -deadZone && rightThrottle < deadZone; // -1 0 + isTankTurning |= leftThrottle < deadZone && rightThrottle < -deadZone; // 0 -1 + + if (toForward || toBackward) + { + isTankTurning = false; + } + } + // Automatic gear change when changing driving direction if (Math::Abs(forwardSpeed) < invalidDirectionThreshold) { - if (throttle < -ZeroTolerance && wheelVehicle->GetCurrentGear() >= 0 && wheelVehicle->GetTargetGear() >= 0) + int targetGear = wheelVehicle->GetTargetGear(); + if (toBackward && currentGear > 0 && targetGear >= 0) { - wheelVehicle->SetCurrentGear(-1); + currentGear = -1; } - else if (throttle > ZeroTolerance && wheelVehicle->GetCurrentGear() <= 0 && wheelVehicle->GetTargetGear() <= 0) + else if (!toBackward && currentGear <= 0 && targetGear <= 0) { - wheelVehicle->SetCurrentGear(1); + currentGear = 1; + } + else if (isTankTurning && currentGear <= 0) + { + currentGear = 1; + } + + if (wheelVehicle->GetCurrentGear() != currentGear) + { + wheelVehicle->SetCurrentGear(currentGear); } } // Automatic break when changing driving direction - if (throttle > 0.0f) + if (toForward) { if (forwardSpeed < -invalidDirectionThreshold) { brake = 1.0f; + leftBrake = 1.0f; + rightBrake = 1.0f; } } - else if (throttle < 0.0f) + else if (toBackward) { if (forwardSpeed > invalidDirectionThreshold) { brake = 1.0f; + leftBrake = 1.0f; + rightBrake = 1.0f; } } else { - if (forwardSpeed < breakThreshold && forwardSpeed > -breakThreshold) + if (forwardSpeed < breakThreshold && forwardSpeed > -breakThreshold && !isTankTurning) // not accelerating, very slow speed -> stop { brake = 1.0f; + leftBrake = 1.0f; + rightBrake = 1.0f; } } // Block throttle if user is changing driving direction - if ((throttle > 0.0f && wheelVehicle->GetTargetGear() < 0) || (throttle < 0.0f && wheelVehicle->GetTargetGear() > 0)) + if ((toForward && currentGear < 0) || (toBackward && currentGear > 0)) { throttle = 0.0f; + leftThrottle = 0; + rightThrottle = 0; } throttle = Math::Abs(throttle); + + if (isTank) + { + // invert acceleration when moving to backward because tank inputs can be < 0 + if (currentGear < 0) + { + float lt = -leftThrottle; + float rt = -rightThrottle; + float lb = leftBrake; + float rb = rightBrake; + leftThrottle = rt; + rightThrottle = lt; + leftBrake = rb; + rightBrake = lb; + } + } } else { throttle = Math::Max(throttle, 0.0f); } + + // Force brake the another side track to turn more faster. + if (Math::Abs(leftThrottle) > deadZone && Math::Abs(rightThrottle) < deadZone) + { + rightBrake = 1.0f; + } + if (Math::Abs(rightThrottle) > deadZone && Math::Abs(leftThrottle) < deadZone) + { + leftBrake = 1.0f; + } + // @formatter:off // Reference: PhysX SDK docs // TODO: expose input control smoothing data @@ -1518,11 +1626,23 @@ void PhysicsBackend::EndSimulateScene(void* scene) PxVehicleDriveNWSmoothAnalogRawInputsAndSetAnalogInputs(padSmoothing, steerVsForwardSpeed, rawInputData, scenePhysX->LastDeltaTime, false, *(PxVehicleDriveNW*)drive); break; } + case WheeledVehicle::DriveTypes::Tank: + { + PxVehicleDriveTankRawInputData driveMode = vehicleDriveMode == WheeledVehicle::DriveModes::Standard ? PxVehicleDriveTankControlModel::eSTANDARD : PxVehicleDriveTankControlModel::eSPECIAL; + PxVehicleDriveTankRawInputData rawInputData = PxVehicleDriveTankRawInputData(driveMode); + rawInputData.setAnalogAccel(Math::Max(Math::Abs(leftThrottle), Math::Abs(rightThrottle))); + rawInputData.setAnalogLeftBrake(leftBrake); + rawInputData.setAnalogRightBrake(rightBrake); + rawInputData.setAnalogLeftThrust(leftThrottle); + rawInputData.setAnalogRightThrust(rightThrottle); + + PxVehicleDriveTankSmoothAnalogRawInputsAndSetAnalogInputs(padSmoothing, rawInputData, scenePhysX->LastDeltaTime, *(PxVehicleDriveTank*)drive); + break; + } } } else - { - const float deadZone = 0.1f; + { switch (wheelVehicle->_driveTypeCurrent) { case WheeledVehicle::DriveTypes::Drive4W: @@ -1547,6 +1667,26 @@ void PhysicsBackend::EndSimulateScene(void* scene) PxVehicleDriveNWSmoothDigitalRawInputsAndSetAnalogInputs(keySmoothing, steerVsForwardSpeed, rawInputData, scenePhysX->LastDeltaTime, false, *(PxVehicleDriveNW*)drive); break; } + case WheeledVehicle::DriveTypes::Tank: + { + // Convert analogic inputs to digital inputs. + leftThrottle = Math::Round(leftThrottle); + rightThrottle = Math::Round(rightThrottle); + leftBrake = Math::Round(leftBrake); + rightBrake = Math::Round(rightBrake); + + PxVehicleDriveTankRawInputData driveMode = vehicleDriveMode == WheeledVehicle::DriveModes::Standard ? PxVehicleDriveTankControlModel::eSTANDARD : PxVehicleDriveTankControlModel::eSPECIAL; + PxVehicleDriveTankRawInputData rawInputData = PxVehicleDriveTankRawInputData(driveMode); + rawInputData.setAnalogAccel(Math::Max(Math::Abs(leftThrottle), Math::Abs(rightThrottle))); + rawInputData.setAnalogLeftBrake(leftBrake); + rawInputData.setAnalogRightBrake(rightBrake); + rawInputData.setAnalogLeftThrust(leftThrottle); + rawInputData.setAnalogRightThrust(rightThrottle); + + // Needs to pass analogic values to vehicle to maintein current moviment direction because digital inputs accept only true/false values to tracks thrust instead of -1 to 1 + PxVehicleDriveTankSmoothAnalogRawInputsAndSetAnalogInputs(padSmoothing, rawInputData, scenePhysX->LastDeltaTime, *(PxVehicleDriveTank *)drive); + break; + } } } } @@ -2803,7 +2943,7 @@ void PhysicsBackend::SetHingeJointLimit(void* joint, const LimitAngularRange& va void PhysicsBackend::SetHingeJointDrive(void* joint, const HingeJointDrive& value) { auto jointPhysX = (PxRevoluteJoint*)joint; - jointPhysX->setDriveVelocity(value.Velocity); + jointPhysX->setDriveVelocity(Math::Max(value.Velocity, 0.0f)); jointPhysX->setDriveForceLimit(Math::Max(value.ForceLimit, 0.0f)); jointPhysX->setDriveGearRatio(Math::Max(value.GearRatio, 0.0f)); jointPhysX->setRevoluteJointFlag(PxRevoluteJointFlag::eDRIVE_FREESPIN, value.FreeSpin); @@ -3086,6 +3226,147 @@ int32 PhysicsBackend::MoveController(void* controller, void* shape, const Vector #if WITH_VEHICLE +PxVehicleDifferential4WData CreatePxVehicleDifferential4WData(const WheeledVehicle::DifferentialSettings& settings) +{ + PxVehicleDifferential4WData differential4WData; + differential4WData.mType = (PxVehicleDifferential4WData::Enum)settings.Type; + differential4WData.mFrontRearSplit = settings.FrontRearSplit; + differential4WData.mFrontLeftRightSplit = settings.FrontLeftRightSplit; + differential4WData.mRearLeftRightSplit = settings.RearLeftRightSplit; + differential4WData.mCentreBias = settings.CentreBias; + differential4WData.mFrontBias = settings.FrontBias; + differential4WData.mRearBias = settings.RearBias; + return differential4WData; +} + +PxVehicleDifferentialNWData CreatePxVehicleDifferentialNWData(const WheeledVehicle::DifferentialSettings& settings, const Array>& wheels) +{ + PxVehicleDifferentialNWData differentialNwData; + for (int32 i = 0; i < wheels.Count(); i++) + differentialNwData.setDrivenWheel(i, true); + + return differentialNwData; +} + +PxVehicleEngineData CreatePxVehicleEngineData(const WheeledVehicle::EngineSettings& settings) +{ + PxVehicleEngineData engineData; + engineData.mMOI = M2ToCm2(settings.MOI); + engineData.mPeakTorque = M2ToCm2(settings.MaxTorque); + engineData.mMaxOmega = RpmToRadPerS(settings.MaxRotationSpeed); + engineData.mDampingRateFullThrottle = M2ToCm2(0.15f); + engineData.mDampingRateZeroThrottleClutchEngaged = M2ToCm2(2.0f); + engineData.mDampingRateZeroThrottleClutchDisengaged = M2ToCm2(0.35f); + return engineData; +} + +PxVehicleGearsData CreatePxVehicleGearsData(const WheeledVehicle::GearboxSettings& settings) +{ + PxVehicleGearsData gears; + + // Total gears is forward gears + neutral/rear gears + int gearsAmount = settings.ForwardGearsRatios + 2; + + // Setup gears torque/top speed relations + // Higher torque = less speed + // Low torque = high speed + + // Example: + // ForwardGearsRatios = 4 + // GearRev = -5 + // Gear0 = 0 + // Gear1 = 4.2 + // Gear2 = 3.4 + // Gear3 = 2.6 + // Gear4 = 1.8 + // Gear5 = 1 + + gears.mRatios[0] = -(gearsAmount - 2); // reverse + gears.mRatios[1] = 0; // neutral + + // Setup all gears except neutral and reverse + for (int i = gearsAmount; i > 2; i--) { + + float gearsRatios = settings.ForwardGearsRatios; + float currentGear = i - 2; + + gears.mRatios[i] = Math::Lerp(gearsRatios, 1.0f, (currentGear / gearsRatios)); + } + + // reset unused gears + for (int i = gearsAmount; i < PxVehicleGearsData::eGEARSRATIO_COUNT; i++) + gears.mRatios[i] = 0; + + gears.mSwitchTime = Math::Max(settings.SwitchTime, 0.0f); + gears.mNbRatios = Math::Clamp(gearsAmount, 2, (int)PxVehicleGearsData::eGEARSRATIO_COUNT); + return gears; +} + +PxVehicleAutoBoxData CreatePxVehicleAutoBoxData() +{ + return PxVehicleAutoBoxData(); +} + +PxVehicleClutchData CreatePxVehicleClutchData(const WheeledVehicle::GearboxSettings& settings) +{ + PxVehicleClutchData clutch; + clutch.mStrength = M2ToCm2(settings.ClutchStrength); + return clutch; +} + +PxVehicleSuspensionData CreatePxVehicleSuspensionData(const WheeledVehicle::Wheel& settings, const PxReal wheelSprungMass) +{ + PxVehicleSuspensionData suspensionData; + const float suspensionFrequency = 7.0f; + suspensionData.mMaxCompression = settings.SuspensionMaxRaise; + suspensionData.mMaxDroop = settings.SuspensionMaxDrop; + suspensionData.mSprungMass = wheelSprungMass; + suspensionData.mSpringStrength = Math::Square(suspensionFrequency) * suspensionData.mSprungMass; + suspensionData.mSpringDamperRate = settings.SuspensionDampingRate * 2.0f * Math::Sqrt(suspensionData.mSpringStrength * suspensionData.mSprungMass); + return suspensionData; +} + +PxVehicleTireData CreatePxVehicleTireData(const WheeledVehicle::Wheel& settings) +{ + PxVehicleTireData tire; + int32 tireIndex = WheelTireTypes.Find(settings.TireFrictionScale); + if (tireIndex == -1) + { + // New tire type + tireIndex = WheelTireTypes.Count(); + WheelTireTypes.Add(settings.TireFrictionScale); + WheelTireFrictionsDirty = true; + } + tire.mType = tireIndex; + tire.mLatStiffX = settings.TireLateralMax; + tire.mLatStiffY = settings.TireLateralStiffness; + tire.mLongitudinalStiffnessPerUnitGravity = settings.TireLongitudinalStiffness; + return tire; +} + +PxVehicleWheelData CreatePxVehicleWheelData(const WheeledVehicle::Wheel& settings) +{ + PxVehicleWheelData wheelData; + wheelData.mMass = settings.Mass; + wheelData.mRadius = settings.Radius; + wheelData.mWidth = settings.Width; + wheelData.mMOI = 0.5f * wheelData.mMass * Math::Square(wheelData.mRadius); + wheelData.mDampingRate = M2ToCm2(settings.DampingRate); + wheelData.mMaxSteer = settings.MaxSteerAngle * DegreesToRadians; + wheelData.mMaxBrakeTorque = M2ToCm2(settings.MaxBrakeTorque); + wheelData.mMaxHandBrakeTorque = M2ToCm2(settings.MaxHandBrakeTorque); + return wheelData; +} + +PxVehicleAckermannGeometryData CreatePxVehicleAckermannGeometryData(PxVehicleWheelsSimData* wheelsSimData) +{ + PxVehicleAckermannGeometryData ackermann; + ackermann.mAxleSeparation = Math::Abs(wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eFRONT_LEFT).z - wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eREAR_LEFT).z); + ackermann.mFrontWidth = Math::Abs(wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eFRONT_RIGHT).x - wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eFRONT_LEFT).x); + ackermann.mRearWidth = Math::Abs(wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eREAR_RIGHT).x - wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eREAR_LEFT).x); + return ackermann; +} + bool SortWheels(WheeledVehicle::Wheel const& a, WheeledVehicle::Wheel const& b) { return (int32)a.Type < (int32)b.Type; @@ -3158,42 +3439,14 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) data.Collider = wheel.Collider; data.LocalOrientation = wheel.Collider->GetLocalOrientation(); - PxVehicleSuspensionData suspensionData; - const float suspensionFrequency = 7.0f; - suspensionData.mMaxCompression = wheel.SuspensionMaxRaise; - suspensionData.mMaxDroop = wheel.SuspensionMaxDrop; - suspensionData.mSprungMass = sprungMasses[i]; - suspensionData.mSpringStrength = Math::Square(suspensionFrequency) * suspensionData.mSprungMass; - suspensionData.mSpringDamperRate = wheel.SuspensionDampingRate * 2.0f * Math::Sqrt(suspensionData.mSpringStrength * suspensionData.mSprungMass); - - PxVehicleTireData tire; - int32 tireIndex = WheelTireTypes.Find(wheel.TireFrictionScale); - if (tireIndex == -1) - { - // New tire type - tireIndex = WheelTireTypes.Count(); - WheelTireTypes.Add(wheel.TireFrictionScale); - WheelTireFrictionsDirty = true; - } - tire.mType = tireIndex; - tire.mLatStiffX = wheel.TireLateralMax; - tire.mLatStiffY = wheel.TireLateralStiffness; - tire.mLongitudinalStiffnessPerUnitGravity = wheel.TireLongitudinalStiffness; - - PxVehicleWheelData wheelData; - wheelData.mMass = wheel.Mass; - wheelData.mRadius = wheel.Radius; - wheelData.mWidth = wheel.Width; - wheelData.mMOI = 0.5f * wheelData.mMass * Math::Square(wheelData.mRadius); - wheelData.mDampingRate = M2ToCm2(wheel.DampingRate); - wheelData.mMaxSteer = wheel.MaxSteerAngle * DegreesToRadians; - wheelData.mMaxBrakeTorque = M2ToCm2(wheel.MaxBrakeTorque); - wheelData.mMaxHandBrakeTorque = M2ToCm2(wheel.MaxHandBrakeTorque); - PxVec3 centreOffset = centerOfMassOffset.transformInv(offsets[i]); PxVec3 forceAppPointOffset(centreOffset.x, wheel.SuspensionForceOffset, centreOffset.z); - wheelsSimData->setTireData(i, tire); + const PxVehicleTireData& tireData = CreatePxVehicleTireData(wheel); + const PxVehicleWheelData& wheelData = CreatePxVehicleWheelData(wheel); + const PxVehicleSuspensionData& suspensionData = CreatePxVehicleSuspensionData(wheel, sprungMasses[i]); + + wheelsSimData->setTireData(i,tireData); wheelsSimData->setWheelData(i, wheelData); wheelsSimData->setSuspensionData(i, suspensionData); wheelsSimData->setSuspTravelDirection(i, centerOfMassOffset.rotate(PxVec3(0.0f, -1.0f, 0.0f))); @@ -3260,48 +3513,19 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) case WheeledVehicle::DriveTypes::Drive4W: { PxVehicleDriveSimData4W driveSimData; + const PxVehicleDifferential4WData& differentialData = CreatePxVehicleDifferential4WData(differential); + const PxVehicleEngineData& engineData = CreatePxVehicleEngineData(engine); + const PxVehicleGearsData& gearsData = CreatePxVehicleGearsData(gearbox); + const PxVehicleAutoBoxData& autoBoxData = CreatePxVehicleAutoBoxData(); + const PxVehicleClutchData& cluchData = CreatePxVehicleClutchData(gearbox); + const PxVehicleAckermannGeometryData& geometryData = CreatePxVehicleAckermannGeometryData(wheelsSimData); - // Differential - PxVehicleDifferential4WData differential4WData; - differential4WData.mType = (PxVehicleDifferential4WData::Enum)differential.Type; - differential4WData.mFrontRearSplit = differential.FrontRearSplit; - differential4WData.mFrontLeftRightSplit = differential.FrontLeftRightSplit; - differential4WData.mRearLeftRightSplit = differential.RearLeftRightSplit; - differential4WData.mCentreBias = differential.CentreBias; - differential4WData.mFrontBias = differential.FrontBias; - differential4WData.mRearBias = differential.RearBias; - driveSimData.setDiffData(differential4WData); - - // Engine - PxVehicleEngineData engineData; - engineData.mMOI = M2ToCm2(engine.MOI); - engineData.mPeakTorque = M2ToCm2(engine.MaxTorque); - engineData.mMaxOmega = RpmToRadPerS(engine.MaxRotationSpeed); - engineData.mDampingRateFullThrottle = M2ToCm2(0.15f); - engineData.mDampingRateZeroThrottleClutchEngaged = M2ToCm2(2.0f); - engineData.mDampingRateZeroThrottleClutchDisengaged = M2ToCm2(0.35f); + driveSimData.setDiffData(differentialData); driveSimData.setEngineData(engineData); - - // Gears - PxVehicleGearsData gears; - gears.mSwitchTime = Math::Max(gearbox.SwitchTime, 0.0f); - driveSimData.setGearsData(gears); - - // Auto Box - PxVehicleAutoBoxData autoBox; - driveSimData.setAutoBoxData(autoBox); - - // Clutch - PxVehicleClutchData clutch; - clutch.mStrength = M2ToCm2(gearbox.ClutchStrength); - driveSimData.setClutchData(clutch); - - // Ackermann steer accuracy - PxVehicleAckermannGeometryData ackermann; - ackermann.mAxleSeparation = Math::Abs(wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eFRONT_LEFT).z - wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eREAR_LEFT).z); - ackermann.mFrontWidth = Math::Abs(wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eFRONT_RIGHT).x - wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eFRONT_LEFT).x); - ackermann.mRearWidth = Math::Abs(wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eREAR_RIGHT).x - wheelsSimData->getWheelCentreOffset(PxVehicleDrive4WWheelOrder::eREAR_LEFT).x); - driveSimData.setAckermannGeometryData(ackermann); + driveSimData.setGearsData(gearsData); + driveSimData.setAutoBoxData(autoBoxData); + driveSimData.setClutchData(cluchData); + driveSimData.setAckermannGeometryData(geometryData); // Create vehicle drive auto drive4W = PxVehicleDrive4W::allocate(wheels.Count()); @@ -3315,36 +3539,18 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) case WheeledVehicle::DriveTypes::DriveNW: { PxVehicleDriveSimDataNW driveSimData; + const PxVehicleDifferentialNWData& differentialData = CreatePxVehicleDifferentialNWData(differential, wheels); + const PxVehicleEngineData& engineData = CreatePxVehicleEngineData(engine); + const PxVehicleGearsData& gearsData = CreatePxVehicleGearsData(gearbox); + const PxVehicleAutoBoxData& autoBoxData = CreatePxVehicleAutoBoxData(); + const PxVehicleClutchData& cluchData = CreatePxVehicleClutchData(gearbox); + const PxVehicleAckermannGeometryData& geometryData = CreatePxVehicleAckermannGeometryData(wheelsSimData); - // Differential - PxVehicleDifferentialNWData differentialNwData; - for (int32 i = 0; i < wheels.Count(); i++) - differentialNwData.setDrivenWheel(i, true); - driveSimData.setDiffData(differentialNwData); - - // Engine - PxVehicleEngineData engineData; - engineData.mMOI = M2ToCm2(engine.MOI); - engineData.mPeakTorque = M2ToCm2(engine.MaxTorque); - engineData.mMaxOmega = RpmToRadPerS(engine.MaxRotationSpeed); - engineData.mDampingRateFullThrottle = M2ToCm2(0.15f); - engineData.mDampingRateZeroThrottleClutchEngaged = M2ToCm2(2.0f); - engineData.mDampingRateZeroThrottleClutchDisengaged = M2ToCm2(0.35f); + driveSimData.setDiffData(differentialData); driveSimData.setEngineData(engineData); - - // Gears - PxVehicleGearsData gears; - gears.mSwitchTime = Math::Max(gearbox.SwitchTime, 0.0f); - driveSimData.setGearsData(gears); - - // Auto Box - PxVehicleAutoBoxData autoBox; - driveSimData.setAutoBoxData(autoBox); - - // Clutch - PxVehicleClutchData clutch; - clutch.mStrength = M2ToCm2(gearbox.ClutchStrength); - driveSimData.setClutchData(clutch); + driveSimData.setGearsData(gearsData); + driveSimData.setAutoBoxData(autoBoxData); + driveSimData.setClutchData(cluchData); // Create vehicle drive auto driveNW = PxVehicleDriveNW::allocate(wheels.Count()); @@ -3364,6 +3570,32 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) vehicle = driveNo; break; } + case WheeledVehicle::DriveTypes::Tank: + { + PxVehicleDriveSimData4W driveSimData; + const PxVehicleDifferential4WData &differentialData = CreatePxVehicleDifferential4WData(differential); + const PxVehicleEngineData &engineData = CreatePxVehicleEngineData(engine); + const PxVehicleGearsData &gearsData = CreatePxVehicleGearsData(gearbox); + const PxVehicleAutoBoxData &autoBoxData = CreatePxVehicleAutoBoxData(); + const PxVehicleClutchData &cluchData = CreatePxVehicleClutchData(gearbox); + const PxVehicleAckermannGeometryData &geometryData = CreatePxVehicleAckermannGeometryData(wheelsSimData); + + driveSimData.setDiffData(differentialData); + driveSimData.setEngineData(engineData); + driveSimData.setGearsData(gearsData); + driveSimData.setAutoBoxData(autoBoxData); + driveSimData.setClutchData(cluchData); + driveSimData.setAckermannGeometryData(geometryData); + + // Create vehicle drive + auto driveTank = PxVehicleDriveTank::allocate(wheels.Count()); + driveTank->setup(PhysX, actorPhysX, *wheelsSimData, driveSimData, wheels.Count()); + driveTank->setToRestState(); + driveTank->mDriveDynData.forceGearChange(PxVehicleGearsData::eFIRST); + driveTank->mDriveDynData.setUseAutoGears(gearbox.AutoGear); + vehicle = driveTank; + break; + } default: CRASH; } @@ -3385,6 +3617,9 @@ void PhysicsBackend::DestroyVehicle(void* vehicle, int32 driveType) case WheeledVehicle::DriveTypes::NoDrive: ((PxVehicleNoDrive*)vehicle)->free(); break; + case WheeledVehicle::DriveTypes::Tank: + ((PxVehicleDriveTank*)vehicle)->free(); + break; } } @@ -3395,42 +3630,11 @@ void PhysicsBackend::UpdateVehicleWheels(WheeledVehicle* actor) for (uint32 i = 0; i < wheelsSimData->getNbWheels(); i++) { auto& wheel = actor->_wheels[i]; - - // Update suspension data - PxVehicleSuspensionData suspensionData = wheelsSimData->getSuspensionData(i); - const float suspensionFrequency = 7.0f; - suspensionData.mMaxCompression = wheel.SuspensionMaxRaise; - suspensionData.mMaxDroop = wheel.SuspensionMaxDrop; - suspensionData.mSpringStrength = Math::Square(suspensionFrequency) * suspensionData.mSprungMass; - suspensionData.mSpringDamperRate = wheel.SuspensionDampingRate * 2.0f * Math::Sqrt(suspensionData.mSpringStrength * suspensionData.mSprungMass); + const PxVehicleSuspensionData& suspensionData = CreatePxVehicleSuspensionData(wheel, wheelsSimData->getSuspensionData(i).mSprungMass); + const PxVehicleTireData& tireData = CreatePxVehicleTireData(wheel); + const PxVehicleWheelData& wheelData = CreatePxVehicleWheelData(wheel); wheelsSimData->setSuspensionData(i, suspensionData); - - // Update tire data - PxVehicleTireData tire; - int32 tireIndex = WheelTireTypes.Find(wheel.TireFrictionScale); - if (tireIndex == -1) - { - // New tire type - tireIndex = WheelTireTypes.Count(); - WheelTireTypes.Add(wheel.TireFrictionScale); - WheelTireFrictionsDirty = true; - } - tire.mType = tireIndex; - tire.mLatStiffX = wheel.TireLateralMax; - tire.mLatStiffY = wheel.TireLateralStiffness; - tire.mLongitudinalStiffnessPerUnitGravity = wheel.TireLongitudinalStiffness; - wheelsSimData->setTireData(i, tire); - - // Update wheel data - PxVehicleWheelData wheelData; - wheelData.mMass = wheel.Mass; - wheelData.mRadius = wheel.Radius; - wheelData.mWidth = wheel.Width; - wheelData.mMOI = 0.5f * wheelData.mMass * Math::Square(wheelData.mRadius); - wheelData.mDampingRate = M2ToCm2(wheel.DampingRate); - wheelData.mMaxSteer = wheel.MaxSteerAngle * DegreesToRadians; - wheelData.mMaxBrakeTorque = M2ToCm2(wheel.MaxBrakeTorque); - wheelData.mMaxHandBrakeTorque = M2ToCm2(wheel.MaxHandBrakeTorque); + wheelsSimData->setTireData(i, tireData); wheelsSimData->setWheelData(i, wheelData); } } @@ -3444,28 +3648,24 @@ void PhysicsBackend::SetVehicleEngine(void* vehicle, const void* value) case PxVehicleTypes::eDRIVE4W: { auto drive4W = (PxVehicleDrive4W*)drive; + const PxVehicleEngineData& engineData = CreatePxVehicleEngineData(engine); PxVehicleDriveSimData4W& driveSimData = drive4W->mDriveSimData; - PxVehicleEngineData engineData; - engineData.mMOI = M2ToCm2(engine.MOI); - engineData.mPeakTorque = M2ToCm2(engine.MaxTorque); - engineData.mMaxOmega = RpmToRadPerS(engine.MaxRotationSpeed); - engineData.mDampingRateFullThrottle = M2ToCm2(0.15f); - engineData.mDampingRateZeroThrottleClutchEngaged = M2ToCm2(2.0f); - engineData.mDampingRateZeroThrottleClutchDisengaged = M2ToCm2(0.35f); driveSimData.setEngineData(engineData); break; } case PxVehicleTypes::eDRIVENW: { auto drive4W = (PxVehicleDriveNW*)drive; + const PxVehicleEngineData& engineData = CreatePxVehicleEngineData(engine); PxVehicleDriveSimDataNW& driveSimData = drive4W->mDriveSimData; - PxVehicleEngineData engineData; - engineData.mMOI = M2ToCm2(engine.MOI); - engineData.mPeakTorque = M2ToCm2(engine.MaxTorque); - engineData.mMaxOmega = RpmToRadPerS(engine.MaxRotationSpeed); - engineData.mDampingRateFullThrottle = M2ToCm2(0.15f); - engineData.mDampingRateZeroThrottleClutchEngaged = M2ToCm2(2.0f); - engineData.mDampingRateZeroThrottleClutchDisengaged = M2ToCm2(0.35f); + driveSimData.setEngineData(engineData); + break; + } + case PxVehicleTypes::eDRIVETANK: + { + auto driveTank = (PxVehicleDriveTank*)drive; + const PxVehicleEngineData &engineData = CreatePxVehicleEngineData(engine); + PxVehicleDriveSimData &driveSimData = driveTank->mDriveSimData; driveSimData.setEngineData(engineData); break; } @@ -3481,16 +3681,9 @@ void PhysicsBackend::SetVehicleDifferential(void* vehicle, const void* value) case PxVehicleTypes::eDRIVE4W: { auto drive4W = (PxVehicleDrive4W*)drive; + const PxVehicleDifferential4WData& differentialData = CreatePxVehicleDifferential4WData(differential); PxVehicleDriveSimData4W& driveSimData = drive4W->mDriveSimData; - PxVehicleDifferential4WData differential4WData; - differential4WData.mType = (PxVehicleDifferential4WData::Enum)differential.Type; - differential4WData.mFrontRearSplit = differential.FrontRearSplit; - differential4WData.mFrontLeftRightSplit = differential.FrontLeftRightSplit; - differential4WData.mRearLeftRightSplit = differential.RearLeftRightSplit; - differential4WData.mCentreBias = differential.CentreBias; - differential4WData.mFrontBias = differential.FrontBias; - differential4WData.mRearBias = differential.RearBias; - driveSimData.setDiffData(differential4WData); + driveSimData.setDiffData(differentialData); break; } } @@ -3507,41 +3700,37 @@ void PhysicsBackend::SetVehicleGearbox(void* vehicle, const void* value) case PxVehicleTypes::eDRIVE4W: { auto drive4W = (PxVehicleDrive4W*)drive; + const PxVehicleGearsData& gearData = CreatePxVehicleGearsData(gearbox); + const PxVehicleClutchData& clutchData = CreatePxVehicleClutchData(gearbox); + const PxVehicleAutoBoxData& autoBoxData = CreatePxVehicleAutoBoxData(); PxVehicleDriveSimData4W& driveSimData = drive4W->mDriveSimData; - - // Gears - PxVehicleGearsData gears; - gears.mSwitchTime = Math::Max(gearbox.SwitchTime, 0.0f); - driveSimData.setGearsData(gears); - - // Auto Box - PxVehicleAutoBoxData autoBox; - driveSimData.setAutoBoxData(autoBox); - - // Clutch - PxVehicleClutchData clutch; - clutch.mStrength = M2ToCm2(gearbox.ClutchStrength); - driveSimData.setClutchData(clutch); + driveSimData.setGearsData(gearData); + driveSimData.setAutoBoxData(autoBoxData); + driveSimData.setClutchData(clutchData); break; } case PxVehicleTypes::eDRIVENW: { auto drive4W = (PxVehicleDriveNW*)drive; + const PxVehicleGearsData& gearData = CreatePxVehicleGearsData(gearbox); + const PxVehicleClutchData& clutchData = CreatePxVehicleClutchData(gearbox); + const PxVehicleAutoBoxData& autoBoxData = CreatePxVehicleAutoBoxData(); PxVehicleDriveSimDataNW& driveSimData = drive4W->mDriveSimData; - - // Gears - PxVehicleGearsData gears; - gears.mSwitchTime = Math::Max(gearbox.SwitchTime, 0.0f); - driveSimData.setGearsData(gears); - - // Auto Box - PxVehicleAutoBoxData autoBox; - driveSimData.setAutoBoxData(autoBox); - - // Clutch - PxVehicleClutchData clutch; - clutch.mStrength = M2ToCm2(gearbox.ClutchStrength); - driveSimData.setClutchData(clutch); + driveSimData.setGearsData(gearData); + driveSimData.setAutoBoxData(autoBoxData); + driveSimData.setClutchData(clutchData); + break; + } + case PxVehicleTypes::eDRIVETANK: + { + auto driveTank = (PxVehicleDriveTank *)drive; + const PxVehicleGearsData &gearData = CreatePxVehicleGearsData(gearbox); + const PxVehicleClutchData &clutchData = CreatePxVehicleClutchData(gearbox); + const PxVehicleAutoBoxData &autoBoxData = CreatePxVehicleAutoBoxData(); + PxVehicleDriveSimData &driveSimData = driveTank->mDriveSimData; + driveSimData.setGearsData(gearData); + driveSimData.setAutoBoxData(autoBoxData); + driveSimData.setClutchData(clutchData); break; } } From 6a2d7e9444244e820ae102ae9eb158abbd7a4b6b Mon Sep 17 00:00:00 2001 From: "Mr. Capybara" Date: Sun, 31 Dec 2023 22:11:30 -0400 Subject: [PATCH 02/15] auto sort wheels based on wheel position --- .../Engine/Physics/Actors/WheeledVehicle.cpp | 7 ++-- Source/Engine/Physics/Actors/WheeledVehicle.h | 22 ---------- .../Physics/PhysX/PhysicsBackendPhysX.cpp | 40 +++++++++++++++---- 3 files changed, 37 insertions(+), 32 deletions(-) diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.cpp b/Source/Engine/Physics/Actors/WheeledVehicle.cpp index 9051c7eed..a68bc23c7 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.cpp +++ b/Source/Engine/Physics/Actors/WheeledVehicle.cpp @@ -59,9 +59,7 @@ void WheeledVehicle::SetWheels(const Array &value) { auto &oldWheel = _wheels.Get()[wheelIndex]; auto &newWheel = value.Get()[wheelIndex]; - if (oldWheel.Type != newWheel.Type || - Math::NotNearEqual(oldWheel.SuspensionForceOffset, newWheel.SuspensionForceOffset) || - oldWheel.Collider != newWheel.Collider) + if (Math::NotNearEqual(oldWheel.SuspensionForceOffset, newWheel.SuspensionForceOffset) || oldWheel.Collider != newWheel.Collider) { softUpdate = false; break; @@ -352,6 +350,9 @@ void WheeledVehicle::OnDebugDrawSelected() // Draw wheels axes if (wheelIndex % 2 == 0 && wheelIndex + 1 < _wheels.Count()) { + if (!_wheels[wheelIndex].Collider || !_wheels[wheelIndex + 1].Collider) + continue; + const Vector3 wheelPos = _wheels[wheelIndex].Collider->GetPosition(); const Vector3 nextWheelPos = _wheels[wheelIndex + 1].Collider->GetPosition(); DEBUG_DRAW_LINE(wheelPos, nextWheelPos, Color::Yellow, 0, false); diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.h b/Source/Engine/Physics/Actors/WheeledVehicle.h index 49c00b17a..acf20310f 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.h +++ b/Source/Engine/Physics/Actors/WheeledVehicle.h @@ -158,23 +158,6 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo API_FIELD(Attributes="Limit(0)") float ClutchStrength = 10.0f; }; - /// - /// Vehicle wheel types. - /// - API_ENUM() enum class WheelTypes - { - // Left wheel of the front axle. - FrontLeft, - // Right wheel of the front axle. - FrontRight, - // Left wheel of the rear axle. - RearLeft, - // Right wheel of the rear axle. - RearRight, - // Non-drivable wheel. - NoDrive, - }; - /// /// Vehicle wheel settings. /// @@ -183,11 +166,6 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo DECLARE_SCRIPTING_TYPE_MINIMAL(Wheel); API_AUTO_SERIALIZATION(); - /// - /// Wheel placement type. - /// - API_FIELD(Attributes="EditorOrder(0)") WheelTypes Type = WheelTypes::FrontLeft; - /// /// Combined mass of the wheel and the tire in kg. Typically, a wheel has mass between 20Kg and 80Kg but can be lower and higher depending on the vehicle. /// diff --git a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp index 99bc093fc..e55b80008 100644 --- a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp +++ b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp @@ -58,7 +58,9 @@ #if WITH_PVD #include #endif - +#if USE_EDITOR +#include "Editor/Editor.h" +#endif // Temporary memory size used by the PhysX during the simulation. Must be multiply of 4kB and 16bit aligned. #define PHYSX_SCRATCH_BLOCK_SIZE (1024 * 128) @@ -3367,18 +3369,42 @@ PxVehicleAckermannGeometryData CreatePxVehicleAckermannGeometryData(PxVehicleWhe return ackermann; } -bool SortWheels(WheeledVehicle::Wheel const& a, WheeledVehicle::Wheel const& b) +bool SortWheelsFrontToBack(WheeledVehicle::Wheel const& a, WheeledVehicle::Wheel const& b) { - return (int32)a.Type < (int32)b.Type; + return a.Collider && b.Collider && a.Collider->GetLocalPosition().Z > b.Collider->GetLocalPosition().Z; } void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) { - // TODO: handle PxVehicleDrive4WWheelOrder internally rather than sorting wheels directly on the vehicle - if (actor->_driveType == WheeledVehicle::DriveTypes::Drive4W) +#if USE_EDITOR + if (Editor::IsPlayMode) +#endif { - // Drive4W requires wheels to match order from PxVehicleDrive4WWheelOrder enum - Sorting::QuickSort(actor->_wheels.Get(), actor->_wheels.Count(), SortWheels); + // Physx vehicles needs to have all wheels sorted to apply controls correctly. + // Its needs to know what wheels are on front to turn wheel to correctly side + // and needs to know wheel side to apply throttle to correctly direction for each track when using tanks. + + if (actor->_driveType == WheeledVehicle::DriveTypes::Drive4W) + Sorting::QuickSort(actor->_wheels.Get(), actor->_wheels.Count(), SortWheelsFrontToBack); + + // sort wheels by side + if (actor->_driveType == WheeledVehicle::DriveTypes::Tank) + { + for (int i = 0; i < actor->_wheels.Count() - 1; i += 2) + { + auto a = actor->_wheels[i]; + auto b = actor->_wheels[i + 1]; + + if (!a.Collider || !b.Collider) + continue; + + if (a.Collider->GetLocalPosition().X > b.Collider->GetLocalPosition().X) + { + actor->_wheels[i] = b; + actor->_wheels[i + 1] = a; + } + } + } } // Get wheels From 967569c3e27b222b7735df2ad145dfab8d41cc2a Mon Sep 17 00:00:00 2001 From: "Mr. Capybara" Date: Sun, 31 Dec 2023 22:43:04 -0400 Subject: [PATCH 03/15] Small fix vehicle tank inputs --- Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp index e55b80008..cb1d094a4 100644 --- a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp +++ b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp @@ -1403,8 +1403,8 @@ void PhysicsBackend::EndSimulateScene(void* scene) // Converting default vehicle controls to tank controls. if (throttle != 0 || steering != 0) { - leftThrottle = throttle + steering; - rightThrottle = throttle - steering; + leftThrottle = Math::Clamp(throttle + steering, -1.0f, 1.0f); + rightThrottle = Math::Clamp(throttle - steering, -1.0f, 1.0f); } } @@ -1418,10 +1418,12 @@ void PhysicsBackend::EndSimulateScene(void* scene) if (leftThrottle < -deadZone && rightThrottle > deadZone) { leftThrottle = 0; + leftBrake = 1; } else if (leftThrottle > deadZone && rightThrottle < -deadZone) { rightThrottle = 0; + rightBrake = 1; } } From 07de7a26dd55fd0aaec3ea637cc7a4ec02f60d41 Mon Sep 17 00:00:00 2001 From: "Mr. Capybara" Date: Mon, 1 Jan 2024 11:15:16 -0400 Subject: [PATCH 04/15] Expose vehicle pad smooth inputs --- .../Engine/Physics/Actors/WheeledVehicle.cpp | 13 ++++ Source/Engine/Physics/Actors/WheeledVehicle.h | 66 ++++++++++++++++++- .../Physics/PhysX/PhysicsBackendPhysX.cpp | 56 ++++++++-------- 3 files changed, 104 insertions(+), 31 deletions(-) diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.cpp b/Source/Engine/Physics/Actors/WheeledVehicle.cpp index a68bc23c7..243d9cc48 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.cpp +++ b/Source/Engine/Physics/Actors/WheeledVehicle.cpp @@ -48,6 +48,16 @@ const Array &WheeledVehicle::GetWheels() const return _wheels; } +WheeledVehicle::DriveControlSettings WheeledVehicle::GetDriveControl() const +{ + return _driveControl; +} + +void WheeledVehicle::SetDriveControl(DriveControlSettings &value) +{ + _driveControl = value; +} + void WheeledVehicle::SetWheels(const Array &value) { #if WITH_VEHICLE @@ -376,11 +386,13 @@ void WheeledVehicle::Serialize(SerializeStream &stream, const void *otherObj) SERIALIZE_MEMBER(DriveType, _driveType); SERIALIZE_MEMBER(DriveModes, _driveMode); SERIALIZE_MEMBER(Wheels, _wheels); + SERIALIZE_MEMBER(DriveControl, _driveControl); SERIALIZE(UseReverseAsBrake); SERIALIZE(UseAnalogSteering); SERIALIZE_MEMBER(Engine, _engine); SERIALIZE_MEMBER(Differential, _differential); SERIALIZE_MEMBER(Gearbox, _gearbox); + } void WheeledVehicle::Deserialize(DeserializeStream &stream, ISerializeModifier *modifier) @@ -390,6 +402,7 @@ void WheeledVehicle::Deserialize(DeserializeStream &stream, ISerializeModifier * DESERIALIZE_MEMBER(DriveType, _driveType); DESERIALIZE_MEMBER(DriveModes, _driveMode); DESERIALIZE_MEMBER(Wheels, _wheels); + DESERIALIZE_MEMBER(DriveControl, _driveControl); DESERIALIZE(UseReverseAsBrake); DESERIALIZE(UseAnalogSteering); DESERIALIZE_MEMBER(Engine, _engine); diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.h b/Source/Engine/Physics/Actors/WheeledVehicle.h index acf20310f..bfa3dc294 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.h +++ b/Source/Engine/Physics/Actors/WheeledVehicle.h @@ -83,6 +83,55 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo OpenRearDrive, }; + /// + /// Vehicle drive control settings. + /// + API_STRUCT() struct DriveControlSettings : ISerializable + { + DECLARE_SCRIPTING_TYPE_MINIMAL(DriveControlSettings); + API_AUTO_SERIALIZATION(); + + /// + /// Acceleration input sensitive. + /// + API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(10)") float RiseRateAcceleration = 6.0f; + + /// + /// Deceleration input sensitive. + /// + API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(11)") float FallRateAcceleration = 10.0f; + + /// + /// Brake input sensitive. + /// + API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(12)") float RiseRateBrake = 6.0f; + + /// + /// Release brake sensitive. + /// + API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(13)") float FallRateBrake = 10.0f; + + /// + /// Brake input sensitive. + /// + API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(14)") float RiseRateHandBrake = 12.0f; + + /// + /// Release handbrake sensitive. + /// + API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(15)") float FallRateHandBrake = 12.0f; + + /// + /// Steer input sensitive. + /// + API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(16)") float RiseRateSteer = 2.5f; + + /// + /// Release steer input sensitive. + /// + API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(17)") float FallRateSteer = 5.0f; + }; + /// /// Vehicle differential settings. /// @@ -321,6 +370,7 @@ private: Array> _wheelsData; float _throttle = 0.0f, _steering = 0.0f, _brake = 0.0f, _handBrake = 0.0f, _tankLeftThrottle, _tankRightThrottle, _tankLeftBrake, _tankRightBrake; Array _wheels; + DriveControlSettings _driveControl; EngineSettings _engine; DifferentialSettings _differential; GearboxSettings _gearbox; @@ -365,6 +415,16 @@ public: /// API_PROPERTY(Attributes="EditorOrder(4), EditorDisplay(\"Vehicle\")") const Array& GetWheels() const; + /// + /// Gets the vehicle drive control settings. + /// + API_PROPERTY(Attributes = "EditorOrder(5), EditorDisplay(\"Vehicle\")") DriveControlSettings GetDriveControl() const; + + /// + /// Sets the vehicle drive control settings. + /// + API_PROPERTY() void SetDriveControl(DriveControlSettings& value); + /// /// Sets the vehicle wheels settings. /// @@ -373,7 +433,7 @@ public: /// /// Gets the vehicle engine settings. /// - API_PROPERTY(Attributes="EditorOrder(5), EditorDisplay(\"Vehicle\")") EngineSettings GetEngine() const; + API_PROPERTY(Attributes="EditorOrder(6), EditorDisplay(\"Vehicle\")") EngineSettings GetEngine() const; /// /// Sets the vehicle engine settings. @@ -383,7 +443,7 @@ public: /// /// Gets the vehicle differential settings. /// - API_PROPERTY(Attributes="EditorOrder(6), EditorDisplay(\"Vehicle\")") DifferentialSettings GetDifferential() const; + API_PROPERTY(Attributes="EditorOrder(7), EditorDisplay(\"Vehicle\")") DifferentialSettings GetDifferential() const; /// /// Sets the vehicle differential settings. @@ -393,7 +453,7 @@ public: /// /// Gets the vehicle gearbox settings. /// - API_PROPERTY(Attributes="EditorOrder(7), EditorDisplay(\"Vehicle\")") GearboxSettings GetGearbox() const; + API_PROPERTY(Attributes="EditorOrder(8), EditorDisplay(\"Vehicle\")") GearboxSettings GetGearbox() const; /// /// Sets the vehicle gearbox settings. diff --git a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp index cb1d094a4..19743396c 100644 --- a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp +++ b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp @@ -1557,38 +1557,38 @@ void PhysicsBackend::EndSimulateScene(void* scene) // @formatter:off // Reference: PhysX SDK docs // TODO: expose input control smoothing data - static constexpr PxVehiclePadSmoothingData padSmoothing = - { - { - 6.0f, // rise rate eANALOG_INPUT_ACCEL - 6.0f, // rise rate eANALOG_INPUT_BRAKE - 12.0f, // rise rate eANALOG_INPUT_HANDBRAKE - 2.5f, // rise rate eANALOG_INPUT_STEER_LEFT - 2.5f, // rise rate eANALOG_INPUT_STEER_RIGHT - }, - { - 10.0f, // fall rate eANALOG_INPUT_ACCEL - 10.0f, // fall rate eANALOG_INPUT_BRAKE - 12.0f, // fall rate eANALOG_INPUT_HANDBRAKE - 5.0f, // fall rate eANALOG_INPUT_STEER_LEFT - 5.0f, // fall rate eANALOG_INPUT_STEER_RIGHT - } - }; - static constexpr PxVehicleKeySmoothingData keySmoothing = + PxVehiclePadSmoothingData padSmoothing = + { + { + wheelVehicle->_driveControl.RiseRateAcceleration, // rise rate eANALOG_INPUT_ACCEL + wheelVehicle->_driveControl.RiseRateBrake, // rise rate eANALOG_INPUT_BRAKE + wheelVehicle->_driveControl.RiseRateHandBrake, // rise rate eANALOG_INPUT_HANDBRAKE + wheelVehicle->_driveControl.RiseRateSteer, // rise rate eANALOG_INPUT_STEER_LEFT + wheelVehicle->_driveControl.RiseRateSteer, // rise rate eANALOG_INPUT_STEER_RIGHT + }, + { + wheelVehicle->_driveControl.FallRateAcceleration, // fall rate eANALOG_INPUT_ACCEL + wheelVehicle->_driveControl.FallRateBrake, // fall rate eANALOG_INPUT_BRAKE + wheelVehicle->_driveControl.FallRateHandBrake, // fall rate eANALOG_INPUT_HANDBRAKE + wheelVehicle->_driveControl.FallRateSteer, // fall rate eANALOG_INPUT_STEER_LEFT + wheelVehicle->_driveControl.FallRateSteer, // fall rate eANALOG_INPUT_STEER_RIGHT + } + }; + PxVehicleKeySmoothingData keySmoothing = { { - 3.0f, // rise rate eANALOG_INPUT_ACCEL - 3.0f, // rise rate eANALOG_INPUT_BRAKE - 10.0f, // rise rate eANALOG_INPUT_HANDBRAKE - 2.5f, // rise rate eANALOG_INPUT_STEER_LEFT - 2.5f, // rise rate eANALOG_INPUT_STEER_RIGHT + wheelVehicle->_driveControl.RiseRateAcceleration, // rise rate eANALOG_INPUT_ACCEL + wheelVehicle->_driveControl.RiseRateBrake, // rise rate eANALOG_INPUT_BRAKE + wheelVehicle->_driveControl.RiseRateHandBrake, // rise rate eANALOG_INPUT_HANDBRAKE + wheelVehicle->_driveControl.RiseRateSteer, // rise rate eANALOG_INPUT_STEER_LEFT + wheelVehicle->_driveControl.RiseRateSteer, // rise rate eANALOG_INPUT_STEER_RIGHT }, { - 5.0f, // fall rate eANALOG_INPUT__ACCEL - 5.0f, // fall rate eANALOG_INPUT__BRAKE - 10.0f, // fall rate eANALOG_INPUT__HANDBRAKE - 5.0f, // fall rate eANALOG_INPUT_STEER_LEFT - 5.0f // fall rate eANALOG_INPUT_STEER_RIGHT + wheelVehicle->_driveControl.FallRateAcceleration, // fall rate eANALOG_INPUT_ACCEL + wheelVehicle->_driveControl.FallRateBrake, // fall rate eANALOG_INPUT_BRAKE + wheelVehicle->_driveControl.FallRateHandBrake, // fall rate eANALOG_INPUT_HANDBRAKE + wheelVehicle->_driveControl.FallRateSteer, // fall rate eANALOG_INPUT_STEER_LEFT + wheelVehicle->_driveControl.FallRateSteer, // fall rate eANALOG_INPUT_STEER_RIGHT } }; // Reference: PhysX SDK docs From 42b20b4e764008052c059e172601c81516ccb7de Mon Sep 17 00:00:00 2001 From: "Mr. Capybara" Date: Tue, 2 Jan 2024 20:39:24 -0400 Subject: [PATCH 05/15] Expose vehicle relationship between speed and steer to driver --- .../Engine/Physics/Actors/WheeledVehicle.cpp | 43 +++++++++++- Source/Engine/Physics/Actors/WheeledVehicle.h | 67 ++++++++++++++++--- .../Physics/PhysX/PhysicsBackendPhysX.cpp | 41 +++++++----- 3 files changed, 126 insertions(+), 25 deletions(-) diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.cpp b/Source/Engine/Physics/Actors/WheeledVehicle.cpp index 243d9cc48..29d1a9e63 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.cpp +++ b/Source/Engine/Physics/Actors/WheeledVehicle.cpp @@ -55,6 +55,48 @@ WheeledVehicle::DriveControlSettings WheeledVehicle::GetDriveControl() const void WheeledVehicle::SetDriveControl(DriveControlSettings &value) { + // Don't let have an invalid steer vs speed list. + if (&value.SteerVsSpeed == nullptr) + value.SteerVsSpeed = Array(); + + if (value.SteerVsSpeed.Count() < 1) + value.SteerVsSpeed.Add(WheeledVehicle::SteerControl()); + else // physx backend requires the max of 4 values only + while (value.SteerVsSpeed.Count() > 4) + value.SteerVsSpeed.RemoveLast(); + + // Maintain all values clamped to have a ordened speed list + int steerVsSpeedCount = value.SteerVsSpeed.Count(); + for (int i = 0; i < steerVsSpeedCount; i++) + { + // Apply only on changed value + if (_driveControl.SteerVsSpeed[i].Speed != value.SteerVsSpeed[i].Speed || _driveControl.SteerVsSpeed[i].Steer != value.SteerVsSpeed[i].Steer) + { + WheeledVehicle::SteerControl &steerVsSpeed = value.SteerVsSpeed[i]; + steerVsSpeed.Steer = Math::Saturate(steerVsSpeed.Steer); + steerVsSpeed.Speed = Math::Max(steerVsSpeed.Speed, 10.0f); + + // Clamp speeds to have an ordened list. + if (i >= 1) + { + WheeledVehicle::SteerControl &lastSteerVsSpeed = value.SteerVsSpeed[i - 1]; + WheeledVehicle::SteerControl &nextSteerVsSpeed = value.SteerVsSpeed[Math::Clamp(i + 1, 0, steerVsSpeedCount - 1)]; + float minSpeed = lastSteerVsSpeed.Speed; + float maxSpeed = nextSteerVsSpeed.Speed; + + if (i + 1 < steerVsSpeedCount - 1) + steerVsSpeed.Speed = Math::Clamp(steerVsSpeed.Speed, minSpeed, maxSpeed); + else + steerVsSpeed.Speed = Math::Max(steerVsSpeed.Speed, minSpeed); + } + else if (steerVsSpeedCount > 1) + { + WheeledVehicle::SteerControl &nextSteerVsSpeed = value.SteerVsSpeed[i + 1]; + steerVsSpeed.Speed = Math::Min(steerVsSpeed.Speed, nextSteerVsSpeed.Speed); + } + } + } + _driveControl = value; } @@ -392,7 +434,6 @@ void WheeledVehicle::Serialize(SerializeStream &stream, const void *otherObj) SERIALIZE_MEMBER(Engine, _engine); SERIALIZE_MEMBER(Differential, _differential); SERIALIZE_MEMBER(Gearbox, _gearbox); - } void WheeledVehicle::Deserialize(DeserializeStream &stream, ISerializeModifier *modifier) diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.h b/Source/Engine/Physics/Actors/WheeledVehicle.h index bfa3dc294..7d7c782df 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.h +++ b/Source/Engine/Physics/Actors/WheeledVehicle.h @@ -40,6 +40,45 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo Special }; + /// + /// Storage the relationship between speed and steer. + /// + API_STRUCT() struct SteerControl : ISerializable + { + DECLARE_SCRIPTING_TYPE_MINIMAL(SteerControl); + API_AUTO_SERIALIZATION(); + + /// + /// The vehicle speed. + /// + API_FIELD(Attributes = "Limit(0)") float Speed; + + /// + /// The target max steer of the speed. + /// + API_FIELD(Attributes = "Limit(0, 1)") float Steer; + + /// + /// Create a Steer/Speed relationship structure. + /// + SteerControl() + { + Speed = 1000; + Steer = 1; + } + + /// + /// Create a Steer/Speed relationship structure. + /// The vehicle speed. + /// The target max steer of the speed. + /// + SteerControl(float speed, float steer) + { + Speed = speed; + Steer = steer; + } + }; + /// /// Vehicle engine settings. /// @@ -94,42 +133,54 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo /// /// Acceleration input sensitive. /// - API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(10)") float RiseRateAcceleration = 6.0f; + API_FIELD(Attributes="Limit(0), EditorOrder(10)") float RiseRateAcceleration = 6.0f; /// /// Deceleration input sensitive. /// - API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(11)") float FallRateAcceleration = 10.0f; + API_FIELD(Attributes="Limit(0), EditorOrder(11)") float FallRateAcceleration = 10.0f; /// /// Brake input sensitive. /// - API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(12)") float RiseRateBrake = 6.0f; + API_FIELD(Attributes="Limit(0), EditorOrder(12)") float RiseRateBrake = 6.0f; /// /// Release brake sensitive. /// - API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(13)") float FallRateBrake = 10.0f; + API_FIELD(Attributes="Limit(0), EditorOrder(13)") float FallRateBrake = 10.0f; /// /// Brake input sensitive. /// - API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(14)") float RiseRateHandBrake = 12.0f; + API_FIELD(Attributes="Limit(0), EditorOrder(14)") float RiseRateHandBrake = 12.0f; /// /// Release handbrake sensitive. /// - API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(15)") float FallRateHandBrake = 12.0f; + API_FIELD(Attributes="Limit(0), EditorOrder(15)") float FallRateHandBrake = 12.0f; /// /// Steer input sensitive. /// - API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(16)") float RiseRateSteer = 2.5f; + API_FIELD(Attributes="Limit(0), EditorOrder(16)") float RiseRateSteer = 2.5f; /// /// Release steer input sensitive. /// - API_FIELD(Attributes="Limit(0), EditorDisplay(\"Inputs\"), EditorOrder(17)") float FallRateSteer = 5.0f; + API_FIELD(Attributes="Limit(0), EditorOrder(17)") float FallRateSteer = 5.0f; + + /// + /// Vehicle control relationship between speed and steer. The higher is the speed, + /// decrease steer to make vehicle more maneuverable (limited only 4 relationships). + /// + API_FIELD() Array SteerVsSpeed = Array + { + SteerControl(800, 1.0f), + SteerControl(1500, 0.7f), + SteerControl(2500, 0.5f), + SteerControl(5000, 0.2f), + }; }; /// diff --git a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp index 19743396c..98fbf7596 100644 --- a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp +++ b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp @@ -1554,9 +1554,7 @@ void PhysicsBackend::EndSimulateScene(void* scene) leftBrake = 1.0f; } - // @formatter:off - // Reference: PhysX SDK docs - // TODO: expose input control smoothing data + // Smooth input controls PxVehiclePadSmoothingData padSmoothing = { { @@ -1591,20 +1589,31 @@ void PhysicsBackend::EndSimulateScene(void* scene) wheelVehicle->_driveControl.FallRateSteer, // fall rate eANALOG_INPUT_STEER_RIGHT } }; - // Reference: PhysX SDK docs - // TODO: expose steer vs forward curve into per-vehicle (up to 8 points, values clamped into 0/1 range) - static constexpr PxF32 steerVsForwardSpeedData[] = + + // Reduce steer by speed to make vehicle more easier to maneuver + + constexpr int steerVsSpeedN = 8; + PxF32 steerVsForwardSpeedData[steerVsSpeedN]; + const int lastSteerVsSpeedIndex = wheelVehicle->_driveControl.SteerVsSpeed.Count() - 1; + int steerVsSpeedIndex = 0; + + // Steer vs speed data structure example: + // array: + // speed, steer + // 1000, 1.0, + // 2000, 0.7, + // 5000, 0.5, + // .. + + // fill the steerVsForwardSpeedData with the speed and steer + for (int i = 0; i < 8; i += 2) { - 0.0f, 1.0f, - 20.0f, 0.9f, - 65.0f, 0.8f, - 120.0f, 0.7f, - PX_MAX_F32, PX_MAX_F32, - PX_MAX_F32, PX_MAX_F32, - PX_MAX_F32, PX_MAX_F32, - PX_MAX_F32, PX_MAX_F32, - }; - const PxFixedSizeLookupTable<8> steerVsForwardSpeed(steerVsForwardSpeedData, 4); + steerVsForwardSpeedData[i] = wheelVehicle->_driveControl.SteerVsSpeed[steerVsSpeedIndex].Speed; + steerVsForwardSpeedData[i + 1] = wheelVehicle->_driveControl.SteerVsSpeed[steerVsSpeedIndex].Steer; + steerVsSpeedIndex = Math::Min(steerVsSpeedIndex + 1, lastSteerVsSpeedIndex); + } + const PxFixedSizeLookupTable steerVsForwardSpeed(steerVsForwardSpeedData, 4); + // @formatter:on if (wheelVehicle->UseAnalogSteering) { From a102bad87ed0461b3c80bd9252c045bbb7d9c8fd Mon Sep 17 00:00:00 2001 From: "Mr. Capybara" Date: Tue, 2 Jan 2024 22:28:23 -0400 Subject: [PATCH 06/15] Change wheel steer angle limit --- Source/Engine/Physics/Actors/WheeledVehicle.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.h b/Source/Engine/Physics/Actors/WheeledVehicle.h index 7d7c782df..f7288a240 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.h +++ b/Source/Engine/Physics/Actors/WheeledVehicle.h @@ -282,9 +282,9 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo API_FIELD(Attributes="EditorOrder(3)") float Width = 20.0f; /// - /// Max steer angle that can be achieved by the wheel (in degrees). + /// Max steer angle that can be achieved by the wheel (in degrees, -180 to 180). /// - API_FIELD(Attributes="Limit(0), EditorDisplay(\"Steering\"), EditorOrder(10)") float MaxSteerAngle = 0.0f; + API_FIELD(Attributes="Limit(-180, 180), EditorDisplay(\"Steering\"), EditorOrder(10)") float MaxSteerAngle = 0.0f; /// /// Damping rate applied to wheel. Specified in kilograms metres-squared per second (kg m^2 s^-1). From 14a4c92aa846d4bdea766c7ec90270d72af39295 Mon Sep 17 00:00:00 2001 From: "Mr. Capybara" Date: Sat, 6 Jan 2024 14:49:38 -0400 Subject: [PATCH 07/15] Add Vehicle anti roll bar and suspension sprung force multiplier --- .../Engine/Physics/Actors/WheeledVehicle.cpp | 47 ++++++++---- Source/Engine/Physics/Actors/WheeledVehicle.h | 37 +++++++++- .../Physics/PhysX/PhysicsBackendPhysX.cpp | 73 ++++++++++++++++--- Source/Engine/Physics/PhysicsBackend.h | 1 + 4 files changed, 130 insertions(+), 28 deletions(-) diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.cpp b/Source/Engine/Physics/Actors/WheeledVehicle.cpp index 29d1a9e63..66a13fd7f 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.cpp +++ b/Source/Engine/Physics/Actors/WheeledVehicle.cpp @@ -56,9 +56,6 @@ WheeledVehicle::DriveControlSettings WheeledVehicle::GetDriveControl() const void WheeledVehicle::SetDriveControl(DriveControlSettings &value) { // Don't let have an invalid steer vs speed list. - if (&value.SteerVsSpeed == nullptr) - value.SteerVsSpeed = Array(); - if (value.SteerVsSpeed.Count() < 1) value.SteerVsSpeed.Add(WheeledVehicle::SteerControl()); else // physx backend requires the max of 4 values only @@ -171,6 +168,20 @@ void WheeledVehicle::SetGearbox(const GearboxSettings &value) _gearbox = value; } +void WheeledVehicle::SetAntiRollBars(const Array &value) +{ + _antiRollBars = value; +#if WITH_VEHICLE + if (_vehicle) + PhysicsBackend::UpdateVehicleAntiRollBars(this); +#endif +} + +const Array &WheeledVehicle::GetAntiRollBars() const +{ + return _antiRollBars; +} + void WheeledVehicle::SetThrottle(float value) { _throttle = Math::Clamp(value, -1.0f, 1.0f); @@ -398,20 +409,24 @@ void WheeledVehicle::OnDebugDrawSelected() DEBUG_DRAW_WIRE_SPHERE(BoundingSphere(data.State.TireContactPoint, 5.0f), Color::Green, 0, false); } } - - // Draw wheels axes - if (wheelIndex % 2 == 0 && wheelIndex + 1 < _wheels.Count()) - { - if (!_wheels[wheelIndex].Collider || !_wheels[wheelIndex + 1].Collider) - continue; - - const Vector3 wheelPos = _wheels[wheelIndex].Collider->GetPosition(); - const Vector3 nextWheelPos = _wheels[wheelIndex + 1].Collider->GetPosition(); - DEBUG_DRAW_LINE(wheelPos, nextWheelPos, Color::Yellow, 0, false); - } } - // Center of mass + // Draw anti roll bars axes + int wheelsCount = _wheels.Count(); + for (int i = 0; i < GetAntiRollBars().Count(); i++) + { + int axleIndex = GetAntiRollBars()[i].Axle; + int leftWheelIndex = axleIndex * 2; + int rightWheelIndex = leftWheelIndex + 1; + + if (leftWheelIndex >= wheelsCount || rightWheelIndex >= wheelsCount) + continue; + if (!_wheels[leftWheelIndex].Collider || !_wheels[rightWheelIndex].Collider) + continue; + + DEBUG_DRAW_LINE(_wheels[leftWheelIndex].Collider->GetPosition(), _wheels[rightWheelIndex].Collider->GetPosition(), Color::Yellow, 0, false); + } + // Center of mass DEBUG_DRAW_WIRE_SPHERE(BoundingSphere(_transform.LocalToWorld(_centerOfMassOffset), 10.0f), Color::Blue, 0, false); RigidBody::OnDebugDrawSelected(); @@ -434,6 +449,7 @@ void WheeledVehicle::Serialize(SerializeStream &stream, const void *otherObj) SERIALIZE_MEMBER(Engine, _engine); SERIALIZE_MEMBER(Differential, _differential); SERIALIZE_MEMBER(Gearbox, _gearbox); + SERIALIZE_MEMBER(AntiRollBars, _antiRollBars); } void WheeledVehicle::Deserialize(DeserializeStream &stream, ISerializeModifier *modifier) @@ -449,6 +465,7 @@ void WheeledVehicle::Deserialize(DeserializeStream &stream, ISerializeModifier * DESERIALIZE_MEMBER(Engine, _engine); DESERIALIZE_MEMBER(Differential, _differential); DESERIALIZE_MEMBER(Gearbox, _gearbox); + DESERIALIZE_MEMBER(AntiRollBars, _antiRollBars); // [Deprecated on 13.06.2023, expires on 13.06.2025] _fixInvalidForwardDir |= modifier->EngineBuild < 6341; diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.h b/Source/Engine/Physics/Actors/WheeledVehicle.h index f7288a240..2048a2f46 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.h +++ b/Source/Engine/Physics/Actors/WheeledVehicle.h @@ -306,6 +306,11 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo /// API_FIELD(Attributes="EditorOrder(4)") ScriptingObjectReference Collider; + /// + /// Spring sprung mass force multiplier. + /// + API_FIELD(Attributes="Limit(0.01f), EditorDisplay(\"Suspension\"), EditorOrder(19)") float SprungMassMultiplier = 1.0f; + /// /// Spring damper rate of suspension unit. /// @@ -407,6 +412,25 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo #endif }; + /// + /// Vehicle axle anti roll bar. + /// + API_STRUCT() struct AntiRollBar : ISerializable + { + DECLARE_SCRIPTING_TYPE_MINIMAL(AntiRollBar); + API_AUTO_SERIALIZATION(); + + /// + /// The specific axle with wheels to apply anti roll. + /// + API_FIELD() int Axle; + + /// + /// The anti roll stiffness. + /// + API_FIELD() float Stiffness; + }; + private: struct WheelData { @@ -420,7 +444,8 @@ private: DriveModes _driveMode = DriveModes::Standard; Array> _wheelsData; float _throttle = 0.0f, _steering = 0.0f, _brake = 0.0f, _handBrake = 0.0f, _tankLeftThrottle, _tankRightThrottle, _tankLeftBrake, _tankRightBrake; - Array _wheels; + Array _wheels; + Array _antiRollBars; DriveControlSettings _driveControl; EngineSettings _engine; DifferentialSettings _differential; @@ -511,6 +536,16 @@ public: /// API_PROPERTY() void SetGearbox(const GearboxSettings& value); + // + /// Sets axles anti roll bars to increase vehicle estability. + /// + API_PROPERTY() void SetAntiRollBars(const Array& value); + + // + /// Gets axles anti roll bars. + /// + API_PROPERTY() const Array& GetAntiRollBars() const; + public: /// /// Sets the input for vehicle throttle. It is the analog accelerator pedal value in range (0,1) where 1 represents the pedal fully pressed and 0 represents the pedal in its rest state. diff --git a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp index 98fbf7596..70ac6a2af 100644 --- a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp +++ b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #endif #if WITH_CLOTH @@ -3333,7 +3334,7 @@ PxVehicleSuspensionData CreatePxVehicleSuspensionData(const WheeledVehicle::Whee const float suspensionFrequency = 7.0f; suspensionData.mMaxCompression = settings.SuspensionMaxRaise; suspensionData.mMaxDroop = settings.SuspensionMaxDrop; - suspensionData.mSprungMass = wheelSprungMass; + suspensionData.mSprungMass = wheelSprungMass * settings.SprungMassMultiplier; suspensionData.mSpringStrength = Math::Square(suspensionFrequency) * suspensionData.mSprungMass; suspensionData.mSpringDamperRate = settings.SuspensionDampingRate * 2.0f * Math::Sqrt(suspensionData.mSpringStrength * suspensionData.mSprungMass); return suspensionData; @@ -3380,23 +3381,29 @@ PxVehicleAckermannGeometryData CreatePxVehicleAckermannGeometryData(PxVehicleWhe return ackermann; } -bool SortWheelsFrontToBack(WheeledVehicle::Wheel const& a, WheeledVehicle::Wheel const& b) +PxVehicleAntiRollBarData CreatePxPxVehicleAntiRollBarData(const WheeledVehicle::AntiRollBar& settings, int leftWheelIndex, int rightWheelIndex) +{ + PxVehicleAntiRollBarData antiRollBar; + antiRollBar.mWheel0 = leftWheelIndex; + antiRollBar.mWheel1 = rightWheelIndex; + antiRollBar.mStiffness = settings.Stiffness; + return antiRollBar; +} + +bool SortWheelsFrontToBack(WheeledVehicle::Wheel const &a, WheeledVehicle::Wheel const &b) { return a.Collider && b.Collider && a.Collider->GetLocalPosition().Z > b.Collider->GetLocalPosition().Z; } void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) { -#if USE_EDITOR - if (Editor::IsPlayMode) -#endif + // Physx vehicles needs to have all wheels sorted to apply controls correctly. + // Its needs to know what wheels are on front to turn wheel to correctly side + // and needs to know wheel side to apply throttle to correctly direction for each track when using tanks. + // Anti roll bars needs to have all wheels sorted to get correctly wheel index too. + if (actor->_driveType != WheeledVehicle::DriveTypes::NoDrive) { - // Physx vehicles needs to have all wheels sorted to apply controls correctly. - // Its needs to know what wheels are on front to turn wheel to correctly side - // and needs to know wheel side to apply throttle to correctly direction for each track when using tanks. - - if (actor->_driveType == WheeledVehicle::DriveTypes::Drive4W) - Sorting::QuickSort(actor->_wheels.Get(), actor->_wheels.Count(), SortWheelsFrontToBack); + Sorting::QuickSort(actor->_wheels.Get(), actor->_wheels.Count(), SortWheelsFrontToBack); // sort wheels by side if (actor->_driveType == WheeledVehicle::DriveTypes::Tank) @@ -3468,7 +3475,8 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) // TODO: get gravityDirection from scenePhysX->Scene->getGravity() PxVehicleComputeSprungMasses(wheels.Count(), offsets, centerOfMassOffset.p, mass, 1, sprungMasses); PxVehicleWheelsSimData* wheelsSimData = PxVehicleWheelsSimData::allocate(wheels.Count()); - for (int32 i = 0; i < wheels.Count(); i++) + int wheelsCount = wheels.Count(); + for (int32 i = 0; i < wheelsCount; i++) { auto& wheel = *wheels[i]; @@ -3514,6 +3522,19 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) wheelsSimData->disableWheel(i); } } + // Add Anti roll bars for wheels axles + for (int i = 0; i < actor->GetAntiRollBars().Count(); i++) + { + int axleIndex = actor->GetAntiRollBars()[i].Axle; + int leftWheelIndex = axleIndex * 2; + int rightWheelIndex = leftWheelIndex + 1; + + if (leftWheelIndex >= wheelsCount || rightWheelIndex >= wheelsCount) + continue; + + const PxVehicleAntiRollBarData &antiRollBar = CreatePxPxVehicleAntiRollBarData(actor->GetAntiRollBars()[i], leftWheelIndex, rightWheelIndex); + wheelsSimData->addAntiRollBarData(antiRollBar); + } for (auto child : actor->Children) { auto collider = ScriptingObject::Cast(child); @@ -3676,6 +3697,34 @@ void PhysicsBackend::UpdateVehicleWheels(WheeledVehicle* actor) } } +void PhysicsBackend::UpdateVehicleAntiRollBars(WheeledVehicle *actor) +{ + int wheelsCount = actor->_wheels.Count(); + auto drive = (PxVehicleWheels *)actor->_vehicle; + PxVehicleWheelsSimData *wheelsSimData = &drive->mWheelsSimData; + + // Update Anti roll bars for wheels axles + for (int i = 0; i < actor->GetAntiRollBars().Count(); i++) + { + int axleIndex = actor->GetAntiRollBars()[i].Axle; + int leftWheelIndex = axleIndex * 2; + int rightWheelIndex = leftWheelIndex + 1; + + if (leftWheelIndex >= wheelsCount || rightWheelIndex >= wheelsCount) + continue; + + const PxVehicleAntiRollBarData &antiRollBar = CreatePxPxVehicleAntiRollBarData(actor->GetAntiRollBars()[i], leftWheelIndex, rightWheelIndex); + if (wheelsSimData->getNbAntiRollBarData() - 1 < i) + { + wheelsSimData->addAntiRollBarData(antiRollBar); + } + else + { + wheelsSimData->setAntiRollBarData(axleIndex, antiRollBar); + } + } +} + void PhysicsBackend::SetVehicleEngine(void* vehicle, const void* value) { auto drive = (PxVehicleDrive*)vehicle; diff --git a/Source/Engine/Physics/PhysicsBackend.h b/Source/Engine/Physics/PhysicsBackend.h index f20683e31..63c14fdc3 100644 --- a/Source/Engine/Physics/PhysicsBackend.h +++ b/Source/Engine/Physics/PhysicsBackend.h @@ -262,6 +262,7 @@ public: static void* CreateVehicle(class WheeledVehicle* actor); static void DestroyVehicle(void* vehicle, int32 driveType); static void UpdateVehicleWheels(WheeledVehicle* actor); + static void UpdateVehicleAntiRollBars(WheeledVehicle* actor); static void SetVehicleEngine(void* vehicle, const void* value); static void SetVehicleDifferential(void* vehicle, const void* value); static void SetVehicleGearbox(void* vehicle, const void* value); From eca17c77997b191d847108a243b434d4124ef093 Mon Sep 17 00:00:00 2001 From: "Mr. Capybara" Date: Sun, 7 Jan 2024 10:35:00 -0400 Subject: [PATCH 08/15] Change vehicle drive mode to vehiclde drive control --- Source/Engine/Physics/Actors/WheeledVehicle.cpp | 12 ------------ Source/Engine/Physics/Actors/WheeledVehicle.h | 16 +++++----------- .../Engine/Physics/PhysX/PhysicsBackendPhysX.cpp | 2 +- 3 files changed, 6 insertions(+), 24 deletions(-) diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.cpp b/Source/Engine/Physics/Actors/WheeledVehicle.cpp index 66a13fd7f..2f92ef3a8 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.cpp +++ b/Source/Engine/Physics/Actors/WheeledVehicle.cpp @@ -33,16 +33,6 @@ void WheeledVehicle::SetDriveType(DriveTypes value) Setup(); } -void WheeledVehicle::SetDriveMode(DriveModes value) -{ - _driveMode = value; -} - -WheeledVehicle::DriveModes WheeledVehicle::GetDriveMode() const -{ - return _driveMode; -} - const Array &WheeledVehicle::GetWheels() const { return _wheels; @@ -441,7 +431,6 @@ void WheeledVehicle::Serialize(SerializeStream &stream, const void *otherObj) SERIALIZE_GET_OTHER_OBJ(WheeledVehicle); SERIALIZE_MEMBER(DriveType, _driveType); - SERIALIZE_MEMBER(DriveModes, _driveMode); SERIALIZE_MEMBER(Wheels, _wheels); SERIALIZE_MEMBER(DriveControl, _driveControl); SERIALIZE(UseReverseAsBrake); @@ -457,7 +446,6 @@ void WheeledVehicle::Deserialize(DeserializeStream &stream, ISerializeModifier * RigidBody::Deserialize(stream, modifier); DESERIALIZE_MEMBER(DriveType, _driveType); - DESERIALIZE_MEMBER(DriveModes, _driveMode); DESERIALIZE_MEMBER(Wheels, _wheels); DESERIALIZE_MEMBER(DriveControl, _driveControl); DESERIALIZE(UseReverseAsBrake); diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.h b/Source/Engine/Physics/Actors/WheeledVehicle.h index 2048a2f46..396f74543 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.h +++ b/Source/Engine/Physics/Actors/WheeledVehicle.h @@ -130,6 +130,11 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo DECLARE_SCRIPTING_TYPE_MINIMAL(DriveControlSettings); API_AUTO_SERIALIZATION(); + /// + /// Gets or sets the drive mode, used by vehicles specify the way of the tracks control. + /// + API_FIELD(Attributes="EditorOrder(0), EditorDisplay(\"Tanks\")") WheeledVehicle::DriveModes DriveMode; + /// /// Acceleration input sensitive. /// @@ -441,7 +446,6 @@ private: void* _vehicle = nullptr; DriveTypes _driveType = DriveTypes::Drive4W, _driveTypeCurrent; - DriveModes _driveMode = DriveModes::Standard; Array> _wheelsData; float _throttle = 0.0f, _steering = 0.0f, _brake = 0.0f, _handBrake = 0.0f, _tankLeftThrottle, _tankRightThrottle, _tankLeftBrake, _tankRightBrake; Array _wheels; @@ -476,16 +480,6 @@ public: /// API_PROPERTY() void SetDriveType(DriveTypes value); - /// - /// Used only for tanks, set the drive mode. - /// - API_PROPERTY() void SetDriveMode(DriveModes value); - - /// - /// Gets the vehicle driving mode. Used only on tanks - /// - API_PROPERTY(Attributes="EditorOrder(3), EditorDisplay(\"Vehicle\")") DriveModes GetDriveMode() const; - /// /// Gets the vehicle wheels settings. /// diff --git a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp index 70ac6a2af..7fbcfc1be 100644 --- a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp +++ b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp @@ -1397,7 +1397,7 @@ void PhysicsBackend::EndSimulateScene(void* scene) float leftBrake = Math::Max(wheelVehicle->_tankLeftBrake, wheelVehicle->_handBrake); float rightBrake = Math::Max(wheelVehicle->_tankRightBrake, wheelVehicle->_handBrake); - WheeledVehicle::DriveModes vehicleDriveMode = wheelVehicle->_driveMode; + WheeledVehicle::DriveModes vehicleDriveMode = wheelVehicle->_driveControl.DriveMode; if (isTank) { From 7f420c0131e244902bc21a228f0d9a379bf0ba52 Mon Sep 17 00:00:00 2001 From: "Mr. Capybara" Date: Sun, 7 Jan 2024 10:46:21 -0400 Subject: [PATCH 09/15] add validation to vehicle drive --- Source/Engine/Physics/Actors/WheeledVehicle.cpp | 10 ++++++++++ Source/Engine/Physics/Actors/WheeledVehicle.h | 16 ++++++++-------- 2 files changed, 18 insertions(+), 8 deletions(-) diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.cpp b/Source/Engine/Physics/Actors/WheeledVehicle.cpp index 2f92ef3a8..9891de208 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.cpp +++ b/Source/Engine/Physics/Actors/WheeledVehicle.cpp @@ -45,6 +45,16 @@ WheeledVehicle::DriveControlSettings WheeledVehicle::GetDriveControl() const void WheeledVehicle::SetDriveControl(DriveControlSettings &value) { + value.RiseRateAcceleration = Math::Max(value.RiseRateAcceleration, 0.01f); + value.RiseRateBrake = Math::Max(value.RiseRateBrake, 0.01f); + value.RiseRateHandBrake = Math::Max(value.RiseRateHandBrake, 0.01f); + value.RiseRateSteer = Math::Max(value.RiseRateSteer, 0.01f); + + value.FallRateAcceleration = Math::Max(value.FallRateAcceleration, 0.01f); + value.FallRateBrake = Math::Max(value.FallRateBrake, 0.01f); + value.FallRateHandBrake = Math::Max(value.FallRateHandBrake, 0.01f); + value.FallRateSteer = Math::Max(value.FallRateSteer, 0.01f); + // Don't let have an invalid steer vs speed list. if (value.SteerVsSpeed.Count() < 1) value.SteerVsSpeed.Add(WheeledVehicle::SteerControl()); diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.h b/Source/Engine/Physics/Actors/WheeledVehicle.h index 396f74543..7cf8310d0 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.h +++ b/Source/Engine/Physics/Actors/WheeledVehicle.h @@ -138,42 +138,42 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo /// /// Acceleration input sensitive. /// - API_FIELD(Attributes="Limit(0), EditorOrder(10)") float RiseRateAcceleration = 6.0f; + API_FIELD(Attributes="EditorOrder(10)") float RiseRateAcceleration = 6.0f; /// /// Deceleration input sensitive. /// - API_FIELD(Attributes="Limit(0), EditorOrder(11)") float FallRateAcceleration = 10.0f; + API_FIELD(Attributes="EditorOrder(11)") float FallRateAcceleration = 10.0f; /// /// Brake input sensitive. /// - API_FIELD(Attributes="Limit(0), EditorOrder(12)") float RiseRateBrake = 6.0f; + API_FIELD(Attributes="EditorOrder(12)") float RiseRateBrake = 6.0f; /// /// Release brake sensitive. /// - API_FIELD(Attributes="Limit(0), EditorOrder(13)") float FallRateBrake = 10.0f; + API_FIELD(Attributes="EditorOrder(13)") float FallRateBrake = 10.0f; /// /// Brake input sensitive. /// - API_FIELD(Attributes="Limit(0), EditorOrder(14)") float RiseRateHandBrake = 12.0f; + API_FIELD(Attributes="EditorOrder(14)") float RiseRateHandBrake = 12.0f; /// /// Release handbrake sensitive. /// - API_FIELD(Attributes="Limit(0), EditorOrder(15)") float FallRateHandBrake = 12.0f; + API_FIELD(Attributes="EditorOrder(15)") float FallRateHandBrake = 12.0f; /// /// Steer input sensitive. /// - API_FIELD(Attributes="Limit(0), EditorOrder(16)") float RiseRateSteer = 2.5f; + API_FIELD(Attributes="EditorOrder(16)") float RiseRateSteer = 2.5f; /// /// Release steer input sensitive. /// - API_FIELD(Attributes="Limit(0), EditorOrder(17)") float FallRateSteer = 5.0f; + API_FIELD(Attributes="EditorOrder(17)") float FallRateSteer = 5.0f; /// /// Vehicle control relationship between speed and steer. The higher is the speed, From e6389511fffd63ae3e56c39f0e27b0868e3b6061 Mon Sep 17 00:00:00 2001 From: ruan Date: Sat, 13 Jan 2024 09:47:00 -0400 Subject: [PATCH 10/15] Fix tank inputs without initialized values --- Source/Engine/Physics/Actors/WheeledVehicle.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.h b/Source/Engine/Physics/Actors/WheeledVehicle.h index 7cf8310d0..a0b67cc75 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.h +++ b/Source/Engine/Physics/Actors/WheeledVehicle.h @@ -26,6 +26,7 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo DriveNW, // Non-drivable vehicle. NoDrive, + // Tank Drive. Can have more than 4 wheel. Not use steer, control acceleration for each tank track. Tank, }; /// @@ -447,7 +448,7 @@ private: void* _vehicle = nullptr; DriveTypes _driveType = DriveTypes::Drive4W, _driveTypeCurrent; Array> _wheelsData; - float _throttle = 0.0f, _steering = 0.0f, _brake = 0.0f, _handBrake = 0.0f, _tankLeftThrottle, _tankRightThrottle, _tankLeftBrake, _tankRightBrake; + float _throttle = 0.0f, _steering = 0.0f, _brake = 0.0f, _handBrake = 0.0f, _tankLeftThrottle = 0.0f, _tankRightThrottle = 0.0f, _tankLeftBrake = 0.0f, _tankRightBrake = 0.0f; Array _wheels; Array _antiRollBars; DriveControlSettings _driveControl; From 7f957483519b78189c1e4a73e7146e3b1c5ce10f Mon Sep 17 00:00:00 2001 From: ruan Date: Sat, 20 Jan 2024 19:44:08 -0400 Subject: [PATCH 11/15] Fix brake not working on tanks --- Source/Engine/Physics/Actors/WheeledVehicle.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.cpp b/Source/Engine/Physics/Actors/WheeledVehicle.cpp index 9891de208..b3e714e51 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.cpp +++ b/Source/Engine/Physics/Actors/WheeledVehicle.cpp @@ -194,7 +194,10 @@ void WheeledVehicle::SetSteering(float value) void WheeledVehicle::SetBrake(float value) { - _brake = Math::Saturate(value); + value = Math::Saturate(value); + _brake = value; + _tankLeftBrake = value; + _tankRightBrake = value; } void WheeledVehicle::SetHandbrake(float value) @@ -227,11 +230,11 @@ void WheeledVehicle::ClearInput() _throttle = 0; _steering = 0; _brake = 0; + _handBrake = 0; _tankLeftThrottle = 0; _tankRightThrottle = 0; _tankLeftBrake = 0; _tankRightBrake = 0; - _handBrake = 0; } float WheeledVehicle::GetForwardSpeed() const @@ -423,10 +426,10 @@ void WheeledVehicle::OnDebugDrawSelected() continue; if (!_wheels[leftWheelIndex].Collider || !_wheels[rightWheelIndex].Collider) continue; - + DEBUG_DRAW_LINE(_wheels[leftWheelIndex].Collider->GetPosition(), _wheels[rightWheelIndex].Collider->GetPosition(), Color::Yellow, 0, false); } - // Center of mass + // Center of mass DEBUG_DRAW_WIRE_SPHERE(BoundingSphere(_transform.LocalToWorld(_centerOfMassOffset), 10.0f), Color::Blue, 0, false); RigidBody::OnDebugDrawSelected(); From 34583d7a55775a0da9a4fc9861ec1246e5bc6772 Mon Sep 17 00:00:00 2001 From: ruan Date: Sun, 21 Jan 2024 09:00:46 -0400 Subject: [PATCH 12/15] Fix crash when disable vehicle --- Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp index 7fbcfc1be..e55965034 100644 --- a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp +++ b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp @@ -3588,7 +3588,6 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) // Create vehicle drive auto drive4W = PxVehicleDrive4W::allocate(wheels.Count()); drive4W->setup(PhysX, actorPhysX, *wheelsSimData, driveSimData, Math::Max(wheels.Count() - 4, 0)); - drive4W->setToRestState(); drive4W->mDriveDynData.forceGearChange(PxVehicleGearsData::eFIRST); drive4W->mDriveDynData.setUseAutoGears(gearbox.AutoGear); vehicle = drive4W; @@ -3613,7 +3612,6 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) // Create vehicle drive auto driveNW = PxVehicleDriveNW::allocate(wheels.Count()); driveNW->setup(PhysX, actorPhysX, *wheelsSimData, driveSimData, wheels.Count()); - driveNW->setToRestState(); driveNW->mDriveDynData.forceGearChange(PxVehicleGearsData::eFIRST); driveNW->mDriveDynData.setUseAutoGears(gearbox.AutoGear); vehicle = driveNW; @@ -3624,7 +3622,6 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) // Create vehicle drive auto driveNo = PxVehicleNoDrive::allocate(wheels.Count()); driveNo->setup(PhysX, actorPhysX, *wheelsSimData); - driveNo->setToRestState(); vehicle = driveNo; break; } @@ -3648,7 +3645,6 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) // Create vehicle drive auto driveTank = PxVehicleDriveTank::allocate(wheels.Count()); driveTank->setup(PhysX, actorPhysX, *wheelsSimData, driveSimData, wheels.Count()); - driveTank->setToRestState(); driveTank->mDriveDynData.forceGearChange(PxVehicleGearsData::eFIRST); driveTank->mDriveDynData.setUseAutoGears(gearbox.AutoGear); vehicle = driveTank; From e5160f288551d0cf8c3d0d0016edf7a7f035dad5 Mon Sep 17 00:00:00 2001 From: ruan Date: Sun, 21 Jan 2024 10:42:55 -0400 Subject: [PATCH 13/15] Small fix --- Source/Engine/Physics/Actors/WheeledVehicle.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.h b/Source/Engine/Physics/Actors/WheeledVehicle.h index a0b67cc75..2bc42d6d7 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.h +++ b/Source/Engine/Physics/Actors/WheeledVehicle.h @@ -134,7 +134,7 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo /// /// Gets or sets the drive mode, used by vehicles specify the way of the tracks control. /// - API_FIELD(Attributes="EditorOrder(0), EditorDisplay(\"Tanks\")") WheeledVehicle::DriveModes DriveMode; + API_FIELD(Attributes="EditorOrder(0), EditorDisplay(\"Tanks\")") WheeledVehicle::DriveModes DriveMode = WheeledVehicle::DriveModes::Standard; /// /// Acceleration input sensitive. From 19ddcad16f20ba484d96d04a6cde54e0ff013cfb Mon Sep 17 00:00:00 2001 From: ruan Date: Sun, 21 Jan 2024 11:03:29 -0400 Subject: [PATCH 14/15] Small clean code --- .../Engine/Physics/Actors/WheeledVehicle.cpp | 52 +++++++++---------- .../Physics/PhysX/PhysicsBackendPhysX.cpp | 3 -- 2 files changed, 26 insertions(+), 29 deletions(-) diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.cpp b/Source/Engine/Physics/Actors/WheeledVehicle.cpp index b3e714e51..803b080c4 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.cpp +++ b/Source/Engine/Physics/Actors/WheeledVehicle.cpp @@ -14,7 +14,7 @@ #include "Engine/Core/Log.h" #endif -WheeledVehicle::WheeledVehicle(const SpawnParams ¶ms) +WheeledVehicle::WheeledVehicle(const SpawnParams& params) : RigidBody(params) { _useCCD = 1; @@ -33,7 +33,7 @@ void WheeledVehicle::SetDriveType(DriveTypes value) Setup(); } -const Array &WheeledVehicle::GetWheels() const +const Array& WheeledVehicle::GetWheels() const { return _wheels; } @@ -43,7 +43,7 @@ WheeledVehicle::DriveControlSettings WheeledVehicle::GetDriveControl() const return _driveControl; } -void WheeledVehicle::SetDriveControl(DriveControlSettings &value) +void WheeledVehicle::SetDriveControl(DriveControlSettings& value) { value.RiseRateAcceleration = Math::Max(value.RiseRateAcceleration, 0.01f); value.RiseRateBrake = Math::Max(value.RiseRateBrake, 0.01f); @@ -69,15 +69,15 @@ void WheeledVehicle::SetDriveControl(DriveControlSettings &value) // Apply only on changed value if (_driveControl.SteerVsSpeed[i].Speed != value.SteerVsSpeed[i].Speed || _driveControl.SteerVsSpeed[i].Steer != value.SteerVsSpeed[i].Steer) { - WheeledVehicle::SteerControl &steerVsSpeed = value.SteerVsSpeed[i]; + WheeledVehicle::SteerControl& steerVsSpeed = value.SteerVsSpeed[i]; steerVsSpeed.Steer = Math::Saturate(steerVsSpeed.Steer); steerVsSpeed.Speed = Math::Max(steerVsSpeed.Speed, 10.0f); // Clamp speeds to have an ordened list. if (i >= 1) { - WheeledVehicle::SteerControl &lastSteerVsSpeed = value.SteerVsSpeed[i - 1]; - WheeledVehicle::SteerControl &nextSteerVsSpeed = value.SteerVsSpeed[Math::Clamp(i + 1, 0, steerVsSpeedCount - 1)]; + WheeledVehicle::SteerControl& lastSteerVsSpeed = value.SteerVsSpeed[i - 1]; + WheeledVehicle::SteerControl& nextSteerVsSpeed = value.SteerVsSpeed[Math::Clamp(i + 1, 0, steerVsSpeedCount - 1)]; float minSpeed = lastSteerVsSpeed.Speed; float maxSpeed = nextSteerVsSpeed.Speed; @@ -88,7 +88,7 @@ void WheeledVehicle::SetDriveControl(DriveControlSettings &value) } else if (steerVsSpeedCount > 1) { - WheeledVehicle::SteerControl &nextSteerVsSpeed = value.SteerVsSpeed[i + 1]; + WheeledVehicle::SteerControl& nextSteerVsSpeed = value.SteerVsSpeed[i + 1]; steerVsSpeed.Speed = Math::Min(steerVsSpeed.Speed, nextSteerVsSpeed.Speed); } } @@ -97,7 +97,7 @@ void WheeledVehicle::SetDriveControl(DriveControlSettings &value) _driveControl = value; } -void WheeledVehicle::SetWheels(const Array &value) +void WheeledVehicle::SetWheels(const Array& value) { #if WITH_VEHICLE // Don't recreate whole vehicle when some wheel properties are only changed (eg. suspension) @@ -106,8 +106,8 @@ void WheeledVehicle::SetWheels(const Array &value) bool softUpdate = true; for (int32 wheelIndex = 0; wheelIndex < value.Count(); wheelIndex++) { - auto &oldWheel = _wheels.Get()[wheelIndex]; - auto &newWheel = value.Get()[wheelIndex]; + auto& oldWheel = _wheels.Get()[wheelIndex]; + auto& newWheel = value.Get()[wheelIndex]; if (Math::NotNearEqual(oldWheel.SuspensionForceOffset, newWheel.SuspensionForceOffset) || oldWheel.Collider != newWheel.Collider) { softUpdate = false; @@ -131,7 +131,7 @@ WheeledVehicle::EngineSettings WheeledVehicle::GetEngine() const return _engine; } -void WheeledVehicle::SetEngine(const EngineSettings &value) +void WheeledVehicle::SetEngine(const EngineSettings& value) { #if WITH_VEHICLE if (_vehicle) @@ -145,7 +145,7 @@ WheeledVehicle::DifferentialSettings WheeledVehicle::GetDifferential() const return _differential; } -void WheeledVehicle::SetDifferential(const DifferentialSettings &value) +void WheeledVehicle::SetDifferential(const DifferentialSettings& value) { #if WITH_VEHICLE if (_vehicle) @@ -159,7 +159,7 @@ WheeledVehicle::GearboxSettings WheeledVehicle::GetGearbox() const return _gearbox; } -void WheeledVehicle::SetGearbox(const GearboxSettings &value) +void WheeledVehicle::SetGearbox(const GearboxSettings& value) { #if WITH_VEHICLE if (_vehicle) @@ -168,7 +168,7 @@ void WheeledVehicle::SetGearbox(const GearboxSettings &value) _gearbox = value; } -void WheeledVehicle::SetAntiRollBars(const Array &value) +void WheeledVehicle::SetAntiRollBars(const Array& value) { _antiRollBars = value; #if WITH_VEHICLE @@ -177,7 +177,7 @@ void WheeledVehicle::SetAntiRollBars(const Array &value) #endif } -const Array &WheeledVehicle::GetAntiRollBars() const +const Array& WheeledVehicle::GetAntiRollBars() const { return _antiRollBars; } @@ -298,12 +298,12 @@ void WheeledVehicle::SetTargetGear(int32 value) #endif } -void WheeledVehicle::GetWheelState(int32 index, WheelState &result) +void WheeledVehicle::GetWheelState(int32 index, WheelState& result) { if (index >= 0 && index < _wheels.Count()) { const auto collider = _wheels[index].Collider.Get(); - for (auto &wheelData : _wheelsData) + for (auto& wheelData : _wheelsData) { if (wheelData.Collider == collider) { @@ -344,10 +344,10 @@ void WheeledVehicle::Setup() #if USE_EDITOR -void WheeledVehicle::DrawPhysicsDebug(RenderView &view) +void WheeledVehicle::DrawPhysicsDebug(RenderView& view) { // Wheels shapes - for (const auto &data : _wheelsData) + for (const auto& data : _wheelsData) { int32 wheelIndex = 0; for (; wheelIndex < _wheels.Count(); wheelIndex++) @@ -357,7 +357,7 @@ void WheeledVehicle::DrawPhysicsDebug(RenderView &view) } if (wheelIndex == _wheels.Count()) break; - const auto &wheel = _wheels[wheelIndex]; + const auto& wheel = _wheels[wheelIndex]; if (wheel.Collider && wheel.Collider->GetParent() == this && !wheel.Collider->GetIsTrigger()) { const Vector3 currentPos = wheel.Collider->GetPosition(); @@ -378,7 +378,7 @@ void WheeledVehicle::DrawPhysicsDebug(RenderView &view) void WheeledVehicle::OnDebugDrawSelected() { // Wheels shapes - for (const auto &data : _wheelsData) + for (const auto& data : _wheelsData) { int32 wheelIndex = 0; for (; wheelIndex < _wheels.Count(); wheelIndex++) @@ -388,7 +388,7 @@ void WheeledVehicle::OnDebugDrawSelected() } if (wheelIndex == _wheels.Count()) break; - const auto &wheel = _wheels[wheelIndex]; + const auto& wheel = _wheels[wheelIndex]; if (wheel.Collider && wheel.Collider->GetParent() == this && !wheel.Collider->GetIsTrigger()) { const Vector3 currentPos = wheel.Collider->GetPosition(); @@ -437,7 +437,7 @@ void WheeledVehicle::OnDebugDrawSelected() #endif -void WheeledVehicle::Serialize(SerializeStream &stream, const void *otherObj) +void WheeledVehicle::Serialize(SerializeStream& stream, const void *otherObj) { RigidBody::Serialize(stream, otherObj); @@ -454,7 +454,7 @@ void WheeledVehicle::Serialize(SerializeStream &stream, const void *otherObj) SERIALIZE_MEMBER(AntiRollBars, _antiRollBars); } -void WheeledVehicle::Deserialize(DeserializeStream &stream, ISerializeModifier *modifier) +void WheeledVehicle::Deserialize(DeserializeStream& stream, ISerializeModifier *modifier) { RigidBody::Deserialize(stream, modifier); @@ -550,14 +550,14 @@ void WheeledVehicle::BeginPlay(SceneBeginData *data) #endif #if USE_EDITOR - GetSceneRendering()->AddPhysicsDebug(this); + GetSceneRendering()->AddPhysicsDebug(this); #endif } void WheeledVehicle::EndPlay() { #if USE_EDITOR - GetSceneRendering()->RemovePhysicsDebug(this); + GetSceneRendering()->RemovePhysicsDebug(this); #endif #if WITH_VEHICLE diff --git a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp index b9d0387d8..c5ebd0620 100644 --- a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp +++ b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp @@ -59,9 +59,6 @@ #if WITH_PVD #include #endif -#if USE_EDITOR -#include "Editor/Editor.h" -#endif // Temporary memory size used by the PhysX during the simulation. Must be multiply of 4kB and 16bit aligned. #define PHYSX_SCRATCH_BLOCK_SIZE (1024 * 128) From 8351a75676e2634bba8d4b0c6f72a354ade4482b Mon Sep 17 00:00:00 2001 From: Wojtek Figat Date: Fri, 23 Feb 2024 14:43:14 +0100 Subject: [PATCH 15/15] Codestyle fixes #2192 --- .../Engine/Physics/Actors/WheeledVehicle.cpp | 64 +++--- Source/Engine/Physics/Actors/WheeledVehicle.h | 36 ++-- .../Physics/PhysX/PhysicsBackendPhysX.cpp | 189 +++++++++--------- 3 files changed, 140 insertions(+), 149 deletions(-) diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.cpp b/Source/Engine/Physics/Actors/WheeledVehicle.cpp index 803b080c4..e0ae1f3c4 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.cpp +++ b/Source/Engine/Physics/Actors/WheeledVehicle.cpp @@ -43,7 +43,7 @@ WheeledVehicle::DriveControlSettings WheeledVehicle::GetDriveControl() const return _driveControl; } -void WheeledVehicle::SetDriveControl(DriveControlSettings& value) +void WheeledVehicle::SetDriveControl(DriveControlSettings value) { value.RiseRateAcceleration = Math::Max(value.RiseRateAcceleration, 0.01f); value.RiseRateBrake = Math::Max(value.RiseRateBrake, 0.01f); @@ -58,28 +58,29 @@ void WheeledVehicle::SetDriveControl(DriveControlSettings& value) // Don't let have an invalid steer vs speed list. if (value.SteerVsSpeed.Count() < 1) value.SteerVsSpeed.Add(WheeledVehicle::SteerControl()); - else // physx backend requires the max of 4 values only + else // PhysX backend requires the max of 4 values only while (value.SteerVsSpeed.Count() > 4) value.SteerVsSpeed.RemoveLast(); - // Maintain all values clamped to have a ordened speed list - int steerVsSpeedCount = value.SteerVsSpeed.Count(); - for (int i = 0; i < steerVsSpeedCount; i++) + // Maintain all values clamped to have an ordered speed list + const int32 steerVsSpeedCount = value.SteerVsSpeed.Count(); + for (int32 i = 0; i < steerVsSpeedCount; i++) { // Apply only on changed value - if (_driveControl.SteerVsSpeed[i].Speed != value.SteerVsSpeed[i].Speed || _driveControl.SteerVsSpeed[i].Steer != value.SteerVsSpeed[i].Steer) + if (Math::NotNearEqual(_driveControl.SteerVsSpeed[i].Speed, value.SteerVsSpeed[i].Speed) || + Math::NotNearEqual(_driveControl.SteerVsSpeed[i].Steer, value.SteerVsSpeed[i].Steer)) { - WheeledVehicle::SteerControl& steerVsSpeed = value.SteerVsSpeed[i]; + SteerControl& steerVsSpeed = value.SteerVsSpeed[i]; steerVsSpeed.Steer = Math::Saturate(steerVsSpeed.Steer); steerVsSpeed.Speed = Math::Max(steerVsSpeed.Speed, 10.0f); - // Clamp speeds to have an ordened list. + // Clamp speeds to have an ordered list if (i >= 1) { - WheeledVehicle::SteerControl& lastSteerVsSpeed = value.SteerVsSpeed[i - 1]; - WheeledVehicle::SteerControl& nextSteerVsSpeed = value.SteerVsSpeed[Math::Clamp(i + 1, 0, steerVsSpeedCount - 1)]; - float minSpeed = lastSteerVsSpeed.Speed; - float maxSpeed = nextSteerVsSpeed.Speed; + const SteerControl& lastSteerVsSpeed = value.SteerVsSpeed[i - 1]; + const SteerControl& nextSteerVsSpeed = value.SteerVsSpeed[Math::Clamp(i + 1, 0, steerVsSpeedCount - 1)]; + const float minSpeed = lastSteerVsSpeed.Speed; + const float maxSpeed = nextSteerVsSpeed.Speed; if (i + 1 < steerVsSpeedCount - 1) steerVsSpeed.Speed = Math::Clamp(steerVsSpeed.Speed, minSpeed, maxSpeed); @@ -88,7 +89,7 @@ void WheeledVehicle::SetDriveControl(DriveControlSettings& value) } else if (steerVsSpeedCount > 1) { - WheeledVehicle::SteerControl& nextSteerVsSpeed = value.SteerVsSpeed[i + 1]; + const SteerControl& nextSteerVsSpeed = value.SteerVsSpeed[i + 1]; steerVsSpeed.Speed = Math::Min(steerVsSpeed.Speed, nextSteerVsSpeed.Speed); } } @@ -108,7 +109,8 @@ void WheeledVehicle::SetWheels(const Array& value) { auto& oldWheel = _wheels.Get()[wheelIndex]; auto& newWheel = value.Get()[wheelIndex]; - if (Math::NotNearEqual(oldWheel.SuspensionForceOffset, newWheel.SuspensionForceOffset) || oldWheel.Collider != newWheel.Collider) + if (Math::NotNearEqual(oldWheel.SuspensionForceOffset, newWheel.SuspensionForceOffset) || + oldWheel.Collider != newWheel.Collider) { softUpdate = false; break; @@ -321,7 +323,7 @@ void WheeledVehicle::Setup() return; // Release previous - void *scene = GetPhysicsScene()->GetPhysicsScene(); + void* scene = GetPhysicsScene()->GetPhysicsScene(); if (_vehicle) { PhysicsBackend::RemoveVehicle(scene, this); @@ -414,21 +416,20 @@ void WheeledVehicle::OnDebugDrawSelected() } } - // Draw anti roll bars axes - int wheelsCount = _wheels.Count(); - for (int i = 0; i < GetAntiRollBars().Count(); i++) + // Anti roll bars axes + const int32 wheelsCount = _wheels.Count(); + for (int32 i = 0; i < GetAntiRollBars().Count(); i++) { - int axleIndex = GetAntiRollBars()[i].Axle; - int leftWheelIndex = axleIndex * 2; - int rightWheelIndex = leftWheelIndex + 1; - + const int32 axleIndex = GetAntiRollBars()[i].Axle; + const int32 leftWheelIndex = axleIndex * 2; + const int32 rightWheelIndex = leftWheelIndex + 1; if (leftWheelIndex >= wheelsCount || rightWheelIndex >= wheelsCount) continue; if (!_wheels[leftWheelIndex].Collider || !_wheels[rightWheelIndex].Collider) continue; - DEBUG_DRAW_LINE(_wheels[leftWheelIndex].Collider->GetPosition(), _wheels[rightWheelIndex].Collider->GetPosition(), Color::Yellow, 0, false); } + // Center of mass DEBUG_DRAW_WIRE_SPHERE(BoundingSphere(_transform.LocalToWorld(_centerOfMassOffset), 10.0f), Color::Blue, 0, false); @@ -437,7 +438,7 @@ void WheeledVehicle::OnDebugDrawSelected() #endif -void WheeledVehicle::Serialize(SerializeStream& stream, const void *otherObj) +void WheeledVehicle::Serialize(SerializeStream& stream, const void* otherObj) { RigidBody::Serialize(stream, otherObj); @@ -454,7 +455,7 @@ void WheeledVehicle::Serialize(SerializeStream& stream, const void *otherObj) SERIALIZE_MEMBER(AntiRollBars, _antiRollBars); } -void WheeledVehicle::Deserialize(DeserializeStream& stream, ISerializeModifier *modifier) +void WheeledVehicle::Deserialize(DeserializeStream& stream, ISerializeModifier* modifier) { RigidBody::Deserialize(stream, modifier); @@ -472,7 +473,7 @@ void WheeledVehicle::Deserialize(DeserializeStream& stream, ISerializeModifier * _fixInvalidForwardDir |= modifier->EngineBuild < 6341; } -void WheeledVehicle::OnColliderChanged(Collider *c) +void WheeledVehicle::OnColliderChanged(Collider* c) { RigidBody::OnColliderChanged(c); @@ -495,7 +496,7 @@ void WheeledVehicle::OnActiveInTreeChanged() Setup(); } -void WheeledVehicle::OnPhysicsSceneChanged(PhysicsScene *previous) +void WheeledVehicle::OnPhysicsSceneChanged(PhysicsScene* previous) { RigidBody::OnPhysicsSceneChanged(previous); @@ -518,10 +519,9 @@ void WheeledVehicle::OnTransformChanged() // Transform all vehicle children around the vehicle origin to fix the vehicle facing direction const Quaternion rotationDelta(0.0f, -0.7071068f, 0.0f, 0.7071068f); const Vector3 origin = GetPosition(); - for (Actor *child : Children) + for (Actor* child : Children) { Transform trans = child->GetTransform(); - ; const Vector3 pivotOffset = trans.Translation - origin; if (pivotOffset.IsZero()) { @@ -541,7 +541,7 @@ void WheeledVehicle::OnTransformChanged() } } -void WheeledVehicle::BeginPlay(SceneBeginData *data) +void WheeledVehicle::BeginPlay(SceneBeginData* data) { RigidBody::BeginPlay(data); @@ -550,14 +550,14 @@ void WheeledVehicle::BeginPlay(SceneBeginData *data) #endif #if USE_EDITOR - GetSceneRendering()->AddPhysicsDebug(this); + GetSceneRendering()->AddPhysicsDebug(this); #endif } void WheeledVehicle::EndPlay() { #if USE_EDITOR - GetSceneRendering()->RemovePhysicsDebug(this); + GetSceneRendering()->RemovePhysicsDebug(this); #endif #if WITH_VEHICLE diff --git a/Source/Engine/Physics/Actors/WheeledVehicle.h b/Source/Engine/Physics/Actors/WheeledVehicle.h index 2bc42d6d7..b2522bce3 100644 --- a/Source/Engine/Physics/Actors/WheeledVehicle.h +++ b/Source/Engine/Physics/Actors/WheeledVehicle.h @@ -29,16 +29,16 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo // Tank Drive. Can have more than 4 wheel. Not use steer, control acceleration for each tank track. Tank, }; + /// - /// Vehicle driving types. - /// Used only on tanks to specify the drive mode. + /// Vehicle driving types. Used only on tanks to specify the drive mode. /// API_ENUM() enum class DriveModes { - // Drive turning the vehicle using only one track + // Drive turning the vehicle using only one track. Standard, // Drive turning the vehicle using all tracks inverse direction. - Special + Special, }; /// @@ -52,21 +52,14 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo /// /// The vehicle speed. /// - API_FIELD(Attributes = "Limit(0)") float Speed; + API_FIELD(Attributes = "Limit(0)") float Speed = 1000; /// /// The target max steer of the speed. /// - API_FIELD(Attributes = "Limit(0, 1)") float Steer; + API_FIELD(Attributes = "Limit(0, 1)") float Steer = 1; - /// - /// Create a Steer/Speed relationship structure. - /// - SteerControl() - { - Speed = 1000; - Steer = 1; - } + SteerControl() = default; /// /// Create a Steer/Speed relationship structure. @@ -177,8 +170,7 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo API_FIELD(Attributes="EditorOrder(17)") float FallRateSteer = 5.0f; /// - /// Vehicle control relationship between speed and steer. The higher is the speed, - /// decrease steer to make vehicle more maneuverable (limited only 4 relationships). + /// Vehicle control relationship between speed and steer. The higher is the speed, decrease steer to make vehicle more maneuverable (limited only 4 relationships). /// API_FIELD() Array SteerVsSpeed = Array { @@ -249,7 +241,7 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo /// /// Number of gears to move to forward /// - API_FIELD(Attributes = "Limit(1, 30)") int ForwardGearsRatios = 5; + API_FIELD(Attributes = "Limit(1, 30)") int32 ForwardGearsRatios = 5; /// /// Time it takes to switch gear. Specified in seconds (s). @@ -429,7 +421,7 @@ API_CLASS(Attributes="ActorContextMenu(\"New/Physics/Wheeled Vehicle\"), ActorTo /// /// The specific axle with wheels to apply anti roll. /// - API_FIELD() int Axle; + API_FIELD() int32 Axle; /// /// The anti roll stiffness. @@ -449,8 +441,8 @@ private: DriveTypes _driveType = DriveTypes::Drive4W, _driveTypeCurrent; Array> _wheelsData; float _throttle = 0.0f, _steering = 0.0f, _brake = 0.0f, _handBrake = 0.0f, _tankLeftThrottle = 0.0f, _tankRightThrottle = 0.0f, _tankLeftBrake = 0.0f, _tankRightBrake = 0.0f; - Array _wheels; - Array _antiRollBars; + Array _wheels; + Array _antiRollBars; DriveControlSettings _driveControl; EngineSettings _engine; DifferentialSettings _differential; @@ -494,7 +486,7 @@ public: /// /// Sets the vehicle drive control settings. /// - API_PROPERTY() void SetDriveControl(DriveControlSettings& value); + API_PROPERTY() void SetDriveControl(DriveControlSettings value); /// /// Sets the vehicle wheels settings. @@ -532,7 +524,7 @@ public: API_PROPERTY() void SetGearbox(const GearboxSettings& value); // - /// Sets axles anti roll bars to increase vehicle estability. + /// Sets axles anti roll bars to increase vehicle stability. /// API_PROPERTY() void SetAntiRollBars(const Array& value); diff --git a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp index f0b6914ae..8339b0f23 100644 --- a/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp +++ b/Source/Engine/Physics/PhysX/PhysicsBackendPhysX.cpp @@ -59,6 +59,7 @@ #if WITH_PVD #include #endif + // Temporary memory size used by the PhysX during the simulation. Must be multiply of 4kB and 16bit aligned. #define PHYSX_SCRATCH_BLOCK_SIZE (1024 * 128) @@ -975,6 +976,7 @@ void PhysicalMaterial::UpdatePhysicsMaterial() bool CollisionCooking::CookConvexMesh(CookingInput& input, BytesContainer& output) { + PROFILE_CPU(); ENSURE_CAN_COOK; if (input.VertexCount == 0) LOG(Warning, "Empty mesh data for collision cooking."); @@ -1018,6 +1020,7 @@ bool CollisionCooking::CookConvexMesh(CookingInput& input, BytesContainer& outpu bool CollisionCooking::CookTriangleMesh(CookingInput& input, BytesContainer& output) { + PROFILE_CPU(); ENSURE_CAN_COOK; if (input.VertexCount == 0 || input.IndexCount == 0) LOG(Warning, "Empty mesh data for collision cooking."); @@ -1052,6 +1055,7 @@ bool CollisionCooking::CookTriangleMesh(CookingInput& input, BytesContainer& out bool CollisionCooking::CookHeightField(int32 cols, int32 rows, const PhysicsBackend::HeightFieldSample* data, WriteStream& stream) { + PROFILE_CPU(); ENSURE_CAN_COOK; PxHeightFieldDesc heightFieldDesc; @@ -1444,45 +1448,45 @@ void PhysicsBackend::EndSimulateScene(void* scene) const float breakThreshold = 8.0f; const float forwardSpeed = wheelVehicle->GetForwardSpeed(); int currentGear = wheelVehicle->GetCurrentGear(); - // Tank tracks direction: 1 forward -1 backward 0 neutral + // Tank tracks direction: 1 forward -1 backward 0 neutral bool toForward = false; toForward |= throttle > deadZone; - toForward |= (leftThrottle > deadZone) && (rightThrottle > deadZone); // 1 1 + toForward |= (leftThrottle > deadZone) && (rightThrottle > deadZone); // 1 1 bool toBackward = false; toBackward |= throttle < -deadZone; - toBackward |= (leftThrottle < -deadZone) && (rightThrottle < -deadZone); // -1 -1 - toBackward |= (leftThrottle < -deadZone) && (rightThrottle < deadZone); // -1 0 - toBackward |= (leftThrottle < deadZone) && (rightThrottle < -deadZone); // 0 -1 + toBackward |= (leftThrottle < -deadZone) && (rightThrottle < -deadZone); // -1 -1 + toBackward |= (leftThrottle < -deadZone) && (rightThrottle < deadZone); // -1 0 + toBackward |= (leftThrottle < deadZone) && (rightThrottle < -deadZone); // 0 -1 bool isTankTurning = false; if (isTank) - { - isTankTurning |= leftThrottle > deadZone && rightThrottle < -deadZone; // 1 -1 - isTankTurning |= leftThrottle < -deadZone && rightThrottle > deadZone; // -1 1 - isTankTurning |= leftThrottle < deadZone && rightThrottle > deadZone; // 0 1 - isTankTurning |= leftThrottle > deadZone && rightThrottle < deadZone; // 1 0 - isTankTurning |= leftThrottle < -deadZone && rightThrottle < deadZone; // -1 0 - isTankTurning |= leftThrottle < deadZone && rightThrottle < -deadZone; // 0 -1 + { + isTankTurning |= leftThrottle > deadZone && rightThrottle < -deadZone; // 1 -1 + isTankTurning |= leftThrottle < -deadZone && rightThrottle > deadZone; // -1 1 + isTankTurning |= leftThrottle < deadZone && rightThrottle > deadZone; // 0 1 + isTankTurning |= leftThrottle > deadZone && rightThrottle < deadZone; // 1 0 + isTankTurning |= leftThrottle < -deadZone && rightThrottle < deadZone; // -1 0 + isTankTurning |= leftThrottle < deadZone && rightThrottle < -deadZone; // 0 -1 if (toForward || toBackward) { isTankTurning = false; } } - + // Automatic gear change when changing driving direction if (Math::Abs(forwardSpeed) < invalidDirectionThreshold) { - int targetGear = wheelVehicle->GetTargetGear(); + int targetGear = wheelVehicle->GetTargetGear(); if (toBackward && currentGear > 0 && targetGear >= 0) { currentGear = -1; } else if (!toBackward && currentGear <= 0 && targetGear <= 0) { - currentGear = 1; + currentGear = 1; } else if (isTankTurning && currentGear <= 0) { @@ -1565,24 +1569,25 @@ void PhysicsBackend::EndSimulateScene(void* scene) leftBrake = 1.0f; } - // Smooth input controls + // Smooth input controls + // @formatter:off PxVehiclePadSmoothingData padSmoothing = + { { - { - wheelVehicle->_driveControl.RiseRateAcceleration, // rise rate eANALOG_INPUT_ACCEL - wheelVehicle->_driveControl.RiseRateBrake, // rise rate eANALOG_INPUT_BRAKE - wheelVehicle->_driveControl.RiseRateHandBrake, // rise rate eANALOG_INPUT_HANDBRAKE - wheelVehicle->_driveControl.RiseRateSteer, // rise rate eANALOG_INPUT_STEER_LEFT - wheelVehicle->_driveControl.RiseRateSteer, // rise rate eANALOG_INPUT_STEER_RIGHT - }, - { - wheelVehicle->_driveControl.FallRateAcceleration, // fall rate eANALOG_INPUT_ACCEL - wheelVehicle->_driveControl.FallRateBrake, // fall rate eANALOG_INPUT_BRAKE - wheelVehicle->_driveControl.FallRateHandBrake, // fall rate eANALOG_INPUT_HANDBRAKE - wheelVehicle->_driveControl.FallRateSteer, // fall rate eANALOG_INPUT_STEER_LEFT - wheelVehicle->_driveControl.FallRateSteer, // fall rate eANALOG_INPUT_STEER_RIGHT - } - }; + wheelVehicle->_driveControl.RiseRateAcceleration, // rise rate eANALOG_INPUT_ACCEL + wheelVehicle->_driveControl.RiseRateBrake, // rise rate eANALOG_INPUT_BRAKE + wheelVehicle->_driveControl.RiseRateHandBrake, // rise rate eANALOG_INPUT_HANDBRAKE + wheelVehicle->_driveControl.RiseRateSteer, // rise rate eANALOG_INPUT_STEER_LEFT + wheelVehicle->_driveControl.RiseRateSteer, // rise rate eANALOG_INPUT_STEER_RIGHT + }, + { + wheelVehicle->_driveControl.FallRateAcceleration, // fall rate eANALOG_INPUT_ACCEL + wheelVehicle->_driveControl.FallRateBrake, // fall rate eANALOG_INPUT_BRAKE + wheelVehicle->_driveControl.FallRateHandBrake, // fall rate eANALOG_INPUT_HANDBRAKE + wheelVehicle->_driveControl.FallRateSteer, // fall rate eANALOG_INPUT_STEER_LEFT + wheelVehicle->_driveControl.FallRateSteer, // fall rate eANALOG_INPUT_STEER_RIGHT + } + }; PxVehicleKeySmoothingData keySmoothing = { { @@ -1600,6 +1605,7 @@ void PhysicsBackend::EndSimulateScene(void* scene) wheelVehicle->_driveControl.FallRateSteer, // fall rate eANALOG_INPUT_STEER_RIGHT } }; + // @formatter:on // Reduce steer by speed to make vehicle more easier to maneuver @@ -1659,14 +1665,13 @@ void PhysicsBackend::EndSimulateScene(void* scene) rawInputData.setAnalogRightBrake(rightBrake); rawInputData.setAnalogLeftThrust(leftThrottle); rawInputData.setAnalogRightThrust(rightThrottle); - PxVehicleDriveTankSmoothAnalogRawInputsAndSetAnalogInputs(padSmoothing, rawInputData, scenePhysX->LastDeltaTime, *(PxVehicleDriveTank*)drive); break; } } } else - { + { switch (wheelVehicle->_driveTypeCurrent) { case WheeledVehicle::DriveTypes::Drive4W: @@ -1693,7 +1698,7 @@ void PhysicsBackend::EndSimulateScene(void* scene) } case WheeledVehicle::DriveTypes::Tank: { - // Convert analogic inputs to digital inputs. + // Convert analog inputs to digital inputs leftThrottle = Math::Round(leftThrottle); rightThrottle = Math::Round(rightThrottle); leftBrake = Math::Round(leftBrake); @@ -1707,8 +1712,8 @@ void PhysicsBackend::EndSimulateScene(void* scene) rawInputData.setAnalogLeftThrust(leftThrottle); rawInputData.setAnalogRightThrust(rightThrottle); - // Needs to pass analogic values to vehicle to maintein current moviment direction because digital inputs accept only true/false values to tracks thrust instead of -1 to 1 - PxVehicleDriveTankSmoothAnalogRawInputsAndSetAnalogInputs(padSmoothing, rawInputData, scenePhysX->LastDeltaTime, *(PxVehicleDriveTank *)drive); + // Needs to pass analog values to vehicle to maintain current movement direction because digital inputs accept only true/false values to tracks thrust instead of -1 to 1 + PxVehicleDriveTankSmoothAnalogRawInputsAndSetAnalogInputs(padSmoothing, rawInputData, scenePhysX->LastDeltaTime, *(PxVehicleDriveTank*)drive); break; } } @@ -2428,7 +2433,7 @@ void PhysicsBackend::SetRigidActorPose(void* actor, const Vector3& position, con actorPhysX->setGlobalPose(trans, wakeUp); } else - actorPhysX->setKinematicTarget(trans); + actorPhysX->setKinematicTarget(trans); } else { @@ -2964,7 +2969,7 @@ void PhysicsBackend::SetHingeJointLimit(void* joint, const LimitAngularRange& va void PhysicsBackend::SetHingeJointDrive(void* joint, const HingeJointDrive& value) { auto jointPhysX = (PxRevoluteJoint*)joint; - jointPhysX->setDriveVelocity(Math::Max(value.Velocity, 0.0f)); + jointPhysX->setDriveVelocity(value.Velocity); jointPhysX->setDriveForceLimit(Math::Max(value.ForceLimit, 0.0f)); jointPhysX->setDriveGearRatio(Math::Max(value.GearRatio, 0.0f)); jointPhysX->setRevoluteJointFlag(PxRevoluteJointFlag::eDRIVE_FREESPIN, value.FreeSpin); @@ -3265,7 +3270,6 @@ PxVehicleDifferentialNWData CreatePxVehicleDifferentialNWData(const WheeledVehic PxVehicleDifferentialNWData differentialNwData; for (int32 i = 0; i < wheels.Count(); i++) differentialNwData.setDrivenWheel(i, true); - return differentialNwData; } @@ -3286,7 +3290,7 @@ PxVehicleGearsData CreatePxVehicleGearsData(const WheeledVehicle::GearboxSetting PxVehicleGearsData gears; // Total gears is forward gears + neutral/rear gears - int gearsAmount = settings.ForwardGearsRatios + 2; + const int32 gearsCount = Math::Clamp(settings.ForwardGearsRatios + 2, 2, PxVehicleGearsData::eGEARSRATIO_COUNT); // Setup gears torque/top speed relations // Higher torque = less speed @@ -3302,24 +3306,23 @@ PxVehicleGearsData CreatePxVehicleGearsData(const WheeledVehicle::GearboxSetting // Gear4 = 1.8 // Gear5 = 1 - gears.mRatios[0] = -(gearsAmount - 2); // reverse - gears.mRatios[1] = 0; // neutral + gears.mRatios[0] = (float)-(gearsCount - 2); // Reverse + gears.mRatios[1] = 0; // Neutral // Setup all gears except neutral and reverse - for (int i = gearsAmount; i > 2; i--) { - - float gearsRatios = settings.ForwardGearsRatios; - float currentGear = i - 2; - - gears.mRatios[i] = Math::Lerp(gearsRatios, 1.0f, (currentGear / gearsRatios)); + for (int32 i = gearsCount; i > 2; i--) + { + float gearsRatios = (float)settings.ForwardGearsRatios; + float currentGear = i - 2.0f; + gears.mRatios[i] = Math::Lerp(gearsRatios, 1.0f, currentGear / gearsRatios); } - // reset unused gears - for (int i = gearsAmount; i < PxVehicleGearsData::eGEARSRATIO_COUNT; i++) + // Reset unused gears + for (int32 i = gearsCount; i < PxVehicleGearsData::eGEARSRATIO_COUNT; i++) gears.mRatios[i] = 0; gears.mSwitchTime = Math::Max(settings.SwitchTime, 0.0f); - gears.mNbRatios = Math::Clamp(gearsAmount, 2, (int)PxVehicleGearsData::eGEARSRATIO_COUNT); + gears.mNbRatios = gearsCount; return gears; } @@ -3388,7 +3391,7 @@ PxVehicleAckermannGeometryData CreatePxVehicleAckermannGeometryData(PxVehicleWhe return ackermann; } -PxVehicleAntiRollBarData CreatePxPxVehicleAntiRollBarData(const WheeledVehicle::AntiRollBar& settings, int leftWheelIndex, int rightWheelIndex) +PxVehicleAntiRollBarData CreatePxPxVehicleAntiRollBarData(const WheeledVehicle::AntiRollBar& settings, int32 leftWheelIndex, int32 rightWheelIndex) { PxVehicleAntiRollBarData antiRollBar; antiRollBar.mWheel0 = leftWheelIndex; @@ -3397,7 +3400,7 @@ PxVehicleAntiRollBarData CreatePxPxVehicleAntiRollBarData(const WheeledVehicle:: return antiRollBar; } -bool SortWheelsFrontToBack(WheeledVehicle::Wheel const &a, WheeledVehicle::Wheel const &b) +bool SortWheelsFrontToBack(WheeledVehicle::Wheel const& a, WheeledVehicle::Wheel const& b) { return a.Collider && b.Collider && a.Collider->GetLocalPosition().Z > b.Collider->GetLocalPosition().Z; } @@ -3412,17 +3415,15 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) { Sorting::QuickSort(actor->_wheels.Get(), actor->_wheels.Count(), SortWheelsFrontToBack); - // sort wheels by side + // Sort wheels by side if (actor->_driveType == WheeledVehicle::DriveTypes::Tank) { - for (int i = 0; i < actor->_wheels.Count() - 1; i += 2) + for (int32 i = 0; i < actor->_wheels.Count() - 1; i += 2) { auto a = actor->_wheels[i]; auto b = actor->_wheels[i + 1]; - if (!a.Collider || !b.Collider) continue; - if (a.Collider->GetLocalPosition().X > b.Collider->GetLocalPosition().X) { actor->_wheels[i] = b; @@ -3482,7 +3483,7 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) // TODO: get gravityDirection from scenePhysX->Scene->getGravity() PxVehicleComputeSprungMasses(wheels.Count(), offsets, centerOfMassOffset.p, mass, 1, sprungMasses); PxVehicleWheelsSimData* wheelsSimData = PxVehicleWheelsSimData::allocate(wheels.Count()); - int wheelsCount = wheels.Count(); + int32 wheelsCount = wheels.Count(); for (int32 i = 0; i < wheelsCount; i++) { auto& wheel = *wheels[i]; @@ -3498,7 +3499,7 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) const PxVehicleWheelData& wheelData = CreatePxVehicleWheelData(wheel); const PxVehicleSuspensionData& suspensionData = CreatePxVehicleSuspensionData(wheel, sprungMasses[i]); - wheelsSimData->setTireData(i,tireData); + wheelsSimData->setTireData(i, tireData); wheelsSimData->setWheelData(i, wheelData); wheelsSimData->setSuspensionData(i, suspensionData); wheelsSimData->setSuspTravelDirection(i, centerOfMassOffset.rotate(PxVec3(0.0f, -1.0f, 0.0f))); @@ -3530,16 +3531,15 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) } } // Add Anti roll bars for wheels axles - for (int i = 0; i < actor->GetAntiRollBars().Count(); i++) + for (int32 i = 0; i < actor->GetAntiRollBars().Count(); i++) { - int axleIndex = actor->GetAntiRollBars()[i].Axle; - int leftWheelIndex = axleIndex * 2; - int rightWheelIndex = leftWheelIndex + 1; - + int32 axleIndex = actor->GetAntiRollBars()[i].Axle; + int32 leftWheelIndex = axleIndex * 2; + int32 rightWheelIndex = leftWheelIndex + 1; if (leftWheelIndex >= wheelsCount || rightWheelIndex >= wheelsCount) continue; - const PxVehicleAntiRollBarData &antiRollBar = CreatePxPxVehicleAntiRollBarData(actor->GetAntiRollBars()[i], leftWheelIndex, rightWheelIndex); + const PxVehicleAntiRollBarData& antiRollBar = CreatePxPxVehicleAntiRollBarData(actor->GetAntiRollBars()[i], leftWheelIndex, rightWheelIndex); wheelsSimData->addAntiRollBarData(antiRollBar); } for (auto child : actor->Children) @@ -3582,14 +3582,14 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) const PxVehicleEngineData& engineData = CreatePxVehicleEngineData(engine); const PxVehicleGearsData& gearsData = CreatePxVehicleGearsData(gearbox); const PxVehicleAutoBoxData& autoBoxData = CreatePxVehicleAutoBoxData(); - const PxVehicleClutchData& cluchData = CreatePxVehicleClutchData(gearbox); + const PxVehicleClutchData& clutchData = CreatePxVehicleClutchData(gearbox); const PxVehicleAckermannGeometryData& geometryData = CreatePxVehicleAckermannGeometryData(wheelsSimData); driveSimData.setDiffData(differentialData); driveSimData.setEngineData(engineData); driveSimData.setGearsData(gearsData); driveSimData.setAutoBoxData(autoBoxData); - driveSimData.setClutchData(cluchData); + driveSimData.setClutchData(clutchData); driveSimData.setAckermannGeometryData(geometryData); // Create vehicle drive @@ -3607,14 +3607,13 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) const PxVehicleEngineData& engineData = CreatePxVehicleEngineData(engine); const PxVehicleGearsData& gearsData = CreatePxVehicleGearsData(gearbox); const PxVehicleAutoBoxData& autoBoxData = CreatePxVehicleAutoBoxData(); - const PxVehicleClutchData& cluchData = CreatePxVehicleClutchData(gearbox); - const PxVehicleAckermannGeometryData& geometryData = CreatePxVehicleAckermannGeometryData(wheelsSimData); + const PxVehicleClutchData& clutchData = CreatePxVehicleClutchData(gearbox); driveSimData.setDiffData(differentialData); driveSimData.setEngineData(engineData); driveSimData.setGearsData(gearsData); driveSimData.setAutoBoxData(autoBoxData); - driveSimData.setClutchData(cluchData); + driveSimData.setClutchData(clutchData); // Create vehicle drive auto driveNW = PxVehicleDriveNW::allocate(wheels.Count()); @@ -3635,18 +3634,18 @@ void* PhysicsBackend::CreateVehicle(WheeledVehicle* actor) case WheeledVehicle::DriveTypes::Tank: { PxVehicleDriveSimData4W driveSimData; - const PxVehicleDifferential4WData &differentialData = CreatePxVehicleDifferential4WData(differential); - const PxVehicleEngineData &engineData = CreatePxVehicleEngineData(engine); - const PxVehicleGearsData &gearsData = CreatePxVehicleGearsData(gearbox); - const PxVehicleAutoBoxData &autoBoxData = CreatePxVehicleAutoBoxData(); - const PxVehicleClutchData &cluchData = CreatePxVehicleClutchData(gearbox); - const PxVehicleAckermannGeometryData &geometryData = CreatePxVehicleAckermannGeometryData(wheelsSimData); + const PxVehicleDifferential4WData& differentialData = CreatePxVehicleDifferential4WData(differential); + const PxVehicleEngineData& engineData = CreatePxVehicleEngineData(engine); + const PxVehicleGearsData& gearsData = CreatePxVehicleGearsData(gearbox); + const PxVehicleAutoBoxData& autoBoxData = CreatePxVehicleAutoBoxData(); + const PxVehicleClutchData& clutchData = CreatePxVehicleClutchData(gearbox); + const PxVehicleAckermannGeometryData& geometryData = CreatePxVehicleAckermannGeometryData(wheelsSimData); driveSimData.setDiffData(differentialData); driveSimData.setEngineData(engineData); driveSimData.setGearsData(gearsData); driveSimData.setAutoBoxData(autoBoxData); - driveSimData.setClutchData(cluchData); + driveSimData.setClutchData(clutchData); driveSimData.setAckermannGeometryData(geometryData); // Create vehicle drive @@ -3700,24 +3699,24 @@ void PhysicsBackend::UpdateVehicleWheels(WheeledVehicle* actor) } } -void PhysicsBackend::UpdateVehicleAntiRollBars(WheeledVehicle *actor) +void PhysicsBackend::UpdateVehicleAntiRollBars(WheeledVehicle* actor) { int wheelsCount = actor->_wheels.Count(); - auto drive = (PxVehicleWheels *)actor->_vehicle; - PxVehicleWheelsSimData *wheelsSimData = &drive->mWheelsSimData; + auto drive = (PxVehicleWheels*)actor->_vehicle; + PxVehicleWheelsSimData* wheelsSimData = &drive->mWheelsSimData; - // Update Anti roll bars for wheels axles - for (int i = 0; i < actor->GetAntiRollBars().Count(); i++) + // Update anti roll bars for wheels axles + const auto& antiRollBars = actor->GetAntiRollBars(); + for (int32 i = 0; i < antiRollBars.Count(); i++) { - int axleIndex = actor->GetAntiRollBars()[i].Axle; - int leftWheelIndex = axleIndex * 2; - int rightWheelIndex = leftWheelIndex + 1; - + const int32 axleIndex = antiRollBars.Get()[i].Axle; + const int32 leftWheelIndex = axleIndex * 2; + const int32 rightWheelIndex = leftWheelIndex + 1; if (leftWheelIndex >= wheelsCount || rightWheelIndex >= wheelsCount) continue; - const PxVehicleAntiRollBarData &antiRollBar = CreatePxPxVehicleAntiRollBarData(actor->GetAntiRollBars()[i], leftWheelIndex, rightWheelIndex); - if (wheelsSimData->getNbAntiRollBarData() - 1 < i) + const PxVehicleAntiRollBarData& antiRollBar = CreatePxPxVehicleAntiRollBarData(antiRollBars.Get()[i], leftWheelIndex, rightWheelIndex); + if ((int32)wheelsSimData->getNbAntiRollBarData() - 1 < i) { wheelsSimData->addAntiRollBarData(antiRollBar); } @@ -3753,8 +3752,8 @@ void PhysicsBackend::SetVehicleEngine(void* vehicle, const void* value) case PxVehicleTypes::eDRIVETANK: { auto driveTank = (PxVehicleDriveTank*)drive; - const PxVehicleEngineData &engineData = CreatePxVehicleEngineData(engine); - PxVehicleDriveSimData &driveSimData = driveTank->mDriveSimData; + const PxVehicleEngineData& engineData = CreatePxVehicleEngineData(engine); + PxVehicleDriveSimData& driveSimData = driveTank->mDriveSimData; driveSimData.setEngineData(engineData); break; } @@ -3812,11 +3811,11 @@ void PhysicsBackend::SetVehicleGearbox(void* vehicle, const void* value) } case PxVehicleTypes::eDRIVETANK: { - auto driveTank = (PxVehicleDriveTank *)drive; - const PxVehicleGearsData &gearData = CreatePxVehicleGearsData(gearbox); - const PxVehicleClutchData &clutchData = CreatePxVehicleClutchData(gearbox); - const PxVehicleAutoBoxData &autoBoxData = CreatePxVehicleAutoBoxData(); - PxVehicleDriveSimData &driveSimData = driveTank->mDriveSimData; + auto driveTank = (PxVehicleDriveTank*)drive; + const PxVehicleGearsData& gearData = CreatePxVehicleGearsData(gearbox); + const PxVehicleClutchData& clutchData = CreatePxVehicleClutchData(gearbox); + const PxVehicleAutoBoxData& autoBoxData = CreatePxVehicleAutoBoxData(); + PxVehicleDriveSimData& driveSimData = driveTank->mDriveSimData; driveSimData.setGearsData(gearData); driveSimData.setAutoBoxData(autoBoxData); driveSimData.setClutchData(clutchData);