//Integration method 3
//upVec is a vector perpendicular (in the upwards direction) to the direction the bullet is travelling in
//is only needed if we calculate the lift force
public static void Heuns(float timeStep, Vector3 currentPos, Vector3 currentVel, Vector3 upVec, BulletData bulletData, out Vector3 newPos, out Vector3 newVel)
{
//Add all factors that affects the acceleration
//Gravity
Vector3 accFactorEuler = gravityVec;
//Drag
accFactorEuler += BulletPhysics.CalculateBulletDragAcc(currentVel, bulletData);
//Lift
accFactorEuler += BulletPhysics.CalculateBulletLiftAcc(currentVel, bulletData, upVec);
//Calculate the new velocity and position
//y_k+1 = y_k + timeStep * 0.5 * (f(t_k, y_k) + f(t_k+1, y_k+1))
//Where f(t_k+1, y_k+1) is calculated with Forward Euler: y_k+1 = y_k + timeStep * f(t_k, y_k)
//Step 1. Find new pos and new vel with Forward Euler
Vector3 newVelEuler = currentVel + timeStep * accFactorEuler;
//New position with Forward Euler (is not needed here)
//Vector3 newPosEuler = currentPos + timeStep * currentVel;
//Step 2. Heuns method's final step
//If we take drag into account, then acceleration is not constant - it also depends on the velocity
//So we have to calculate another acceleration factor
//Gravity
Vector3 accFactorHeuns = gravityVec;
//Drag
//This assumes that windspeed is constant between the steps, which it should be because wind doesnt change that often
accFactorHeuns += BulletPhysics.CalculateBulletDragAcc(newVelEuler, bulletData);
//Lift
accFactorHeuns += BulletPhysics.CalculateBulletLiftAcc(newVelEuler, bulletData, upVec);
newVel = currentVel + timeStep * 0.5f * (accFactorEuler + accFactorHeuns);
newPos = currentPos + timeStep * 0.5f * (currentVel + newVelEuler);
}