AIDrivingComponent.zig

gistfile1.txt

const std = @import("std");
const basis = @import("basis");
const vhl = @import("vhl");
const means = @import("../means.zig");

const GameObjectComponent = basis.component_contexts.GameObjectComponent;

const Message = basis.messaging.Message;
const MessageParameters = basis.messaging.MessageParameters;

const AutoGearBoxComponent = vhl.components.AutoGearBoxComponent;
const VehicleControllerComponent = vhl.components.VehicleControllerComponent;
const VehicleControllerPtr = basis.physics.vehicle_controller.VehicleControllerPtr;
const VehicleSpeedController = vhl.vehicle_speed_controller.VehicleSpeedController;

const PropagatedValue = basis.network.PropagatedValue;
const PropagatedValueHandle = basis.network.PropagatedValueHandle;

const Driveline = means.ai.driveline.Driveline;

const Vec3 = basis.math.Vec3;

const zigfsm = means.thirdparty.zigfsm;

const ENABLE_DEBUG_DRAW = false;

//----------------------------------------------------

const State = enum {
    Inactive,
    FollowDriveline,
    ReverseTurn,
    Stopped,
};

const Event = enum {
    MoveTo,
    Stop,
    Deactivate,
    StartReverseTurn,
    ReverseTurnComplete,
    AtDestination,
};

const TransitionTable = [_]zigfsm.Transition(State, Event){
    // MoveTo.
    .{ .event = .MoveTo, .from = .Inactive, .to = .FollowDriveline },
    .{ .event = .MoveTo, .from = .ReverseTurn, .to = .FollowDriveline },
    .{ .event = .MoveTo, .from = .Stopped, .to = .FollowDriveline },
    .{ .event = .MoveTo, .from = .FollowDriveline, .to = .FollowDriveline },
    // Stop.
    .{ .event = .Stop, .from = .FollowDriveline, .to = .Stopped },
    .{ .event = .Stop, .from = .ReverseTurn, .to = .Stopped },
    // Reverse turn handling.
    .{ .event = .StartReverseTurn, .from = .FollowDriveline, .to = .ReverseTurn },
    .{ .event = .ReverseTurnComplete, .from = .ReverseTurn, .to = .FollowDriveline },
    // Deactivate.
    .{ .event = .Deactivate, .from = .FollowDriveline, .to = .Inactive },
    .{ .event = .Deactivate, .from = .ReverseTurn, .to = .Inactive },
    .{ .event = .Deactivate, .from = .Stopped, .to = .Inactive },
    // At destination.
    .{ .event = .AtDestination, .from = .FollowDriveline, .to = .Stopped },
    .{ .event = .AtDestination, .from = .ReverseTurn, .to = .Stopped },
};

const AIDrivingFSM = zigfsm.StateMachineFromTable(State, Event, &TransitionTable, .Inactive, &.{});

//----------------------------------------------------

