// File: tomfunkhouser[P]gaps/apps/sflprocess/sflprocess.cpp
// Function: CreateOverlapObjectRelationships(gaps::R3SurfelScene*, double, double, double, double)
static int
CreateOverlapObjectRelationships(R3SurfelScene *scene,
  RNLength max_gap_distance, RNLength max_plane_offset, RNAngle max_normal_angle,
  RNScalar min_overlap)
{
  // Check max gap
  if (max_gap_distance == 0) return 1;
  
  // Start statistics
  RNTime start_time;
  start_time.Read();
  int count = 0;
  if (print_verbose) {
    printf("Creating object overlap relationships ...\n");
    fflush(stdout);
  }

  // Create a pointset for each object
  R3SurfelPointSet **pointsets = new R3SurfelPointSet * [ scene->NObjects() ];
  RNScalar max_resolution = 8.0/(max_gap_distance*max_gap_distance);
  for (int i = 0; i < scene->NObjects(); i++) {
    R3SurfelObject *object = scene->Object(i);
    R3SurfelNodeSet nodes;
    pointsets[i] = new R3SurfelPointSet();
    nodes.InsertNodes(object, max_resolution);
    for (int j = 0; j < nodes.NNodes(); j++) {
      R3SurfelNode *node = nodes.Node(j);
      for (int k = 0; k < node->NBlocks(); k++) {
        R3SurfelBlock *block = node->Block(k);
        pointsets[i]->InsertPoints(block);
      }
    }
  }

  // Consider every object
  for (int i0 = 0; i0 < scene->NObjects(); i0++) {
    R3SurfelObject *object0 = scene->Object(i0);
    R3SurfelPointSet *pointset0 = pointsets[i0];
    if (pointset0->NPoints() == 0) continue;
    const R3Box& bbox0 = pointset0->BBox();

#ifdef USE_GRID
    // Create grid
    R3Grid *grid0 = CreateGrid(pointset0, max_gap_distance);
    if (!grid0) continue;
#else
    // Create kdtree
    RNArray<R3SurfelPoint *> array0;
    for (int j = 0; j < pointset0->NPoints(); j++) array0.Insert(pointset0->Point(j));
    R3Kdtree<R3SurfelPoint *> kdtree(array0, SurfelPointPosition);
#endif
                                     
    // Consider every other object
    for (int i1 = 0; i1 < scene->NObjects(); i1++) {
      R3SurfelObject *object1 = scene->Object(i1);
      if (object0 == object1) continue;
      R3SurfelPointSet *pointset1 = pointsets[i1];
      if (pointset1->NPoints() == 0) continue;
      const R3Box& bbox1 = pointset1->BBox();

      // Check bboxes for max_gap_distance
      if (R3Distance(bbox0, bbox1) > max_gap_distance) continue;

      // Check pointset1 for overlap with grid0
      int npoints = 0;
      for (int j1 = 0; j1 < pointset1->NPoints(); j1++) {
        R3SurfelPoint *point1 = pointset1->Point(j1);
#ifdef USE_GRID
        // Check if point1 is in marked grid cell
        const R3Point& world_position = point1->Position();
        R3Point grid_position = grid0->GridPosition(world_position);
        int ix = (int) (grid_position.X() + 0.5);
        if ((ix < 0) || (ix >= grid0->XResolution())) continue;
        int iy = (int) (grid_position.Y() + 0.5);
        if ((iy < 0) || (iy >= grid0->YResolution())) continue;
        int iz = (int) (grid_position.Z() + 0.5);
        if ((iz < 0) || (iz >= grid0->ZResolution())) continue;
        if (grid0->GridValue(ix, iy, iz) <= 0) continue;
        // grid0->SetGridValue(ix, iy, iz, 0);
#else
        // Check if point1 is near compatible point in kdtree
        SurfelPointCompatibilityParameters compatibility_parameters;
        compatibility_parameters.max_gap_distance = max_gap_distance;
        compatibility_parameters.max_normal_angle = max_normal_angle;
        compatibility_parameters.max_plane_offset = max_plane_offset;
        if (!kdtree.FindAny(point1, 0, max_gap_distance, AreSurfelPointsCompatible, &compatibility_parameters)) continue;
#endif
        npoints++;
      }

      // Compute/check overlap
      RNScalar overlap = (RNScalar) npoints / (RNScalar) pointset1->NPoints();

      // Create object relationship
      if (overlap > min_overlap) {
        RNScalar operands[] = { overlap };
        int noperands = sizeof(operands) / sizeof(RNScalar);
        R3SurfelObjectRelationship *relationship = new R3SurfelObjectRelationship(R3_SURFEL_OBJECT_OVERLAP_RELATIONSHIP, object0, object1, operands, noperands);
        scene->InsertObjectRelationship(relationship);
        count++;
      }
    }

#ifdef USE_GRID
    // Delete grid
    delete grid0;
#endif
  }

  // Delete pointsets
  for (int i = 0; i < scene->NObjects(); i++) {
    delete pointsets[i];
  }

  // Print statistics
  if (print_verbose) {
    printf("  Time = %.2f seconds\n", start_time.Elapsed());
    printf("  # Objects = %d\n", scene->NObjects());
    printf("  # Relationships = %d\n", count);
    fflush(stdout);
  }
  
  // Return success
  return 1;
}

