// File: /workspace/github_cpp_makefile_star2_O1/tomfunkhouser[P]gaps/pkgs/RGBD/RGBDImage.cpp
// Function: gaps::RGBDImage::PixelWorldNormal(int, int) const
R3Vector RGBDImage::
PixelWorldNormal(int ix, int iy) const
{
  // Allocate buffer of points
  const int r = 2;
  R3Point *points = new R3Point[(2*r+1) * (2*r+1)];
  int npoints = 0;
  
  // Fill buffer of points in neighborhood r of (ix, iy)
  for (int s = -r; s <= r; s++) {
    int i = ix + s;
    if ((i < 0) || (i >= width)) continue;
    for (int t = -r; t <= r; t++) {
      int j = iy + t;
      if ((j < 0) || (j >= height)) continue;
      R3Point camera_position;
      R2Point image_position(i + 0.5, j + 0.5);
      if (RGBDTransformImageToCamera(image_position, camera_position, this)) {
        points[npoints++] = camera_position;
      }
    }
  }

  // Compute normal at pixel
  if (npoints < 3) return R3zero_vector;
  R3Point centroid = R3Centroid(npoints, points);
  R3Triad triad = R3PrincipleAxes(centroid, npoints, points);
  R3Vector normal = (triad[2].Z() > 0) ? triad[2] : -triad[2];

  // Transform into world coordinates
  normal.Transform(camera_to_world);
  normal.Normalize();
  
  // Delete buffer of points
  delete [] points;

  // Return normal in world coordinates
  return normal;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x88, %rsp
movl %ecx, %r12d
movl %edx, 0x18(%rsp)
movq %rsi, %r15
movq %rdi, %r14
movl $0x258, %edi            # imm = 0x258
callq 0x11040
movq %rax, 0x8(%rsp)
xorl %ebx, %ebx
movq 0x8(%rsp), %rax
leaq (%rax,%rbx), %rdi
callq 0x4be44
addq $0x18, %rbx
cmpq $0x258, %rbx            # imm = 0x258
jne 0x1db2b
movq %r14, 0x10(%rsp)
movl $0xfffffffe, %ecx       # imm = 0xFFFFFFFE
xorl %r13d, %r13d
leaq 0x40(%rsp), %rbx
movl %ecx, %eax
addl 0x18(%rsp), %eax
movl %ecx, 0x1c(%rsp)
js 0x1dbfb
cmpl 0x28(%r15), %eax
jge 0x1dbfb
xorps %xmm0, %xmm0
cvtsi2sd %eax, %xmm0
addsd 0x9f49f(%rip), %xmm0    # 0xbd020
movsd %xmm0, 0x20(%rsp)
movl $0xfffffffe, %r14d      # imm = 0xFFFFFFFE
movl %r12d, %ebp
addl %r14d, %ebp
js 0x1dbf2
cmpl 0x2c(%r15), %ebp
jge 0x1dbf2
movq %rbx, %rdi
callq 0x4be44
xorps %xmm1, %xmm1
cvtsi2sd %ebp, %xmm1
addsd 0x9f46e(%rip), %xmm1    # 0xbd020
leaq 0x28(%rsp), %rbp
movq %rbp, %rdi
movsd 0x20(%rsp), %xmm0
callq 0x6fee6
movq %rbp, %rdi
movq %rbx, %rsi
movq %r15, %rdx
callq 0x16507
testl %eax, %eax
je 0x1dbf2
movslq %r13d, %rax
incl %r13d
leaq (%rax,%rax,2), %rax
movq 0x8(%rsp), %rcx
leaq (%rcx,%rax,8), %rdi
movq %rbx, %rsi
callq 0x4c1b4
incl %r14d
cmpl $0x3, %r14d
jne 0x1db8d
movl 0x1c(%rsp), %ecx
incl %ecx
cmpl $0x3, %ecx
jne 0x1db58
cmpl $0x3, %r13d
jge 0x1dc29
leaq 0xa1329(%rip), %rsi     # 0xbef40
movq 0x10(%rsp), %rbx
movq %rbx, %rdi
callq 0x4c8ee
jmp 0x1dcad
leaq 0x28(%rsp), %rbx
movq %rbx, %rdi
movl %r13d, %esi
movq 0x8(%rsp), %r14
movq %r14, %rdx
xorl %ecx, %ecx
callq 0x2b455
leaq 0x40(%rsp), %rdi
movq %rbx, %rsi
movl %r13d, %edx
movq %r14, %rcx
xorl %r8d, %r8d
xorl %r9d, %r9d
callq 0x2b4df
leaq 0x70(%rsp), %rsi
movsd 0x10(%rsi), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x1dc7f
movq 0x10(%rsp), %rbx
movq %rbx, %rdi
callq 0x4c8ee
jmp 0x1dc8c
movq 0x10(%rsp), %rbx
movq %rbx, %rdi
callq 0x4d574
addq $0x30, %r15
movq %rbx, %rdi
movq %r15, %rsi
callq 0x4d4ba
movq %rbx, %rdi
callq 0x4cc00
movq 0x8(%rsp), %rdi
callq 0x117b0
movq %rbx, %rax
addq $0x88, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq %rax, %rbx
movl $0x258, %esi            # imm = 0x258
movq 0x8(%rsp), %rdi
callq 0x11ba0
movq %rbx, %rdi
callq 0x11a70