crestboat.cs

using Crest;
using System.Collections.Generic;
using Unity.Entities;
using Unity.Mathematics;
using UnityEngine;

namespace AiGame.Boats
{
    public class KinematicBuoyancy : MonoBehaviour
    {
        public bool Sink;
        public float SinkGravity = -0.1f;
        public ForcePoint[] ForcePoints;
        
        public Vector3 BoatVelocity;

        [SerializeField]
        private float GlobalHeightOffset;
        [SerializeField]
        private Vector3 CenterOfMass;
        [SerializeField]
        private float WaterDensity = 1000;
        [SerializeField]
        private float CenterForce = 100f;

        [Header("Forces")]
        [SerializeField]
        float ForceHeightOffset = 0f;
        [SerializeField]
        float ForceMultiplier = 10f;
        [SerializeField]
        float ObjectWidth = 12f;

        [Header("Drag")]
        [SerializeField]
        private float DragInWaterUp = 3f;
        [SerializeField]
        private float DragInWaterRight = 2f;
        [SerializeField]
        private float DragInWaterForward = 1f;

        private Vector3[] QueryPoints;
        private Vector3[] Displacements;
        private Vector3[] Velocities;
        private ICollProvider CollisionProvider;
        private List<BuoyancyForces> Forces = new List<BuoyancyForces>();

        private int QueryHash;
        private SampleFlowHelper SampleFlowHelper = new SampleFlowHelper();
        private float TotalWeight;

        private float SeaLevel;
        private bool ForcesUpdated;

        public void Init()
        {
            SeaLevel = OceanRenderer.Instance.SeaLevel;

            QueryPoints = new Vector3[ForcePoints.Length + 1];
            Displacements = new Vector3[ForcePoints.Length + 1];
            Velocities = new Vector3[ForcePoints.Length + 1];

            CollisionProvider = OceanRenderer.Instance.CollisionProvider;
            QueryHash = GetHashCode();

            TotalWeight = 0f;
            foreach (var pt in ForcePoints)
            {
                TotalWeight += pt.Weight;
            }
        }

        public bool TryGetForces(DynamicBuffer<BuoyancyForces> forces)
        {
            if (ForcesUpdated)
            {
                forces.Clear();
                foreach (var force in Forces)
                {
                    forces.Add(force);
                }
                ForcesUpdated = false;
                return true;
            } else
            {
                return false;
            }
        }

        private void Update()
        {
            Forces.Clear();
            UpdateWaterQueries();

            Vector3 waterSurfaceVel = Velocities[ForcePoints.Length];

            {
                SampleFlowHelper.Init(transform.position, ObjectWidth);
                Vector2 surfaceFlow = Vector2.zero;
                SampleFlowHelper.Sample(ref surfaceFlow);
                waterSurfaceVel += new Vector3(surfaceFlow.x, 0, surfaceFlow.y);
            }

            // Buoyancy
            UpdateBuoyancy();
            UpdateDrag(waterSurfaceVel);
            ForcesUpdated = true;
        }

        private void UpdateWaterQueries()
        {
            Vector3 centerOfMass = new Vector3(0, CenterOfMass.y, 0);

            // Update query points
            for (int i = 0; i < ForcePoints.Length; i++)
            {
                QueryPoints[i] = transform.TransformPoint(ForcePoints[i].OffsetPosition + centerOfMass);
            }
            QueryPoints[ForcePoints.Length] = transform.position + centerOfMass;

            CollisionProvider.Query(QueryHash, ObjectWidth, QueryPoints, Displacements, null, Velocities);
        }

        void UpdateBuoyancy()
        {
            Vector3 centerOfMass = new Vector3(0, CenterOfMass.y, 0);
            float verticalGravity = Physics.gravity.y;
            if (Sink)
            {
                verticalGravity = SinkGravity;
            }

            float archimedesForceMagnitude = WaterDensity * Mathf.Abs(verticalGravity);
            int centerIndex = ForcePoints.Length;
            float waterHeight = SeaLevel + GlobalHeightOffset + Displacements[centerIndex].y;
            float heightDiff = waterHeight - QueryPoints[centerIndex].y;

            BuoyancyForce bforce;

            if (Sink)
            {
                bforce = new BuoyancyForce { Gravity = SinkGravity };
                Forces.Add(bforce);
                return;
            }

            if (heightDiff > 0)
            {
                bforce = new BuoyancyForce
                {
                    Force = archimedesForceMagnitude * heightDiff * Vector3.up * CenterForce,
                    Position = centerOfMass,
                    ForceMode = Unity.Physics.Extensions.ForceMode.Force
                };

            }
            else
            {
                bforce = new BuoyancyForce { Gravity = math.abs(heightDiff) };
            }
            Forces.Add(bforce);

            for (int i = 0; i < ForcePoints.Length; i++)
            {
                waterHeight = SeaLevel + GlobalHeightOffset + Displacements[i].y;
                heightDiff = waterHeight - QueryPoints[i].y;


                if (heightDiff > 0)
                {
                    bforce = new BuoyancyForce
                    {
                        Force = archimedesForceMagnitude * heightDiff * Vector3.up * ForcePoints[i].Weight * ForceMultiplier / TotalWeight,
                        Position = ForcePoints[i].OffsetPosition + centerOfMass,
                        ForceMode = Unity.Physics.Extensions.ForceMode.Force
                    };

                }
                else
                {
                    bforce = new BuoyancyForce { Gravity = math.abs(heightDiff) };
                }
                Forces.Add(bforce);
            }
        }

