changed: redoing grapple again, freshly started

Assets/Scripts/Core/Graph Tasks/DoGrapplePull.cs

@@ -1,8 +1,12 @@
+using System;
 using Drawing;
+using log4net.Appender;
 using NodeCanvas.Framework;
 using ParadoxNotion.Design;
 using ParadoxNotion.Services;
 using Reset.Units;
+using Sirenix.Serialization;
+using Unity.Cinemachine;
 using UnityEngine;
 
 
@@ -28,10 +32,11 @@ namespace NodeCanvas.Tasks.Actions {
 		public BBParameter<AnimationCurve> endDeaccelerationCurve;
 		
 		private float startTime;
+		
+		private Vector3 velocityOnStart;
+
 
 		private Vector3 originalDirection;
-		public float originalDistance;
-		public Vector3 originalLocation;
 		
 		public float breakAtDistance;
 		public float breakAtDotProduct;
@@ -40,17 +45,19 @@ namespace NodeCanvas.Tasks.Actions {
 		private Vector3 smoothedInput;
 		private Vector3 smoothedInputRefVelocity;
 
-		private Vector3 gizmoHookPoint;
+		private Vector3 gizmoSwingDirection;
-		private Vector3 gizmoCenterPoint;
+		private Vector3 gizmoPointDirection;
-		private Vector3 gizmoDirection;
+		private Vector3 gizmoFinalDirection;
-		private Vector3 gizmoStartPos;
+		private Vector3 gizmosSmoothedInput;
-		private Vector3 gizmoEndPos;
 
 		private Transform camera;
 
+		private float referenceSpeed;
+
 		private Vector3 finalDirection;
-		
+		private float yChangeMultipler;
-	//Use for initialization. This is called only once in the lifetime of the task.
+
+		//Use for initialization. This is called only once in the lifetime of the task.
 		//Return null if init was successfull. Return an error string otherwise
 		protected override string OnInit(){
 			
@@ -63,37 +70,57 @@ namespace NodeCanvas.Tasks.Actions {
 		protected override void OnExecute(){
 			camera = Camera.main.transform;
 			MonoManager.current.onLateUpdate += DrawGrappleGizmo;
-			originalLocation = agent.transform.position;
+
-			originalDirection = agent.transform.position.DirectionTo(grapplePoint.value);
+			// Get the current move direction
-			originalDistance = Vector3.Distance(agent.transform.position, grapplePoint.value);
+			velocityOnStart = agent.outputMoveDirection;
+
+			// Use the current move direciton to initialize the direction
+			finalDirection = velocityOnStart;
+			
 			startTime = Time.time;
 			currentSpeed = pullSpeedCurve.value[0].value * pullSpeedRange.value.y;
-			finalDirection = Vector3.zero;
+
+			smoothedInput = agent.GetComponent<CharacterController>().velocity.normalized.Flatten(null, 0f, 0f);
 		}
 
 		//Called once per frame while the action is active.
 		protected override void OnUpdate(){		
-			// Add input changes
+			// Calculate input
 			Vector2 rawInput = agent.GetComponent<PlayerControls>().rawMoveInput;
-			Vector3 input = Quaternion.LookRotation(agent.transform.position.DirectionTo(grapplePoint.value)) * new Vector3(rawInput.x, rawInput.y, 0f);
+			Vector3 input = new Vector3(rawInput.x, rawInput.y, 0f);
-			// input = Quaternion.LookRotation(agent.transform.position.DirectionTo(grapplePoint.value)) * (camera.rotation * input);
+			
-
+			smoothedInput = Vector3.SmoothDamp(smoothedInput, input, ref smoothedInputRefVelocity, 1f); 
-			smoothedInput = Vector3.SmoothDamp(smoothedInput, input, ref smoothedInputRefVelocity, 5f * Time.deltaTime); 
 			
 			// Create the distance variables
-			Vector3 dirToPoint = agent.transform.position.DirectionTo(grapplePoint.value + (smoothedInput * 6));
+			float currentDist = Vector3.Distance(agent.transform.position, grapplePoint.value);
-			float currentDist = Vector3.Distance(agent.transform.position, grapplePoint.value); 
 
-			var center = agent.transform.position + agent.transform.position.DirectionTo(grapplePoint.value).normalized * 5f;
+			// Calculate the direction on XZ
+			Vector3 directionToPoint = agent.transform.position.DirectionTo(grapplePoint.value);
+			Vector3 directionToSwing = Quaternion.LookRotation(input) * directionToPoint;
 			
-			// finalDirection = Vector3.Slerp(finalDirection, dirToPoint, originalDistance);
+			finalDirection = Vector3.Slerp(directionToPoint, directionToSwing, (elapsedTime * .6f));	
-			// finalDirection = Vector3.Slerp(relativeStart, relativeEnd, currentDist / 5f);
-			finalDirection = Vector3.Slerp(finalDirection, dirToPoint + smoothedInput, currentDist / 5f);
 			
-			gizmoHookPoint = grapplePoint.value + (smoothedInput * 5);
+			// Flatten it
-				
+			// finalDirection.Flatten(null, 0f); // don't wanna flatten it to use the natural y as gravity
+			
+			// Some math for getting the Y
+			yChangeMultipler = Mathf.Lerp(yChangeMultipler, 0f, elapsedTime * .5f); // Starts at 1 so that the player has more ability to change height on start of swing, then smooths to zero
+
+			// Add Y, this is a test
+			if (Vector3.Dot(finalDirection, Vector3.down) < 0) { // This should make you go up
+				float yTarget = 10f;
+
+				finalDirection = Vector3.Slerp(finalDirection, finalDirection.Flatten(null, yTarget), 1f * Time.deltaTime);
+			} else {
+				float yTarget = -10f;
+
+				finalDirection = Vector3.Slerp(finalDirection, finalDirection.Flatten(null, yTarget), 1f * Time.deltaTime);
+			}
+			
+			Debug.Log(Vector3.Dot(directionToSwing, Vector3.down));
+			
+			// Speed
 			float evaluatedSpeed = pullSpeedCurve.value.Evaluate(Mathf.Clamp((Time.time - startTime) / 6f, 0f, Mathf.Infinity));
-
 			float speedAgainstCurve = Mathf.Lerp(pullSpeedRange.value.x, pullSpeedRange.value.y, evaluatedSpeed);
 			
 			// Find how far from 0-1 the player is from the max and minimum distance
@@ -116,16 +143,20 @@ namespace NodeCanvas.Tasks.Actions {
 			// Soften the speed changes
 			currentSpeed = Mathf.Lerp(currentSpeed, speedAgainstCurve, 10f * Time.deltaTime);
 			
-			Debug.Log(dirToPoint);
+			// Gizmos
-			dirToPoint.y *= 2.5f;
+			gizmosSmoothedInput = smoothedInput;
-
+			gizmoPointDirection = directionToPoint;
-			gizmoDirection = finalDirection;
+			gizmoSwingDirection = directionToSwing;
-			// agent.GetComponent<CharacterController>().Move((dirToPoint.normalized + smoothedInput) * currentSpeed * Time.deltaTime);
+			gizmoFinalDirection = finalDirection;
-			agent.AddToCurrentDirection(finalDirection, currentSpeed * Time.deltaTime);
+			
-
+			agent.SetNewDirection(finalDirection.normalized.Flatten(null, 0));
-			// Debug.Log( $"{ endDeaccelerationCurve.value.Evaluate(((slowdownDistance.value - currentDist)) )}");
+			agent.SetNewGravity(finalDirection.y);
-
+			agent.SmoothToSpeed(5f, 1f * Time.deltaTime, out referenceSpeed);
-			if (Vector3.Dot(originalDirection, dirToPoint) < breakAtDotProduct) {
+			
+			// agent.SmoothToDirection(finalDirection.Flatten(null, 0).normalized * evaluatedSpeed, 1f  * Time.deltaTime, out referenceDirection);
+			// agent.SmoothToGravitation(finalDirection.y, 1f, out referenceGravity);
+			
+			if (Vector3.Dot(velocityOnStart, directionToPoint.normalized) < breakAtDotProduct) {
 				EndAction(true);
 			} else if (currentDist < breakAtDistance) {
 				EndAction(true);
@@ -133,22 +164,52 @@ namespace NodeCanvas.Tasks.Actions {
 		}
 
 		public void DrawGrappleGizmo(){
+			// Destination gizmos
 			using (Draw.WithColor(Color.blue)){
-				Draw.Arrow(agent.transform.position, agent.transform.position + gizmoDirection);
+				Vector3 offsetTowardsCamera = grapplePoint.value.DirectionTo(camera.transform.position);
-				Draw.SolidCircle(grapplePoint.value, grapplePoint.value.DirectionTo(camera.position), 1f);
 				
-			}
+				// Grapple Point
+				Draw.SolidCircle(grapplePoint.value + offsetTowardsCamera, grapplePoint.value.DirectionTo(camera.transform.position), .4f);
+				Draw.Label2D(grapplePoint.value + offsetTowardsCamera * 2f  + Vector3.up, "Grapple Point");
 
-			using (Draw.WithColor(Color.yellow)) {
+				using (Draw.WithLineWidth(1.5f)) {
-				Draw.Arrow(grapplePoint.value, gizmoHookPoint);
+					// Final Direction
-			}
+					Draw.Line(agent.transform.position + Vector3.up, agent.transform.position + Vector3.up + gizmoFinalDirection.normalized * 2f);
+					Draw.ArrowheadArc(agent.transform.position + Vector3.up, gizmoFinalDirection.normalized, 2f, 15f);
 
+					Color inactiveBlue = new(0, 0, 1, .1f);
+					Color swingColor = Color.Lerp(Color.blue, inactiveBlue, (elapsedTime * .6f));
+					Color dirColor = Color.Lerp(inactiveBlue, Color.blue, (elapsedTime * .6f));
+
+					// Swing Direction
+					using (Draw.WithColor(swingColor)) {
+						float swingLength = 2.2f;
+						Vector3 swingStart = agent.transform.position + Vector3.up * .4f;
+						Vector3 swingDir = swingStart + gizmoSwingDirection.normalized * swingLength;
+						Draw.DashedLine(swingStart, swingDir, .2f, .2f);
+						Draw.ArrowheadArc(swingStart, gizmoSwingDirection.normalized, swingLength, 15f);
+						Draw.Label2D(swingDir + Vector3.up * .4f, "Swing Direction");
+					}
+
+					// Point Direction
+					using (Draw.WithColor(dirColor)) {
+						float pointLength = 1.2f;
+						Vector3 pointStart = agent.transform.position + Vector3.up * .2f;
+						Vector3 pointDir = pointStart + gizmoPointDirection.normalized * pointLength;
+						Draw.DashedLine(pointStart, pointDir, .2f, .2f);
+						Draw.ArrowheadArc(pointStart, gizmoPointDirection.normalized, pointLength, 15f);
+						Draw.Label2D(pointDir + Vector3.up * .4f, "Grapple Point Direction");
+					}
+				}
+			}
+			
+			// Input Gizmos
 			using (Draw.WithColor(Color.red)) {
-				Draw.SolidCircle(gizmoCenterPoint, gizmoCenterPoint.DirectionTo(camera.position), .2f);
+				// Input left and right, up and down for size 
-			}
+				Vector3 inputGizmoOffset = (agent.transform.position + camera.rotation * Vector3.back + Vector3.up * .5f);
-
+				Vector3 inputGizmoPosition = inputGizmoOffset + camera.rotation * Vector3.Lerp(Vector3.left, Vector3.right, (gizmosSmoothedInput.x - 1 ) / 2f + 1f);
-			using (Draw.WithColor(Color.magenta)) {
+				Draw.Line(inputGizmoOffset + camera.rotation * Vector3.left, inputGizmoOffset + camera.rotation * Vector3.right);
-				Draw.Line(agent.transform.position, grapplePoint.value);
+				Draw.SolidCircle(inputGizmoPosition, inputGizmoPosition.DirectionTo(camera.position), Mathf.Lerp(.2f, .6f, (gizmosSmoothedInput.y - 1 ) / 2f + 1f));
 			}
 		}
 		

Assets/Scripts/Player/UnitMovementHandler.cs

@@ -1,10 +1,14 @@
 using System;
+using System.Numerics;
 using NUnit.Framework.Internal;
 using UnityEngine;
 using ParadoxNotion.Design;
 using Sirenix.OdinInspector;
 using UnityEditor.Rendering;
 using UnityEngine.Serialization;
+using Quaternion = UnityEngine.Quaternion;
+using Vector2 = UnityEngine.Vector2;
+using Vector3 = UnityEngine.Vector3;
 
 public enum PlayerFacingDirection{
     TowardsTarget = 0,
@@ -81,6 +85,8 @@ namespace Reset.Units{
 
         private float directionChangeDot;
         private bool moveCallDisabledNextFrame;
+
+        private bool movedThisFrame;
         
         private CharacterController controller;
         private PlayerControls controls;
@@ -89,6 +95,8 @@ namespace Reset.Units{
         private Vector3 moveSmooth;
         public float gravitySmooth;
 
+        private bool relativeToCamera;
+        
         [ShowInInspector, PropertyOrder(2)]
         public UnitMovementData data = new();
 
@@ -113,8 +121,6 @@ namespace Reset.Units{
             UpdateCurrentRotation();
             
             DoMovement();
-            
-
         }
 
         // Add directly to the direction
@@ -123,9 +129,31 @@ namespace Reset.Units{
             additionalSpeed = power;
         }
 
-        public void SetNewDirection(Vector3 inputDirection, float power){
+        public void SmoothToDirection(Vector3 desiredDirection, float value, out Vector3 referenceDirection){
-            additionalMoveDirection = inputDirection.normalized;
+            // referenceDirection = outputMoveDirection;
-            additionalSpeed = power * Time.deltaTime;
+
+            additionalMoveDirection = Vector3.Slerp(outputMoveDirection, desiredDirection, value);
+            
+            referenceDirection = outputMoveDirection;
+        }
+
+        public void SmoothToSpeed(float desiredSpeed, float smoothing, out float referenceSpeed){
+            additionalSpeed = Mathf.Lerp(outputSpeed, desiredSpeed, smoothing * Time.deltaTime);
+            
+            referenceSpeed = additionalSpeed;
+        }
+
+        // public void SmoothToGravity(float desiredGravity, float smoothing, out float referenceGravity){
+        //     outputMoveDirection.y = Mathf.Lerp(outputMoveDirection.y, desiredGravity, smoothing);
+        //     referenceGravity = desiredGravity;
+        // }
+
+        public void SetNewDirection(Vector3 inputDirection){ // NOTE: If smoothing desired add a default bool for smoothing maybe?
+            additionalMoveDirection = inputDirection.Flatten(null, 0f, null);
+        }
+
+        public void SetNewGravity(float value){
+            additionalMoveDirection.y = value;
         }
 
         // Hold a new rotation to be moved to during PlayerFacingDirection.SpecifiedRotation
@@ -151,7 +179,7 @@ namespace Reset.Units{
             if (float.IsPositiveInfinity(speed)) {
                 return;
             }
-
+            
             outputSpeed = Mathf.Lerp(outputSpeed, Mathf.Max(data.moveSpeed, speed), priority);
         }
         
@@ -210,6 +238,7 @@ namespace Reset.Units{
 
         // Update the speed, called every frame
         private void UpdateCurrentSpeed(){
+            Debug.Log(data.moveSpeed);
             outputSpeed = Mathf.Lerp(outputSpeed, data.moveSpeed, data.moveSpeedSoothing * Time.deltaTime);
         }
 
@@ -228,7 +257,7 @@ namespace Reset.Units{
             float gravityMoveDirection = data.jumpPower + (Physics.gravity.y * data.gravityPower);
             
             // Commit gravity to move direction, ignoring XZ since those are done in UpdateMovementDirection
-            outputMoveDirection.y = Mathf.SmoothDamp(outputMoveDirection.y, gravityMoveDirection, ref gravitySmooth, .1f * Time.deltaTime);
+            // outputMoveDirection.y = Mathf.SmoothDamp(outputMoveDirection.y, gravityMoveDirection, ref gravitySmooth, .1f * Time.deltaTime);
         }
 
         // Update the rotation, called every frame
@@ -291,17 +320,8 @@ namespace Reset.Units{
         }
         
         // Custom move from input
-        public void DoMovement(Vector3 moveDir, float speed, float gravityScale, bool relativeToCamera = true){
+        public void DoMovement(Vector3 moveDir, float speed, float gravityScale){
-            if (moveCallDisabledNextFrame) {
+            // Debug.Log($"moveDir: {moveDir}, agent velocity: {transform.InverseTransformDirection(controller.velocity.normalized)}");
-                // Vector3 velocity = transform.InverseTransformDirection(controller.velocity.normalized);
-                // outputMoveDirection = new Vector3(velocity.x, outputMoveDirection.y, velocity.z);
-                                
-                
-                moveCallDisabledNextFrame = false;
-                return;
-            }
-            
-            Debug.Log($"moveDir: {moveDir}, agent velocity: {transform.InverseTransformDirection(controller.velocity.normalized)}");
 
             // Seperate the different move directions. Additonal becomes it's own because it needs to be added independently of the other two
             Vector3 moveXZDir = new Vector3(moveDir.x, 0f, moveDir.z);
@@ -316,11 +336,9 @@ namespace Reset.Units{
             // Construct the direction and move
             Vector3 finalDir = moveXZDir + moveYDir;
 
-            if (relativeToCamera) {
+
                 controller.Move((Camera.main.transform.rotation.Flatten(0, null, 0) * finalDir) + addDir);    
-            } else {
+
-                controller.Move(finalDir + addDir);
-            }
         }
 
         void LateUpdate(){