// File: tomfunkhouser[P]gaps/pkgs/GSV/GSVScan.cpp
// Function: gaps::GSVScan::UpdatePoseOrientations()
void GSVScan::
UpdatePoseOrientations(void)
{
  // Get useful variables
  if (NScanlines() == 0) return;
  if (!survey) return;
  GSVLaser *laser = survey->Laser();
  if (!laser) return;
  GSVSegment *segment = survey->Segment();
  if (!segment) return;
  int mid_beam_index = laser->MaxBeamIndex()/2;
  if (mid_beam_index < 0) return;
  R3Vector front = segment->Front(Timestamp());

  // Read points
  if (!ReadPoints()) return;
  
  // Create list of points on middle beam index
  int npositions = 0;
  const int max_positions = 4096;
  R3Point positions[max_positions];
  for (int e = 0; e < NScanlines(); e++) {
    GSVScanline *scanline = Scanline(e);
    if (npositions >= max_positions) break;
    for (int k = 0; k < scanline->NPoints(); k++) {
      const GSVPoint& point = scanline->Point(k);
      if (point.BeamIndex() != mid_beam_index) continue;
      R3Vector v = point.Position() - scanline->Pose().Viewpoint();
      v.Normalize();
      if (npositions >= max_positions) break;
      positions[npositions++] = R3zero_point + v;
    }
  }

  // Release points
  if (!ReleasePoints()) return;
  
  // Compute PCA coordinate system of points
  if (npositions < 3) return;
  R3Point centroid = R3Centroid(npositions, positions);
  R3Triad principle_axes = R3PrincipleAxes(centroid, npositions, positions);

  // Compute scan orientation
  R3Vector axis2 = principle_axes[2];
  if (axis2.Dot(front) < 0) axis2.Flip();
  R3Vector axis0 = axis2 % R3posz_vector;
  axis0.Normalize();
  R3Vector axis1 = axis2 % axis0;
  axis1.Normalize();
  R3Triad triad(axis0, axis1, axis2);

  // Update pose orientation for every scanline
  for (int i = 0; i < NScanlines(); i++) {
    GSVScanline *scanline = Scanline(i);
    R3Point viewpoint = scanline->Pose().Viewpoint();
    GSVPose pose(viewpoint, triad);
    scanline->SetPose(pose);
  }
}

// Assembly:
subq $0x18268, %rsp          # imm = 0x18268
movq %rdi, 0x18260(%rsp)
movq 0x18260(%rsp), %rdi
movq %rdi, 0x48(%rsp)
callq 0x3a0d0
cmpl $0x0, %eax
jne 0x560db
jmp 0x565a5
movq 0x48(%rsp), %rax
cmpq $0x0, 0x18(%rax)
jne 0x560ec
jmp 0x565a5
movq 0x48(%rsp), %rax
movq 0x18(%rax), %rdi
callq 0x44bf0
movq %rax, 0x18258(%rsp)
cmpq $0x0, 0x18258(%rsp)
jne 0x56112
jmp 0x565a5
movq 0x48(%rsp), %rax
movq 0x18(%rax), %rdi
callq 0x53f50
movq %rax, 0x18250(%rsp)
cmpq $0x0, 0x18250(%rsp)
jne 0x56138
jmp 0x565a5
movq 0x18258(%rsp), %rdi
callq 0x44ae0
movl $0x2, %ecx
cltd
idivl %ecx
movl %eax, 0x1824c(%rsp)
cmpl $0x0, 0x1824c(%rsp)
jge 0x56163
jmp 0x565a5
movq 0x48(%rsp), %rdi
movq 0x18250(%rsp), %rax
movq %rax, 0x40(%rsp)
callq 0x56770
movq 0x40(%rsp), %rsi
leaq 0x18230(%rsp), %rdi
callq 0x48b60
movq 0x48(%rsp), %rdi
callq 0x557c0
cmpl $0x0, %eax
jne 0x561a0
jmp 0x565a5
movl $0x0, 0x1822c(%rsp)
movl $0x1000, 0x18228(%rsp) # imm = 0x1000
leaq 0x220(%rsp), %rax
movq %rax, %rcx
addq $0x18000, %rcx          # imm = 0x18000
movq %rcx, 0x30(%rsp)
movq %rax, 0x38(%rsp)
movq 0x38(%rsp), %rdi
movq %rdi, 0x28(%rsp)
callq 0x17fd80
movq 0x28(%rsp), %rax
movq 0x30(%rsp), %rcx
addq $0x18, %rax
cmpq %rcx, %rax
movq %rax, 0x38(%rsp)
jne 0x561d2
movl $0x0, 0x21c(%rsp)
movq 0x48(%rsp), %rdi
movl 0x21c(%rsp), %eax
movl %eax, 0x24(%rsp)
callq 0x3a0d0
movl %eax, %ecx
movl 0x24(%rsp), %eax
cmpl %ecx, %eax
jge 0x56393
movq 0x48(%rsp), %rdi
movl 0x21c(%rsp), %esi
callq 0x3a0f0
movq %rax, 0x210(%rsp)
cmpl $0x1000, 0x1822c(%rsp) # imm = 0x1000
jl 0x56252
jmp 0x56393
movl $0x0, 0x20c(%rsp)
movl 0x20c(%rsp), %eax
movl %eax, 0x20(%rsp)
movq 0x210(%rsp), %rdi
callq 0x3a120
movl %eax, %ecx
movl 0x20(%rsp), %eax
cmpl %ecx, %eax
jge 0x5637b
movq 0x210(%rsp), %rdi
movl 0x20c(%rsp), %esi
callq 0x3a130
movq %rax, 0x200(%rsp)
movq 0x200(%rsp), %rdi
callq 0x566d0
cmpl 0x1824c(%rsp), %eax
je 0x562ba
jmp 0x56365
movq 0x200(%rsp), %rsi
leaq 0x1d0(%rsp), %rdi
callq 0x3a200
movq 0x210(%rsp), %rdi
callq 0x44c60
movq %rax, %rdi
callq 0x3a330
movq %rax, %rdx
leaq 0x1e8(%rsp), %rdi
leaq 0x1d0(%rsp), %rsi
callq 0x1800f0
leaq 0x1e8(%rsp), %rdi
callq 0x181650
cmpl $0x1000, 0x1822c(%rsp) # imm = 0x1000
jl 0x56318
jmp 0x5637b
leaq 0x1b8(%rsp), %rdi
leaq 0x6acf79(%rip), %rsi    # 0x7032a0
leaq 0x1e8(%rsp), %rdx
callq 0x180350
movl 0x1822c(%rsp), %eax
movl %eax, %ecx
addl $0x1, %ecx
movl %ecx, 0x1822c(%rsp)
cltq
leaq 0x220(%rsp), %rdi
imulq $0x18, %rax, %rax
addq %rax, %rdi
leaq 0x1b8(%rsp), %rsi
callq 0x180420
movl 0x20c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x20c(%rsp)
jmp 0x5625d
jmp 0x5637d
movl 0x21c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x21c(%rsp)
jmp 0x56204
movq 0x48(%rsp), %rdi
callq 0x55960
cmpl $0x0, %eax
jne 0x563a7
jmp 0x565a5
cmpl $0x3, 0x1822c(%rsp)
jge 0x563b6
jmp 0x565a5
movl 0x1822c(%rsp), %esi
leaq 0x220(%rsp), %rdx
leaq 0x1a0(%rsp), %rdi
xorl %eax, %eax
movl %eax, %ecx
callq 0x1172c0
movl 0x1822c(%rsp), %edx
leaq 0x220(%rsp), %rcx
leaq 0x158(%rsp), %rdi
leaq 0x1a0(%rsp), %rsi
xorl %eax, %eax
movl %eax, %r9d
movq %r9, %r8
callq 0x117380
leaq 0x158(%rsp), %rdi
movl $0x2, %esi
callq 0x4ad30
movq %rax, %rsi
leaq 0x140(%rsp), %rdi
callq 0x180f30
leaq 0x140(%rsp), %rdi
leaq 0x18230(%rsp), %rsi
callq 0x181500
movaps %xmm0, %xmm1
xorps %xmm0, %xmm0
ucomisd %xmm1, %xmm0
jbe 0x56452
leaq 0x140(%rsp), %rdi
callq 0x1816d0
leaq 0x128(%rsp), %rdi
leaq 0x140(%rsp), %rsi
leaq 0x6acf87(%rip), %rdx    # 0x7033f0
callq 0x181d30
leaq 0x128(%rsp), %rdi
callq 0x181650
leaq 0x110(%rsp), %rdi
leaq 0x140(%rsp), %rsi
leaq 0x128(%rsp), %rdx
callq 0x181d30
leaq 0x110(%rsp), %rdi
callq 0x181650
leaq 0xc8(%rsp), %rdi
leaq 0x128(%rsp), %rsi
leaq 0x110(%rsp), %rdx
leaq 0x140(%rsp), %rcx
callq 0x15d8d0
movl $0x0, 0xc4(%rsp)
movq 0x48(%rsp), %rdi
movl 0xc4(%rsp), %eax
movl %eax, 0x1c(%rsp)
callq 0x3a0d0
movl %eax, %ecx
movl 0x1c(%rsp), %eax
cmpl %ecx, %eax
jge 0x565a5
movq 0x48(%rsp), %rdi
movl 0xc4(%rsp), %esi
callq 0x3a0f0
movq %rax, 0xb8(%rsp)
movq 0xb8(%rsp), %rdi
callq 0x44c60
movq %rax, %rdi
callq 0x3a330
movq %rax, %rsi
leaq 0xa0(%rsp), %rdi
movq %rdi, 0x8(%rsp)
callq 0x17fdd0
movq 0x8(%rsp), %rsi
leaq 0x60(%rsp), %rdi
movq %rdi, 0x10(%rsp)
leaq 0xc8(%rsp), %rdx
callq 0x5e4b0
movq 0x10(%rsp), %rsi
movq 0xb8(%rsp), %rdi
callq 0x57740
jmp 0x5656b
leaq 0x60(%rsp), %rdi
callq 0x5e680
movl 0xc4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xc4(%rsp)
jmp 0x564d5
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x58(%rsp)
movl %eax, 0x54(%rsp)
leaq 0x60(%rsp), %rdi
callq 0x5e680
jmp 0x565ad
addq $0x18268, %rsp          # imm = 0x18268
retq
movq 0x58(%rsp), %rdi
callq 0x29c10
nopw (%rax,%rax)