Improve agent's movement. Debug agent rotate to X, Y axis

Assets/Scripts/NodalPathfinding/NodalPathfinding2DAgent.cs

@@ -34,6 +34,10 @@ namespace BS {
                 Gizmos.color = Color.red;
                 Gizmos.DrawSphere(destination + new Vector3(0, 0, 1), 0.2f);
             }
+            Gizmos.color = Color.red;
+            if (rb == null)
+                rb = GetComponent<Rigidbody2D>();
+            Gizmos.DrawLine(transform.position, transform.position + new Vector3(rb.velocity.x, rb.velocity.y));
         }
 
 		private void Start()
@@ -75,9 +79,9 @@ namespace BS {
 			if (path.Count > 0)
 			{
 				transform.position += (path[0] - transform.position).normalized * moveSpeed * Time.fixedDeltaTime;
-				//rb.velocity = Vector3.Lerp(rb.velocity, (path[0] - transform.position).normalized * moveSpeed, 0.5f);
+				rb.velocity = Vector3.Lerp(rb.velocity, (path[0] - transform.position).normalized * moveSpeed, 0.1f);
 				//Debug.Log((path[0] - transform.position).normalized.magnitude);
-				rb.velocity = (path[0] - transform.position).normalized * moveSpeed;
+				//rb.velocity = (path[0] - transform.position).normalized * moveSpeed;
 
 				if (path.Count > 1 && Vector2.Distance(transform.position, path[0]) < 0.1f && Vector2.Dot(path[1] - path[0], path[0] - transform.position) < 0)
 				{
@@ -93,8 +97,11 @@ namespace BS {
 					path.RemoveAt(0);
 				}
 			}
-			if (rb.velocity.magnitude > 0.01f)
-				transform.rotation = Quaternion.Lerp(transform.rotation, Quaternion.FromToRotation(Vector3.up, rb.velocity), 0.05f);
+            if (rb.velocity.magnitude > 0.01f)
+            {
+                float angle = Vector2.SignedAngle(Vector2.up, rb.velocity);
+                transform.rotation = Quaternion.Lerp(transform.rotation, Quaternion.AngleAxis(angle, Vector3.forward), 0.2f);
+            }
 		}
 	}
 }
\ No newline at end of file