VelocityEstimator.EstimateVelocityCoroutine C# (CSharp) Method

	IEnumerator EstimateVelocityCoroutine()
	{
		sampleCount = 0;

		Vector3 previousPosition = transform.position;
		Quaternion previousRotation = transform.rotation;
		while ( true )
		{
			yield return new WaitForEndOfFrame();

			float velocityFactor = 1.0f / Time.deltaTime;
			float angularVelocityFactor = Mathf.Deg2Rad / Time.deltaTime;

			int v = sampleCount % velocitySamples.Length;
			int w = sampleCount % angularVelocitySamples.Length;
			sampleCount++;

			// Estimate linear velocity
			velocitySamples[v] = velocityFactor * ( transform.position - previousPosition );

			// Estimate angular velocity
			float angle;
			Vector3 axis;
			Quaternion deltaRotation = transform.rotation * Quaternion.Inverse( previousRotation );
			deltaRotation.ToAngleAxis( out angle, out axis );
			angularVelocitySamples[w] = angle * axis * angularVelocityFactor;

			previousPosition = transform.position;
			previousRotation = transform.rotation;
		}
	}
}