Calculating pitch angle to hit a target using a projectile that has fixed speed

Question

I am trying to calculate a vertical angle needed to hit a target at [distance, elevation] 2D coordinates relative to shooter given a fixed projectile velocity, air drag and gravitational acceleration.

Although I operate in 3D world (minecraft) the task itself eliminates the 3D problem. Once you calculate the rotation around your axis, which is in my case just atan2(-x3D, -z3D) what I'm solving is described on this picture:

From a wikipedia article about balistics I have picked this equation. As you can see, it seems to directly solve for my angle:

$$ \theta = arctan \bigg( \frac{v^2 \pm \sqrt{v^4-g(gx^2+2yv^2)}}{gx} \bigg) $$

So I rewrote the equation in javascript as Math.atan((v*v+Math.sqrt(v*v*v*v - g*(g*x*x+2*y*v*v)))/g*x); where v is speed, g is gravitational acceleration, x is distance and y is elevation of the target.

I implemented the whole algorithm as following:

//Assumed constants
var g = 9.81;  //Gravitational acceleration
var v = 35.9;  //Experimantally measured arrow speed
//The point of player head 3D [x,y,z]
var mepoint = this.bot.entity.position.offset(0, this.bot.entity.height, 0);
//The point of the target center assuming we're at [0, 0, 0] again using 3D [x,y,z]
var point = this.target.position.offset(0, this.target.height/2, 0).minus(mepoint);
//Transfer the coordinates with regards to the bot rotation
//That is, the 2D X coordinate is the distance as seen from the top
var x = Math.sqrt(point.x*point.x+point.z*point.z);
//The 2D Y coordinate is just plain difference between elevation
var y = point.y;
console.log("|BOT: TaskShoot| Target 2D coords [distance, elevation]: ",[x, y]);
//Calculate the horizontal angle - look at the target 3D [x,z] coordinates
var yaw = Math.atan2(-point.x, -point.z);

//Calculate the pitch using the equation from wikipedia
var pitch =  Math.atan((v*v+Math.sqrt(v*v*v*v - g*(g*x*x+2*y*v*v)))/g*x);
//Since it has 2 solutions, solve for bot + and - variant
var pitch2 = Math.atan((v*v-Math.sqrt(v*v*v*v - g*(g*x*x+2*y*v*v)))/g*x);          
console.log("|BOT: TaskShoot| Angles: ",[pitch, pitch2]);
//NaN test: !(NaN>=0 || NaN<=0)  
//Test two pitches and avoid NaNs. If both pitches are NaN, the target is out of range.
if(!isNaN(pitch))
  this.bot.look(yaw, pitch, false, function() {_this.finished();});
else if(!isNaN(pitch2))
  this.bot.look(yaw, pitch2, false, function() {_this.finished();});
else
  this.bot.look(yaw, 0, false, function() {_this.finishedError(123, "Couldn't calculate aim pitch - out of range? Aiming straight.");}); 

But this calculation allways returns almost the same values for the angles:

|BOT: TaskShoot| Target 2D coords [distance, elevation]:  [ 13.027689861406742, 3.032749999999993 ]
|BOT: TaskShoot| Angles:  [ 1.5704302724148764, -1.5643119398157057 ]

1.5704 is about pi/2. I never got significantly different result than this. So what's wrong with my calculation and how should I fix it? Did I even pick the right formula?

Answer 1

Refactoring is your friend !!!
Separate the issues and try to get your code as generic as possible.
Separate and factorise the steps of the computing.
Keep comments short.

• Boilerplate :

// ------------------------

var g = 9.81; //Gravitational acceleration
var v = 35.9; //Experimantally measured arrow speed

var sqrt = Math.sqrt,
    atan2 = Math.atan2,
    abs = Math.abs,
    sq = function (x) {
        return x * x;
    };

• Function dealing with entities

// ------------------------

function getAngleToEntity(srcEntity, tgtEntity) {
    return getAngleTo(
    srcEntity.position.offset(0, srcEntity.height / 2, 0),
    tgtEntity.position.offset(0, srcEntity.height / 2, 0),
    v);
}

• Function just dealing with vectors/point

// ------------------------

function getAngleTo(srcPoint, dstPoint, speed) {
    // delta = dst - src
    var delta = dstPoint.minus(srcPoint);
    // distance as seen from the top
    var x = Math.sqrt(sq(delta.x) + sq(delta.z));
    // elevation
    var y = delta.y;
    //Calculate the direct angle
    var straight = Math.atan2(y, x);
    //Calculate the pitch using the equation from wikipedia
    var sqSpeed = sq(speed),
        qdSpeed = sq(sqSpeed);
    var rightTerm = qdSpeed - g * (g * sq(x) + 2 * sq(y));
    if (rightTerm < 0) {
        // handle not-able-to-reach case
        console.log('no reach');
        // return ... ??? ;
    }
    rightTerm = sqrt(rightTerm);
    var angle1 = atan2((sqSpeed + rightTerm), (g * x));
    var angle2 = atan2((sqSpeed - rightTerm), (g * x));
    // Choose the angle closest from straight angle.
    var resAngle = abs(angle1 - straight) < abs(angle2 - straight) ? angle1 : angle2;
    // log
    console.log("|BOT: TaskShoot| Target 2D coords [distance, elevation]: ", [x, y]);
    console.log("|BOT: TaskShoot| Angles: ", [angle1, angle2]);
    //
    return resAngle;
}

• Unit tests (add some others, and check results !! ) :

// ------------------------

function Vector(x, y, z) { this.x = x;
    this.y = y;
    this.z = z;
    this.minus = function (other) {
        return new Vector(this.x - other.x, this.y - other.y, this.z - other.z);
    }
}

// ------------------------

// some unit tests here
getAngleTo(new Vector(0, 0, 0), new Vector(30, 0, 90), v);

Answer 2

Try fixing parenthesis:

var pitch =  Math.atan((v*v+Math.sqrt(v*v*v*v - g*(g*x*x+2*y*v*v)))/(g*x));
var pitch2 = Math.atan((v*v-Math.sqrt(v*v*v*v - g*(g*x*x+2*y*v*v)))/(g*x));  

Consider also using atan2 function. Maybe in your case it won't fix anything but it's always good to know about it. https://en.wikipedia.org/wiki/Atan2