        void UpdateDrag(Vector3 waterSurfaceVel)
        {
            Vector3 centerOfMass = new Vector3(0, CenterOfMass.y, 0);

            // Apply drag relative to water
            Vector3 velocityRelativeToWater = BoatVelocity - waterSurfaceVel;

            var forcePosition = centerOfMass + ForceHeightOffset * Vector3.up;

            BuoyancyForce up = new BuoyancyForce
            {
                Force = Vector3.up * Vector3.Dot(Vector3.up, -velocityRelativeToWater) * DragInWaterUp,
                Position = forcePosition,
                ForceMode = Unity.Physics.Extensions.ForceMode.Acceleration
            };
            Forces.Add(up);

            BuoyancyForce left = new BuoyancyForce
            {
                Force = transform.right * Vector3.Dot(transform.right, -velocityRelativeToWater) * DragInWaterRight,
                Position = forcePosition,
                ForceMode = Unity.Physics.Extensions.ForceMode.Acceleration
            };
            Forces.Add(left);

            BuoyancyForce forward = new BuoyancyForce
            {
                Force = transform.forward * Vector3.Dot(transform.forward, -velocityRelativeToWater) * DragInWaterForward,
                Position = forcePosition,
                ForceMode = Unity.Physics.Extensions.ForceMode.Acceleration
            };
            Forces.Add(forward);
        }

        private void OnDrawGizmosSelected()
        {
            Gizmos.color = Color.yellow;
            Gizmos.DrawCube(transform.TransformPoint(CenterOfMass), Vector3.one * 0.25f);

            for (int i = 0; i < ForcePoints.Length; i++)
            {
                var point = ForcePoints[i];

                var transformedPoint = transform.TransformPoint(point.OffsetPosition + new Vector3(0, CenterOfMass.y, 0));

                Gizmos.color = Color.red;
                Gizmos.DrawCube(transformedPoint, Vector3.one * 0.5f);
            }

        }
    }
}

using Unity.Entities;
using Unity.Mathematics;
using Unity.Physics;
using Unity.Physics.Extensions;
using Unity.Transforms;
using UnityEngine;

namespace AiGame.Boats
{
    [System.Serializable]
    public struct KinematicBodySteering
    {
        public float3 Gravity;
        public float Mass;
        public float LinearDamp;
        public float AngularDamp;
        public PhysicsMass PhysicsMass;

        public float3 Position;
        public quaternion Rotation;
        public float3 Linear;
        public float3 Angular;

        public void SetMass()
        {
            PhysicsMass = PhysicsMass.CreateDynamic(MassProperties.UnitSphere, Mass);
        }

        public void Update(LocalToWorld hullTransform, float step)
        {
            Position = new float3(hullTransform.Position.x, Position.y, hullTransform.Position.z);
            Rotation = CorrectRotation(hullTransform, step);
        }

