// File: /workspace/github_cpp_makefile_star2_O1/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, 0x8(%rsp)
movq %rdx, %r15
movq %rsi, 0x10(%rsp)
movq %rdi, %rbx
leaq 0xc9a49(%rip), %rax     # 0xe5ba8
movsd (%rax), %xmm0
movsd %xmm0, (%rsp)
leaq 0x18(%rdx), %rsi
leaq 0x30(%rsp), %rdi
callq 0x3ca36
movq %rbx, 0x18(%rsp)
movslq 0x14(%rbx), %rbx
movl $0x28, %ecx
movq %rbx, %rax
mulq %rcx
movq $-0x1, %rdi
cmovnoq %rax, %rdi
callq 0xe030
movq %rax, %r12
testq %rbx, %rbx
jle 0x1c1ec
movq 0x18(%rsp), %rax
movq 0x8(%rax), %rax
leaq 0x20(%r12), %rcx
xorl %edx, %edx
movabsq $-0x4010000000000000, %rsi # imm = 0xBFF0000000000000
movabsq $0x47efffffe0000000, %rdi # imm = 0x47EFFFFFE0000000
xorl %r8d, %r8d
movq (%rax,%r8,8), %r9
movq %r9, -0x20(%rcx)
movq %rdx, -0x18(%rcx)
movq %rsi, -0x10(%rcx)
movq %rdx, -0x8(%rcx)
movq %rdi, (%rcx)
incq %r8
addq $0x28, %rcx
cmpq %r8, %rbx
jne 0x1c1c9
movb 0xc9a7e(%rip), %al      # 0xe5c70
testb %al, %al
je 0x1c3d6
movq 0x10(%rsp), %rcx
movslq 0x70(%rcx), %rax
leaq (%rax,%rax,4), %r14
leaq (%r12,%r14,8), %rbx
leaq 0x18(%rcx), %rdi
leaq 0x30(%rsp), %rsi
callq 0x49efc
movsd %xmm0, 0x10(%r12,%r14,8)
movsd %xmm0, 0x20(%r12,%r14,8)
leaq 0xc9a0a(%rip), %rdi     # 0xe5c38
movq %rbx, %rsi
callq 0x245e4
movq %r15, 0x20(%rsp)
cmpl $0x0, 0xc99fe(%rip)     # 0xe5c40
je 0x1c356
leaq 0xc99e9(%rip), %rdi     # 0xe5c38
callq 0x246e0
movq %rax, %rbx
movq (%rax), %rbp
cmpq %r15, %rbp
je 0x1c343
cmpl $0x0, 0x14(%rbp)
jle 0x1c34d
xorl %r13d, %r13d
movq 0x8(%rbp), %rax
movq (%rax,%r13,8), %r15
movq 0x8(%r15), %rdi
cmpq %rbp, %rdi
jne 0x1c285
movq 0x10(%r15), %rdi
movslq 0x70(%rdi), %rax
leaq (%rax,%rax,4), %rax
leaq (%r12,%rax,8), %r14
movsd 0x10(%r12,%rax,8), %xmm0
ucomisd 0x85da8(%rip), %xmm0    # 0xa2048
jne 0x1c2b8
jp 0x1c2b8
addq $0x18, %rdi
leaq 0x30(%rsp), %rsi
callq 0x49efc
movsd %xmm0, 0x10(%r14)
movsd 0x20(%r14), %xmm1
testb $0x2, 0x38(%r15)
jne 0x1c2e2
movq 0x18(%rsp), %rdi
movq (%rdi), %rax
movq %r15, %rsi
movsd %xmm1, 0x28(%rsp)
callq *0x2b8(%rax)
movsd 0x28(%rsp), %xmm1
movsd 0x28(%r15), %xmm0
addsd 0x20(%rbx), %xmm0
subsd 0x10(%rbx), %xmm0
addsd 0x10(%r14), %xmm0
ucomisd %xmm0, %xmm1
jbe 0x1c32c
movq %r15, 0x8(%r14)
movsd %xmm0, 0x20(%r14)
movsd 0x85d30(%rip), %xmm0    # 0xa2040
leaq 0xc9921(%rip), %rdi     # 0xe5c38
movq %r14, %rsi
ucomisd %xmm1, %xmm0
jbe 0x1c327
callq 0x2477a
jmp 0x1c32c
callq 0x245e4
incq %r13
movslq 0x14(%rbp), %rax
cmpq %rax, %r13
jl 0x1c270
movq 0x20(%rsp), %r15
jmp 0x1c34d
movsd 0x20(%rbx), %xmm0
movsd %xmm0, (%rsp)
cmpq %r15, %rbp
jne 0x1c23b
cmpq $0x0, 0x8(%rsp)
je 0x1c3a7
cmpq 0x10(%rsp), %r15
je 0x1c3a7
movslq 0x70(%r15), %rax
leaq (%rax,%rax,4), %rax
movq 0x8(%r12,%rax,8), %rbx
testq %rbx, %rbx
je 0x1c3a2
leaq (%r12,%rax,8), %r14
addq $0x8, %r14
movq 0x8(%rsp), %rdi
movl 0xc(%rdi), %edx
movq %rbx, %rsi
callq 0x70682
movq (%r14), %rax
movq 0x8(%rax), %rcx
cmpq %r15, %rcx
movq %rcx, %r15
jne 0x1c3a2
movq 0x10(%rax), %r15
testq %rbx, %rbx
jne 0x1c35e
cmpl $0x0, 0xc9892(%rip)     # 0xe5c40
jle 0x1c3ba
movl $0x0, 0xc9886(%rip)     # 0xe5c40
movq %r12, %rdi
callq 0xe5b0
movsd (%rsp), %xmm0
addq $0x48, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
callq 0xe900
jmp 0x1c1fa