Master' Thesis Code

Monte Carlo Simulations for 3D Path Planning of quadrotors in wind and urban enviornment

V 6.5 of Christopher Milcsik's Master's Thesis ( IMPACT OF WINDS ON DRONE PACKAGE DELIVERY OPTIMAL ROUTING) Matlab Code

MiniHeap_two code: copy, paste and save as MiniHeap_two for program to run.

classdef MinHeap_two
    properties
        elements
        positions
    end
    methods
        function obj = MinHeap_two()
            obj.elements = [];
            obj.positions = containers.Map('KeyType', 'double', 'ValueType', 'double');
        end

function obj = insert(obj, index, priority)
    % Check if the index already exists in the heap
    if isKey(obj.positions, index)
        currentPosition = obj.positions(index);

        % Ensure the currentPosition is valid
        if currentPosition > 0 && currentPosition <= size(obj.elements, 1)
            currentPriority = obj.elements(currentPosition, 1);  % Get current priority

            % Case 1: New priority is better, remove the old element and insert the new one
            if priority < currentPriority
                obj.elements(currentPosition, :) = [];  % Remove the existing element
                obj.positions.remove(index);

                % Adjust positions for elements after the removed element
                for i = currentPosition:size(obj.elements, 1)
                    obj.positions(obj.elements(i, 2)) = i;
                end

                % Clean up the heap after removal
                obj = heapifyDown(obj, currentPosition);
                [obj, ~] = verifyAndFixMinHeap(obj);
            else
                % If the current priority is better or the same, no need to insert
                return;
            end

        else
            % Case 2: Handle invalid position and potential duplicate log
            duplicateCount = 0;
            duplicatePosition = -1;

            % Check for duplicates in the heap
            for i = 1:size(obj.elements, 1)
                if obj.elements(i, 2) == index
                    duplicateCount = duplicateCount + 1;
                    duplicatePosition = i;
                end
            end

            % Handle duplicate logging
            if duplicateCount > 1
                currentPriority = obj.elements(currentPosition, 1);
                duplicatePriority = obj.elements(duplicatePosition, 1);

                % Case 3: If the duplicate has better priority, remove the current element
                if duplicatePriority < currentPriority
                    obj.elements(currentPosition, :) = [];
                    obj.positions.remove(index);

                    % Adjust positions after removal
                    for i = currentPosition:size(obj.elements, 1)
                        obj.positions(obj.elements(i, 2)) = i;
                    end

                    % Clean up after removal
                    obj = heapifyDown(obj, currentPosition);
                else
                    % Case 4: Otherwise, remove the duplicate
                    obj.elements(duplicatePosition, :) = [];
                    obj.positions.remove(index);

                    % Adjust positions for elements after removal
                    for i = duplicatePosition:size(obj.elements, 1)
                        obj.positions(obj.elements(i, 2)) = i;
                    end

                    % Clean up after removing duplicate
                    obj = heapifyDown(obj, duplicatePosition);
                end
            end
            [obj, ~] = verifyAndFixMinHeap(obj);
            return;
        end
        return;
    end

    % Case 5: Insert the new element at the end of the heap
    obj.elements = [obj.elements; priority, index];
    obj.positions(index) = size(obj.elements, 1);

    % Clean up the heap by "bubbling up" the new element
    obj = heapifyUp(obj, size(obj.elements, 1));
     [obj, ~] = verifyAndFixMinHeap(obj);
end



function obj = insertbatch(obj, indices, priorities)
    % Step 1: Handle conflicts and remove existing elements if necessary
    existingIndices = indices(isKey(obj.positions, (indices)));  % Filter out existing indices
    for i = 1:length(existingIndices)
        idx = cell2mat(existingIndices(i));
        currentPosition = obj.positions(idx);
        % Ensure currentPosition is within bounds before accessing obj.elements
        if currentPosition > 0 && currentPosition <= size(obj.elements, 1)
            currentPriority = obj.elements(currentPosition, 1);  % Current priority
            % Get the priority of the new element for this index
            newPriority = priorities(cell2mat(indices) == idx);
            % If the new priority is better, remove the existing one
            if newPriority < currentPriority
                obj.elements(currentPosition, :) = [];  % Remove existing element
                obj.positions.remove(idx);
                % Adjust positions after removal
                for j = currentPosition:size(obj.elements, 1)
                    obj.positions(obj.elements(j, 2)) = j;
                end
            else
                % If current priority is better, continue to the next index
                continue;
            end
        else
            % Invalid position handling or checking for double logging
            duplicateCount = 0;
            duplicatePosition = -1;
            % Check for duplicate entries in obj.elements
            for j = 1:size(obj.elements, 1)
                if obj.elements(j, 2) == idx
                    duplicateCount = duplicateCount + 1;
                    duplicatePosition = j;
                end
            end
            % If duplicates exist, resolve by comparing priorities
            if duplicateCount > 1
                currentPriority = obj.elements(currentPosition, 1);
                duplicatePriority = obj.elements(duplicatePosition, 1);
                if duplicatePriority < currentPriority
                    % Remove current element with worse priority
                    obj.elements(currentPosition, :) = [];
                    obj.positions.remove(idx);
                    % Adjust positions after removal
                    for j = currentPosition:size(obj.elements, 1)
                        obj.positions(obj.elements(j, 2)) = j;
                    end
                else
                    % Remove duplicate with worse priority
                    obj.elements(duplicatePosition, :) = [];
                    obj.positions.remove(idx);
                    % Adjust positions after removal
                    for j = duplicatePosition:size(obj.elements, 1)
                        obj.positions(obj.elements(j, 2)) = j;
                    end
                end
            end
        end
    end
    % Step 2: Insert all new elements into the heap
    if ~isempty(indices)
        % Convert indices and priorities to numeric arrays
        indicesNumeric = cell2mat(indices);
        prioritiesNumeric = priorities(:);
        % Append the new elements to the heap
        obj.elements = [obj.elements; [prioritiesNumeric, indicesNumeric]];
        % Update positions for the new elements
        for i = 1:length(indicesNumeric)
            obj.positions(indicesNumeric(i)) = size(obj.elements, 1) - length(indicesNumeric) + i;
        end
        % Step 3: Perform heapify for all new elements
        for i = (size(obj.elements, 1) - length(indicesNumeric) + 1):size(obj.elements, 1)
            obj = heapifyUp(obj, i);
        end
    end
