// File: /workspace/github_cpp_makefile_star2_O2/tomfunkhouser[P]gaps/pkgs/R3Shapes/R3Mesh.cpp
// Function: gaps::R3Mesh::DijkstraDistance(gaps::R3MeshVertex const*, gaps::R3MeshVertex const*, gaps::RNArray<gaps::R3MeshEdge*>*) const
RNLength R3Mesh::
DijkstraDistance(const R3MeshVertex *source_vertex, const R3MeshVertex *destination_vertex, RNArray<R3MeshEdge *> *edges) const
{
  // Initialize return value
  RNLength destination_distance = RN_INFINITY;
  R3Point destination_position = VertexPosition(destination_vertex);

  // Allocate temporary data
  DijkstraData *vertex_data = new DijkstraData [ NVertices() ];
  if (!vertex_data) {
    RNFail("Unable to allocate temporary data for geodesic distances.\n");
    return RN_INFINITY;
  }

  // Initialize all data
  for (int i = 0; i < NVertices(); i++) {
    vertex_data[i].vertex = Vertex(i);
    vertex_data[i].edge = NULL;
    vertex_data[i].distance_to_destination = RN_UNKNOWN;
    vertex_data[i].heappointer = NULL;
    vertex_data[i].distance = FLT_MAX;
  }

  // Initialize priority queue
  DijkstraData tmp;
  static RNHeap<DijkstraData *> heap(&tmp, &(tmp.distance), &(tmp.heappointer));
  DijkstraData *data = &vertex_data[ VertexID(source_vertex) ];
  data->distance_to_destination = R3Distance(VertexPosition(source_vertex), destination_position);
  data->distance = data->distance_to_destination;
  heap.Push(data);

  // Visit other vertices computing shortest distance
  while (!heap.IsEmpty()) {
    DijkstraData *data = heap.Pop();
    R3MeshVertex *vertex = data->vertex;
    if (vertex == destination_vertex) { destination_distance = data->distance; break; }
    for (int i = 0; i < VertexValence(vertex); i++) {
      R3MeshEdge *edge = EdgeOnVertex(vertex, i);
      R3MeshVertex *neighbor_vertex = VertexAcrossEdge(edge, vertex);
      DijkstraData *neighbor_data = &vertex_data [ VertexID(neighbor_vertex) ];
      if (neighbor_data->distance_to_destination == RN_UNKNOWN) {
        const R3Point& neighbor_position = VertexPosition(neighbor_vertex);
        neighbor_data->distance_to_destination = R3Distance(neighbor_position, destination_position);
      }
      RNScalar old_distance = neighbor_data->distance;
      RNScalar new_distance = EdgeLength(edge) + data->distance - data->distance_to_destination + neighbor_data->distance_to_destination;
      if (new_distance < old_distance) {
        neighbor_data->edge = edge;
        neighbor_data->distance = new_distance;
        if (old_distance < FLT_MAX) heap.Update(neighbor_data);
        else heap.Push(neighbor_data);
      }
    }
  }

  // Fill in path of edges
  if (edges) {
    const R3MeshVertex *vertex = destination_vertex;
    while (vertex != source_vertex) {
      DijkstraData *data = &vertex_data [ VertexID(vertex) ];
      if (!data->edge) break;
      edges->Insert(data->edge);
      vertex = VertexAcrossEdge(data->edge, vertex);
    }
  }

  // Delete temporary data
  heap.Empty();
  delete [] vertex_data;

  // Return distance
  return destination_distance;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x48, %rsp
movq %rcx, %r14
movq %rdx, %r13
movq %rsi, %r15
movq %rdi, %rbp
leaq 0xc0c45(%rip), %rax     # 0xdabb8
movsd (%rax), %xmm0
movsd %xmm0, 0x8(%rsp)
leaq 0x18(%rdx), %rsi
leaq 0x30(%rsp), %rdi
callq 0x37b5c
movslq 0x14(%rbp), %rbx
pushq $0x28
popq %rcx
movq %rbx, %rax
mulq %rcx
pushq $-0x1
popq %rdi
cmovnoq %rax, %rdi
callq 0xe030
movq %rax, %r12
xorl %eax, %eax
testl %ebx, %ebx
movl $0x0, %edx
cmovgl %ebx, %edx
movq %rbp, 0x20(%rsp)
movq 0x8(%rbp), %rcx
leaq (%rdx,%rdx,4), %rdx
movabsq $-0x4010000000000000, %rsi # imm = 0xBFF0000000000000
movabsq $0x47efffffe0000000, %rdi # imm = 0x47EFFFFFE0000000
cmpq %rax, %rdx
je 0x1a000
movq (%rcx), %r8
movq %r8, (%r12,%rax,8)
andq $0x0, 0x8(%r12,%rax,8)
movq %rsi, 0x10(%r12,%rax,8)
andq $0x0, 0x18(%r12,%rax,8)
movq %rdi, 0x20(%r12,%rax,8)
addq $0x5, %rax
addq $0x8, %rcx
jmp 0x19fd4
movq %r13, 0x10(%rsp)
movq %r14, 0x18(%rsp)
movb 0xc0c70(%rip), %al      # 0xdac80
testb %al, %al
je 0x1a1cb
movslq 0x70(%r15), %rax
imulq $0x28, %rax, %r14
leaq (%r12,%r14), %rbx
movq %r15, (%rsp)
leaq 0x18(%r15), %rdi
leaq 0x30(%rsp), %rsi
callq 0x4321a
movsd %xmm0, 0x10(%r12,%r14)
movsd %xmm0, 0x20(%r12,%r14)
leaq 0xc0bfd(%rip), %rdi     # 0xdac48
movq %rbx, %rsi
callq 0x21f7e
cmpl $0x0, 0xc0bf6(%rip)     # 0xdac50
je 0x1a13d
leaq 0xc0be1(%rip), %rdi     # 0xdac48
callq 0x22086
movq %rax, %r14
movq (%rax), %rbp
movq 0x10(%rsp), %r13
cmpq %r13, %rbp
je 0x1a148
xorl %r13d, %r13d
movslq 0x14(%rbp), %rax
cmpq %rax, %r13
jge 0x1a053
movq 0x8(%rbp), %rax
movq (%rax,%r13,8), %r15
movq 0x8(%r15), %rdi
cmpq %rbp, %rdi
jne 0x1a0a1
movq 0x10(%r15), %rdi
movslq 0x70(%rdi), %rax
imulq $0x28, %rax, %rax
leaq (%r12,%rax), %rbx
movsd 0x10(%r12,%rax), %xmm0
ucomisd 0x78f64(%rip), %xmm0    # 0x93020
jne 0x1a0d3
jp 0x1a0d3
addq $0x18, %rdi
leaq 0x30(%rsp), %rsi
callq 0x4321a
movsd %xmm0, 0x10(%rbx)
movsd 0x20(%rbx), %xmm0
movsd %xmm0, 0x28(%rsp)
movq 0x20(%rsp), %rdi
movq %r15, %rsi
callq 0x10db2
movsd 0x28(%rsp), %xmm1
addsd 0x20(%r14), %xmm0
subsd 0x10(%r14), %xmm0
addsd 0x10(%rbx), %xmm0
ucomisd %xmm0, %xmm1
jbe 0x1a135
movq %r15, 0x8(%rbx)
movsd %xmm0, 0x20(%rbx)
movsd 0x78eff(%rip), %xmm0    # 0x93018
leaq 0xc0b28(%rip), %rdi     # 0xdac48
movq %rbx, %rsi
ucomisd %xmm1, %xmm0
jbe 0x1a130
callq 0x22126
jmp 0x1a135
callq 0x21f7e
incq %r13
jmp 0x1a083
movq (%rsp), %r15
movq 0x10(%rsp), %r13
jmp 0x1a158
movsd 0x20(%r14), %xmm0
movsd %xmm0, 0x8(%rsp)
movq (%rsp), %r15
movq 0x18(%rsp), %r14
testq %r14, %r14
je 0x1a19e
cmpq %r15, %r13
je 0x1a19e
movslq 0x70(%r13), %rax
imulq $0x28, %rax, %rax
movq 0x8(%r12,%rax), %rsi
testq %rsi, %rsi
je 0x1a19e
leaq (%r12,%rax), %rbx
addq $0x8, %rbx
movq %r14, %rdi
callq 0x22bfc
movq (%rbx), %rax
movq 0x8(%rax), %rcx
cmpq %r13, %rcx
movq %rcx, %r13
jne 0x1a162
movq 0x10(%rax), %r13
jmp 0x1a162
cmpl $0x0, 0xc0aab(%rip)     # 0xdac50
jle 0x1a1ae
andl $0x0, 0xc0aa2(%rip)     # 0xdac50
movq %r12, %rdi
callq 0xe5d0
movsd 0x8(%rsp), %xmm0
addq $0x48, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
leaq 0xc0aae(%rip), %rdi     # 0xdac80
callq 0xe840
testl %eax, %eax
je 0x1a018
xorpd %xmm0, %xmm0
movupd %xmm0, 0xc0a5d(%rip)    # 0xdac48
leaq 0xc0a56(%rip), %rsi     # 0xdac48
andq $0x0, 0xc0a76(%rip)     # 0xdac70
movupd %xmm0, 0xc0a5e(%rip)    # 0xdac60
movl $0x1, 0xc0a6c(%rip)     # 0xdac78
movabsq $0x1800000020, %rax     # imm = 0x1800000020
movq %rax, 0xc0a3b(%rip)     # 0xdac58
leaq 0x7d4c(%rip), %rdi      # 0x21f70
leaq 0xc026d(%rip), %rdx     # 0xda498
callq 0xe3c0
leaq 0xc0a49(%rip), %rdi     # 0xdac80
callq 0xe2f0
jmp 0x1a018
nop