// File: /workspace/github_cpp_makefile_star2_O3/tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelUtils.cpp
// Function: gaps::EstimateSupportPlane(gaps::R3SurfelPointSet*, double, double*)
R3Plane
EstimateSupportPlane(R3SurfelPointSet *pointset,
  RNLength accuracy, RNScalar *npoints)
{
  // Check point set
  if (pointset->NPoints() == 0) {
    if (npoints) *npoints = 0;
    return R3null_plane;
  }

  // Determine zres
  const R3Box& bbox = pointset->BBox();
  RNScalar zmin = bbox.ZMin();
  RNScalar zlength = bbox.ZLength();
  if (zlength == 0) return R3Plane(0, 0, 1, -(bbox.ZMin()));
  int zres = (int) (2 * zlength / accuracy) + 4;

  // Initialize votes
  RNScalar *votes = new RNScalar [zres];
  for (int i = 0; i < zres; i++) votes[i] = 0;
  
  // Cast votes
  int step = 10 * pointset->NPoints() / zres + 1;
  for (int j = 0; j < pointset->NPoints(); j += step) {
    const R3SurfelPoint *point = pointset->Point(j);
    R3Point position = point->Position();
    int iz = (int) (zres * (position.Z() - zmin) / zlength);
    if (iz >= zres) iz = zres - 1;
    for (int k = iz; k >= 0; k--) votes[k] += 0.01;
    votes[iz]++;
  }

  // Blur votes
  RNScalar *copy = new RNScalar [ zres ];
  for (int i = 0; i < zres; i++) copy[i] = votes[i];
  votes[0] = 0.75*copy[0] + 0.25*copy[1];
  votes[zres-1] = 0.75*copy[zres-1] + 0.25*copy[zres-2];
  for (int i = 1; i < zres-1; i++) {
    votes[i] = 0.5 * copy[i];
    votes[i] += 0.25 * copy[i-1];
    votes[i] += 0.25 * copy[i+1];
  }
  delete [] copy;

  // Find z with most votes
  RNScalar best_z = 0;
  RNScalar best_vote = 0;
  for (int i = 0; i < zres; i++) {
    if (votes[i] > best_vote) {
      best_vote = votes[i];
      best_z = zlength * i / zres + zmin;
    }
  }

  // Delete votes
  delete [] votes;

  // Fill in return value
  if (npoints) *npoints = step * best_vote;

  // Return ground plane
  return R3Plane(0, 0, 1, -best_z);
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x78, %rsp
movq %rdi, %r14
movl 0x10(%rsi), %ebx
testl %ebx, %ebx
je 0x22355
movq %rsi, %r12
movsd 0x30(%rsi), %xmm3
movsd 0x48(%rsi), %xmm2
subsd %xmm3, %xmm2
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm2
jne 0x22378
jp 0x22378
xorpd 0xbb061(%rip), %xmm3    # 0xdd3a0
movsd 0xbae01(%rip), %xmm2    # 0xdd148
xorpd %xmm0, %xmm0
movq %r14, %rdi
callq 0x828d8
jmp 0x22370
testq %rdx, %rdx
je 0x22361
movq $0x0, (%rdx)
leaq 0x512f38(%rip), %rsi    # 0x5352a0
movq %r14, %rdi
callq 0x828b6
movq %r14, %rax
jmp 0x2269d
movapd %xmm3, 0x30(%rsp)
movq %rdx, 0x50(%rsp)
movq %r14, 0x58(%rsp)
movsd %xmm2, 0x20(%rsp)
movapd %xmm2, %xmm1
addsd %xmm2, %xmm1
divsd %xmm0, %xmm1
cvttsd2si %xmm1, %r13d
leal 0x4(%r13), %r14d
movslq %r14d, %rax
shlq $0x3, %rax
cmpl $-0x4, %r13d
movq $-0x1, %rdi
cmovgeq %rax, %rdi
movq %rdi, 0x48(%rsp)
callq 0x13030
movq %rax, %r15
movq %r13, 0x10(%rsp)
cmpl $-0x3, %r13d
jl 0x223ec
cmpl $0x2, %r14d
movl $0x1, %edx
cmovgel %r14d, %edx
shlq $0x3, %rdx
movq %r15, %rdi
xorl %esi, %esi
callq 0x132a0
leal (%rbx,%rbx), %eax
leal (%rax,%rax,4), %eax
cltd
idivl %r14d
incl %eax
movl %eax, 0xc(%rsp)
movl %r14d, %ebp
xorps %xmm0, %xmm0
cvtsi2sd %r14d, %xmm0
movsd %xmm0, 0x18(%rsp)
testl %ebx, %ebx
jle 0x224d8
movq 0x10(%rsp), %rax
leal 0x3(%rax), %r13d
movslq 0xc(%rsp), %rbx
xorl %r14d, %r14d
movq 0x8(%r12), %rax
movq %r14, %rcx
shlq $0x4, %rcx
movq (%rax,%rcx), %rdx
movq 0x8(%rax,%rcx), %rax
xorps %xmm0, %xmm0
cvtss2sd (%rax), %xmm0
addsd 0x10(%rdx), %xmm0
xorps %xmm1, %xmm1
cvtss2sd 0x4(%rax), %xmm1
addsd 0x18(%rdx), %xmm1
xorps %xmm2, %xmm2
cvtss2sd 0x8(%rax), %xmm2
addsd 0x20(%rdx), %xmm2
leaq 0x60(%rsp), %rdi
callq 0x83dc2
movsd 0xbb66d(%rip), %xmm1    # 0xddae0
movsd 0x70(%rsp), %xmm0
subsd 0x30(%rsp), %xmm0
mulsd 0x18(%rsp), %xmm0
divsd 0x20(%rsp), %xmm0
cvttsd2si %xmm0, %eax
cmpl %eax, %ebp
cmovlel %r13d, %eax
testl %eax, %eax
js 0x224b1
movl %eax, %ecx
movsd (%r15,%rcx,8), %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, (%r15,%rcx,8)
addq $-0x1, %rcx
jb 0x2249b
cltq
movsd (%r15,%rax,8), %xmm0
addsd 0xbac87(%rip), %xmm0    # 0xdd148
movsd %xmm0, (%r15,%rax,8)
addq %rbx, %r14
movslq 0x10(%r12), %rax
cmpq %rax, %r14
jl 0x22426
movq 0x48(%rsp), %rdi
callq 0x13030
movq %rax, %r12
movq 0x10(%rsp), %r14
movslq %r14d, %rbx
cmpl $-0x3, %r14d
jl 0x2252b
cmpl $0x2, %ebp
movl $0x1, %edx
cmovgel %ebp, %edx
shlq $0x3, %rdx
movq %r12, %rdi
movq %r15, %rsi
callq 0x133f0
movsd (%r12), %xmm2
movsd 0x8(%r12), %xmm0
movsd 0x10(%r12,%rbx,8), %xmm1
mulsd 0xbb5c7(%rip), %xmm1    # 0xddaf0
jmp 0x22533
movsd 0xbb5b5(%rip), %xmm1    # 0xddae8
movsd 0xbb5b5(%rip), %xmm3    # 0xddaf0
mulsd %xmm0, %xmm3
movsd 0xbb5b1(%rip), %xmm4    # 0xddaf8
mulsd %xmm4, %xmm2
addsd %xmm3, %xmm2
mulsd 0x18(%r12,%rbx,8), %xmm4
movsd %xmm2, (%r15)
addsd %xmm1, %xmm4
movsd %xmm4, 0x18(%r15,%rbx,8)
cmpl $-0x1, %r14d
movq 0x58(%rsp), %rbx
jl 0x225c0
leal 0x3(%r14), %eax
movl %eax, %eax
decq %rax
xorl %ecx, %ecx
movsd 0xbabb4(%rip), %xmm1    # 0xdd138
movsd 0xbb564(%rip), %xmm2    # 0xddaf0
mulsd %xmm1, %xmm0
movsd (%r12,%rcx,8), %xmm3
mulsd %xmm2, %xmm3
addsd %xmm0, %xmm3
movsd 0x10(%r12,%rcx,8), %xmm0
movapd %xmm0, %xmm4
mulsd %xmm2, %xmm4
addsd %xmm3, %xmm4
movsd %xmm4, 0x8(%r15,%rcx,8)
incq %rcx
cmpq %rcx, %rax
jne 0x2258c
movq %r12, %rdi
callq 0x13680
xorpd %xmm6, %xmm6
xorpd %xmm7, %xmm7
cmpl $-0x3, %r14d
movapd 0x30(%rsp), %xmm3
movsd 0x20(%rsp), %xmm4
movsd 0x18(%rsp), %xmm5
jl 0x2263f
cmpl $0x2, %ebp
movl $0x1, %eax
cmovgel %ebp, %eax
xorpd %xmm0, %xmm0
xorl %ecx, %ecx
xorpd %xmm1, %xmm1
movsd (%r15,%rcx,8), %xmm7
xorps %xmm2, %xmm2
cvtsi2sd %ecx, %xmm2
mulsd %xmm4, %xmm2
divsd %xmm5, %xmm2
addsd %xmm3, %xmm2
movapd %xmm0, %xmm6
cmpltsd %xmm7, %xmm6
andpd %xmm6, %xmm2
andnpd %xmm1, %xmm6
orpd %xmm2, %xmm6
maxsd %xmm0, %xmm7
incq %rcx
movapd %xmm7, %xmm0
movapd %xmm6, %xmm1
cmpq %rcx, %rax
jne 0x225fd
movapd %xmm7, 0x20(%rsp)
movapd %xmm6, 0x30(%rsp)
movq %r15, %rdi
callq 0x13680
movq 0x50(%rsp), %rax
testq %rax, %rax
je 0x22674
xorps %xmm0, %xmm0
cvtsi2sdl 0xc(%rsp), %xmm0
movapd 0x20(%rsp), %xmm1
mulsd %xmm0, %xmm1
movsd %xmm1, (%rax)
movapd 0x30(%rsp), %xmm3
xorpd 0xbad1e(%rip), %xmm3    # 0xdd3a0
movsd 0xbaabe(%rip), %xmm2    # 0xdd148
xorpd %xmm0, %xmm0
xorpd %xmm1, %xmm1
movq %rbx, %rdi
callq 0x828d8
movq %rbx, %rax
addq $0x78, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq