BoatAttack/Assets/Unity Physics Items/BuoyantObject2.cs

using System;
using System.Collections.Generic;
using UnityEngine;
using Unity.Mathematics;
using Unity.Entities;
//using Unity.Physics;

namespace WaterSystem
{
	public class BuoyantObject2 : MonoBehaviour//, IConvertGameObjectToEntity
	{
		public BuoyancyType _buoyancyType; // type of buoyancy to calculate
		public float density; // density of the object, this is calculated off it's volume and mass
		public float volume; // volume of the object, this is calculated via it's colliders
		public float voxelResolution = 0.51f; // voxel resolution, represents the half size of a voxel when creating the voxel representation
		private Bounds voxelBounds; // bounds of the voxels
		public Vector3 centerOfMass = Vector3.zero; // Center Of Mass offset

		private const float DAMPFER = 0.005f;
		private const float WATER_DENSITY = 1000;

		private float baseDrag; // reference to original drag
		private float baseAngularDrag; // reference to original angular drag
		private int _guid; // GUID for the height system
		private Vector3 localArchimedesForce;

		[SerializeField]
		private Vector3[] voxels; // voxel position
		private float3[] samplePoints; // sample points for height calc
		private float3[] heights; // water height array(only size of 1 when simple or non-physical)
		private float3[] normals; // water normal array(only used when non-physical and size of 1 also when simple)
		[SerializeField]
		Collider[] colliders; // colliders attatched ot this object
		Rigidbody RB;
		private DebugDrawing[] debugInfo; // For drawing force gizmos
		public float percentSubmerged = 0f;

		[ContextMenu("Initialize")]
		void Init()
		{
			voxels = null;

			if (_buoyancyType == BuoyancyType.NonPhysicalVoxel || _buoyancyType == BuoyancyType.PhysicalVoxel) // If voxel based we need colliders and voxels
			{
				SetupColliders();
				SliceIntoVoxels();
				samplePoints = new float3[voxels.Length];
			}

			if (_buoyancyType == BuoyancyType.Physical || _buoyancyType == BuoyancyType.PhysicalVoxel) // If physical, then we need a rigidbody
			{
				// The object must have a RidigBody
				RB = GetComponent<Rigidbody>();
				if (RB == null)
				{
					RB = gameObject.AddComponent<Rigidbody>();
					Debug.LogError(string.Format("Buoyancy:Object \"{0}\" had no Rigidbody. Rigidbody has been added.", name));
				}
				RB.centerOfMass = centerOfMass + voxelBounds.center;
				baseDrag = RB.drag;
				baseAngularDrag = RB.angularDrag;
				samplePoints = new float3[voxels.Length];
			}

			if (_buoyancyType == BuoyancyType.NonPhysical || _buoyancyType == BuoyancyType.Physical)
			{
				voxels = new Vector3[1];
				voxels[0] = centerOfMass;
				samplePoints = new float3[1];
			}

			float archimedesForceMagnitude = WATER_DENSITY * -Physics.gravity.y * volume;
			localArchimedesForce = new Vector3(0, archimedesForceMagnitude, 0) / samplePoints.Length;
		}

		private void Start()
		{
			_guid = gameObject.GetInstanceID();

			Init();

			if (_buoyancyType == BuoyancyType.NonPhysical || _buoyancyType == BuoyancyType.Physical)
			{
				debugInfo = new DebugDrawing[1];
				heights = new float3[1];// new NativeSlice<float3>();
				normals = new float3[1];//new NativeSlice<float3>();
			}
			else
			{
				debugInfo = new DebugDrawing[voxels.Length];
				heights = new float3[voxels.Length]; //new NativeSlice<float3>();
			}
		}

		void SetupColliders()
		{
			// The object must have a Collider
			colliders = GetComponentsInChildren<Collider>(false);
			if (colliders.Length == 0)
			{
				colliders = new Collider[1];
				colliders[0] = gameObject.AddComponent<BoxCollider>();
				Debug.LogError(string.Format("Buoyancy:Object \"{0}\" had no coll. BoxCollider has been added.", name));
			}
		}

		void Update()
		{
			for (var i = 0; i < samplePoints.Length; i++)
			{
				samplePoints[i] = transform.TransformPoint(voxels[i]);
			}

			if (_buoyancyType == BuoyancyType.PhysicalVoxel || _buoyancyType == BuoyancyType.Physical) // if acurate the are more points so only heights are needed
			{
				GerstnerWavesJobs.UpdateSamplePoints(samplePoints, _guid, false);
				GerstnerWavesJobs.GetData(_guid, ref heights);
			}
			else
			{
				GerstnerWavesJobs.UpdateSamplePoints(samplePoints, _guid, true);
				GerstnerWavesJobs.GetSimpleData(_guid, ref heights, ref normals);

				if (_buoyancyType == BuoyancyType.NonPhysical)
				{
					Vector3 vec = transform.position;
					vec.y = heights[0].y;
					transform.position = vec;
					transform.up = Vector3.Slerp(transform.up, normals[0], Time.deltaTime);
				}
				else if (_buoyancyType == BuoyancyType.NonPhysicalVoxel)
				{
					// do the voxel non-physical
				}
			}
		}

		private void FixedUpdate()
		{
			float submergedAmount = 0f;

			if (_buoyancyType == BuoyancyType.PhysicalVoxel)
			{
				Physics.autoSyncTransforms = false;
				for (var i = 0; i < voxels.Length; i++) BuoyancyForce(voxels[i], heights[i].y, ref submergedAmount, ref debugInfo[i]);
				Physics.SyncTransforms();
				Physics.autoSyncTransforms = true;
				UpdateDrag(submergedAmount);
			}
			else if (_buoyancyType == BuoyancyType.Physical)
			{
				BuoyancyForce(Vector3.zero, heights[0].y, ref submergedAmount, ref debugInfo[0]);
				UpdateDrag(submergedAmount);
			}
		}

		private void BuoyancyForce(Vector3 position, float waterHeight, ref float submergedAmount, ref DebugDrawing _debug)
		{
			var wp = transform.TransformPoint(position);
			float waterLevel = waterHeight;

			_debug.position = wp;
			_debug.waterHeight = waterLevel;
			_debug.force = Vector3.zero;

			if (wp.y - voxelResolution < waterLevel)
			{
				float k = Mathf.Clamp01(waterLevel - (wp.y - voxelResolution)) / (voxelResolution * 2f);

				submergedAmount += k / voxels.Length;//(math.clamp(waterLevel - (wp.y - voxelResolution), 0f, voxelResolution * 2f) / (voxelResolution * 2f)) / voxels.Count;

				var velocity = RB.GetPointVelocity(wp);
				velocity.y *= 2f;
				var localDampingForce = DAMPFER * RB.mass * -velocity;
				var force = localDampingForce + Mathf.Sqrt(k) * localArchimedesForce;//\
				RB.AddForceAtPosition(force, wp);

				_debug.force = force; // For drawing force gizmos
			}
		}

		private void UpdateDrag(float submergedAmount)
		{
			percentSubmerged = Mathf.Lerp(percentSubmerged, submergedAmount, 0.25f);
			RB.drag = baseDrag + (baseDrag * (percentSubmerged * 10f));
			RB.angularDrag = baseAngularDrag + (percentSubmerged * 0.5f);
		}

		private void SliceIntoVoxels()
		{
			Quaternion rot = transform.rotation;
			Vector3 pos = transform.position;
			Vector3 size = transform.localScale;
			transform.SetPositionAndRotation(Vector3.zero, Quaternion.identity);
			transform.localScale = Vector3.one;

			voxels = null;
			var points = new List<Vector3>();

			var rawBounds = VoxelBounds();
			voxelBounds = rawBounds;
			voxelBounds.size = RoundVector(rawBounds.size, voxelResolution);
			for (float ix = -voxelBounds.extents.x; ix < voxelBounds.extents.x; ix += voxelResolution)
			{
				for (float iy = -voxelBounds.extents.y; iy < voxelBounds.extents.y; iy += voxelResolution)
				{
					for (float iz = -voxelBounds.extents.z; iz < voxelBounds.extents.z; iz += voxelResolution)
					{
						float x = (voxelResolution * 0.5f) + ix;
						float y = (voxelResolution * 0.5f) + iy;
						float z = (voxelResolution * 0.5f) + iz;

						var p = new Vector3(x, y, z) + voxelBounds.center;

						bool inside = false;
						for (var i = 0; i < colliders.Length; i++)
						{
							if (PointIsInsideCollider(colliders[i], p))
							{
								inside = true;
							}
						}
						if (inside)
							points.Add(p);
					}
				}
			}

			voxels = points.ToArray();
			transform.SetPositionAndRotation(pos, rot);
			transform.localScale = size;
			var voxelVolume = Mathf.Pow(voxelResolution, 3f) * voxels.Length;
			var rawVolume = rawBounds.size.x * rawBounds.size.y * rawBounds.size.z;
			volume = Mathf.Min(rawVolume, voxelVolume);
			density = gameObject.GetComponent<Rigidbody>().mass / volume;
		}

		private Bounds VoxelBounds()
		{
			Bounds bounds = new Bounds();
			foreach (Collider nextCollider in colliders)
			{
				bounds.Encapsulate(nextCollider.bounds);
			}
			return bounds;
		}

