// File: /workspace/github_cpp_makefile_star2_O2/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, 0x1c(%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 0x45d4a
addq $0x18, %rbx
cmpq $0x258, %rbx            # imm = 0x258
jne 0x1c3d9
movq %r14, 0x10(%rsp)
xorl %r13d, %r13d
pushq $-0x2
popq %rcx
leaq 0x40(%rsp), %rbx
cmpl $0x3, %ecx
je 0x1c4b1
movl %ecx, %eax
addl 0x1c(%rsp), %eax
js 0x1c4aa
cmpl 0x28(%r15), %eax
jge 0x1c4aa
movl %ecx, 0x18(%rsp)
cvtsi2sd %eax, %xmm0
addsd 0x8bbe5(%rip), %xmm0    # 0xa8018
movsd %xmm0, 0x20(%rsp)
pushq $-0x2
popq %r14
cmpl $0x3, %r14d
je 0x1c4a6
movl %r12d, %ebp
addl %r14d, %ebp
js 0x1c4a1
cmpl 0x2c(%r15), %ebp
jge 0x1c4a1
movq %rbx, %rdi
callq 0x45d4a
cvtsi2sd %ebp, %xmm1
addsd 0x8bbb3(%rip), %xmm1    # 0xa8018
leaq 0x28(%rsp), %rbp
movq %rbp, %rdi
movsd 0x20(%rsp), %xmm0
callq 0x66640
movq %rbp, %rdi
movq %rbx, %rsi
movq %r15, %rdx
callq 0x15cb3
testl %eax, %eax
je 0x1c4a1
movslq %r13d, %rax
incl %r13d
imulq $0x18, %rax, %rdi
addq 0x8(%rsp), %rdi
movq %rbx, %rsi
callq 0x46036
incl %r14d
jmp 0x1c43d
movl 0x18(%rsp), %ecx
incl %ecx
jmp 0x1c404
cmpl $0x3, %r13d
jge 0x1c4d0
leaq 0x8d9b2(%rip), %rsi     # 0xa9e70
movq 0x10(%rsp), %rbx
movq %rbx, %rdi
callq 0x4651c
jmp 0x1c554
leaq 0x28(%rsp), %rbx
movq %rbx, %rdi
movl %r13d, %esi
movq 0x8(%rsp), %r14
movq %r14, %rdx
xorl %ecx, %ecx
callq 0x28400
leaq 0x40(%rsp), %rdi
movq %rbx, %rsi
movl %r13d, %edx
movq %r14, %rcx
xorl %r8d, %r8d
xorl %r9d, %r9d
callq 0x28489
leaq 0x70(%rsp), %rsi
movsd 0x10(%rsi), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x1c526
movq 0x10(%rsp), %rbx
movq %rbx, %rdi
callq 0x4651c
jmp 0x1c533
movq 0x10(%rsp), %rbx
movq %rbx, %rdi
callq 0x46e72
addq $0x30, %r15
movq %rbx, %rdi
movq %r15, %rsi
callq 0x46db8
movq %rbx, %rdi
callq 0x4680e
movq 0x8(%rsp), %rdi
callq 0x117a0
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 0x11bc0
movq %rbx, %rdi
callq 0x11a80
nop