        public void UpdateWithForces(DynamicBuffer<BuoyancyForces> forces, float step, LocalToWorld hullTransform)
        {
            Position = new float3(hullTransform.Position.x, Position.y, hullTransform.Position.z);
            Translation translation = new Translation { Value = Position };
            Rotation rotation = new Rotation { Value = Rotation };
            float4x4 localToWorld = float4x4.TRS(Position, Rotation, new float3(1f, 1f, 1f));

            float3 targetAngularVelocity = default;

            for (int i = 0; i < forces.Length; i++)
            {
                BuoyancyForce force = forces[i];
                if (!force.Force.Equals(default))
                {

                    float3 point = localToWorld.LocalToWorld(force.Position);
                    PhysicsMass.GetImpulseFromForce(force.Force, force.ForceMode, step, out float3 impulse, out PhysicsMass impulseMass);

                    Linear += impulse * impulseMass.InverseMass;

                    var worldFromEntity = new RigidTransform(rotation.Value, translation.Value);
                    var worldFromMotion = math.mul(worldFromEntity, impulseMass.Transform);
                    float3 angularImpulseWorldSpace = math.cross(point - worldFromMotion.pos, impulse);
                    float3 angularImpulseInertiaSpace = math.rotate(math.inverse(worldFromMotion.rot), angularImpulseWorldSpace);

                    targetAngularVelocity += angularImpulseInertiaSpace * impulseMass.InverseInertia;
                }

                if (force.Gravity > 0f)
                {
                    Linear.y = math.lerp(Linear.y, -force.Gravity, 1 - math.exp(-10 * step));
                }

            }


            Linear *= math.clamp(1.0f - LinearDamp * step, 0.0f, 1.0f);
            Position += Linear * step;

            Angular = math.lerp(Angular, targetAngularVelocity, step);
            Angular *= math.clamp(1.0f - AngularDamp * step, 0.0f, 1.0f);
            IntegrateOrientation(ref Rotation, Angular, step);
            
            Rotation = CorrectRotation(hullTransform, step);
        }

        public quaternion CorrectRotation(LocalToWorld hullTransform, float step)
        {
            float heading = Quaternion.LookRotation(hullTransform.Forward).eulerAngles.y;
            Quaternion currentRotation = Rotation;
            Vector3 correctionEuler = currentRotation.eulerAngles;
            correctionEuler.y = heading;
            Quaternion hullRotation = Quaternion.Euler(new Vector3(0f, heading, 0f));
            Quaternion targetRotation = Quaternion.Euler(new Vector3(0f, currentRotation.eulerAngles.y, 0f));
            
            float angleDiff = Quaternion.Angle(hullRotation, targetRotation);
            if (angleDiff > 0.1f)
            {
                Quaternion target = Quaternion.RotateTowards(currentRotation, Quaternion.Euler(correctionEuler), step * angleDiff * 10f);
                return target;
            } else
            {
                return Rotation;
            }
        }

        private static void IntegrateOrientation(ref quaternion orientation, float3 angularVelocity, float timestep)
        {
            quaternion dq = IntegrateAngularVelocity(angularVelocity, timestep);
            quaternion r = math.mul(orientation, dq);
            orientation = math.normalizesafe(r);
        }

        // Returns a non-normalized quaternion that approximates the change in angle angularVelocity * timestep.
        private static quaternion IntegrateAngularVelocity(float3 angularVelocity, float timestep)
        {
            float3 halfDeltaTime = new float3(timestep * 0.5f);
            float3 halfDeltaAngle = angularVelocity * halfDeltaTime;
            return new quaternion(new float4(halfDeltaAngle, 1.0f));
        }
    }
}

struct KinematicSteerJob
        {
            public float DeltaTime;
            public float FixedDeltaTime;

            public void Execute(ref PhysicsMass mass, ref KinematicBoatComponent boat, ref PhysicsVelocity velocity, ref Translation translation, ref Rotation rotation,
                in LocalToWorld localToWorld)
            {
                float3 forcePosition;
                float3 motorForce;
                float3 angularVelocity;

                if (boat.AiControlled)
                {
                    if (!boat.AiDirection.Equals(default)  && boat.AiTurnSpeed > 0f)
                    {
                        angularVelocity = MathHelper.AngularVelocityToTarget(rotation.Value, boat.AiDirection, boat.AiTurnSpeed, localToWorld.Up);
                        velocity.SetAngularVelocityWorldSpace(mass, in rotation, in angularVelocity);
                    }
                    

                    if (boat.AiSpeed > 0f)
                    {
                        boat.SteeringState.ApplyForwardForce(boat.SteeringConfig, localToWorld, boat.AiSpeed, out forcePosition, out motorForce);
                        velocity.AddForceAtPosition(mass, translation, rotation, motorForce, forcePosition, FixedDeltaTime, ForceMode.Acceleration);
                    }
                    return;
                }

                boat.SteeringState.Update(DeltaTime, boat.SteeringConfig);

                if (boat.SteeringState.ApplyMotorForce(boat.SteeringConfig, localToWorld, out forcePosition, out motorForce))
                {
                    velocity.AddForceAtPosition(mass, translation, rotation, motorForce, forcePosition, FixedDeltaTime, ForceMode.Acceleration);
                }


                float3 steer = boat.SteeringState.GetSteeringDirection(boat.SteeringConfig, localToWorld);
                
                angularVelocity = MathHelper.AngularVelocityToTarget(rotation.Value, steer, 1f, localToWorld.Up);
                velocity.SetAngularVelocityWorldSpace(mass, in rotation, in angularVelocity);

                //if (boat.SteeringState.ApplySteering(boat.SteeringConfig, localToWorld, out float3 steeringForce))
                //{
                //    velocity.ApplyTorque(mass, steeringForce, FixedDeltaTime);
                //}
            }
        }
        
        public static float3 AngularVelocityToTarget(quaternion fromRotation, float3 toDirection, float turnSpeed, float3 up)
        {
            var wanted = quaternion.LookRotation(toDirection, up);
            return AngularVelocityToTarget(fromRotation, wanted, turnSpeed);
        }

        public static float3 AngularVelocityToTarget(quaternion fromRotation, quaternion toRotation, float turnSpeed)
        {
            quaternion delta = math.mul(toRotation, math.inverse(fromRotation));
            delta.ToAngleAxis(out float3 axis, out float angle);

            // We get an infinite axis in the event that our rotation is already aligned.
            if (float.IsInfinity(axis.x))
            {
                return default;
            }

            if (angle > 180f)
            {
                angle -= 360f;
            }

            // Here I drop down to 0.9f times the desired movement,
            // since we'd rather undershoot and ease into the correct angle
            // than overshoot and oscillate around it in the event of errors.
            return (math.radians(0.9f) * angle / turnSpeed) * math.normalizesafe(axis);
        }
        public static void ToAngleAxis(this quaternion q, out float3 axis, out float angle)
        {
            q = math.normalizesafe(q);
            
            angle = 2.0f * (float)math.acos(q.value.w);
            angle = math.degrees(angle);
            float den = (float)math.sqrt(1.0 - q.value.w * q.value.w);
            if (den > 0.0001f)
            {
                axis = q.value.xyz / den;
            }
            else
            {
                // This occurs when the angle is zero. 
                // Not a problem: just set an arbitrary normalized axis.
                axis = new float3(1, 0, 0);
            }
        }

