Net.Like.Xue.Tokyo/Packages-Local/Com.Project.B.Unity/Vehicle/WorldCarService.cs

using System;
using System.Collections;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Linq;
using BITKit;
using BITKit.Entities;
using BITKit.Physics;
using BITKit.WorldNode;
using Microsoft.Extensions.DependencyInjection;
using Microsoft.Extensions.Logging;
using Net.Project.B.UX;
using Net.Project.B.World;
using Net.Project.B.WorldNode;
using Project.B.CharacterController;
using Project.B.Player;
using UnityEditor;
using UnityEngine;
using UnityEngine.AI;
using UnityEngine.InputSystem;
using UnityEngine.UIElements;

namespace Net.Project.B.Vehicle
{
   public class WorldCarService:IDisposable
   {
      private class RuntimeCarInfo
      {
         public float CurrentSteerAngle = 0f;

         public float InnerAngle;
         public float OuterAngle;

         public float WheelBase;
         public float TrackWidth;

         public Vector3 AngularVelocity;
      }

      private readonly ILogger<WorldCarService> _logger;
      
      private readonly IFixedTicker _ticker;

      private readonly IUXHud _uxHud;
      
      private readonly IEntitiesService _entitiesService;

      private readonly ConcurrentDictionary<int, UnityVehicleNode> _vehicles = new();

      private readonly ConcurrentDictionary<int, RuntimeCarInfo> _runtimeCarInfos=new();

      private readonly IWorldSeatService _seatService;

      private readonly ICarKeyMap<InputAction> _carKeyMap;

      private readonly InputActionGroup _inputActionGroup = new();

      private readonly HashSet<int> _isControlVehicles = new();

      private readonly HashSet<Rigidbody> _waitLoaded = new();
      
      private float _currentVertical;
      private float _currentHorizontal;
      private float _currentHandBrake;
      
      public WorldCarService(IEntitiesService entitiesService, IFixedTicker ticker, ICarKeyMap<InputAction> carKeyMap, IWorldSeatService seatService, ILogger<WorldCarService> logger, IUXHud uxHud)
      {
         _entitiesService = entitiesService;
         _ticker = ticker;
         _carKeyMap = carKeyMap;
         _seatService = seatService;
         _logger = logger;
         _uxHud = uxHud;

         _entitiesService.OnAdd += OnAdd;
         _entitiesService.OnRemove += OnRemove;
         
         _ticker.Add(OnTick);

         _inputActionGroup.RegisterCallback(_carKeyMap.VerticalKey, OnVertical);
         _inputActionGroup.RegisterCallback(_carKeyMap.HorizontalKey, OnHorizontal);
         _inputActionGroup.RegisterCallback(_carKeyMap.HandBrakeKey, OnHandBrake);
         
         _inputActionGroup.allowInput.AddElement(this);

         _seatService.OnSeatOccupied += OnSeatOccupied;

      }

      private void OnSeatOccupied(UnitySeatNode arg1, IEntity arg2, IEntity arg3)
      {
         foreach (var (vehicleId, vehicleNode) in _vehicles)
         {
            if (!vehicleNode.seatObject) continue;
            var vehicleSeatId = vehicleNode.seatObject.GetInstanceID();
            if (vehicleSeatId != arg1.Id) continue;

            if (arg3 is not null)
            {
               if (_isControlVehicles.Add(vehicleId)) ;
               {
                  if (vehicleNode.rigidbody.velocity.sqrMagnitude < 1)
                  {
                     foreach (var wheelCollider in vehicleNode.wheels)
                     {
                        wheelCollider.motorTorque = 0;
                        wheelCollider.brakeTorque = vehicleNode.handbrakeForce;
                     }
                  }
               }
            }
            else
            {
               if (_isControlVehicles.Remove(vehicleId))
               {
                  foreach (var wheelCollider in vehicleNode.wheels)
                  {
                     wheelCollider.motorTorque = 0;
                     wheelCollider.brakeTorque = 0;
                  }
               }
            }
         }
      }

      private void OnHandBrake(InputAction.CallbackContext obj)
      {
         _currentHandBrake = obj.ReadValue<float>();
      }

      private void OnHorizontal(InputAction.CallbackContext obj)
      {
         _currentHorizontal = obj.ReadValue<float>();
      }

      private void OnVertical(InputAction.CallbackContext obj)
      {
         _currentVertical = obj.ReadValue<float>();
      }

