// File: 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:
subq $0x128, %rsp            # imm = 0x128
movq %rdi, 0x118(%rsp)
movq %rsi, 0x110(%rsp)
movq %rdx, 0x108(%rsp)
movq %rcx, 0x100(%rsp)
movq 0x118(%rsp), %rdi
movq %rdi, 0x30(%rsp)
leaq 0x150be5(%rip), %rax    # 0x1784f0
movsd (%rax), %xmm0
movsd %xmm0, 0xf8(%rsp)
movq 0x108(%rsp), %rsi
callq 0x13660
movq %rax, %rsi
leaq 0xe0(%rsp), %rdi
callq 0x6eb90
movq 0x30(%rsp), %rdi
callq 0x135e0
cltq
movl $0x28, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
callq 0xf030
movq %rax, 0xd8(%rsp)
cmpq $0x0, 0xd8(%rsp)
jne 0x2799c
leaq 0x10b976(%rip), %rdi    # 0x1332f2
movb $0x0, %al
callq 0xd4f30
leaq 0x150b66(%rip), %rax    # 0x1784f0
movsd (%rax), %xmm0
movsd %xmm0, 0x120(%rsp)
jmp 0x27eb2
movl $0x0, 0xd4(%rsp)
movq 0x30(%rsp), %rdi
movl 0xd4(%rsp), %eax
movl %eax, 0x2c(%rsp)
callq 0x135e0
movl %eax, %ecx
movl 0x2c(%rsp), %eax
cmpl %ecx, %eax
jge 0x27a94
movq 0x30(%rsp), %rdi
movl 0xd4(%rsp), %esi
callq 0x13630
movq %rax, %rcx
movq 0xd8(%rsp), %rax
movslq 0xd4(%rsp), %rdx
imulq $0x28, %rdx, %rdx
addq %rdx, %rax
movq %rcx, (%rax)
movq 0xd8(%rsp), %rax
movslq 0xd4(%rsp), %rcx
imulq $0x28, %rcx, %rcx
addq %rcx, %rax
movq $0x0, 0x8(%rax)
movq 0xd8(%rsp), %rax
movslq 0xd4(%rsp), %rcx
imulq $0x28, %rcx, %rcx
addq %rcx, %rax
movsd 0x10b5e2(%rip), %xmm0   # 0x133018
movsd %xmm0, 0x10(%rax)
movq 0xd8(%rsp), %rax
movslq 0xd4(%rsp), %rcx
imulq $0x28, %rcx, %rcx
addq %rcx, %rax
movq $0x0, 0x18(%rax)
movq 0xd8(%rsp), %rax
movslq 0xd4(%rsp), %rcx
imulq $0x28, %rcx, %rcx
addq %rcx, %rax
movsd 0x10b5a7(%rip), %xmm0   # 0x133020
movsd %xmm0, 0x20(%rax)
movl 0xd4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xd4(%rsp)
jmp 0x279a7
cmpb $0x0, 0x150b25(%rip)    # 0x1785c0
jne 0x27b00
leaq 0x150b1c(%rip), %rdi    # 0x1785c0
callq 0xf8e0
cmpl $0x0, %eax
je 0x27b00
leaq 0xc8(%rsp), %rdx
leaq 0xc0(%rsp), %rcx
leaq 0x150ac3(%rip), %rdi    # 0x178588
leaq 0xa8(%rsp), %rsi
movl $0x1, %r8d
callq 0x396f0
jmp 0x27ada
leaq 0x11caf(%rip), %rdi     # 0x39790
leaq 0x150aa0(%rip), %rsi    # 0x178588
leaq 0x147a09(%rip), %rdx    # 0x16f4f8
callq 0xf420
leaq 0x150ac5(%rip), %rdi    # 0x1785c0
callq 0xf310
movq 0x30(%rsp), %rdi
movq 0xd8(%rsp), %rax
movq %rax, 0x20(%rsp)
movq 0x110(%rsp), %rsi
callq 0x36910
movq 0x30(%rsp), %rdi
movl %eax, %ecx
movq 0x20(%rsp), %rax
movslq %ecx, %rcx
imulq $0x28, %rcx, %rcx
addq %rcx, %rax
movq %rax, 0x90(%rsp)
movq 0x110(%rsp), %rsi
callq 0x13660
movq %rax, %rdi
leaq 0xe0(%rsp), %rsi
callq 0x86c40
movq 0x90(%rsp), %rax
movsd %xmm0, 0x10(%rax)
movq 0x90(%rsp), %rax
movsd 0x10(%rax), %xmm0
movq 0x90(%rsp), %rax
movsd %xmm0, 0x20(%rax)
movq 0x90(%rsp), %rsi
leaq 0x1509f8(%rip), %rdi    # 0x178588
callq 0x397d0
leaq 0x1509ec(%rip), %rdi    # 0x178588
callq 0x399f0
cmpl $0x0, %eax
setne %al
xorb $-0x1, %al
testb $0x1, %al
jne 0x27bb2
jmp 0x27dd4
leaq 0x1509cf(%rip), %rdi    # 0x178588
callq 0x39a10
movq %rax, 0x88(%rsp)
movq 0x88(%rsp), %rax
movq (%rax), %rax
movq %rax, 0x80(%rsp)
movq 0x80(%rsp), %rax
cmpq 0x108(%rsp), %rax
jne 0x27c2b
movq 0x88(%rsp), %rax
movsd 0x20(%rax), %xmm0
movsd %xmm0, 0xf8(%rsp)
jmp 0x27dd4
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0xa0(%rsp)
movl %eax, 0x9c(%rsp)
leaq 0x15099f(%rip), %rdi    # 0x1785c0
callq 0xf300
jmp 0x27ec3
movl $0x0, 0x7c(%rsp)
movq 0x30(%rsp), %rdi
movl 0x7c(%rsp), %eax
movl %eax, 0x1c(%rsp)
movq 0x80(%rsp), %rsi
callq 0x36b50
movl %eax, %ecx
movl 0x1c(%rsp), %eax
cmpl %ecx, %eax
jge 0x27dcf
movq 0x30(%rsp), %rdi
movq 0x80(%rsp), %rsi
movl 0x7c(%rsp), %edx
callq 0x36ec0
movq 0x30(%rsp), %rdi
movq %rax, 0x70(%rsp)
movq 0x70(%rsp), %rsi
movq 0x80(%rsp), %rdx
callq 0x36f50
movq 0x30(%rsp), %rdi
movq %rax, 0x68(%rsp)
movq 0xd8(%rsp), %rax
movq %rax, 0x10(%rsp)
movq 0x68(%rsp), %rsi
callq 0x36910
movl %eax, %ecx
movq 0x10(%rsp), %rax
movslq %ecx, %rcx
imulq $0x28, %rcx, %rcx
addq %rcx, %rax
movq %rax, 0x60(%rsp)
movq 0x60(%rsp), %rax
movsd 0x10(%rax), %xmm0
movsd 0x10b342(%rip), %xmm1   # 0x133018
ucomisd %xmm1, %xmm0
jne 0x27d0e
jp 0x27d0e
movq 0x30(%rsp), %rdi
movq 0x68(%rsp), %rsi
callq 0x13660
movq %rax, 0x58(%rsp)
movq 0x58(%rsp), %rdi
leaq 0xe0(%rsp), %rsi
callq 0x86c40
movq 0x60(%rsp), %rax
movsd %xmm0, 0x10(%rax)
movq 0x30(%rsp), %rdi
movq 0x60(%rsp), %rax
movsd 0x20(%rax), %xmm0
movsd %xmm0, 0x50(%rsp)
movq 0x70(%rsp), %rsi
callq 0x13cc0
movq 0x88(%rsp), %rax
addsd 0x20(%rax), %xmm0
movq 0x88(%rsp), %rax
subsd 0x10(%rax), %xmm0
movq 0x60(%rsp), %rax
addsd 0x10(%rax), %xmm0
movsd %xmm0, 0x48(%rsp)
movsd 0x48(%rsp), %xmm1
movsd 0x50(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x27dbd
movq 0x70(%rsp), %rcx
movq 0x60(%rsp), %rax
movq %rcx, 0x8(%rax)
movsd 0x48(%rsp), %xmm0
movq 0x60(%rsp), %rax
movsd %xmm0, 0x20(%rax)
movsd 0x10b291(%rip), %xmm0   # 0x133020
ucomisd 0x50(%rsp), %xmm0
jbe 0x27daa
movq 0x60(%rsp), %rsi
leaq 0x1507e5(%rip), %rdi    # 0x178588
callq 0x39b20
jmp 0x27dbb
movq 0x60(%rsp), %rsi
leaq 0x1507d2(%rip), %rdi    # 0x178588
callq 0x397d0
jmp 0x27dbd
jmp 0x27dbf
movl 0x7c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x7c(%rsp)
jmp 0x27c33
jmp 0x27b95
cmpq $0x0, 0x100(%rsp)
je 0x27e79
movq 0x108(%rsp), %rax
movq %rax, 0x40(%rsp)
movq 0x40(%rsp), %rax
cmpq 0x110(%rsp), %rax
je 0x27e77
movq 0x30(%rsp), %rdi
movq 0xd8(%rsp), %rax
movq %rax, 0x8(%rsp)
movq 0x40(%rsp), %rsi
callq 0x36910
movl %eax, %ecx
movq 0x8(%rsp), %rax
movslq %ecx, %rcx
imulq $0x28, %rcx, %rcx
addq %rcx, %rax
movq %rax, 0x38(%rsp)
movq 0x38(%rsp), %rax
cmpq $0x0, 0x8(%rax)
jne 0x27e3f
jmp 0x27e77
movq 0x100(%rsp), %rdi
movq 0x38(%rsp), %rax
movq 0x8(%rax), %rsi
callq 0x376d0
movq 0x30(%rsp), %rdi
movq 0x38(%rsp), %rax
movq 0x8(%rax), %rsi
movq 0x40(%rsp), %rdx
callq 0x36f50
movq %rax, 0x40(%rsp)
jmp 0x27df0
jmp 0x27e79
leaq 0x150708(%rip), %rdi    # 0x178588
callq 0x39c20
movq 0xd8(%rsp), %rax
movq %rax, (%rsp)
cmpq $0x0, %rax
je 0x27ea0
movq (%rsp), %rdi
callq 0xf670
movsd 0xf8(%rsp), %xmm0
movsd %xmm0, 0x120(%rsp)
movsd 0x120(%rsp), %xmm0
addq $0x128, %rsp            # imm = 0x128
retq
movq 0xa0(%rsp), %rdi
callq 0xf890