// File: tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelPointGraph.cpp
// Function: gaps::R3SurfelPointGraph::PointNormal(int, int) const
R3Vector R3SurfelPointGraph::
PointNormal(int point_index, RNBoolean fast_and_approximate) const
{
  // Get point index
  if (point_index < 0) return R3zero_vector;
  if (NNeighbors(point_index) < 2) return R3zero_vector;
  R3SurfelPoint *point = Point(point_index);

  // Check algorithm
  R3Vector normal;
  if (fast_and_approximate) {
    // Compute normal with vector cross product
    R3Point positions[3];
    int index1 = (int) (RNRandomScalar() * NNeighbors(point_index));
    int index2 = (index1 + NNeighbors(point_index)/2) % NNeighbors(point_index);
    const R3SurfelPoint *neighbor1 = Neighbor(point_index, index1);
    const R3SurfelPoint *neighbor2 = Neighbor(point_index, index2);
    positions[0] = point->Position();
    positions[1] = neighbor1->Position();
    positions[2] = neighbor2->Position();
    R3Vector v1 = positions[1] - positions[0];
    R3Vector v2 = positions[2] - positions[0];
    normal = v1 % v2;
    normal.Normalize();
  }
  else {
    // Create neighborhood points
    int npositions = NNeighbors(point_index) + 1;
    R3Point *positions = new R3Point [npositions];
    positions[0] = point->Position();
    for (int j = 0; j < NNeighbors(point_index); j++) {
      const R3SurfelPoint *neighbor = Neighbor(point_index, j);
      positions[j+1] = neighbor->Position();
    }

    // Compute normal of neighborhood points with PCA 
    R3Point centroid = R3Centroid(npositions, positions);
    R3Triad triad = R3PrincipleAxes(centroid, npositions, positions);

    // Delete neighborhood points
    delete [] positions;

    // Return normal
    normal = triad[2];
  }

  // Flip normal so that positive in max dimension
  int dim = normal.MaxDimension();
  if (normal[dim] < 0) normal.Flip();

  // Return normal
  return normal;
}