      private void OnTick(float obj)
      {
         foreach (var rigidbody in _waitLoaded)
         {
            if (!rigidbody)
            {
               _waitLoaded.Remove(rigidbody);
               break;
            }
            if (Physics.Raycast(rigidbody.worldCenterOfMass+Vector3.up, Vector3.down, out var hit,64, LayerMask.GetMask("Default")))
            {
               Debug.DrawRay(rigidbody.worldCenterOfMass,Vector3.down,Color.yellow,obj);
               rigidbody.isKinematic = false;
               _waitLoaded.Remove(rigidbody);
               break;
            }
         }
         
         foreach (var (id,vehicleNode) in _vehicles)
         {
            var runtimeInfo = _runtimeCarInfos.GetOrCreate(id);

            if (_isControlVehicles.Contains(id))
            {
               RotateSteeringWheel();

               Steering();
               
               Acceleration();
               
               HandBraking();

               foreach (var wheelCollider in vehicleNode.wheels)
               {
                  var text = $"" +
                             $"Drive Wheel: {wheelCollider.name}" +
                             $"\nTorque: {wheelCollider.motorTorque:F2}" +
                             $"\nBrake: {wheelCollider.brakeTorque:F2}" +
                             $"\nGround:{wheelCollider.isGrounded}" +
                             $"\nRPM:{wheelCollider.rpm}";
                  wheelCollider.GetWorldPose(out var pos,out _);
                  
                  DrawXXL.DrawText.Write(text,pos);
               }
            }

            //Slowdown();

            /*
            foreach (var wheelCollider in vehicleNode.wheels)
            {
               if (wheelCollider.isGrounded)
               {
                  vehicleNode.rigidbody.AddForce(Vector3.down * vehicleNode.rigidbody.velocity.GetLength());
                  break;
               }
            }
            */

            for (var index = 0; index < vehicleNode.wheelMeshes.Length; index++)
            {
               UpdateWheel(vehicleNode.wheels[index], vehicleNode.wheelMeshes[index]);
            }

            continue;
            void Steering()
            {
               if(_isControlVehicles.Contains(id) is false)return;
            runtimeInfo.CurrentSteerAngle = vehicleNode.maxSteerAngle * _currentHorizontal;

            switch (vehicleNode.steeringWheelColliders.Length)
            {
                case 0:
                {
                    var newRot = Quaternion.Euler(Vector3.up * ( runtimeInfo.CurrentSteerAngle * Time.fixedDeltaTime));

                    newRot = vehicleNode.rigidbody.rotation * newRot;

                    vehicleNode.rigidbody.MoveRotation(newRot);
                }
                    break;
                case 2 when vehicleNode.allowAckermannSteering:
                {
                   // 获取当前车轮的转向角度（用于计算）
                   var steerAngle = vehicleNode.maxSteerAngle*_currentHorizontal;
                   var normalizedSteerAngle =
                      Mathf.Clamp(steerAngle, -vehicleNode.maxSteerAngle, vehicleNode.maxSteerAngle);

                   // 计算内外侧车轮的转向角度（阿克曼转向公式）
                   runtimeInfo.InnerAngle = Mathf.Atan(Mathf.Tan(normalizedSteerAngle * Mathf.Deg2Rad) *
                                                       (runtimeInfo.WheelBase /
                                                        (runtimeInfo.WheelBase + runtimeInfo.TrackWidth))) *
                                            Mathf.Rad2Deg;

                   runtimeInfo.OuterAngle = Mathf.Atan(Mathf.Tan(normalizedSteerAngle * Mathf.Deg2Rad) *
                                                       (runtimeInfo.TrackWidth /
                                                        (runtimeInfo.WheelBase + runtimeInfo.TrackWidth))) *
                                            Mathf.Rad2Deg;

                   // 设置前轮的转向角度
                   switch (runtimeInfo.CurrentSteerAngle)
                   {
                      case > 0:
                         vehicleNode.steeringWheelColliders[0].steerAngle = runtimeInfo.OuterAngle;
                         vehicleNode.steeringWheelColliders[1].steerAngle = runtimeInfo.InnerAngle;
                         break;
                      case < 0:
                         vehicleNode.steeringWheelColliders[0].steerAngle = runtimeInfo.InnerAngle;
                         vehicleNode.steeringWheelColliders[1].steerAngle = runtimeInfo.OuterAngle;
                         break;
                      case 0:
                         vehicleNode.steeringWheelColliders[0].steerAngle = 0;
                         vehicleNode.steeringWheelColliders[1].steerAngle = 0;
                         break;
                   }

                   break;
                }
                default:
                {
                    runtimeInfo.InnerAngle = runtimeInfo.OuterAngle = runtimeInfo.CurrentSteerAngle;
                    
                    foreach (var steeringWheelCollider in vehicleNode.steeringWheelColliders)
                    {
                        steeringWheelCollider.steerAngle = runtimeInfo.CurrentSteerAngle;
                    }

                    break;
                }
            }
        }

            void Acceleration()
            {
               var selfVelocityZ = vehicleNode.rigidbody.transform.InverseTransformDirection(vehicleNode.rigidbody.velocity).z;

               float motorTorque = 0;
               float brakeTorque = 0;

               // 驱动逻辑
               if (_currentVertical != 0)
               {

                  // 判断是否方向相反，如果相反就刹车
                  if ((_currentVertical > 0 && selfVelocityZ < -vehicleNode.stopThreshold) ||
                      (_currentVertical < 0 && selfVelocityZ > vehicleNode.stopThreshold))
                  {
                     brakeTorque = vehicleNode.brakePower;
                  }
                  else
                  {
                     // 正常加速
                     motorTorque = _currentVertical * vehicleNode.horsePower;
                  }

               }

               // 应用驱动轮
               foreach (var wheelCollider in vehicleNode.driveWheelColliders)
               {
                  wheelCollider.motorTorque = motorTorque;
                  wheelCollider.brakeTorque = brakeTorque;
               }
            }


            void HandBraking()
            {
               var handBrakeTorque = (_currentHandBrake > 0) ? _currentHandBrake * vehicleNode.handbrakeForce : 0;

               foreach (var wheelCollider in vehicleNode.handBrakeWheelColliders)
               {
                  wheelCollider.brakeTorque = handBrakeTorque;
               }
            }


            void RotateSteeringWheel()
            {
               if(!vehicleNode.steeringWheel)return;
               
               var currentXAngle =
                  vehicleNode.steeringWheel.transform.localEulerAngles.x; // Maximum steer angle in degrees

               var normalizedSteerAngle =
                  Mathf.Clamp(vehicleNode.steeringWheelColliders.First().steerAngle, -vehicleNode.maxSteerAngle,
                     vehicleNode.maxSteerAngle);
               var rotation = Mathf.Lerp(vehicleNode.steeringWheelAngle,-vehicleNode.steeringWheelAngle,
                  (normalizedSteerAngle + vehicleNode.maxSteerAngle) / (2 * vehicleNode.maxSteerAngle));

               // Set the local rotation of the steering wheel
               vehicleNode.steeringWheel.localRotation = Quaternion.Lerp(vehicleNode.steeringWheel.localRotation, Quaternion.Euler(currentXAngle, 0, rotation),8*obj);
            }

            void UpdateWheel(WheelCollider col, Transform mesh)
            {
               col.GetWorldPose(out Vector3 position, out Quaternion rotation);
               mesh.SetPositionAndRotation(position, rotation);
            }
            
            void Slowdown()
            {
               if (!vehicleNode.rigidbody || vehicleNode.rigidbody.isKinematic)return;
               {
                  if (_currentVertical == 0 && _currentHandBrake == 0)
                  {
                     vehicleNode  .rigidbody.velocity =
                        Vector3.Lerp(  vehicleNode  .rigidbody.velocity, Vector3.zero, Time.deltaTime * 0.5f);
                  }
               }
            }
         }
      }