// Assembly:
subq $0x228, %rsp            # imm = 0x228
movq %rdi, 0x218(%rsp)
movsd %xmm0, 0x210(%rsp)
movsd %xmm1, 0x208(%rsp)
movsd %xmm2, 0x200(%rsp)
movsd %xmm3, 0x1f8(%rsp)
movsd 0x210(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jne 0x25a87
jp 0x25a87
movl $0x1, 0x224(%rsp)
jmp 0x2630a
leaq 0x1e8(%rsp), %rdi
callq 0x16dd30
leaq 0x1e8(%rsp), %rdi
callq 0x263f0
movl $0x0, 0x1e4(%rsp)
cmpl $0x0, 0x23ed0d(%rip)    # 0x2647c0
je 0x25ad2
leaq 0x1d3253(%rip), %rdi    # 0x1f8d0f
movb $0x0, %al
callq 0x16090
movq 0x2354c6(%rip), %rax    # 0x25af90
movq (%rax), %rdi
callq 0x168c0
movq 0x218(%rsp), %rdi
callq 0x266c0
cltq
movl $0x8, %ecx
mulq %rcx
movq %rax, %rdi
seto %cl
movq $-0x1, %rax
testb $0x1, %cl
cmovneq %rax, %rdi
callq 0x16040
movq %rax, 0x1d8(%rsp)
movsd 0x210(%rsp), %xmm1
mulsd 0x210(%rsp), %xmm1
movsd 0x1d254c(%rip), %xmm0   # 0x1f8070
divsd %xmm1, %xmm0
movsd %xmm0, 0x1d0(%rsp)
movl $0x0, 0x1cc(%rsp)
movl 0x1cc(%rsp), %eax
movl %eax, 0x68(%rsp)
movq 0x218(%rsp), %rdi
callq 0x266c0
movl %eax, %ecx
movl 0x68(%rsp), %eax
cmpl %ecx, %eax
jge 0x25d62
movq 0x218(%rsp), %rdi
movl 0x1cc(%rsp), %esi
callq 0x29d70
movq %rax, 0x1c0(%rsp)
leaq 0x158(%rsp), %rdi
callq 0x5f050
movl $0x60, %edi
callq 0x16740
movq %rax, 0x60(%rsp)
jmp 0x25b9c
movq 0x60(%rsp), %rdi
callq 0x552d0
jmp 0x25ba8
movq 0x60(%rsp), %rdx
movq 0x1d8(%rsp), %rax
movslq 0x1cc(%rsp), %rcx
movq %rdx, (%rax,%rcx,8)
movq 0x1c0(%rsp), %rsi
movsd 0x1d0(%rsp), %xmm0
leaq 0x158(%rsp), %rdi
callq 0x5f250
jmp 0x25be1
movl $0x0, 0x148(%rsp)
movl 0x148(%rsp), %eax
movl %eax, 0x58(%rsp)
leaq 0x158(%rsp), %rdi
callq 0x29da0
movl %eax, 0x5c(%rsp)
jmp 0x25c0a
movl 0x58(%rsp), %eax
movl 0x5c(%rsp), %ecx
cmpl %ecx, %eax
jge 0x25d2d
movl 0x148(%rsp), %esi
leaq 0x158(%rsp), %rdi
callq 0x29dc0
movq %rax, 0x50(%rsp)
jmp 0x25c35
movq 0x50(%rsp), %rax
movq %rax, 0x140(%rsp)
movl $0x0, 0x13c(%rsp)
movl 0x13c(%rsp), %eax
movl %eax, 0x4c(%rsp)
movq 0x140(%rsp), %rdi
callq 0x27710
movl %eax, %ecx
movl 0x4c(%rsp), %eax
cmpl %ecx, %eax
jge 0x25d15
movq 0x140(%rsp), %rdi
movl 0x13c(%rsp), %esi
callq 0x27730
movq %rax, 0x40(%rsp)
jmp 0x25c8e
movq 0x40(%rsp), %rax
movq %rax, 0x130(%rsp)
movq 0x1d8(%rsp), %rax
movslq 0x1cc(%rsp), %rcx
movq (%rax,%rcx,8), %rdi
movq 0x130(%rsp), %rsi
movq (%rdi), %rax
movq 0x10(%rax), %rax
callq *%rax
jmp 0x25cc2
jmp 0x25cc4
movl 0x13c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x13c(%rsp)
jmp 0x25c4d
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x150(%rsp)
movl %eax, 0x14c(%rsp)
jmp 0x25d50
movq 0x60(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x150(%rsp)
movl %eax, 0x14c(%rsp)
movl $0x60, %esi
callq 0x16750
jmp 0x25d50
jmp 0x25d17
movl 0x148(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x148(%rsp)
jmp 0x25bec
leaq 0x158(%rsp), %rdi
callq 0x5f1e0
movl 0x1cc(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x1cc(%rsp)
jmp 0x25b3c
leaq 0x158(%rsp), %rdi
callq 0x5f1e0
jmp 0x26319
movl $0x0, 0x12c(%rsp)
movl 0x12c(%rsp), %eax
movl %eax, 0x3c(%rsp)
movq 0x218(%rsp), %rdi
callq 0x266c0
movl %eax, %ecx
movl 0x3c(%rsp), %eax
cmpl %ecx, %eax
jge 0x2623f
movq 0x218(%rsp), %rdi
movl 0x12c(%rsp), %esi
callq 0x29d70
movq %rax, 0x120(%rsp)
movq 0x1d8(%rsp), %rax
movslq 0x12c(%rsp), %rcx
movq (%rax,%rcx,8), %rax
movq %rax, 0x118(%rsp)
movq 0x118(%rsp), %rdi
callq 0x29df0
cmpl $0x0, %eax
jne 0x25de2
jmp 0x26229
movq 0x118(%rsp), %rdi
callq 0x29e00
movq %rax, 0x110(%rsp)
movq 0x118(%rsp), %rdi
movsd 0x210(%rsp), %xmm0
movl $0x400, %esi            # imm = 0x400
callq 0x8c600
movq %rax, 0x108(%rsp)
cmpq $0x0, 0x108(%rsp)
jne 0x25e2a
jmp 0x26229
movl $0x0, 0x104(%rsp)
movl 0x104(%rsp), %eax
movl %eax, 0x38(%rsp)
movq 0x218(%rsp), %rdi
callq 0x266c0
movl %eax, %ecx
movl 0x38(%rsp), %eax
cmpl %ecx, %eax
jge 0x261fb
movq 0x218(%rsp), %rdi
movl 0x104(%rsp), %esi
callq 0x29d70
movq %rax, 0xf8(%rsp)
movq 0x120(%rsp), %rax
cmpq 0xf8(%rsp), %rax
jne 0x25e8e
jmp 0x261e5
movq 0x1d8(%rsp), %rax
movslq 0x104(%rsp), %rcx
movq (%rax,%rcx,8), %rax
movq %rax, 0xf0(%rsp)
movq 0xf0(%rsp), %rdi
callq 0x29df0
cmpl $0x0, %eax
jne 0x25ec1
jmp 0x261e5
movq 0xf0(%rsp), %rdi
callq 0x29e00
movq %rax, 0xe8(%rsp)
movq 0x110(%rsp), %rdi
movq 0xe8(%rsp), %rsi
callq 0xbfa00
ucomisd 0x210(%rsp), %xmm0
jbe 0x25efb
jmp 0x261e5
movl $0x0, 0xe4(%rsp)
movl $0x0, 0xe0(%rsp)
movl 0xe0(%rsp), %eax
movl %eax, 0x34(%rsp)
movq 0xf0(%rsp), %rdi
callq 0x29df0
movl %eax, %ecx
movl 0x34(%rsp), %eax
cmpl %ecx, %eax
jge 0x260e8
movq 0xf0(%rsp), %rdi
movl 0xe0(%rsp), %esi
callq 0x29e10
movq %rax, 0xd8(%rsp)
movq 0xd8(%rsp), %rsi
leaq 0xb8(%rsp), %rdi
callq 0x29e30
leaq 0xb8(%rsp), %rax
movq %rax, 0xd0(%rsp)
movq 0x108(%rsp), %rsi
movq 0xd0(%rsp), %rdx
leaq 0xa0(%rsp), %rdi
callq 0x29ea0
leaq 0xa0(%rsp), %rdi
callq 0x26ee0
movsd 0x1d20b6(%rip), %xmm1   # 0x1f8060
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x9c(%rsp)
cmpl $0x0, 0x9c(%rsp)
jl 0x25fe5
movl 0x9c(%rsp), %eax
movl %eax, 0x30(%rsp)
movq 0x108(%rsp), %rdi
callq 0x29f30
movl %eax, %ecx
movl 0x30(%rsp), %eax
cmpl %ecx, %eax
jl 0x25fea
jmp 0x260d2
leaq 0xa0(%rsp), %rdi
callq 0x26ef0
movsd 0x1d2061(%rip), %xmm1   # 0x1f8060
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x98(%rsp)
cmpl $0x0, 0x98(%rsp)
jl 0x2603a
movl 0x98(%rsp), %eax
movl %eax, 0x2c(%rsp)
movq 0x108(%rsp), %rdi
callq 0x29f50
movl %eax, %ecx
movl 0x2c(%rsp), %eax
cmpl %ecx, %eax
jl 0x2603f
jmp 0x260d2
leaq 0xa0(%rsp), %rdi
callq 0x26e50
movsd 0x1d200c(%rip), %xmm1   # 0x1f8060
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x94(%rsp)
cmpl $0x0, 0x94(%rsp)
jl 0x2608f
movl 0x94(%rsp), %eax
movl %eax, 0x28(%rsp)
movq 0x108(%rsp), %rdi
callq 0x29f70
movl %eax, %ecx
movl 0x28(%rsp), %eax
cmpl %ecx, %eax
jl 0x26091
jmp 0x260d2
movq 0x108(%rsp), %rdi
movl 0x9c(%rsp), %esi
movl 0x98(%rsp), %edx
movl 0x94(%rsp), %ecx
callq 0x29f90
movaps %xmm0, %xmm1
xorps %xmm0, %xmm0
ucomisd %xmm1, %xmm0
jb 0x260c1
jmp 0x260d2
movl 0xe4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xe4(%rsp)
movl 0xe0(%rsp), %eax
addl $0x1, %eax
movl %eax, 0xe0(%rsp)
jmp 0x25f11
cvtsi2sdl 0xe4(%rsp), %xmm0
movsd %xmm0, 0x20(%rsp)
movq 0xf0(%rsp), %rdi
callq 0x29df0
movsd 0x20(%rsp), %xmm0
cvtsi2sd %eax, %xmm1
divsd %xmm1, %xmm0
movsd %xmm0, 0x88(%rsp)
movsd 0x88(%rsp), %xmm0
ucomisd 0x1f8(%rsp), %xmm0
jbe 0x261e3
movsd 0x88(%rsp), %xmm0
movsd %xmm0, 0x80(%rsp)
movl $0x1, 0x7c(%rsp)
movl $0x38, %edi
callq 0x16740
movq %rax, %rdi
movq %rdi, %rax
movq %rax, 0x18(%rsp)
movq 0x120(%rsp), %rdx
movq 0xf8(%rsp), %rcx
movl 0x7c(%rsp), %r9d
movl $0x2, %esi
leaq 0x80(%rsp), %r8
callq 0x6e400
jmp 0x2618b
movq 0x18(%rsp), %rax
movq %rax, 0x70(%rsp)
movq 0x218(%rsp), %rdi
movq 0x70(%rsp), %rsi
movq (%rdi), %rax
callq *0x60(%rax)
movl 0x1e4(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x1e4(%rsp)
jmp 0x261e3
movq 0x18(%rsp), %rdi
movq %rax, %rcx
movl %edx, %eax
movq %rcx, 0x150(%rsp)
movl %eax, 0x14c(%rsp)
movl $0x38, %esi
callq 0x16750
jmp 0x26319
jmp 0x261e5
movl 0x104(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x104(%rsp)
jmp 0x25e35
movq 0x108(%rsp), %rax
movq %rax, 0x10(%rsp)
cmpq $0x0, %rax
je 0x26227
movq 0x10(%rsp), %rdi
callq 0x10dda0
movq 0x10(%rsp), %rdi
movl $0x250, %esi            # imm = 0x250
callq 0x16750
jmp 0x26229
movl 0x12c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x12c(%rsp)
jmp 0x25d6d
movl $0x0, 0x6c(%rsp)
movl 0x6c(%rsp), %eax
movl %eax, 0xc(%rsp)
movq 0x218(%rsp), %rdi
callq 0x266c0
movl %eax, %ecx
movl 0xc(%rsp), %eax
cmpl %ecx, %eax
jge 0x2629a
movq 0x1d8(%rsp), %rax
movslq 0x6c(%rsp), %rcx
movq (%rax,%rcx,8), %rax
movq %rax, (%rsp)
cmpq $0x0, %rax
je 0x2628b
movq (%rsp), %rdi
movq (%rdi), %rax
callq *0x8(%rax)
jmp 0x2628d
movl 0x6c(%rsp), %eax
addl $0x1, %eax
movl %eax, 0x6c(%rsp)
jmp 0x26247
cmpl $0x0, 0x23e51f(%rip)    # 0x2647c0
je 0x262ff
leaq 0x1e8(%rsp), %rdi
callq 0x16ddf0
leaq 0x1d22c8(%rip), %rdi    # 0x1f857f
movb $0x1, %al
callq 0x16090
movq 0x218(%rsp), %rdi
callq 0x266c0
movl %eax, %esi
leaq 0x1d237c(%rip), %rdi    # 0x1f8650
movb $0x0, %al
callq 0x16090
movl 0x1e4(%rsp), %esi
leaq 0x1d2a51(%rip), %rdi    # 0x1f8d3a
movb $0x0, %al
callq 0x16090
movq 0x234c99(%rip), %rax    # 0x25af90
movq (%rax), %rdi
callq 0x168c0
movl $0x1, 0x224(%rsp)
movl 0x224(%rsp), %eax
addq $0x228, %rsp            # imm = 0x228
retq
movq 0x150(%rsp), %rdi
callq 0x16ca0
nopw %cs:(%rax,%rax)