end





function [obj, index, priority] = extractMin(obj)
    if isempty(obj.elements)
        index = [];
        priority = [];
        return;
    end
    % Get the minimum priority and its corresponding index
    priority = obj.elements(1, 1);  % The minimum priority is always at the top
    index = obj.elements(1, 2);     % The corresponding index
    % Remove the minimum element from the heap
    if size(obj.elements, 1) > 1
        obj.elements(1, :) = obj.elements(end, :);  % Replace the root with the last element
        obj.elements(end, :) = [];  % Remove the last element
        obj = heapifyDown(obj, 1);  % Restore the heap property
    else
        obj.elements = [];  % If only one element, clear the heap
    end
    % Remove the index from the positions map
    if isKey(obj.positions, index)
        remove(obj.positions, index);
    end
     [obj, ~] = verifyAndFixMinHeap(obj);
end


%% extractMin multiple indices
function [obj, indices, priority] = extractMinbatch(obj)
    if isempty(obj.elements)
        indices = [];
        priority = [];
        return;
    end
    % Get the minimum priority and its index
    minPriority = obj.elements(1, 1);
    % Initialize an array to hold indices that are within 10% of minPriority
    indices = [];
    count = 0;  % Counter to stop after 4 elements
    % Loop through all elements to find those within 10% of minPriority
    for i = 1:size(obj.elements, 1)
        if obj.elements(i, 1) <= minPriority * 1.015
            indices = [indices; obj.elements(i, 2)]; % Collect indices
            count = count + 1;
            % Stop after n elements
            if count >= 10
                break;
            end
        end
    end
    % Now, we need to remove the minimum element from the heap
    priority = minPriority; % Store the min priority to return
    if size(obj.elements, 1) > 1
        obj.elements(1, :) = obj.elements(end, :);
        obj.elements(end, :) = [];
        obj = heapifyDown(obj, 1);
    else
        obj.elements = [];
    end
    % Check if the first index exists in the positions map before removing it
    if isKey(obj.positions, indices(1))
        remove(obj.positions, indices(1));
    end
 end


        function obj = heapifyUp(obj, idx)
            while idx > 1
                parentIdx = floor(idx / 2);
                if obj.elements(idx, 1) < obj.elements(parentIdx, 1)
                    obj = swap(obj, idx, parentIdx);
                    idx = parentIdx;
                else
                    break;
                end
            end
        end
        
        function obj = heapifyDown(obj, idx)
            numElements = size(obj.elements, 1);
            while true
                leftIdx = 2 * idx;
                rightIdx = 2 * idx + 1;
                smallest = idx;

                if leftIdx <= numElements && obj.elements(leftIdx, 1) < obj.elements(smallest, 1)
                    smallest = leftIdx;
                end
                if rightIdx <= numElements && obj.elements(rightIdx, 1) < obj.elements(smallest, 1)
                    smallest = rightIdx;
                end
                if smallest ~= idx
                    obj = swap(obj, idx, smallest);
                    idx = smallest;
                else
                    break;
                end
            end
        end
        
        function obj = swap(obj, idx1, idx2)
            obj.positions(obj.elements(idx1, 2)) = idx2;
            obj.positions(obj.elements(idx2, 2)) = idx1;
            temp = obj.elements(idx1, :);
            obj.elements(idx1, :) = obj.elements(idx2, :);
            obj.elements(idx2, :) = temp;
        end
        
        function isEmpty = isEmpty(obj)
            isEmpty = isempty(obj.elements);
        end
    end
end

function [obj, isValid] = verifyAndFixMinHeap(obj)
    % Get the total number of elements in the heap
    numElements = size(obj.elements, 1);
    
    % Start assuming the heap is valid
    isValid = true;
    
    % Loop through each element that has at least one child
    for i = 1:floor(numElements / 2)
        % Get the priority of the current element (parent)
        parentPriority = obj.elements(i, 1);

        % Calculate the index of the left child
        leftChildIdx = 2 * i;
        % Check if the left child exists
        if leftChildIdx <= numElements
            leftChildPriority = obj.elements(leftChildIdx, 1);
            % Verify heap property for left child
            if parentPriority > leftChildPriority
                %fprintf('Heap property violated between parent index %d and left child index %d. Fixing...\n', i, leftChildIdx);
                isValid = false;
                % Fix the heap by applying heapifyDown from the parent index
                obj = heapifyDown(obj, i);
            end
        end

        % Calculate the index of the right child
        rightChildIdx = 2 * i + 1;
        % Check if the right child exists
        if rightChildIdx <= numElements
            rightChildPriority = obj.elements(rightChildIdx, 1);
            % Verify heap property for right child
            if parentPriority > rightChildPriority
                %fprintf('Heap property violated between parent index %d and right child index %d. Fixing...\n', i, rightChildIdx);
                isValid = false;
                % Fix the heap by applying heapifyDown from the parent index
                obj = heapifyDown(obj, i);
            end
        end
    end

    % If no violations were found, print a message
    if isValid
        %fprintf('The min-heap property is valid.\n');
    else
        %fprintf('Heap violations were fixed.\n');
    end
end