I am trying to implement A* search on a grid in MATLAB. I am currently using a priority queue class I found [here][1], but it's a bit slow. I tried to write this simple priority queue class in MATLAB:

    classdef PQ < handle
        
        properties
            nElements;
            priorityList;
            valueList;
        end
        
        methods
            function thePQueue = PQ()
                thePQueue.nElements = 0;
                thePQueue.priorityList{500} = [];
                thePQueue.valueList{500} = [];

                Index = cellfun('isempty', thePQueue.priorityList);
                thePQueue.priorityList(Index) = {NaN};
                thePQueue.valueList(Index) = {NaN};
            end
            
            function push(thePQueue, value, priority)
                thePQueue.nElements = thePQueue.nElements + 1;
                thePQueue.priorityList{thePQueue.nElements} = priority;
                thePQueue.valueList{thePQueue.nElements} = value;
            end
       
        
            function minPriorityElement = pop(thePQueue)
                if ~thePQueue.isEmpty
                    thePQueue.nElements = thePQueue.nElements - 1;
                    [~, minPriorityIndx] = min(cell2mat(thePQueue.priorityList));
                    minPriorityElement = thePQueue.valueList{minPriorityIndx};

                    thePQueue.priorityList(minPriorityIndx) = {NaN};
                    thePQueue.valueList(minPriorityIndx) = {NaN};
                else
                    disp('Queue is empty');
                end
            end
        
            function flagIsEmpty = isEmpty(thePQueue)
                flagIsEmpty = (thePQueue.nElements == 0);
            end
        end
    end

But It's giving me different results compared to the priority queue I mentioned above. I'm not even sure which one is the right one. I won't have more than 500 nodes on my grid. That's why I'm using constant length cells. This priority queue is almost 10 times faster than the one I'm using but I'm not 100% sure if it is right. How can I check it? Is there any other way I could implement the same idea and get a faster result? 

Edit: My question is on hold right now, the reason being: "Questions containing broken code or asking for advice about code not yet written are off-topic...". My question contains a working not-broken code. Please remove the hold!


  [1]: http://stackoverflow.com/a/16927918/2625036