function [ output ] = obstacleCheck(P1, P2, heightThresh, heightMax, thetaThresh)
% Function to check if two points are within the cone to be classified as
% an obstacle.
%INPUTS:
%   P1, P2 - Points to be checked in XYZ space
%   heightThresh - Threshold height above which the second point has to be
%   heightMax - Max height of the cone 
%   thetaThresh - The angle threshold of the cone

%OUTPUT:
%   output - boolean to show compatible or not

P3 = [P1(1), P1(2), P1(3)+heightMax];
delP1 = P3-P1;
delP2 = P2-P1;

if (abs(P2(3)- P1(3))>heightThresh) && (abs(P2(3)- P1(3))<heightMax) && (dot(delP1,delP2)/norm(delP1)/norm(delP2)>cos(thetaThresh))
    output = 1;
else
    output = 0;
end
end