		static Vector3 RoundVector(Vector3 vec, float rounding)
		{
			return new Vector3(Mathf.Ceil(vec.x / rounding) * rounding, Mathf.Ceil(vec.y / rounding) * rounding, Mathf.Ceil(vec.z / rounding) * rounding);
		}

		private bool PointIsInsideCollider(Collider c, Vector3 p)
		{
			Vector3 cp = Physics.ClosestPoint(p, c, Vector3.zero, UnityEngine.Quaternion.identity);
			return Vector3.Distance(cp, p) < 0.01f ? true : false;
		}

		private void OnDrawGizmosSelected()
		{
			const float gizmoSize = 0.05f;
			var matrix = Matrix4x4.TRS(transform.position, transform.rotation, transform.lossyScale);
			Gizmos.matrix = matrix;

			if (voxels != null)
			{
				Gizmos.color = Color.yellow;

				foreach (var p in voxels)
				{
					Gizmos.DrawCube(p, new Vector3(gizmoSize, gizmoSize, gizmoSize));
				}
			}

			if (voxelResolution >= 0.1f)
			{
				Gizmos.DrawWireCube(voxelBounds.center, voxelBounds.size);
				Vector3 center = voxelBounds.center;
				float y = center.y - voxelBounds.extents.y;
				for (float x = -voxelBounds.extents.x; x < voxelBounds.extents.x; x += voxelResolution)
				{
					Gizmos.DrawLine(new Vector3(x, y, -voxelBounds.extents.z + center.z), new Vector3(x, y, voxelBounds.extents.z + center.z));
				}
				for (float z = -voxelBounds.extents.z; z < voxelBounds.extents.z; z += voxelResolution)
				{
					Gizmos.DrawLine(new Vector3(-voxelBounds.extents.x, y, z + center.z), new Vector3(voxelBounds.extents.x, y, z + center.z));
				}
			}
			else
				voxelBounds = VoxelBounds();

			Gizmos.color = Color.red;
			Gizmos.DrawSphere(voxelBounds.center + centerOfMass, 0.2f);

			Gizmos.matrix = Matrix4x4.identity; Gizmos.matrix = Matrix4x4.identity;

			if (debugInfo != null)
			{
				foreach (DebugDrawing debug in debugInfo)
				{
					Gizmos.color = Color.cyan;
					Gizmos.DrawCube(debug.position, new Vector3(gizmoSize, gizmoSize, gizmoSize)); // drawCenter
					Vector3 water = debug.position;
					water.y = debug.waterHeight;
					Gizmos.DrawLine(debug.position, water); // draw the water line
					Gizmos.DrawSphere(water, gizmoSize * 4f);
					if (_buoyancyType == BuoyancyType.Physical || _buoyancyType == BuoyancyType.PhysicalVoxel)
					{
						Gizmos.color = Color.red;
						Gizmos.DrawRay(debug.position, debug.force / RB.mass); // draw force
					}
				}
			}

		}

		public void Convert(Entity entity, EntityManager dstManager, GameObjectConversionSystem conversionSystem)
		{
			Init();

			dstManager.AddComponent(entity, typeof(MoveWithInputTag));

			BuoyantData data = new BuoyantData();
			data.type = _buoyancyType;
			data.voxelResolution = voxelResolution;
			data.localArchimedesForce = localArchimedesForce;
			data.percentSubmerged = 0f;
			data.baseDrag = baseDrag;
			data.baseAngularDrag = baseAngularDrag;
			dstManager.AddComponentData(entity, data);


			dstManager.AddBuffer<VoxelOffset>(entity);
			dstManager.AddBuffer<VoxelHeight>(entity);

			var mass = dstManager.GetComponentData<Unity.Physics.PhysicsMass>(entity);
			mass.CenterOfMass = centerOfMass;
			dstManager.SetComponentData(entity, mass);

			DynamicBuffer<VoxelOffset> offsets = dstManager.GetBuffer<VoxelOffset>(entity);
			DynamicBuffer<VoxelHeight> heights = dstManager.GetBuffer<VoxelHeight>(entity);

			for (int i = 0; i < voxels.Length; i++)
			{
                offsets.Add(new VoxelOffset { Value = voxels[i] - centerOfMass });// transform.TransformPoint(voxels[i]) - transform.position }); // << Is this right?
				heights.Add(new VoxelHeight { Value = float3.zero });
			}
		}

		struct DebugDrawing
		{
			public Vector3 force;
			public Vector3 position;
			public float waterHeight;
		}

		public enum BuoyancyType
		{
			NonPhysical,
			NonPhysicalVoxel,
			Physical,
			PhysicalVoxel
		}
	}
}