// File: 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:
subq $0x118, %rsp            # imm = 0x118
movq %rdi, 0x18(%rsp)
movq %rdi, %rax
movq %rax, 0x20(%rsp)
movq %rdi, 0x110(%rsp)
movq %rsi, 0x108(%rsp)
movl %edx, 0x104(%rsp)
movl %ecx, 0x100(%rsp)
movq 0x108(%rsp), %rax
movq %rax, 0x28(%rsp)
movl $0x2, 0xfc(%rsp)
movl $0x258, %edi            # imm = 0x258
callq 0x12040
movq %rax, 0x30(%rsp)
movq %rax, %rcx
addq $0x258, %rcx            # imm = 0x258
movq %rcx, 0x38(%rsp)
movq %rax, 0x40(%rsp)
movq 0x40(%rsp), %rdi
movq %rdi, 0x10(%rsp)
callq 0x8a990
jmp 0x2956e
movq 0x38(%rsp), %rcx
movq 0x10(%rsp), %rax
addq $0x18, %rax
cmpq %rcx, %rax
movq %rax, 0x40(%rsp)
jne 0x2955d
movq 0x30(%rsp), %rax
movq %rax, 0xf0(%rsp)
movl $0x0, 0xe0(%rsp)
movl $0xfffffffe, 0xdc(%rsp) # imm = 0xFFFFFFFE
cmpl $0x2, 0xdc(%rsp)
jg 0x29726
movl 0x104(%rsp), %eax
addl 0xdc(%rsp), %eax
movl %eax, 0xd8(%rsp)
cmpl $0x0, 0xd8(%rsp)
jl 0x295e7
movq 0x28(%rsp), %rcx
movl 0xd8(%rsp), %eax
cmpl 0x28(%rcx), %eax
jl 0x29614
jmp 0x29710
movq 0x30(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0xe8(%rsp)
movl %eax, 0xe4(%rsp)
movl $0x258, %esi            # imm = 0x258
callq 0x12d80
jmp 0x29832
movl $0xfffffffe, 0xd4(%rsp) # imm = 0xFFFFFFFE
cmpl $0x2, 0xd4(%rsp)
jg 0x2970e
movl 0x100(%rsp), %eax
addl 0xd4(%rsp), %eax
movl %eax, 0xd0(%rsp)
cmpl $0x0, 0xd0(%rsp)
jl 0x2965d
movq 0x28(%rsp), %rcx
movl 0xd0(%rsp), %eax
cmpl 0x2c(%rcx), %eax
jl 0x29662
jmp 0x296f8
leaq 0xb8(%rsp), %rdi
callq 0x8a990
cvtsi2sdl 0xd8(%rsp), %xmm0
movsd 0x13c9a8(%rip), %xmm1   # 0x166028
addsd %xmm1, %xmm0
cvtsi2sdl 0xd0(%rsp), %xmm1
movsd 0x13c993(%rip), %xmm2   # 0x166028
addsd %xmm2, %xmm1
leaq 0xa8(%rsp), %rdi
callq 0xcfd80
movq 0x28(%rsp), %rdx
leaq 0xa8(%rsp), %rdi
leaq 0xb8(%rsp), %rsi
callq 0x1aef0
cmpl $0x0, %eax
je 0x296f6
movq 0xf0(%rsp), %rdi
movl 0xe0(%rsp), %eax
movl %eax, %ecx
addl $0x1, %ecx
movl %ecx, 0xe0(%rsp)
cltq
imulq $0x18, %rax, %rax
addq %rax, %rdi
leaq 0xb8(%rsp), %rsi
callq 0x8b030
jmp 0x296f8
movl 0xd4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xd4(%rsp)
jmp 0x2961f
jmp 0x29710
movl 0xdc(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xdc(%rsp)
jmp 0x295a9
cmpl $0x3, 0xe0(%rsp)
jge 0x29746
movq 0x18(%rsp), %rdi
leaq 0x189854(%rip), %rsi    # 0x1b2f90
callq 0x8bb40
jmp 0x29825
movl 0xe0(%rsp), %esi
movq 0xf0(%rsp), %rdx
leaq 0x90(%rsp), %rdi
xorl %eax, %eax
movl %eax, %ecx
callq 0x43970
movl 0xe0(%rsp), %edx
movq 0xf0(%rsp), %rcx
leaq 0x48(%rsp), %rdi
leaq 0x90(%rsp), %rsi
xorl %eax, %eax
movl %eax, %r9d
movq %r9, %r8
callq 0x43a30
leaq 0x48(%rsp), %rdi
movl $0x2, %esi
callq 0x304e0
movq %rax, %rdi
callq 0x22680
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x297cd
leaq 0x48(%rsp), %rdi
movl $0x2, %esi
callq 0x304e0
movq 0x18(%rsp), %rdi
movq %rax, %rsi
callq 0x8bb40
jmp 0x297e9
leaq 0x48(%rsp), %rdi
movl $0x2, %esi
callq 0x304e0
movq 0x18(%rsp), %rdi
movq %rax, %rsi
callq 0x8cdf0
movq 0x18(%rsp), %rdi
movq 0x28(%rsp), %rsi
addq $0x30, %rsi
callq 0x8cc60
movq 0x18(%rsp), %rdi
callq 0x8c260
movq 0xf0(%rsp), %rax
movq %rax, 0x8(%rsp)
cmpq $0x0, %rax
je 0x29823
movq 0x8(%rsp), %rdi
callq 0x12920
jmp 0x29825
movq 0x20(%rsp), %rax
addq $0x118, %rsp            # imm = 0x118
retq
movq 0xe8(%rsp), %rdi
callq 0x12c20
nop