// File: /workspace/github_cpp_makefile_star2_O1/tomfunkhouser[P]gaps/pkgs/R3Utils/R3Segmentation.cpp
// Function: gaps::R3SegmentationPrimitive::Update(gaps::R3SegmentationPoint*, gaps::RNArray<gaps::R3SegmentationPoint*> const*)
void R3SegmentationPrimitive::
Update(R3SegmentationPoint *seed_point, const RNArray<R3SegmentationPoint *> *points)
{
  // Remember stuff about primitive (so can set same orientation)
  R3Vector previous_vector(0,0,0);
  if (primitive_type == R3_SEGMENTATION_LINE_PRIMITIVE_TYPE) previous_vector = line.Vector();
  if (primitive_type == R3_SEGMENTATION_PLANE_PRIMITIVE_TYPE) previous_vector = plane.Normal();

  // Update bounding box
  bbox = R3null_box;
  if (points) {
    for (int i = 0; i < points->NEntries(); i++) {
      R3SegmentationPoint *point = points->Kth(i);
      bbox.Union(point->position);
    }
  }

  // Initialize everything
  if (seed_point) {
    R3Point seed_position = seed_point->position;
    centroid = seed_position;
    line.Reset(seed_position, line.Vector()); 
    plane.Reset(seed_position, seed_point->normal);
    bbox.Union(seed_position);
  }
  else {
    // Temporary
    RNAbort("Need seed point");
  }

  // Update based on points
  if (points && (points->NEntries() > 0)) {
    // Allocate arrays of point positions and weights
    const int max_positions = 1024;
    R3Point *positions = new R3Point [ max_positions ];
    RNScalar *weights = new RNScalar [ max_positions ];

    // Fill arrays of point positions and weights
    int npositions = 0;
    int skip = points->NEntries() / max_positions + 1;
    for (int i = 0; i < points->NEntries(); i += skip) {
      R3SegmentationPoint *point = points->Kth(i);
      if (npositions >= max_positions-1) break;
      positions[npositions] = point->position;
      if (primitive_type == R3_SEGMENTATION_PLANE_PRIMITIVE_TYPE)
        weights[npositions] = fabs(plane.Normal().Dot(point->normal));
      else if (primitive_type == R3_SEGMENTATION_LINE_PRIMITIVE_TYPE)
        weights[npositions] = 1.0 - fabs(plane.Normal().Dot(point->normal));
      else weights[npositions] = 1.0;
      npositions++;
    }

    // Add seed point with 20% of the total weight
    if (seed_point) {
      positions[npositions] = seed_point->position;
      weights[npositions] = 0.2 * points->NEntries();
      npositions++;
    }

    // Compute centroid
    centroid = R3Centroid(npositions, positions, weights);

    // Update primitive parameters
    if ((primitive_type == R3_SEGMENTATION_NULL_PRIMITIVE_TYPE) && (npositions >= 2)) {
      RNScalar variances[3];
      R3Triad axes = R3PrincipleAxes(centroid, npositions, positions, weights, variances);
      if (variances[0] > RN_EPSILON) {
        if (variances[1] > RN_EPSILON) {
          RNScalar ratio10 = variances[1] / variances[0];
          RNScalar ratio21 = variances[2] / variances[1];
          if (ratio10 < ratio21) {
            primitive_type = R3_SEGMENTATION_LINE_PRIMITIVE_TYPE;
            line.Reset(centroid, axes[0]);
          }
          else {
            primitive_type = R3_SEGMENTATION_PLANE_PRIMITIVE_TYPE;
            plane.Reset(centroid, axes[2]);
          }
        }
      }
    }
    else if ((primitive_type == R3_SEGMENTATION_LINE_PRIMITIVE_TYPE) && (npositions >= 2)) {
      // Compute principle directions
      RNScalar variances[3];
      R3Triad axes = R3PrincipleAxes(centroid, npositions, positions, weights, variances);
      if (variances[0] > RN_EPSILON) {
        // Update line
        R3Vector direction = axes[0];
        line.Reset(centroid, direction);

        // Check if should flip line
        RNScalar dot = direction.Dot(previous_vector);
        if (dot < 0) line.Flip();
      }
    }
    else if ((primitive_type == R3_SEGMENTATION_PLANE_PRIMITIVE_TYPE) && (npositions >= 3)) {
      // Compute principle directions
      RNScalar variances[3];
      R3Triad axes = R3PrincipleAxes(centroid, npositions, positions, weights, variances);
      if (variances[1] > RN_EPSILON) {
        // Update plane
        R3Vector normal = axes[2];
        plane.Reset(centroid, normal);

        // Check if should flip plane
        if (seed_point) {
          RNScalar dot = normal.Dot(seed_point->normal);
          if (dot < 0) plane.Flip();
        }
        else {
          RNScalar dot = normal.Dot(previous_vector);
          if (dot < 0) plane.Flip();
        }
      }
    }

#if 0
    // Rasterize planar grid
    if (primitive_type == R3_SEGMENTATION_PLANAR_GRID_PRIMITIVE_TYPE) {
      // Rasterize points into planar grid
      R3PlanarGrid density(plane, bbox, min_cluster_spacing);
      for (int i = 0; i < points->NEntries(); i++) {
        R3SegmentationPoint *point = points->Kth(i);
        R3Point position = point->position;
        density.RasterizeWorldR3SegmentationPoint(position.X(), position.Y(), position.Z(), 1.0);
      }

      // Reset planar grid
      planar_grid.Reset(plane, bbox, min_cluster_spacing);
      R3Point seed_position = seed_point->position;
      R2R3SegmentationPoint grid_position = planar_grid.GridPosition(seed_position);
      int ix = (int) (grid_position.X() + 0.5);
      int iy = (int) (grid_position.Y() + 0.5);
      FloodCopy(density.grid, planar_grid.grid, ix, iy);
    }
#endif

    // Delete array of point positions and weights
    delete [] positions;
    delete [] weights;
  }
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0xd8, %rsp
movq %rdx, %r13
movq %rsi, %rbp
movq %rdi, %r15
leaq 0x60(%rsp), %rdi
xorpd %xmm0, %xmm0
xorpd %xmm1, %xmm1
xorpd %xmm2, %xmm2
callq 0x9ff66
cmpl $0x2, (%r15)
jne 0x29378
leaq 0x70(%r15), %rsi
leaq 0x60(%rsp), %rdi
callq 0xa0712
cmpl $0x3, (%r15)
jne 0x2938f
leaq 0x88(%r15), %rsi
leaq 0x60(%rsp), %rdi
callq 0xa0712
leaq 0x8(%r15), %r14
leaq 0x10(%r15), %rdi
leaq 0x1488da(%rip), %r12    # 0x171c78
leaq 0x8(%r12), %rsi
callq 0x9f83c
leaq 0x28(%r15), %rdi
addq $0x20, %r12
movq %r12, %rsi
callq 0x9f83c
testq %r13, %r13
je 0x293e6
cmpl $0x0, 0xc(%r13)
jle 0x293e6
xorl %ebx, %ebx
movq (%r13), %rax
movq (%rax,%rbx,8), %rsi
addq $0x8, %rsi
movq %r14, %rdi
callq 0x890f6
incq %rbx
movslq 0xc(%r13), %rax
cmpq %rax, %rbx
jl 0x293c6
testq %rbp, %rbp
je 0x29454
leaq 0x8(%rbp), %rsi
leaq 0x90(%rsp), %r12
movq %r12, %rdi
callq 0x9f4de
leaq 0x40(%r15), %rdi
movq %r12, %rsi
callq 0x9f83c
leaq 0x58(%r15), %rdi
movq %rbp, %rbx
leaq 0x70(%r15), %rbp
movq %r12, %rsi
callq 0x9f83c
movq %rbp, %rdi
movq %rbp, %rsi
callq 0xa0712
movq %rbp, %rdi
movq %rbx, %rbp
callq 0xa0288
leaq 0x88(%r15), %rdi
leaq 0x20(%rbx), %rdx
movq %r12, %rsi
callq 0x9e822
movq %r14, %rdi
movq %r12, %rsi
callq 0x890f6
jmp 0x29462
leaq 0xe2e61(%rip), %rdi     # 0x10c2bc
xorl %eax, %eax
callq 0xc78f2
testq %r13, %r13
je 0x29863
cmpl $0x0, 0xc(%r13)
jle 0x29863
movl $0x6000, %edi           # imm = 0x6000
callq 0x15040
movq %rax, %r14
xorl %ebx, %ebx
leaq (%r14,%rbx), %rdi
callq 0x9f4cc
addq $0x18, %rbx
cmpq $0x6000, %rbx           # imm = 0x6000
jne 0x29485
movq %rbp, 0x20(%rsp)
movq %r14, 0x10(%rsp)
movl $0x2000, %edi           # imm = 0x2000
callq 0x15040
movl 0xc(%r13), %edx
testl %edx, %edx
movq %rax, 0x8(%rsp)
movq %r15, 0x18(%rsp)
jle 0x295b6
shrl $0xa, %edx
leaq 0x88(%r15), %rcx
movq %rcx, 0x28(%rsp)
leaq 0x8(,%rdx,8), %rsi
incq %rdx
xorl %ebx, %ebx
xorl %r12d, %r12d
xorl %ebp, %ebp
movq %rdx, 0x38(%rsp)
movq %rsi, 0x30(%rsp)
movl %ebp, %r14d
cmpl $0x3fe, %ebp            # imm = 0x3FE
jg 0x29596
movq (%r13), %rax
movq (%rax,%rbx), %r14
leaq 0x8(%r14), %rsi
movslq %ebp, %r15
leaq (%r15,%r15,2), %rax
movq 0x10(%rsp), %rcx
leaq (%rcx,%rax,8), %rdi
callq 0x9f83c
movq 0x18(%rsp), %rax
movl (%rax), %eax
cmpl $0x2, %eax
je 0x29554
movsd 0xe1b1c(%rip), %xmm0    # 0x10b050
cmpl $0x3, %eax
jne 0x2957d
addq $0x20, %r14
movq 0x28(%rsp), %rdi
movq %r14, %rsi
callq 0xa01fe
andpd 0xe201e(%rip), %xmm0    # 0x10b570
jmp 0x2957d
addq $0x20, %r14
movq 0x28(%rsp), %rdi
movq %r14, %rsi
callq 0xa01fe
movapd %xmm0, %xmm1
andpd 0xe1fff(%rip), %xmm1    # 0x10b570
movsd 0xe1ad7(%rip), %xmm0    # 0x10b050
subsd %xmm1, %xmm0
movq 0x8(%rsp), %rax
movsd %xmm0, (%rax,%r15,8)
leal 0x1(%rbp), %r14d
movq 0x38(%rsp), %rdx
movq 0x30(%rsp), %rsi
cmpl $0x3ff, %ebp            # imm = 0x3FF
jge 0x295b9
addq %rdx, %r12
movslq 0xc(%r13), %rcx
addq %rsi, %rbx
movl %r14d, %ebp
cmpq %rcx, %r12
jl 0x294f0
jmp 0x295b9
xorl %r14d, %r14d
movq 0x20(%rsp), %rcx
testq %rcx, %rcx
je 0x29602
leaq 0x8(%rcx), %rsi
movslq %r14d, %r14
leaq (%r14,%r14,2), %rax
movq 0x10(%rsp), %r15
leaq (%r15,%rax,8), %rdi
callq 0x9f83c
movq 0x8(%rsp), %rax
xorps %xmm0, %xmm0
cvtsi2sdl 0xc(%r13), %xmm0
mulsd 0xe2a7e(%rip), %xmm0    # 0x10c070
movsd %xmm0, (%rax,%r14,8)
incl %r14d
movq 0x18(%rsp), %rbx
jmp 0x2960c
movq 0x18(%rsp), %rbx
movq 0x10(%rsp), %r15
leaq 0x90(%rsp), %r12
movq %r12, %rdi
movl %r14d, %esi
movq %r15, %rdx
movq %rax, %rbp
movq %rax, %rcx
callq 0x6ed79
leaq 0x40(%rbx), %r13
movq %r13, %rdi
movq %r12, %rsi
callq 0x9f83c
movl (%rbx), %eax
testl %eax, %eax
setne %cl
cmpl $0x2, %r14d
setl %dl
orb %cl, %dl
jne 0x296f2
leaq 0x90(%rsp), %rdi
movq %r15, %rcx
leaq 0x40(%rsp), %r15
movq %r13, %rsi
movl %r14d, %edx
movq %rcx, %r14
movq %rbp, %r8
movq %r15, %r9
callq 0x6ee03
movsd (%r15), %xmm0
leaq 0x1476db(%rip), %rax    # 0x170d58
movsd (%rax), %xmm2
ucomisd %xmm2, %xmm0
jbe 0x2984e
movsd 0x48(%rsp), %xmm1
ucomisd %xmm2, %xmm1
jbe 0x2984e
movapd %xmm1, %xmm2
movhpd 0x50(%rsp), %xmm2       # xmm2 = xmm2[0],mem[0]
unpcklpd %xmm1, %xmm0            # xmm0 = xmm0[0],xmm1[0]
divpd %xmm0, %xmm2
movapd %xmm2, %xmm0
unpckhpd %xmm2, %xmm0            # xmm0 = xmm0[1],xmm2[1]
ucomisd %xmm2, %xmm0
jbe 0x2982e
movl $0x2, (%rbx)
leaq 0x58(%rbx), %rdi
movq %r13, %rsi
callq 0x9f83c
addq $0x70, %rbx
leaq 0x90(%rsp), %rsi
movq %rbx, %rdi
callq 0xa0712
movq %rbx, %rdi
callq 0xa0288
jmp 0x2984e
cmpl $0x2, %r14d
setl %cl
cmpl $0x2, %eax
setne %dl
orb %cl, %dl
jne 0x297a3
leaq 0x90(%rsp), %rdi
movq %r15, %rcx
leaq 0x40(%rsp), %r15
movq %r13, %rsi
movl %r14d, %edx
movq %rcx, %r14
movq 0x8(%rsp), %r8
movq %r15, %r9
callq 0x6ee03
movsd (%r15), %xmm0
leaq 0x14761f(%rip), %rax    # 0x170d58
ucomisd (%rax), %xmm0
jbe 0x2984e
leaq 0x78(%rsp), %r15
leaq 0x90(%rsp), %rsi
movq %r15, %rdi
callq 0x9ff76
leaq 0x58(%rbx), %rdi
movq %r13, %rsi
callq 0x9f83c
addq $0x70, %rbx
movq %rbx, %rdi
movq %r15, %rsi
callq 0xa0712
movq %rbx, %rdi
callq 0xa0288
leaq 0x60(%rsp), %rsi
movq %r15, %rdi
callq 0xa01fe
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
jbe 0x2984e
movq %rbx, %rdi
callq 0xa02f4
jmp 0x2984e
cmpl $0x3, %eax
setne %al
cmpl $0x3, %r14d
setl %cl
orb %al, %cl
jne 0x29851
leaq 0x90(%rsp), %rdi
leaq 0x40(%rsp), %r12
movq %r13, %rsi
movl %r14d, %edx
movq %r15, %rcx
movq 0x8(%rsp), %r8
movq %r12, %r9
callq 0x6ee03
movsd 0x8(%r12), %xmm0
leaq 0x14756f(%rip), %rax    # 0x170d58
ucomisd (%rax), %xmm0
jbe 0x29851
leaq 0xc0(%rsp), %rsi
leaq 0x78(%rsp), %r12
movq %r12, %rdi
callq 0x9ff76
addq $0x88, %rbx
movq %rbx, %rdi
movq %r13, %rsi
movq %r12, %rdx
callq 0x9e822
movq 0x20(%rsp), %rsi
testq %rsi, %rsi
je 0x29875
addq $0x20, %rsi
leaq 0x78(%rsp), %rdi
jmp 0x2987f
movl $0x3, (%rbx)
addq $0x88, %rbx
leaq 0xc0(%rsp), %rdx
movq %rbx, %rdi
movq %r13, %rsi
callq 0x9e822
movq %r14, %r15
movq %r15, %rdi
callq 0x15820
movq 0x8(%rsp), %rdi
callq 0x15820
addq $0xd8, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
leaq 0x78(%rsp), %rdi
leaq 0x60(%rsp), %rsi
callq 0xa01fe
xorpd %xmm1, %xmm1
ucomisd %xmm0, %xmm1
jbe 0x29851
movq %rbx, %rdi
callq 0x2ea5e
jmp 0x29851
movq %rax, %rbx
movl $0x6000, %esi           # imm = 0x6000
movq %r14, %rdi
callq 0x15c30
movq %rbx, %rdi
callq 0x15ae0