// Assembly:
subq $0x258, %rsp            # imm = 0x258
movq %rdi, 0x68(%rsp)
movq %rdi, %rax
movq %rax, 0x70(%rsp)
movq %rdi, 0x250(%rsp)
movq %rsi, 0x248(%rsp)
movl %edx, 0x244(%rsp)
movl %ecx, 0x240(%rsp)
movq 0x248(%rsp), %rax
movq %rax, 0x78(%rsp)
cmpl $0x0, 0x244(%rsp)
jge 0x5d7cf
movq 0x68(%rsp), %rdi
leaq 0x5a09db(%rip), %rsi    # 0x5fe1a0
callq 0xf94f0
jmp 0x5dca4
movq 0x78(%rsp), %rdi
movl 0x244(%rsp), %esi
callq 0x43ef0
cmpl $0x2, %eax
jge 0x5d7fb
movq 0x68(%rsp), %rdi
leaq 0x5a09af(%rip), %rsi    # 0x5fe1a0
callq 0xf94f0
jmp 0x5dca4
movq 0x78(%rsp), %rdi
movl 0x244(%rsp), %esi
callq 0x43e40
movq 0x68(%rsp), %rdi
movq %rax, 0x238(%rsp)
callq 0xf94a0
cmpl $0x0, 0x240(%rsp)
je 0x5da38
leaq 0x1f0(%rsp), %rax
movq %rax, %rcx
addq $0x48, %rcx
movq %rcx, 0x58(%rsp)
movq %rax, 0x60(%rsp)
movq 0x60(%rsp), %rdi
movq %rdi, 0x50(%rsp)
callq 0xf8340
movq 0x50(%rsp), %rax
movq 0x58(%rsp), %rcx
addq $0x18, %rax
cmpq %rcx, %rax
movq %rax, 0x60(%rsp)
jne 0x5d845
callq 0x144f30
movq 0x78(%rsp), %rdi
movsd %xmm0, 0x40(%rsp)
movl 0x244(%rsp), %esi
callq 0x43ef0
movsd 0x40(%rsp), %xmm0
movq 0x78(%rsp), %rdi
cvtsi2sd %eax, %xmm1
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x1ec(%rsp)
movl 0x1ec(%rsp), %eax
movl %eax, 0x48(%rsp)
movl 0x244(%rsp), %esi
callq 0x43ef0
movq 0x78(%rsp), %rdi
movl $0x2, %ecx
cltd
idivl %ecx
movl %eax, %ecx
movl 0x48(%rsp), %eax
addl %ecx, %eax
movl %eax, 0x4c(%rsp)
movl 0x244(%rsp), %esi
callq 0x43ef0
movq 0x78(%rsp), %rdi
movl %eax, %ecx
movl 0x4c(%rsp), %eax
cltd
idivl %ecx
movl %edx, 0x1e8(%rsp)
movl 0x244(%rsp), %esi
movl 0x1ec(%rsp), %edx
callq 0x43f20
movq 0x78(%rsp), %rdi
movq %rax, 0x1e0(%rsp)
movl 0x244(%rsp), %esi
movl 0x1e8(%rsp), %edx
callq 0x43f20
movq %rax, 0x1d8(%rsp)
movq 0x238(%rsp), %rsi
leaq 0x1c0(%rsp), %rdi
callq 0x18730
leaq 0x1f0(%rsp), %rdi
leaq 0x1c0(%rsp), %rsi
callq 0xf89e0
movq 0x1e0(%rsp), %rsi
leaq 0x1a8(%rsp), %rdi
callq 0x18730
leaq 0x1f0(%rsp), %rdi
addq $0x18, %rdi
leaq 0x1a8(%rsp), %rsi
callq 0xf89e0
movq 0x1d8(%rsp), %rsi
leaq 0x190(%rsp), %rdi
callq 0x18730
leaq 0x1f0(%rsp), %rdi
addq $0x30, %rdi
leaq 0x190(%rsp), %rsi
callq 0xf89e0
leaq 0x1f0(%rsp), %rsi
addq $0x18, %rsi
leaq 0x1f0(%rsp), %rdx
leaq 0x178(%rsp), %rdi
callq 0xf86b0
leaq 0x1f0(%rsp), %rsi
addq $0x30, %rsi
leaq 0x1f0(%rsp), %rdx
leaq 0x160(%rsp), %rdi
callq 0xf86b0
leaq 0x148(%rsp), %rdi
leaq 0x178(%rsp), %rsi
leaq 0x160(%rsp), %rdx
callq 0xfa2f0
movq 0x68(%rsp), %rdi
leaq 0x148(%rsp), %rsi
callq 0xfa170
movq 0x68(%rsp), %rdi
callq 0xf9c10
jmp 0x5dc6d
movq 0x78(%rsp), %rdi
movl 0x244(%rsp), %esi
callq 0x43ef0
addl $0x1, %eax
movl %eax, 0x144(%rsp)
movslq 0x144(%rsp), %rax
movq %rax, 0x30(%rsp)
movl $0x18, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
movq %rdi, 0x28(%rsp)
callq 0x13030
movq %rax, %rcx
movq 0x30(%rsp), %rax
movq %rcx, 0x38(%rsp)
cmpq $0x0, %rax
je 0x5dae0
movq 0x38(%rsp), %rax
movq 0x30(%rsp), %rcx
imulq $0x18, %rcx, %rdx
movq %rax, %rcx
addq %rdx, %rcx
movq %rcx, 0x18(%rsp)
movq %rax, 0x20(%rsp)
movq 0x20(%rsp), %rdi
movq %rdi, 0x10(%rsp)
callq 0xf8340
jmp 0x5dac8
movq 0x18(%rsp), %rcx
movq 0x10(%rsp), %rax
addq $0x18, %rax
cmpq %rcx, %rax
movq %rax, 0x20(%rsp)
jne 0x5dab7
movq 0x38(%rsp), %rax
movq %rax, 0x138(%rsp)
movq 0x238(%rsp), %rsi
leaq 0x110(%rsp), %rdi
callq 0x18730
movq 0x138(%rsp), %rdi
leaq 0x110(%rsp), %rsi
callq 0xf89e0
movl $0x0, 0x10c(%rsp)
movq 0x78(%rsp), %rdi
movl 0x10c(%rsp), %eax
movl %eax, 0xc(%rsp)
movl 0x244(%rsp), %esi
callq 0x43ef0
movl %eax, %ecx
movl 0xc(%rsp), %eax
cmpl %ecx, %eax
jge 0x5dbe7
movq 0x78(%rsp), %rdi
movl 0x244(%rsp), %esi
movl 0x10c(%rsp), %edx
callq 0x43f20
movq %rax, 0x100(%rsp)
movq 0x100(%rsp), %rsi
leaq 0xe8(%rsp), %rdi
callq 0x18730
movq 0x138(%rsp), %rdi
movl 0x10c(%rsp), %eax
addl $0x1, %eax
cltq
imulq $0x18, %rax, %rax
addq %rax, %rdi
leaq 0xe8(%rsp), %rsi
callq 0xf89e0
movl 0x10c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x10c(%rsp)
jmp 0x5db22
movq 0x28(%rsp), %rsi
movq 0x38(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x130(%rsp)
movl %eax, 0x12c(%rsp)
callq 0x13a90
jmp 0x5dcb1
movl 0x144(%rsp), %esi
movq 0x138(%rsp), %rdx
leaq 0xd0(%rsp), %rdi
xorl %eax, %eax
movl %eax, %ecx
callq 0x97b20
movl 0x144(%rsp), %edx
movq 0x138(%rsp), %rcx
leaq 0x88(%rsp), %rdi
leaq 0xd0(%rsp), %rsi
xorl %eax, %eax
movl %eax, %r9d
movq %r9, %r8
callq 0x97be0
movq 0x138(%rsp), %rax
movq %rax, (%rsp)
cmpq $0x0, %rax
je 0x5dc4e
movq (%rsp), %rdi
callq 0x13750
leaq 0x88(%rsp), %rdi
movl $0x2, %esi
callq 0x44220
movq 0x68(%rsp), %rdi
movq %rax, %rsi
callq 0xfa170
movq 0x68(%rsp), %rdi
callq 0xf97d0
movq 0x68(%rsp), %rdi
movl %eax, 0x84(%rsp)
movl 0x84(%rsp), %esi
callq 0x31d80
xorps %xmm0, %xmm0
ucomisd (%rax), %xmm0
jbe 0x5dca2
movq 0x68(%rsp), %rdi
callq 0xf9c90
jmp 0x5dca4
movq 0x70(%rsp), %rax
addq $0x258, %rsp            # imm = 0x258
retq
movq 0x130(%rsp), %rdi
callq 0x13970
nop