      private void OnRemove(IEntity obj)
      {
         _vehicles.TryRemove(obj.Id, out _);
      }

      private void OnAdd(IEntity obj)
      {
         if (obj.ServiceProvider.GetService<UnityVehicleNode>() is not { } vehicleNode) return;

         _vehicles.TryAdd(obj.Id, vehicleNode);

         vehicleNode.rigidbody.interpolation = RigidbodyInterpolation.Interpolate;
         vehicleNode.rigidbody.automaticCenterOfMass = false;
         vehicleNode.rigidbody.centerOfMass = default;

         obj.ServiceProvider.QueryComponents(out GameObject gameObject, out Transform transform);
         
         gameObject.layer= LayerMask.NameToLayer("Vehicle");
         gameObject.isStatic = false;

         var colliderReady = false;
         
         foreach (var boxCollider in gameObject.GetComponentsInChildren<BoxCollider>())
         {
            if (boxCollider.attachedRigidbody == vehicleNode.rigidbody && boxCollider.isTrigger is false && boxCollider.gameObject.layer==gameObject.layer)
            {
               colliderReady = true;
               break;
            }
         }

         try
         {
            if (colliderReady is false)
            {
               var meshRenderers = gameObject
                  .GetComponentsInChildren<MeshRenderer>()
                  .Where(x=>x.gameObject.name.Contains("Wheel",StringComparison.CurrentCultureIgnoreCase) is false)
                  .ToArray()
                  ;

               if (meshRenderers.Length == 0)
               {
                  Debug.LogWarning($"没有 MeshRenderer 可用于生成 BoxCollider：{gameObject.name}");        throw new OperationCanceledException();
               }

               Bounds bounds = meshRenderers[0].bounds;
               for (int i = 1; i < meshRenderers.Length; i++)
               {
                  bounds.Encapsulate(meshRenderers[i].bounds);
               }

               var box = gameObject.AddComponent<BoxCollider>();
    
               // 将 world bounds 转换为本地坐标系
               var centerLocal = gameObject.transform.InverseTransformPoint(bounds.center);
               var sizeLocal = gameObject.transform.InverseTransformVector(bounds.size);

               box.center = centerLocal;
               box.size = sizeLocal;
            }
         }
         catch (OperationCanceledException)
         {
            
         }

        

         var colliders = obj.ServiceProvider.GetRequiredService<GameObject>().GetComponentsInChildren<Collider>(true);
         foreach (var x in colliders)
         {
            foreach (var y in colliders)
            {
               Physics.IgnoreCollision(x, y, true);
            }

            if (x.attachedRigidbody == vehicleNode.rigidbody)
               x.gameObject.layer = gameObject.layer;
         }

         if (!vehicleNode.seatObject)
         {
            var nodes = gameObject.GetComponentsInChildren<UnityNode>().Select(x=>x.WorldNode);
            if (nodes.QueryComponent(out UnitySeatNode seatNode))
            {
               seatNode.Rigidbody = vehicleNode.rigidbody;
               vehicleNode.seatObject = seatNode.SeatObject.gameObject;
            }
            else
            {
               _logger.LogWarning(
                  $"VehicleNode {gameObject.name} 没有找到座位节点，请添加 UnitySeatNode 或者 UnitySeatNode 的子节点");
            }
         }

         if (vehicleNode.seatObject)
         {
            vehicleNode.seatObject.layer = LayerMask.NameToLayer("Default");
         }
         

         if (vehicleNode.steeringWheelColliders.Length is 2)
         {
            var runtimeInfo = _runtimeCarInfos.GetOrCreate(obj.Id);

            runtimeInfo.WheelBase = Mathf.Abs(
               vehicleNode.rigidbody.transform
                  .InverseTransformPoint(vehicleNode.steeringWheelColliders[0].transform.position).z -
               vehicleNode.rigidbody.transform
                  .InverseTransformPoint(vehicleNode.handBrakeWheelColliders[0].transform.position).z);
            runtimeInfo.TrackWidth =
               Mathf.Abs(
                  vehicleNode.rigidbody.transform
                     .InverseTransformPoint(vehicleNode.steeringWheelColliders[0].transform.position).x -
                  vehicleNode.rigidbody.transform
                     .InverseTransformPoint(vehicleNode.steeringWheelColliders[1].transform.position).x);
         }
         if (vehicleNode.rigidbody)
         {
            vehicleNode.rigidbody.isKinematic = true;
            _waitLoaded.Add(vehicleNode.rigidbody);

            //var physicsController = vehicleNode.rigidbody.gameObject.AddComponent<UnityCollisionController>();

            //physicsController.OnUnityCollisionEnter += OnUnityCollisionEnter;
            
            void OnUnityCollisionEnter(Collision collision)
            {
               if (_entitiesService.TryGetEntity(collision.transform.gameObject.GetInstanceID(), out var hitEntity))
               {
                  Debug.Log(hitEntity.Id);
               }
            }

            var obstable  =vehicleNode.rigidbody.gameObject.GetOrAddComponent<NavMeshObstacle>();

            var maxVolume = 0f;

            foreach (var collider in colliders.OfType<BoxCollider>())
            {
               if(ReferenceEquals(collider.attachedRigidbody,vehicleNode.rigidbody) is false)continue;
               var volume = collider.bounds.size.x * collider.bounds.size.y * collider.bounds.size.z;
               if (volume > maxVolume)
               {
                  maxVolume = volume;
                  
                  obstable.center = collider.center;
                  obstable.size = collider.size;
               }
            }

            foreach (var wheelCollider in vehicleNode.wheels)
            {
               wheelCollider.brakeTorque = vehicleNode.handbrakeForce;
            }
         }
      }

 

      public void Dispose()
      {
         _entitiesService.OnAdd -= OnAdd;
         _ticker.Remove(OnTick);
         
         _seatService.OnSeatOccupied -= OnSeatOccupied;
         
         _inputActionGroup.allowInput.RemoveElement(this);
         
         _inputActionGroup.UnRegisterCallback(_carKeyMap.VerticalKey, OnVertical);
         _inputActionGroup.UnRegisterCallback(_carKeyMap.HorizontalKey, OnHorizontal);
         _inputActionGroup.UnRegisterCallback(_carKeyMap.HandBrakeKey, OnHandBrake);
      }
   }

}