533 lines
19 KiB
C#
533 lines
19 KiB
C#
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);
|
||
}
|
||
}
|
||
|
||
}
|
||
|