public void insert(HeapNode hn) throws Exception {
        if (this.lastFreePos == heapNodes.length) {
            throw new Exception("Heap overflow");
        }
        this.heapNodes[this.lastFreePos] = hn;
        this.positions[hn.vertex] = this.lastFreePos;
        int childPos = this.lastFreePos, parentPos = (childPos - 1) / 2;
        HeapNode aux;
        while (parentPos >= 0 && this.heapNodes[parentPos].compareTo(this.heapNodes[childPos]) > 0) {
            aux = this.heapNodes[parentPos];
            this.heapNodes[parentPos] = this.heapNodes[childPos];
            this.heapNodes[childPos] = aux;
            this.positions[this.heapNodes[childPos].vertex] = childPos;
            this.positions[this.heapNodes[parentPos].vertex] = parentPos;
            childPos = parentPos;
            parentPos = (childPos - 1) / 2;
        }
        this.lastFreePos++;
    }