I am trying to implement A* search on a grid in MATLAB. I am currently using a priority queue class I found here, but it's a bit slow. I tried to write this simple priority queue class in MATLAB:classdef PQ2 < handle    properties        nElements        indx;        priorityList;        valueList;    end    methods        function thePQueue = PQ2()            thePQueue.nElements = 0;            thePQueue.priorityList = NaN*ones(500,1);            thePQueue.valueList{500} = [];            thePQueue.indx = 1;            thePQueue.nElements = 0;        end        function push(thePQueue, value, priority)            thePQueue.priorityList(thePQueue.indx) = priority;            thePQueue.valueList{thePQueue.indx} = value;            thePQueue.indx = thePQueue.indx + 1;            thePQueue.nElements = thePQueue.nElements + 1;        end        function minPriorityElement = pop(thePQueue)            if ~thePQueue.isEmpty                thePQueue.nElements = thePQueue.nElements - 1;                [~, minPriorityIndx] = min(thePQueue.priorityList);                minPriorityElement = thePQueue.valueList{minPriorityIndx};                thePQueue.priorityList(minPriorityIndx) = NaN;            else                disp('Queue is empty');            end        end        function flagIsEmpty = isEmpty(thePQueue)        flagIsEmpty = (thePQueue.nElements == 0);        end    endendThe above code is at least 3 times faster than the one I've mentioned above. I'm getting exactly the same output from these 2 classes, and therefore I believe that the code is correct, but I'm not 100% certain. How can I check it? Is there any other way I could implement the same idea and get a faster result?
Simple Priority Queue
queue;matlab