pub const AIDrivingComponent = struct {
    const Self = @This();
    pub const RegistrationName = "means.AIDrivingComponent";

    const STEERING_SPEED = 0.13; // How much to turn the wheels during one tick.

    // Order = 60 which makes this component update after VehicleAvatarComponent.
    // This is important if we have a GO with both components. If the AI is active
    // it will tick after the non-AI movement component, overriding the input to the vehicle.
    pub const UpdateOrder = 60;

    //----------------------------------------------------

    context: GameObjectComponent,
    blueprintProperties: ?*const AIDrivingComponentProperties = null,

    autoGearBoxComponent: *AutoGearBoxComponent = undefined,
    vehicleControllerComponent: *VehicleControllerComponent = undefined,

    vehicleController: VehicleControllerPtr = VehicleControllerPtr.initNull(),

    fsm: AIDrivingFSM,
    fsmHandler: AIDrivingFSM.Handler,
    fsmHandlers: [1]*AIDrivingFSM.Handler = undefined,

    // Propagated values:
    active: PropagatedValueHandle(bool),
    reversingEnabled: PropagatedValueHandle(bool),
    acceleration: PropagatedValueHandle(f32),
    brake: PropagatedValueHandle(f32),
    steering: PropagatedValueHandle(f32),

    currentTargetPosition: Vec3 = Vec3.Zero,
    currentArrivalDistanceEpsilon: f32 = 0.0,
    currentMovementProfile: means.ai.AIMovementProfile = means.ai.AIMovementProfile.Normal,
    currentMovementCallback: means.ai.AIMovementCallback = .{},

    currentSteeringValue: f32 = 0.0,
    currentSpeedMultiplier: f32 = 1.0,

    speedController: VehicleSpeedController = VehicleSpeedController.init(),

    driveline: Driveline,

    //----------------------------------------------------

    pub fn init(context: GameObjectComponent) !Self {
        return Self{
            .context = context,
            .fsm = AIDrivingFSM.init(),
            .fsmHandler = zigfsm.Interface.make(AIDrivingFSM.Handler, Self),
            .active = PropagatedValue(bool).init(context, "active", true, true, false),
            .reversingEnabled = PropagatedValue(bool).init(context, "reversingEnabled", true, true, false),
            .acceleration = PropagatedValue(f32).init(context, "acceleration", false, true, 0.0),
            .brake = PropagatedValue(f32).init(context, "brake", false, true, 0.0),
            .steering = PropagatedValue(f32).init(context, "steering", false, true, 0.0),
            .driveline = Driveline.init(context.allocator),
        };
    }

    //----------------------------------------------------

    pub fn create(self: *Self) !void {
        // Set the component instance as the transition handler for its FSM.
        // Turning a single-item ptr into a slice seems to be a bit tricky right now,
        // so running via single-item array for now: https://github.com/ziglang/zig/issues/6391
        self.fsmHandlers[0] = &self.fsmHandler;
        self.fsm.setTransitionHandlers(&self.fsmHandlers);

        self.reversingEnabled.setValueChangedCallback(
            PropagatedValue(bool).Callback.initMethod(self, Self, onReversingEnabledChanged),
        );

        self.driveline.flags = means.ai.driveline.Flags.LowerSpeedWithHighSteeringAngles.asInt();
    }

    pub fn destroy(self: *Self) !void {
        self.active.deinit();
        self.reversingEnabled.deinit();
        self.acceleration.deinit();
        self.brake.deinit();
        self.steering.deinit();

        self.driveline.deinit();
    }

    pub fn onObjectCreated(self: *Self) !void {
        var go = self.context.getGameObject();

        {
            var comp = go.getComponent(VehicleControllerComponent);
            basis.assertd(comp != null, "VehicleControllerComponent not found.");
            self.vehicleControllerComponent = comp.?;

            basis.assertd(
                self.vehicleControllerComponent.controller != null,
                "If we have a vehicle controller component, there should also be a controller.",
            );
            self.vehicleController = self.vehicleControllerComponent.controller.?;
        }

        {
            var comp = go.getComponent(AutoGearBoxComponent);
            basis.assertd(comp != null, "Auto-gearbox component not found.");
            self.autoGearBoxComponent = comp.?;

            // Not gonna do this here. We'll let the VehicleAvatarComponent handle this.
            //self.autoGearBoxComponent.initAutoGearBox(self.vehicleDescription.autoGearBoxParameters.str(), self.vehicleController);
        }

        if (!self.context.inEditor()) {
            // TODO: Init whisker arrays here.
        }
    }

    pub fn update(self: *Self, deltaTime: f32) !void {
        _ = deltaTime;

        if (ENABLE_DEBUG_DRAW) {
            if (self.active.get() and self.context.onServer()) {
                self.driveline.debugDraw(true, true);

                // TODO: Debug draw whisker arrays here.
            }
        }
    }

    pub fn preTick(self: *Self, tickDeltaTime: f32) !void {
        if (!self.active.get()) {
            return;
        }

        if (self.context.onClient()) {
            self.applyVehicleInput(tickDeltaTime);
            return;
        }

        // TODO: Update the rear whiskers here.

        if (self.fsm.currentState() == State.FollowDriveline) {
            var inputData = basis.physics.vehicles.VehicleInputData{
                .acceleration = 0.0,
                .steering = 0.0,
                .brake = 0.0,
                .handbrake = 0.0,
            };

            // TODO: Update the front whiskers here.

            const pos = self.context.transform.getPosition();
            const ori = self.context.transform.getOrientation();
            const linVel = self.context.transform.getLinearVelocity();
            const angVel = self.context.transform.getAngularVelocity();

            try self.driveline.tick(tickDeltaTime, pos, ori, linVel);

            inputData.steering = self.getUpdatedSteeringValue(angVel, self.driveline.steeringValue);

            self.currentSpeedMultiplier = 1.0;

            //mSpeedLoweringLogic.tick(time, inputData.steering, mController->getStateInfo().currentSpeedForward, angVel);

            // float closestObstacleDistance;
            // float closestObstacleAngle;
            // if (mFrontWhiskerArray.getClosestHit(closestObstacleDistance, closestObstacleAngle))
            // {
            //     BASIS_PROFILE("Collision avoidance speed module");

            //     mCollisionAvoidanceSpeedModule.closestObstacleAngle->setValue(closestObstacleAngle);
            //     mCollisionAvoidanceSpeedModule.closestObstacleDistance->setValue(closestObstacleDistance);
            //     mCollisionAvoidanceSpeedModule.engine->process();
            //     mCurrentSpeedMultiplier = mCollisionAvoidanceSpeedModule.speed->getValue();
            // }

            //mCurrentSpeedMultiplier *= mSpeedLoweringLogic.getSpeedMultiplier();
            self.currentSpeedMultiplier = self.currentSpeedMultiplier * self.driveline.speedMultiplier;

            self.speedController.targetSpeed = self.getCurrentMaxSpeed() * self.currentSpeedMultiplier;
            self.speedController.update(tickDeltaTime, &inputData);

            self.acceleration.set(inputData.acceleration);
            self.brake.set(inputData.brake);
            self.steering.set(inputData.steering);

            self.applyVehicleInput(tickDeltaTime);
        } else if (self.fsm.currentState() == State.ReverseTurn) {
            // if (mReverseTurnLogic.isActive())
            // {
            //     mAcceleration.set(mReverseTurnLogic.getAccelerationInput());
            //     mBrake.set(mReverseTurnLogic.getBrakeInput());
            //     mSteering.set(mReverseTurnLogic.getSteeringInput());

            //     applyVehicleInput(dt);
            // }
        } else if (self.fsm.currentState() == State.Stopped) {
            // In the stopped state the acceleration should be 0 and brake 1.
            basis.assert(self.acceleration.get() == 0.0);
            basis.assert(self.brake.get() == 1.0);

            self.applyVehicleInput(tickDeltaTime);
        }
    }

    pub fn tick(self: *Self, tickDeltaTime: f32) !void {
        _ = tickDeltaTime;

        if (!self.active.get()) {
            return;
        }

        if (self.fsm.currentState() == State.FollowDriveline) {
            if (self.driveline.atDestination) {
                self.driveline.reset();
                _ = try self.fsm.do(Event.AtDestination);
            } else if (self.driveline.hasFailed) {
                _ = try self.fsm.do(Event.Stop);
            } else {
                // mTimeSinceLastReverseTurn += dt;
                // if (mTimeSinceLastReverseTurn > MINIMUM_TIME_BETWEEN_REVERSE_TURNS)
                // {
                //     // Check if we are stuck.

                //     mStucknessDetector.tick(time, mTransform);

                //     if (mStucknessDetector.isStuck())
                //     {
                //         mStucknessDetector.reset();

                //         bool headingTowardsTheRight = mDriveline.getSteeringValue() >= 0.0f;
                //         mReverseTurnLogic.getUnstuck(headingTowardsTheRight, mTransform);
                //         mFSM.executeTransition(TransitionStartReverseTurn);
                //     }

                //     // Check if we should turn around.

                //     mReverseTurnDetector.tick(time, mTransform, mDriveline, mVehicleDescription->turnRadius, mClosestRearObstacleDistance);

                //     bool rightReverseTurn;
                //     ReverseTurnDetector::Reason reverseTurnReason;
                //     if (mReverseTurnDetector.shouldDoReverseTurn(rightReverseTurn, &reverseTurnReason))
                //     {
                //         mReverseTurnDetector.reset();

                //         //if (reverseTurnReason == ReverseTurnDetector::ReasonTargetsBehindAgent)
                //         mReverseTurnLogic.turnAround(rightReverseTurn, mTransform);
                //         //else
                //         //	mReverseTurnLogic.reachTarget(rightReverseTurn, mTransform);

                //         mFSM.executeTransition(TransitionStartReverseTurn);
                //     }
                // }
            }
        } else if (self.fsm.currentState() == State.ReverseTurn) {
            // mTimeSinceLastReverseTurn = 0.0f;

            // mReverseTurnLogic.tick(time, mClosestRearObstacleDistance);

            // if (!mReverseTurnLogic.isActive())
            // {
            //     mFSM.executeTransition(TransitionReverseTurnComplete);
            // }
        }
    }

    //----------------------------------------------------
    // Driving API:

    pub fn isActive(self: *const Self) bool {
        return self.active.get();
    }

    pub fn deactivate(self: *Self) !void {
        if (self.fsm.currentState() == State.Inactive) {
            return;
        }

        _ = try self.fsm.do(Event.Deactivate);
    }

    pub fn moveTo(
        self: *Self,
        targetPosition: Vec3,
        arrivalDestinationEpsilon: f32,
        profile: means.ai.AIMovementProfile,
        callback: means.ai.AIMovementCallback,
    ) void {
        self.currentTargetPosition = targetPosition;
        self.currentArrivalDistanceEpsilon = arrivalDestinationEpsilon;
        self.currentMovementProfile = profile;
        self.currentMovementCallback = callback;

        //mTimeSinceLastReverseTurn = BASIS_LARGEST_NUMBER;

        _ = self.fsm.do(Event.MoveTo) catch |err| {
            basis.assertf(false, "moveTo() - Error in transition: {s}", .{@errorName(err)});
        };
    }

    pub fn stop(self: *Self) void {
        if (self.fsm.currentState() == State.Inactive or self.fsm.currentState() == State.Stopped) {
            return;
        }

        _ = self.fsm.do(Event.Stop) catch |err| {
            basis.assertf(false, "stop() - Error in transition: {s}", .{@errorName(err)});
        };
    }

    //----------------------------------------------------

    pub fn onTransition(handler: *AIDrivingFSM.Handler, event: ?Event, from: State, to: State) zigfsm.HandlerResult {
        // Cannot use downcast() here since fsmHandler is not the first field.
        // See zigfsm.Interface.downcast() for more info.
        //const self = zigfsm.Interface.downcast(Self, handler);
        const self = @fieldParentPtr(Self, "fsmHandler", handler);

        if (event) |e| {
            if (e == Event.Stop and (from == State.FollowDriveline or from == State.ReverseTurn)) {
                if (self.driveline.hasFailed) {
                    self.currentMovementCallback.call(means.ai.AIMovementResult.PathNotFound);
                } else {
                    self.currentMovementCallback.call(means.ai.AIMovementResult.Aborted);
                }
            }

            if (e == Event.MoveTo and (from == State.FollowDriveline or from == State.ReverseTurn)) {
                const vehicleRadius = self.getVehicleNavMeshRadius();
                self.driveline.begin(self.currentTargetPosition, self.currentArrivalDistanceEpsilon, vehicleRadius);
            }

            if (e == Event.AtDestination) {
                self.currentMovementCallback.call(means.ai.AIMovementResult.Success);
            }
        }

        if (from != to) {
            // Exit handlers.

            switch (from) {
                State.Inactive => {
                    self.active.set(true);
                },
                State.FollowDriveline => {
                    self.speedController.disable();
                },
                State.ReverseTurn => {
                    //basis.printf("Exiting reverse turn\n", .{});
                    self.reversingEnabled.set(false);
                },
                State.Stopped => {
                    //
                },
            }

            // Enter handlers.

            switch (to) {
                State.Inactive => {
                    self.active.set(false);
                    self.reversingEnabled.set(true);
                },
                State.FollowDriveline => {
                    self.reversingEnabled.set(false);

                    const vehicleRadius = self.getVehicleNavMeshRadius();
                    self.driveline.begin(self.currentTargetPosition, self.currentArrivalDistanceEpsilon, vehicleRadius);

                    //mStucknessDetector.reset();
                    //mReverseTurnDetector.reset();
                    //mSpeedLoweringLogic.reset();

                    self.speedController.enable(self.vehicleController);
                },
                State.ReverseTurn => {
                    //basis.printf("Entering reverse turn\n", .{});
                    self.reversingEnabled.set(true);
                },
                State.Stopped => {
                    self.driveline.reset();

                    self.acceleration.set(0.0);
                    self.brake.set(1.0);
                    self.steering.set(0.0);

                    self.currentSteeringValue = 0.0;
                    self.currentSpeedMultiplier = 1.0;

                    //mTimeSinceLastReverseTurn = 0.0f;
                },
            }
        }

        return zigfsm.HandlerResult.Continue;
    }

    fn onReversingEnabledChanged(self: *Self, enabled: bool, localChange: bool, valueTime: f64) void {
        _ = valueTime;
        _ = localChange;
        self.autoGearBoxComponent.autoGearBox.brakeIntoReverseEnabled = enabled;
    }

    fn applyVehicleInput(self: *Self, deltaTime: f32) void {
        // Update the input. The steering is piped straight through to the vehicle controller.
        // The other parameters are sent to the auto gear box which processes them and updates
        // the vehicle input data struct.

        var inputData = basis.physics.vehicles.VehicleInputData{
            .acceleration = self.acceleration.get(),
            .steering = self.steering.get(),
            .brake = self.brake.get(),
            .handbrake = 0.0,
        };

        self.autoGearBoxComponent.updateAutoGearBox(
            deltaTime,
            inputData.acceleration,
            inputData.brake,
            inputData.handbrake,
            &inputData,
        );

        basis.assert(inputData.acceleration <= 1.0 and inputData.acceleration >= 0.0);
        basis.assert(inputData.steering <= 1.0 and inputData.steering >= -1.0);
        basis.assert(inputData.brake <= 1.0 and inputData.brake >= 0.0);
        basis.assert(inputData.handbrake <= 1.0 and inputData.handbrake >= 0.0);

        self.vehicleController.setInputData(inputData);
    }

    fn getCurrentMaxSpeed(self: *const Self) f32 {
        basis.assert(self.blueprintProperties != null);
        const bpProps = self.blueprintProperties.?;

        // Return the speed based on the movement profile and convert from kph to m/s.

        switch (self.currentMovementProfile) {
            means.ai.AIMovementProfile.Calm => {
                return bpProps.calmSpeed / 3.6;
            },
            means.ai.AIMovementProfile.Normal => {
                return bpProps.normalSpeed / 3.6;
            },
            means.ai.AIMovementProfile.Fast => {
                return bpProps.fastSpeed / 3.6;
            },
        }

        return 0.0;
    }

    fn getUpdatedSteeringValue(self: *Self, angularVelocity: Vec3, steeringTargetValue: f32) f32 {
        if (basis.math.floatsAlmostEqualEpsilon(self.currentSteeringValue, steeringTargetValue, 0.01)) {
            // The "current" steering value is close enough to the target. Use the target.
            self.currentSteeringValue = steeringTargetValue;
        } else {
            if (self.currentSteeringValue < steeringTargetValue) {
                self.currentSteeringValue += STEERING_SPEED;
                if (self.currentSteeringValue > steeringTargetValue)
                    self.currentSteeringValue = steeringTargetValue;
            } else {
                self.currentSteeringValue -= STEERING_SPEED;
                if (self.currentSteeringValue < steeringTargetValue)
                    self.currentSteeringValue = steeringTargetValue;
            }
        }

        const angVelY = angularVelocity.y;

        var counterSpin: f32 = 0.0;

        const HARD_SPIN_THRESHOLD = 0.8;
        const MAX_SPIN_THRESHOLD = 1.5;

        const HARD_SPIN_COUNTER_VALUE = 0.15;
        const MAX_SPIN_COUNTER_VALUE = 0.25;

        if (angVelY < -HARD_SPIN_THRESHOLD) {
            // Vehicle spinning hard to the left, counter by turning to the right.
            counterSpin = basis.math.remapFloat(-angVelY, HARD_SPIN_THRESHOLD, MAX_SPIN_THRESHOLD, HARD_SPIN_COUNTER_VALUE, MAX_SPIN_COUNTER_VALUE);
        } else if (angVelY > HARD_SPIN_THRESHOLD) {
            // Vehicle spinning hard to the right, counter by turning to the left.
            counterSpin = -basis.math.remapFloat(angVelY, HARD_SPIN_THRESHOLD, MAX_SPIN_THRESHOLD, HARD_SPIN_COUNTER_VALUE, MAX_SPIN_COUNTER_VALUE);
        }

        // if (counterSpin != 0.0) {
        //     basis.printf("counterSpin: {}\n", .{counterSpin});
        // }

        self.currentSteeringValue = self.currentSteeringValue + counterSpin;

        // {
        //     BASIS_PROFILE("Collision avoidance steering module");

        //     float closestHitDistance;
        //     float closestHitAngle;
        //     if (mFrontWhiskerArray.getClosestHit(closestHitDistance, closestHitAngle))
        //     {
        //         //BASIS_PRINTF("Closest hit, distance: %.2f, angle: %.2f\n", closestHitDistance, closestHitAngle);

        //         mCollisionAvoidanceSteeringModule.closestObstacleAngle->setValue(closestHitAngle);
        //         mCollisionAvoidanceSteeringModule.closestObstacleDistance->setValue(closestHitDistance);
        //     }
        //     else
        //     {
        //         mCollisionAvoidanceSteeringModule.closestObstacleAngle->setValue(0.0f);
        //         mCollisionAvoidanceSteeringModule.closestObstacleDistance->setValue(25.0f);
        //     }

        //     mCollisionAvoidanceSteeringModule.engine->process();
        //     float collisionAvoidanceSteering = mCollisionAvoidanceSteeringModule.avoidanceSteeringValue->getValue();

        //     //BASIS_PRINTF("Closest hit, distance: %.2f, angle: %.2f, steering: %.2f\n",
        //     //	closestHitDistance, closestHitAngle, collisionAvoidanceSteering);

        //     mCurrentSteeringValue += collisionAvoidanceSteering;
        // }

        self.currentSteeringValue = std.math.clamp(self.currentSteeringValue, -1.0, 1.0);
        return self.currentSteeringValue;
    }

    fn getVehicleNavMeshRadius(self: *const Self) f32 {
        basis.assert(self.blueprintProperties != null);
        return self.blueprintProperties.?.vehicleNavMeshRadius;
    }
};

