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
            indx;
            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};
                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(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

The 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 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