using Unity.Mathematics;
using Unity.Transforms;
using UnityEngine;

namespace AiGame.Boats
{
    public struct SteeringState
    {
        public float Heading;
        public float Motor;
        public float Steer;
        public float Haxis;
        public float Vaxis;

      
        public void Update(float deltaTime, SteeringConfig config)
        {
            if (Vaxis > 0f)
            {
                Motor = math.lerp(Motor, Vaxis, config.MotorForwardAcceleration * deltaTime);
            }
            else if (Vaxis < 0f)
            {
                Motor = math.lerp(Motor, Vaxis, config.MotorReverseAcceleration * deltaTime);
            }
            else
            {
                Motor = math.lerp(Motor, Vaxis, config.MotorDeceleration * deltaTime);
            }

            if (Haxis == 0f)
            {
                Steer = math.lerp(Steer, Haxis, config.SteerDeceleration * deltaTime);
            }
            else
            {
                Steer = math.lerp(Steer, Haxis, config.SteerAcceleration * deltaTime);
            }
        }

        public bool ApplyMotorForce(SteeringConfig config, LocalToWorld localToWorld, out float3 forcePosition, out float3 force)
        {
            float3 up = new float3(0f, 1f, 0f);
           

            forcePosition = localToWorld.Position + config.ForceHeightOffset * up;
            if (config.ForceForwardOffset != 0f)
            {
                forcePosition = forcePosition + config.ForceForwardOffset * localToWorld.Forward;
            }

            force = default;

            if (Motor < 0f)
            {
                force = -localToWorld.Forward * config.EnginePower * math.abs(Motor);
                return true;
            }
            else if (Motor > 0f)
            {
                force = localToWorld.Forward * config.EnginePower * Motor;
                return true;
            }
            else
            {
                return false;
            }
        }

        public void ApplyForwardForce(SteeringConfig config, LocalToWorld localToWorld, float speed, out float3 forcePosition, out float3 force)
        {
            float3 up = new float3(0f, 1f, 0f);

            forcePosition = localToWorld.Position + config.ForceHeightOffset * up;
            if (config.ForceForwardOffset != 0f)
            {
                forcePosition = forcePosition + config.ForceForwardOffset * localToWorld.Forward;
            }

            force = localToWorld.Forward * config.EnginePower * speed;
        }

        public bool ApplySteering(SteeringConfig config, LocalToWorld localToWorld, out float3 force)
        {
            force = default;
            if (Steer != 0f)
            {
                force = localToWorld.Up * config.TurnPower * Steer;
                return true;
            }
            else
            {
                return false;
            }
        }

        public float3 GetSteeringDirection(SteeringConfig config, LocalToWorld localToWorld)
        {
            if (Steer == 0f)
            {
                return localToWorld.Forward;
            }

            float angle = math.radians(config.TurnPower * Steer);
            return math.mul(quaternion.AxisAngle(localToWorld.Up, angle), localToWorld.Forward);
        }
    }
}