// File: 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:
subq $0x298, %rsp            # imm = 0x298
movq %rdi, 0x290(%rsp)
movq %rsi, 0x288(%rsp)
movq %rdx, 0x280(%rsp)
movq 0x290(%rsp), %rax
movq %rax, 0x68(%rsp)
leaq 0x268(%rsp), %rdi
xorps %xmm2, %xmm2
movaps %xmm2, %xmm0
movaps %xmm2, %xmm1
callq 0x12beb0
movq 0x68(%rsp), %rax
cmpl $0x2, (%rax)
jne 0x3c24a
movq 0x68(%rsp), %rdi
addq $0x58, %rdi
callq 0x47560
movq %rax, %rsi
leaq 0x268(%rsp), %rdi
callq 0x12cb70
movq 0x68(%rsp), %rax
cmpl $0x3, (%rax)
jne 0x3c275
movq 0x68(%rsp), %rdi
addq $0x88, %rdi
callq 0x2dca0
movq %rax, %rsi
leaq 0x268(%rsp), %rdi
callq 0x12cb70
movq 0x68(%rsp), %rdi
addq $0x8, %rdi
leaq 0x22d403(%rip), %rsi    # 0x269688
callq 0x1f7f0
cmpq $0x0, 0x280(%rsp)
je 0x3c30d
movl $0x0, 0x264(%rsp)
movl 0x264(%rsp), %eax
movl %eax, 0x64(%rsp)
movq 0x280(%rsp), %rdi
callq 0x1ecf0
movl %eax, %ecx
movl 0x64(%rsp), %eax
cmpl %ecx, %eax
jge 0x3c30b
movq 0x280(%rsp), %rdi
movl 0x264(%rsp), %esi
callq 0x1f6a0
movq 0x68(%rsp), %rdi
movq %rax, 0x258(%rsp)
addq $0x8, %rdi
movq 0x258(%rsp), %rsi
addq $0x8, %rsi
callq 0xfe520
movl 0x264(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x264(%rsp)
jmp 0x3c2a0
jmp 0x3c30d
cmpq $0x0, 0x288(%rsp)
je 0x3c3b9
movq 0x288(%rsp), %rsi
addq $0x8, %rsi
leaq 0x240(%rsp), %rdi
callq 0x12ad90
movq 0x68(%rsp), %rdi
addq $0x40, %rdi
leaq 0x240(%rsp), %rsi
callq 0x12b3e0
movq 0x68(%rsp), %rdi
movq %rdi, %rax
addq $0x58, %rax
movq %rax, 0x58(%rsp)
addq $0x58, %rdi
callq 0x47560
movq 0x58(%rsp), %rdi
movq %rax, %rdx
leaq 0x240(%rsp), %rsi
xorl %ecx, %ecx
callq 0x47570
movq 0x68(%rsp), %rdi
addq $0x88, %rdi
movq 0x288(%rsp), %rdx
addq $0x20, %rdx
leaq 0x240(%rsp), %rsi
callq 0x1295b0
movq 0x68(%rsp), %rdi
addq $0x8, %rdi
leaq 0x240(%rsp), %rsi
callq 0xfe520
jmp 0x3c3c7
leaq 0x1c1f14(%rip), %rdi    # 0x1fe2d4
movb $0x0, %al
callq 0x1778a0
cmpq $0x0, 0x280(%rsp)
je 0x3caa4
movq 0x280(%rsp), %rdi
callq 0x1ecf0
cmpl $0x0, %eax
jle 0x3caa4
movl $0x400, 0x23c(%rsp) # imm = 0x400
movl $0x6000, %edi           # imm = 0x6000
callq 0x16040
movq %rax, 0x40(%rsp)
movq %rax, %rcx
addq $0x6000, %rcx           # imm = 0x6000
movq %rcx, 0x48(%rsp)
movq %rax, 0x50(%rsp)
movq 0x50(%rsp), %rdi
movq %rdi, 0x38(%rsp)
callq 0x12ad40
jmp 0x3c42b
movq 0x48(%rsp), %rcx
movq 0x38(%rsp), %rax
addq $0x18, %rax
cmpq %rcx, %rax
movq %rax, 0x50(%rsp)
jne 0x3c41a
movq 0x40(%rsp), %rax
movq %rax, 0x230(%rsp)
movl $0x2000, %edi           # imm = 0x2000
callq 0x16040
movq %rax, 0x218(%rsp)
movl $0x0, 0x214(%rsp)
movq 0x280(%rsp), %rdi
callq 0x1ecf0
movl $0x400, %ecx            # imm = 0x400
cltd
idivl %ecx
addl $0x1, %eax
movl %eax, 0x210(%rsp)
movl $0x0, 0x20c(%rsp)
movl 0x20c(%rsp), %eax
movl %eax, 0x34(%rsp)
movq 0x280(%rsp), %rdi
callq 0x1ecf0
movl %eax, %ecx
movl 0x34(%rsp), %eax
cmpl %ecx, %eax
jge 0x3c636
movq 0x280(%rsp), %rdi
movl 0x20c(%rsp), %esi
callq 0x1f6a0
movq %rax, 0x200(%rsp)
cmpl $0x3ff, 0x214(%rsp) # imm = 0x3FF
jl 0x3c513
jmp 0x3c636
movq 0x40(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x228(%rsp)
movl %eax, 0x224(%rsp)
movl $0x6000, %esi           # imm = 0x6000
callq 0x16e20
jmp 0x3caac
movq 0x200(%rsp), %rsi
addq $0x8, %rsi
movq 0x230(%rsp), %rdi
movslq 0x214(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rdi
callq 0x12b3e0
movq 0x68(%rsp), %rax
cmpl $0x3, (%rax)
jne 0x3c58c
movq 0x68(%rsp), %rdi
addq $0x88, %rdi
callq 0x2dca0
movq %rax, %rdi
movq 0x200(%rsp), %rsi
addq $0x20, %rsi
callq 0x12c4c0
movaps 0x1c0aff(%rip), %xmm1   # 0x1fd070
pand %xmm1, %xmm0
movq 0x218(%rsp), %rax
movslq 0x214(%rsp), %rcx
movsd %xmm0, (%rax,%rcx,8)
jmp 0x3c60b
movq 0x68(%rsp), %rax
cmpl $0x2, (%rax)
jne 0x3c5ec
movq 0x68(%rsp), %rdi
addq $0x88, %rdi
callq 0x2dca0
movq %rax, %rdi
movq 0x200(%rsp), %rsi
addq $0x20, %rsi
callq 0x12c4c0
movaps %xmm0, %xmm1
movaps 0x1c0aab(%rip), %xmm0   # 0x1fd070
pand %xmm0, %xmm1
movsd 0x1c0a4f(%rip), %xmm0   # 0x1fd020
subsd %xmm1, %xmm0
movq 0x218(%rsp), %rax
movslq 0x214(%rsp), %rcx
movsd %xmm0, (%rax,%rcx,8)
jmp 0x3c609
movq 0x218(%rsp), %rax
movslq 0x214(%rsp), %rcx
movsd 0x1c0a1c(%rip), %xmm0   # 0x1fd020
movsd %xmm0, (%rax,%rcx,8)
jmp 0x3c60b
movl 0x214(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x214(%rsp)
movl 0x210(%rsp), %eax
addl 0x20c(%rsp), %eax
movl %eax, 0x20c(%rsp)
jmp 0x3c497
cmpq $0x0, 0x288(%rsp)
je 0x3c6ac
movq 0x288(%rsp), %rsi
addq $0x8, %rsi
movq 0x230(%rsp), %rdi
movslq 0x214(%rsp), %rax
imulq $0x18, %rax, %rax
addq %rax, %rdi
callq 0x12b3e0
movq 0x280(%rsp), %rdi
callq 0x1ecf0
cvtsi2sd %eax, %xmm1
movsd 0x1c1a4e(%rip), %xmm0   # 0x1fe0d0
mulsd %xmm1, %xmm0
movq 0x218(%rsp), %rax
movslq 0x214(%rsp), %rcx
movsd %xmm0, (%rax,%rcx,8)
movl 0x214(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x214(%rsp)
movl 0x214(%rsp), %esi
movq 0x230(%rsp), %rdx
movq 0x218(%rsp), %rcx
leaq 0x1e8(%rsp), %rdi
callq 0xc9830
movq 0x68(%rsp), %rdi
addq $0x40, %rdi
leaq 0x1e8(%rsp), %rsi
callq 0x12b3e0
movq 0x68(%rsp), %rax
cmpl $0x0, (%rax)
jne 0x3c84e
cmpl $0x2, 0x214(%rsp)
jl 0x3c84e
movq 0x68(%rsp), %rsi
addq $0x40, %rsi
movl 0x214(%rsp), %edx
movq 0x230(%rsp), %rcx
movq 0x218(%rsp), %r8
leaq 0x1d0(%rsp), %r9
leaq 0x188(%rsp), %rdi
callq 0xc98f0
movsd 0x1d0(%rsp), %xmm0
leaq 0x22c001(%rip), %rax    # 0x268748
ucomisd (%rax), %xmm0
jbe 0x3c849
movsd 0x1d8(%rsp), %xmm0
leaq 0x22bfe7(%rip), %rax    # 0x268748
ucomisd (%rax), %xmm0
jbe 0x3c847
movsd 0x1d8(%rsp), %xmm0
divsd 0x1d0(%rsp), %xmm0
movsd %xmm0, 0x180(%rsp)
movsd 0x1e0(%rsp), %xmm0
divsd 0x1d8(%rsp), %xmm0
movsd %xmm0, 0x178(%rsp)
movsd 0x180(%rsp), %xmm1
movsd 0x178(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x3c7fe
movq 0x68(%rsp), %rax
movl $0x2, (%rax)
movq %rax, %rcx
addq $0x58, %rcx
movq %rcx, 0x20(%rsp)
addq $0x40, %rax
movq %rax, 0x28(%rsp)
leaq 0x188(%rsp), %rdi
xorl %esi, %esi
callq 0x36020
movq 0x20(%rsp), %rdi
movq 0x28(%rsp), %rsi
movq %rax, %rdx
xorl %ecx, %ecx
callq 0x47570
jmp 0x3c845
movq 0x68(%rsp), %rax
movl $0x3, (%rax)
movq %rax, %rcx
addq $0x88, %rcx
movq %rcx, 0x10(%rsp)
addq $0x40, %rax
movq %rax, 0x18(%rsp)
leaq 0x188(%rsp), %rdi
movl $0x2, %esi
callq 0x36020
movq 0x10(%rsp), %rdi
movq 0x18(%rsp), %rsi
movq %rax, %rdx
callq 0x1295b0
jmp 0x3c847
jmp 0x3c849
jmp 0x3ca6a
movq 0x68(%rsp), %rax
cmpl $0x2, (%rax)
jne 0x3c934
cmpl $0x2, 0x214(%rsp)
jl 0x3c934
movq 0x68(%rsp), %rsi
addq $0x40, %rsi
movl 0x214(%rsp), %edx
movq 0x230(%rsp), %rcx
movq 0x218(%rsp), %r8
leaq 0x160(%rsp), %r9
leaq 0x118(%rsp), %rdi
callq 0xc98f0
movsd 0x160(%rsp), %xmm0
leaq 0x22be99(%rip), %rax    # 0x268748
ucomisd (%rax), %xmm0
jbe 0x3c92f
leaq 0x118(%rsp), %rdi
xorl %esi, %esi
callq 0x36020
movq %rax, %rsi
leaq 0x100(%rsp), %rdi
callq 0x12bef0
movq 0x68(%rsp), %rsi
movq %rsi, %rdi
addq $0x58, %rdi
addq $0x40, %rsi
leaq 0x100(%rsp), %rdx
xorl %ecx, %ecx
callq 0x47570
leaq 0x100(%rsp), %rdi
leaq 0x268(%rsp), %rsi
callq 0x12c4c0
movsd %xmm0, 0xf8(%rsp)
xorps %xmm0, %xmm0
ucomisd 0xf8(%rsp), %xmm0
jbe 0x3c92d
movq 0x68(%rsp), %rdi
addq $0x58, %rdi
callq 0x475d0
jmp 0x3c92f
jmp 0x3ca68
movq 0x68(%rsp), %rax
cmpl $0x3, (%rax)
jne 0x3ca66
cmpl $0x3, 0x214(%rsp)
jl 0x3ca66
movq 0x68(%rsp), %rsi
addq $0x40, %rsi
movl 0x214(%rsp), %edx
movq 0x230(%rsp), %rcx
movq 0x218(%rsp), %r8
leaq 0xe0(%rsp), %r9
leaq 0x98(%rsp), %rdi
callq 0xc98f0
movsd 0xe8(%rsp), %xmm0
leaq 0x22bdb3(%rip), %rax    # 0x268748
ucomisd (%rax), %xmm0
jbe 0x3ca64
leaq 0x98(%rsp), %rdi
movl $0x2, %esi
callq 0x36020
movq %rax, %rsi
leaq 0x80(%rsp), %rdi
callq 0x12bef0
movq 0x68(%rsp), %rsi
movq %rsi, %rdi
addq $0x88, %rdi
addq $0x40, %rsi
leaq 0x80(%rsp), %rdx
callq 0x1295b0
cmpq $0x0, 0x288(%rsp)
je 0x3ca29
movq 0x288(%rsp), %rsi
addq $0x20, %rsi
leaq 0x80(%rsp), %rdi
callq 0x12c4c0
movsd %xmm0, 0x78(%rsp)
xorps %xmm0, %xmm0
ucomisd 0x78(%rsp), %xmm0
jbe 0x3ca27
movq 0x68(%rsp), %rdi
addq $0x88, %rdi
callq 0x475f0
jmp 0x3ca62
leaq 0x80(%rsp), %rdi
leaq 0x268(%rsp), %rsi
callq 0x12c4c0
movsd %xmm0, 0x70(%rsp)
xorps %xmm0, %xmm0
ucomisd 0x70(%rsp), %xmm0
jbe 0x3ca60
movq 0x68(%rsp), %rdi
addq $0x88, %rdi
callq 0x475f0
jmp 0x3ca62
jmp 0x3ca64
jmp 0x3ca66
jmp 0x3ca68
jmp 0x3ca6a
movq 0x230(%rsp), %rax
movq %rax, 0x8(%rsp)
cmpq $0x0, %rax
je 0x3ca87
movq 0x8(%rsp), %rdi
callq 0x169a0
movq 0x218(%rsp), %rax
movq %rax, (%rsp)
cmpq $0x0, %rax
je 0x3caa2
movq (%rsp), %rdi
callq 0x169a0
jmp 0x3caa4
addq $0x298, %rsp            # imm = 0x298
retq
movq 0x228(%rsp), %rdi
callq 0x16ca0
nopl (%rax)