//----------------------------------------------------

pub const AIDrivingComponentProperties = struct {
    const Self = @This();

    allocator: std.mem.Allocator,

    calmSpeed: f32 = 30.0, // In kph.
    normalSpeed: f32 = 50.0, // In kph.
    fastSpeed: f32 = 70.0, // In kph.

    frontWhiskerMinLength: f32 = 10.0,
    frontWhiskerMaxLength: f32 = 20.0,
    frontWhiskerMinLengthSpeed: f32 = 10.0, // In kph.
    frontWhiskerMaxLengthSpeed: f32 = 70.0, // In kph.

    rearWhiskerLength: f32 = 7.0,

    vehicleNavMeshRadius: f32 = 3.0,

    pub fn init(allocator: std.mem.Allocator) !Self {
        return Self{
            .allocator = allocator,
        };
    }

    pub fn loadJSON(self: *Self, json: []const u8) !void {
        const Props = struct {
            calmSpeed: f32,
            normalSpeed: f32,
            fastSpeed: f32,

            frontWhiskerMinLength: f32,
            frontWhiskerMaxLength: f32,
            frontWhiskerMinLengthSpeed: f32,
            frontWhiskerMaxLengthSpeed: f32,

            rearWhiskerLength: f32,

            vehicleNavMeshRadius: f32,
        };

        const props = try std.json.parseFromSlice(Props, self.allocator, json, .{});
        defer props.deinit();

        self.calmSpeed = props.value.calmSpeed;
        self.normalSpeed = props.value.normalSpeed;
        self.fastSpeed = props.value.fastSpeed;

        self.frontWhiskerMinLength = props.value.frontWhiskerMinLength;
        self.frontWhiskerMaxLength = props.value.frontWhiskerMaxLength;
        self.frontWhiskerMinLengthSpeed = props.value.frontWhiskerMinLengthSpeed;
        self.frontWhiskerMaxLengthSpeed = props.value.frontWhiskerMaxLengthSpeed;

        self.rearWhiskerLength = props.value.rearWhiskerLength;

        self.vehicleNavMeshRadius = props.value.vehicleNavMeshRadius;
    }
};