// File: /workspace/github_cpp_makefile_star2_O3/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, %r15
movq %rsi, (%rsp)
movq %rdi, %rbx
leaq 0xc9a04(%rip), %rax     # 0xe5ba8
movsd (%rax), %xmm0
movsd %xmm0, 0x8(%rsp)
leaq 0x18(%rdx), %rsi
leaq 0x30(%rsp), %rdi
callq 0x3c542
movq %rbx, 0x10(%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 0x1c232
movq 0x10(%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 0x1c20f
movq %r14, 0x18(%rsp)
movb 0xc9a33(%rip), %al      # 0xe5c70
testb %al, %al
je 0x1c42b
movq (%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 0x49404
movsd %xmm0, 0x10(%r12,%r14,8)
movsd %xmm0, 0x20(%r12,%r14,8)
leaq 0xc99c0(%rip), %rdi     # 0xe5c38
movq %rbx, %rsi
callq 0x243f2
cmpl $0x0, 0xc99b9(%rip)     # 0xe5c40
je 0x1c3a6
movq %r15, 0x20(%rsp)
leaq 0xc999f(%rip), %rdi     # 0xe5c38
callq 0x24506
movq %rax, %rbx
movq (%rax), %r14
cmpq %r15, %r14
je 0x1c39b
cmpl $0x0, 0x14(%r14)
jle 0x1c387
xorl %r13d, %r13d
movq 0x8(%r14), %rax
movq (%rax,%r13,8), %r15
movq 0x8(%r15), %rdi
cmpq %r14, %rdi
jne 0x1c2d0
movq 0x10(%r15), %rdi
movslq 0x70(%rdi), %rax
leaq (%rax,%rax,4), %rax
leaq (%r12,%rax,8), %rbp
movsd 0x10(%r12,%rax,8), %xmm0
ucomisd 0x85d5d(%rip), %xmm0    # 0xa2048
jne 0x1c302
jp 0x1c302
addq $0x18, %rdi
leaq 0x30(%rsp), %rsi
callq 0x49404
movsd %xmm0, 0x10(%rbp)
movsd 0x20(%rbp), %xmm2
testb $0x2, 0x38(%r15)
jne 0x1c330
movq 0x10(%rsp), %rdi
movq (%rdi), %rax
movq %r15, %rsi
movsd %xmm2, 0x28(%rsp)
callq *0x2b8(%rax)
movsd 0x28(%rsp), %xmm2
movsd 0x10(%rbp), %xmm0
movsd 0x28(%r15), %xmm1
addsd 0x20(%rbx), %xmm1
subsd 0x10(%rbx), %xmm1
addsd %xmm0, %xmm1
ucomisd %xmm1, %xmm2
jbe 0x1c377
movq %r15, 0x8(%rbp)
movsd %xmm1, 0x20(%rbp)
movsd 0x85ce5(%rip), %xmm0    # 0xa2040
leaq 0xc98d6(%rip), %rdi     # 0xe5c38
movq %rbp, %rsi
ucomisd %xmm2, %xmm0
jbe 0x1c372
callq 0x245ac
jmp 0x1c377
callq 0x243f2
incq %r13
movslq 0x14(%r14), %rax
cmpq %rax, %r13
jl 0x1c2bb
cmpl $0x0, 0xc98b2(%rip)     # 0xe5c40
movq 0x20(%rsp), %r15
jne 0x1c292
jmp 0x1c3a6
movsd 0x20(%rbx), %xmm0
movsd %xmm0, 0x8(%rsp)
movq 0x18(%rsp), %r14
testq %r14, %r14
sete %al
cmpq (%rsp), %r15
sete %cl
orb %al, %cl
jne 0x1c3fb
movslq 0x70(%r15), %rax
leaq (%rax,%rax,4), %rax
movq 0x8(%r12,%rax,8), %rsi
testq %rsi, %rsi
je 0x1c3fb
leaq (%r12,%rax,8), %rbx
addq $0x8, %rbx
movl 0xc(%r14), %edx
movq %r14, %rdi
callq 0x6f8a2
movq (%rbx), %rax
movq 0x8(%rax), %rcx
cmpq %r15, %rcx
movq %rcx, %r15
jne 0x1c3f5
movq 0x10(%rax), %r15
cmpq (%rsp), %r15
jne 0x1c3bc
cmpl $0x0, 0xc983e(%rip)     # 0xe5c40
jle 0x1c40e
movl $0x0, 0xc9832(%rip)     # 0xe5c40
movq %r12, %rdi
callq 0xe5b0
movsd 0x8(%rsp), %xmm0
addq $0x48, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
callq 0xe900
jmp 0x1c245
nop