blog.piflik.de

One of the more recent additions to my Utilities. I wrote this to calculate a parabolic trajectory for a thrown object (in this special case a grenade). More specifically, I calculate the required throw-velocity to accurately hit a specified point from a starting position and a given angle. This velocity can be capped and when the required speed exceeds this cap, the trajectory will not reach the target. The class can also calculate points on the trajectory in order to visualize them. Lastly, it provides a coroutine that can be used to make an object follow this trajectory. The calculation for this is done via a synchronized Leapfrog integration, and does not use Unity’s physics engine. Both this coroutine and the calculation for the trajectory-points can be specified to ‘bounce’ on surfaces, where it will take into account both the bounciness of the surface that is being hit and an optional bounciness value for the object. A bouncing object will only come to rest on a surface that is sufficiently flat.

<pre>using System.Collections;
using UnityEngine;
using System.Collections.Generic;
using System.Linq;
using UnityEngine.Events;

/// <summary>
/// Calculates a parabolic trajectory from a start point to a target point and allows an object to follow this trajectory
/// Does not use Unity's physics engine, but bounces off colliders. If the collider contains a PhysicMaterial, its bounciness will be taken into account
/// </summary>
public class Trajectory {

	private const float GRAVITY = 10;
	private const float DEFAULT_TIMESTEP = 0.01f;
	private const float FLAT_SURFACE = 0.9f; //TODO define flat surface better?

	private Vector3 _startPosition;
	private Vector3 _startVelocity;

	private float _bounciness;

	/// <summary>
	/// Calculates the required starting velocity to hit the target point from the starting position
	/// </summary>
	/// <param name="start">Start point of the trajectory</param>
	/// <param name="end">End point of the trajectory</param>
	/// <param name="angle">Angle in radians</param>
	/// <param name="maxSpeed">Speed cap; if the required speed exceeds this, the trajectory will not reach the target</param>
	/// <returns></returns>
	private Vector3 StartVelocity(Vector3 start, Vector3 end, float angle, float maxSpeed = Mathf.Infinity) {
		Vector3 direction = end - start;
		float distance = direction.magnitude;
		float elevation = direction.y;
		direction = direction.normalized;

		float tan = Mathf.Tan(angle);

		float speed = Mathf.Sqrt(Mathf.Abs(GRAVITY * distance * (tan * tan + 1) / (2 * tan - 2 * elevation / distance)));
		speed = Mathf.Min(maxSpeed, speed);

		return speed * (Mathf.Cos(angle) * direction + Mathf.Sin(angle) * Vector3.up);
	}

	/// <summary>
	/// Create Vector3 array to visualize the trajectory
	/// </summary>
	/// <param name="bounce"></param>
	/// <returns></returns>
	public IEnumerable<Vector3> GetTrajectoryPoints(bool bounce) {

		List<Vector3> points = new List<Vector3>();

		Vector3 position = _startPosition;
		Vector3 velocity = _startVelocity;

		points.Add(position);

		float timer = 0;
		int counter = 0;
		while (timer < 50) {
			timer += DEFAULT_TIMESTEP;
			counter++;

			RaycastHit hit;
			if (NextStep(ref position, ref velocity, DEFAULT_TIMESTEP, out hit)) {

				bool flatSurface = Vector3.Dot(hit.normal, Vector3.up) > FLAT_SURFACE;

				//points.Add(position);

				if(!bounce || Mathf.Approximately(velocity.y, 0) && flatSurface) {
					points.Add(position);
					break;
				}
			}

			if (counter == 5) {
				points.Add(position);
				counter = 0;
			}
		}

		return points;
	}

	/// <summary>
	/// Updates the position and velocity to the next timestpe (synchronized leap-frog integration)
	/// </summary>
	/// <param name="position">Position</param>
	/// <param name="velocity">Velocity</param>
	/// <param name="timeStep">Timestep</param>
	/// <param name="hit">If there was a collision within this timestep, this will contain the information</param>
	/// <returns>True, if collision during timestep</returns>
	private bool NextStep(ref Vector3 position, ref Vector3 velocity, float timeStep, out RaycastHit hit) {
		bool bounced = false;
		Vector3 nextPos = position + timeStep * velocity - 0.5f * timeStep * timeStep * GRAVITY * Vector3.up;
		velocity.y += -GRAVITY * timeStep;

		if (Physics.Linecast(position, nextPos, out hit, -1, QueryTriggerInteraction.Ignore)) {
			nextPos = hit.point;

			PhysicMaterial physMat;
			float collisionDecay = (physMat = hit.collider.material).bounciness > 0 ? physMat.bounciness * _bounciness : _bounciness;
			velocity = collisionDecay * Vector3.Reflect(velocity, hit.normal);

			bounced = true;
		}

		position = nextPos;

		return bounced;
	}

	/// <summary>
	/// A coroutine to follow the trajectory
	/// </summary>
	/// <param name="obj">Object to follow this trajectory</param>
	/// <param name="follow">Rotate object to face move-direction?</param>
	/// <param name="bounce">Does this object bounce off surfaces?</param>
	/// <param name="onBounce">optional action to perform on bounce</param>
	/// <param name="onDestinationReached">optional action to perform when the object reaches the destination</param>
	/// <returns></returns>
	public IEnumerator FollowTrajectory(Transform obj, bool follow, bool bounce, UnityAction<GameObject> onBounce = null, UnityAction<GameObject> onDestinationReached = null) {
		Vector3 position = _startPosition;
		Vector3 velocity = _startVelocity;

		float timer = 0;
		while(timer < 50) {
			timer += Time.deltaTime;

			RaycastHit hit;
			if(NextStep(ref position, ref velocity, Time.deltaTime, out hit)) {

				if (onBounce != null) {
					onBounce.Invoke(obj.gameObject);
				}

				bool flatSurface = Vector3.Dot(hit.normal, Vector3.up) > FLAT_SURFACE;

				if(!bounce || Mathf.Abs(velocity.y) < 0.1f && flatSurface) {
					obj.position = position;
					if (onDestinationReached != null) {
						onDestinationReached.Invoke(obj.gameObject);
					}
					break;
				}
			}

			obj.position = position;

			if (follow) {
				obj.rotation = Quaternion.LookRotation(velocity.normalized, obj.up);
			}

			yield return null;
		}
	}

	public Trajectory (Vector3 start, Vector3 end, float throwAngle, float maxSpeed, bool bounce, float bounciness = 1) {
		_bounciness = bounciness;
		_startPosition = start;
		_startVelocity = StartVelocity(start, end, throwAngle, maxSpeed);
	}

	public void Update(Vector3 start, Vector3 end, float throwAngle, float maxSpeed, bool bounce, float bounciness = 1) {
		_bounciness = bounciness;
		_startPosition = start;
		_startVelocity = StartVelocity(start, end, throwAngle, maxSpeed);
	}

	private Trajectory() {}
}