// File: /workspace/github_cpp_makefile_star2_O2/tomfunkhouser[P]gaps/pkgs/R3Surfels/R3SurfelUtils.cpp
// Function: gaps::CreateClusterObjects(gaps::R3SurfelScene*, gaps::R3SurfelPointGraph*, gaps::R3SurfelObject*, gaps::R3SurfelNode*, double, double, int)
RNArray<R3SurfelObject *> *
CreateClusterObjects(R3SurfelScene *scene, R3SurfelPointGraph *graph, 
  R3SurfelObject *parent_object, R3SurfelNode *parent_node, 
  RNLength max_offplane_distance, RNAngle max_normal_angle,
  int min_points_per_object)
{
  // Get convenient variables
  R3SurfelTree *tree = scene->Tree();
  if (!tree) return 0;
  R3SurfelDatabase *database = tree->Database();
  if (!database) return 0;

  // Check number of points
  if (graph->NPoints() < min_points_per_object) return NULL;

  // Check parent object
  if (!parent_object) parent_object = scene->RootObject();

  // Check parent node
  if (!parent_node) parent_node = tree->RootNode();

  // Create normals
  R3Vector *normals = NULL;
  if ((max_offplane_distance > 0) || (max_normal_angle > 0)) {
    normals = CreateNormals(graph, FALSE);
  }

  // Create cluster for every point
  RNArray<R3SurfelCluster *> clusters;
  for (int i = 0; i < graph->NPoints(); i++) {
    R3SurfelPoint *point = graph->Point(i);
    R3Point position = point->Position();
    R3SurfelCluster *cluster = new R3SurfelCluster();
    cluster->parent = NULL;
    cluster->points.Insert(point);
    cluster->bbox.Reset(position, position);
    cluster->id = i;
    clusters.Insert(cluster);
  }

  // Check clusters
  if (clusters.IsEmpty()) {
    if (normals) delete [] normals;
    return NULL;  
  }

  // Create cluster pairs and add to heap
  R3SurfelClusterPair tmp;
  RNHeap<R3SurfelClusterPair *> heap(&tmp, &tmp.similarity, NULL, FALSE);;
  for (int index0 = 0; index0 < graph->NPoints(); index0++) {
    R3SurfelPoint *point0 = graph->Point(index0);
    R3SurfelCluster *cluster0 = clusters.Kth(index0);
    R3Point position0 = point0->Position();
    const R3Vector& normal0 = (normals) ? normals[index0] : R3zero_vector;
    R3Plane plane0(position0, normal0);
    for (int j = 0; j < graph->NNeighbors(index0); j++) {
      R3SurfelPoint *point1 = graph->Neighbor(index0, j);
      int index1 = graph->PointIndex(point1);
      if (index1 < index0) continue;
      R3Point position1 = point1->Position();

      // Compute plane similarity
      RNScalar plane_similarity = 1;
      if (max_offplane_distance > 0) {
        RNScalar offplane_distance = R3Distance(plane0, position1);
        if (offplane_distance > max_offplane_distance) continue;
        plane_similarity *= 1.0 - 0.9 * offplane_distance / max_offplane_distance;
      }

      // Compute normal similarity
      RNScalar normal_similarity = 1;
      if (max_normal_angle > 0) {
        if (normals) {
          const R3Vector& normal1 = normals[index1];
          RNScalar dot = fabs(normal0.Dot(normal1));
          RNAngle normal_angle = (dot < 1) ? acos(dot) : 0;
          if (normal_angle > max_normal_angle) continue;
          normal_similarity *= 1.0 - 0.9 * normal_angle / max_normal_angle;
        }
      }

      // Compute distance similarity
      RNScalar distance_similarity = 1.0;
      if (graph->MaxDistance() > 0) {
        RNScalar distance = R3Distance(position0, position1);
        distance_similarity = 1.0 - 0.99 * distance / graph->MaxDistance();
      }

      // Create pair
      R3SurfelCluster *cluster1 = clusters.Kth(index1);
      R3SurfelClusterPair *pair = new R3SurfelClusterPair();
      pair->clusters[0] = cluster0;
      pair->clusters[1] = cluster1;
      pair->similarity = distance_similarity * normal_similarity * plane_similarity;
      heap.Push(pair);
    }
  }

  // Check cluster pairs
  if (heap.IsEmpty()) {
    for (int i = 0; i < clusters.NEntries(); i++) delete clusters[i];
    if (normals) delete [] normals;
    return NULL;
  }

  // Merge clusters hierarchically
  while (!heap.IsEmpty()) {
    // Get pair
    R3SurfelClusterPair *pair = heap.Pop();

    // Get clusters
    R3SurfelCluster *cluster0 = pair->clusters[0];
    R3SurfelCluster *cluster1 = pair->clusters[1];
    while (cluster0->parent) cluster0 = cluster0->parent;
    while (cluster1->parent) cluster1 = cluster1->parent;
    if (cluster0 == cluster1) continue;

    // Merge clusters
    cluster0->points.Append(cluster1->points);
    cluster0->bbox.Union(cluster1->bbox);
    cluster1->parent = cluster0;
    cluster1->points.Empty(TRUE);
    cluster1->bbox = R3null_box;

    // Delete pair
    delete pair;
  }

  // Create array of objects
  RNArray<R3SurfelObject *> *objects = new RNArray<R3SurfelObject *>();
  if (!objects) {
    RNFail("Unable to create array of objects\n");
    for (int i = 0; i < clusters.NEntries(); i++) delete clusters[i];
    if (normals) delete [] normals;
    return NULL;
  }

  // Create objects with copies of surfels
  for (int i = 0; i < clusters.NEntries(); i++) {
    R3SurfelCluster *cluster = clusters.Kth(i);

    // Check cluster
    if (cluster->points.NEntries() < min_points_per_object) { delete cluster; continue; }
    if (cluster->parent) { delete cluster; continue; }

    // Create surfels
    R3Point origin = cluster->bbox.Centroid();
    R3Surfel *surfels = new R3Surfel [ cluster->points.NEntries() ];
    for (int j = 0; j < cluster->points.NEntries(); j++) {
      R3SurfelPoint *point = cluster->points.Kth(j);
      R3SurfelBlock *block = point->Block();
      const R3Surfel *surfel = point->Surfel();
      RNCoord x = surfel->X() + block->PositionOrigin().X() - origin.X();
      RNCoord y = surfel->Y() + block->PositionOrigin().Y() - origin.Y();
      RNCoord z = surfel->Z() + block->PositionOrigin().Z() - origin.Z();
      surfels[j].SetPosition(x, y, z);
      surfels[j].SetColor(surfel->Color());
      surfels[j].SetAerial(surfel->IsAerial());
    }

    // Create block
    R3SurfelBlock *block = new R3SurfelBlock(surfels, cluster->points.NEntries(), origin);
    if (!block) {
      RNFail("Unable to create node\n");
      for (int i = 0; i < clusters.NEntries(); i++) delete clusters[i];
      if (normals) delete [] normals;
      delete [] surfels;
      return NULL;
    }

    // Delete surfels
    delete [] surfels;

    // Create node
    char node_name[256];
    sprintf(node_name, "O%d\n", objects->NEntries());
    R3SurfelNode *node = new R3SurfelNode(node_name);
    if (!node) {
      RNFail("Unable to create node\n");
      for (int i = 0; i < clusters.NEntries(); i++) delete clusters[i];
      if (normals) delete [] normals;
      delete block;
      return NULL;
    }

    // Create object
    char object_name[256];
    sprintf(object_name, "O%d\n", objects->NEntries());
    R3SurfelObject *object = new R3SurfelObject(object_name);
    if (!object) {
      RNFail("Unable to create object\n");
      for (int i = 0; i < clusters.NEntries(); i++) delete clusters[i];
      if (normals) delete [] normals;
      delete block;
      delete node;
      return NULL;
    }
     
    // Insert everything
    block->UpdateProperties();
    database->InsertBlock(block);
    node->InsertBlock(block);
    tree->InsertNode(node, parent_node);
    object->InsertNode(node);
    scene->InsertObject(object, parent_object);
    objects->Insert(object);

    // Delete cluster
    delete cluster;

    // Print debug statement
    printf("    %d %d\n", i, block->NSurfels());
  }
  
  // Delete normals
  delete [] normals;

  // Return objects
  return objects;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x2f8, %rsp            # imm = 0x2F8
movl %r8d, 0x24(%rsp)
movsd %xmm1, 0x38(%rsp)
movsd %xmm0, 0x18(%rsp)
movq %rsi, %r12
movq 0x8(%rdi), %rsi
testq %rsi, %rsi
je 0x276be
movq 0x10(%rsi), %rax
movq %rax, 0x60(%rsp)
testq %rax, %rax
je 0x276be
movl 0x24(%rsp), %eax
cmpl %eax, 0x18(%r12)
jge 0x276d6
xorl %r12d, %r12d
movq %r12, %rax
addq $0x2f8, %rsp            # imm = 0x2F8
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
testq %rdx, %rdx
jne 0x276e2
movq 0x10(%rdi), %rax
movq (%rax), %rdx
testq %rcx, %rcx
jne 0x276ee
movq 0x18(%rsi), %rax
movq (%rax), %rcx
xorpd %xmm0, %xmm0
movsd 0x18(%rsp), %xmm1
ucomisd %xmm0, %xmm1
movq %rcx, 0x58(%rsp)
movq %rdi, 0x48(%rsp)
movq %rdx, 0x40(%rsp)
movq %rsi, 0x50(%rsp)
ja 0x27729
movsd 0x38(%rsp), %xmm1
ucomisd %xmm0, %xmm1
ja 0x27729
movq $0x0, 0x10(%rsp)
jmp 0x27738
movq %r12, %rdi
xorl %esi, %esi
callq 0x251da
movq %rax, 0x10(%rsp)
leaq 0x28(%rsp), %rdi
callq 0x9a7ae
xorl %ebx, %ebx
leaq 0x1f0(%rsp), %r14
xorl %r15d, %r15d
movslq 0x18(%r12), %rax
cmpq %rax, %r15
jge 0x277e4
movq 0x10(%r12), %rbp
addq %rbx, %rbp
movq %r14, %rdi
movq %rbp, %rsi
callq 0x16310
pushq $0x58
popq %rdi
callq 0x134f0
movq %rax, %r13
xorpd %xmm0, %xmm0
movapd %xmm0, 0x40(%rax)
movapd %xmm0, 0x30(%rax)
movapd %xmm0, 0x20(%rax)
movapd %xmm0, 0x10(%rax)
movapd %xmm0, (%rax)
andq $0x0, 0x50(%rax)
movq %rax, %rdi
callq 0x28778
andq $0x0, (%r13)
leaq 0x8(%r13), %rdi
movq %rbp, %rsi
callq 0x1784c
movq %r13, %rdi
addq $0x18, %rdi
movq %r14, %rsi
movq %r14, %rdx
callq 0x6494c
movl %r15d, 0x50(%r13)
leaq 0x28(%rsp), %rdi
movq %r13, %rsi
callq 0x1784c
incq %r15
addq $0x10, %rbx
jmp 0x2774f
cmpl $0x0, 0x34(%rsp)
je 0x27d7c
xorpd %xmm1, %xmm1
movsd 0x38(%rsp), %xmm0
ucomisd %xmm1, %xmm0
seta %cl
xorpd %xmm0, %xmm0
leaq 0xb0(%rsp), %rsi
movapd %xmm0, (%rsi)
movupd %xmm0, 0x18(%rsi)
movupd %xmm0, 0x24(%rsi)
movabsq $-0xfffffff0, %rdx      # imm = 0xFFFFFFFF00000010
movq %rdx, 0x10(%rsi)
cmpq $0x0, 0x10(%rsp)
setne %dl
andb %cl, %dl
movb %dl, 0xf(%rsp)
xorl %ebx, %ebx
leaq 0x90(%rsp), %rbp
cltq
cmpq %rax, %rbx
jge 0x27a60
movq %rbx, %r13
shlq $0x4, %r13
movq 0x10(%r12), %rsi
addq %r13, %rsi
movq 0x28(%rsp), %rax
movq (%rax,%rbx,8), %rax
movq %rax, 0x70(%rsp)
leaq 0xf0(%rsp), %rdi
callq 0x16310
imulq $0x18, %rbx, %rdx
movq 0x10(%rsp), %rax
addq %rax, %rdx
movq %rdx, 0x68(%rsp)
testq %rax, %rax
jne 0x27893
leaq 0xa88c5(%rip), %rdx     # 0xd0158
leaq 0x1f0(%rsp), %rdi
leaq 0xf0(%rsp), %rsi
callq 0x76e1c
xorl %r15d, %r15d
movq 0x68(%r12), %rax
movslq 0xc(%rax,%r13), %rcx
cmpq %rcx, %r15
jge 0x27a53
movq (%rax,%r13), %rax
movq (%rax,%r15,8), %rsi
movq %rsi, %r14
subq 0x10(%r12), %r14
shrq $0x4, %r14
movslq %r14d, %rax
cmpq %rax, %rbx
jg 0x27a4b
movq %rbp, %rdi
callq 0x16310
movsd 0x18(%rsp), %xmm0
ucomisd 0xa0a8c(%rip), %xmm0    # 0xc8380
movsd 0xa084c(%rip), %xmm1    # 0xc8148
jbe 0x27936
movq %rbp, %rdi
leaq 0x1f0(%rsp), %rsi
callq 0x49786
movapd %xmm0, %xmm1
movsd 0x18(%rsp), %xmm0
ucomisd %xmm0, %xmm1
mulsd 0xa1204(%rip), %xmm1    # 0xc8b28
divsd %xmm0, %xmm1
addsd 0xa0818(%rip), %xmm1    # 0xc8148
ja 0x27a4b
movsd 0xa080a(%rip), %xmm2    # 0xc8148
cmpb $0x0, 0xf(%rsp)
movsd %xmm1, 0x88(%rsp)
je 0x279ab
movslq %r14d, %rax
imulq $0x18, %rax, %rsi
addq 0x10(%rsp), %rsi
movq 0x68(%rsp), %rdi
callq 0x78bf6
andpd 0xa0a64(%rip), %xmm0    # 0xc83d0
xorpd %xmm2, %xmm2
movsd 0xa07d0(%rip), %xmm1    # 0xc8148
ucomisd %xmm0, %xmm1
jbe 0x27987
callq 0x134a0
movapd %xmm0, %xmm2
movsd 0x38(%rsp), %xmm0
ucomisd %xmm0, %xmm2
mulsd 0xa118f(%rip), %xmm2    # 0xc8b28
divsd %xmm0, %xmm2
addsd 0xa07a3(%rip), %xmm2    # 0xc8148
ja 0x27a4b
movsd %xmm2, 0x80(%rsp)
movsd 0x78(%r12), %xmm0
ucomisd 0xa09bd(%rip), %xmm0    # 0xc8380
movsd 0xa077d(%rip), %xmm0    # 0xc8148
jbe 0x279f4
leaq 0xf0(%rsp), %rdi
movq %rbp, %rsi
callq 0x495ca
mulsd 0xa114b(%rip), %xmm0    # 0xc8b30
divsd 0x78(%r12), %xmm0
addsd 0xa0754(%rip), %xmm0    # 0xc8148
movsd %xmm0, 0x78(%rsp)
movq 0x28(%rsp), %rax
movslq %r14d, %rcx
movq (%rax,%rcx,8), %r14
pushq $0x18
popq %rdi
callq 0x134f0
movsd 0x88(%rsp), %xmm0
movsd 0x80(%rsp), %xmm1
movq 0x70(%rsp), %rcx
movq %rcx, (%rax)
movq %r14, 0x8(%rax)
mulsd 0x78(%rsp), %xmm1
mulsd %xmm1, %xmm0
movsd %xmm0, 0x10(%rax)
leaq 0xb0(%rsp), %rdi
movq %rax, %rsi
callq 0x287b2
incq %r15
jmp 0x278ab
incq %rbx
movl 0x18(%r12), %eax
jmp 0x27841
cmpl $0x0, 0xb8(%rsp)
je 0x27d90
pushq $0x1
popq %rax
leaq 0xb0(%rsp), %r14
testl %eax, %eax
movq 0x10(%rsp), %rbx
je 0x27b09
movq %r14, %rdi
callq 0x288d4
movq %rax, %r13
movq (%rax), %rcx
movq 0x8(%rax), %rax
movq %rcx, %r12
movq (%rcx), %rcx
testq %rcx, %rcx
jne 0x27a98
movq %rax, %rbx
movq (%rax), %rax
testq %rax, %rax
jne 0x27aa3
cmpq %rbx, %r12
je 0x27afd
leaq 0x8(%r12), %rdi
leaq 0x8(%rbx), %rbp
movq %rbp, %rsi
callq 0x9aa76
leaq 0x18(%r12), %rdi
leaq 0x18(%rbx), %r15
movq %r15, %rsi
callq 0x646fe
movq %r12, (%rbx)
movq %rbp, %rdi
pushq $0x1
popq %rsi
callq 0x9a9cc
movq %r15, %rdi
leaq 0xfdefb(%rip), %rsi     # 0x1259e8
callq 0x177ee
movq %r13, %rdi
pushq $0x18
popq %rsi
callq 0x13500
movl 0xb8(%rsp), %eax
jmp 0x27a79
pushq $0x10
popq %rdi
callq 0x134f0
movq %rax, %r12
movq %rax, %rdi
callq 0x9a7ae
xorl %r15d, %r15d
movq %r12, 0x18(%rsp)
movslq 0x34(%rsp), %rax
cmpq %rax, %r15
jge 0x27dce
movq 0x28(%rsp), %rax
movq (%rax,%r15,8), %r14
movl 0x24(%rsp), %eax
cmpl %eax, 0x14(%r14)
jl 0x27b4b
cmpq $0x0, (%r14)
je 0x27b63
movq %r14, %rdi
callq 0x288ba
movq %r14, %rdi
pushq $0x58
popq %rsi
callq 0x13500
incq %r15
jmp 0x27b24
leaq 0x18(%r14), %rsi
leaq 0x90(%rsp), %rdi
callq 0x644c2
movslq 0x14(%r14), %rbp
movq %rbp, %rax
pushq $0x30
popq %rcx
mulq %rcx
movq %rax, %rbx
pushq $-0x1
popq %rax
cmovoq %rax, %rbx
movq %rbx, %rdi
callq 0x13030
movq %rax, %r13
testl %ebp, %ebp
je 0x27bb3
imulq $0x30, %rbp, %rbp
xorl %r12d, %r12d
leaq (%r12,%r13), %rdi
callq 0x29118
addq $0x30, %r12
cmpq %r12, %rbp
jne 0x27ba1
movapd 0x90(%rsp), %xmm0
movsd 0xa0(%rsp), %xmm1
movq %r13, %rax
addq $0x2f, %rax
xorl %ecx, %ecx
movq 0x18(%rsp), %r12
leaq 0x1f0(%rsp), %rbp
movslq 0x14(%r14), %rdx
cmpq %rdx, %rcx
jge 0x27c57
movq 0x8(%r14), %rdx
movq (%rdx,%rcx,8), %rdx
movq (%rdx), %rsi
movq 0x8(%rdx), %rdx
cvtss2sd 0x8(%rdx), %xmm2
addsd 0x20(%rsi), %xmm2
subsd %xmm1, %xmm2
cvtsd2ss %xmm2, %xmm2
cvtps2pd (%rdx), %xmm3
movupd 0x10(%rsi), %xmm4
addpd %xmm3, %xmm4
subpd %xmm0, %xmm4
cvtpd2ps %xmm4, %xmm3
movlpd %xmm3, -0x2f(%rax)
movss %xmm2, -0x27(%rax)
movb 0x2c(%rdx), %sil
movb %sil, -0x3(%rax)
movb 0x2d(%rdx), %sil
movb %sil, -0x2(%rax)
movb 0x2e(%rdx), %sil
movb %sil, -0x1(%rax)
movb 0x2f(%rdx), %dl
andb $0x2, %dl
movb (%rax), %sil
andb $-0x3, %sil
orb %dl, %sil
movb %sil, (%rax)
incq %rcx
addq $0x30, %rax
jmp 0x27bdb
movl $0xc8, %edi
callq 0x134f0
movq %rax, %rbx
movl 0x14(%r14), %edx
xorpd %xmm0, %xmm0
movq %rax, %rdi
movq %r13, %rsi
leaq 0x90(%rsp), %rcx
callq 0x29e7e
movq %r13, %rdi
callq 0x136a0
movl 0xc(%r12), %edx
movq %rbp, %rdi
leaq 0xa103d(%rip), %rsi     # 0xc8cd3
xorl %eax, %eax
callq 0x130e0
movl $0xd0, %edi
callq 0x134f0
movq %rax, %r13
movq %rax, %rdi
movq %rbp, %rsi
callq 0x33394
movl 0xc(%r12), %edx
leaq 0xf0(%rsp), %rdi
leaq 0xa100a(%rip), %rsi     # 0xc8cd3
xorl %eax, %eax
callq 0x130e0
movl $0xf8, %edi
callq 0x134f0
movq %rax, %rbp
movq %rax, %rdi
leaq 0xf0(%rsp), %rsi
callq 0x16360
movq %rbx, %rdi
callq 0x2b540
movq 0x60(%rsp), %rdi
movq (%rdi), %rax
movq %rbx, %rsi
callq *0x10(%rax)
movq (%r13), %rax
movq %r13, %rdi
movq %rbx, %rsi
callq *0x28(%rax)
movq 0x50(%rsp), %rdi
movq %r13, %rsi
movq 0x58(%rsp), %rdx
callq 0x34e72
movq %rbp, %rdi
movq %r13, %rsi
callq 0x17274
movq 0x48(%rsp), %rdi
movq (%rdi), %rax
movq %rbp, %rsi
movq 0x40(%rsp), %rdx
callq *0x18(%rax)
movq %r12, %rdi
movq %rbp, %rsi
callq 0x1784c
movq %r14, %rdi
callq 0x288ba
movq %r14, %rdi
pushq $0x58
popq %rsi
callq 0x13500
movl 0x8(%rbx), %edx
leaq 0xa0f70(%rip), %rdi     # 0xc8cd8
movl %r15d, %esi
xorl %eax, %eax
callq 0x13070
movq 0x10(%rsp), %rbx
jmp 0x27b5e
movq 0x10(%rsp), %rdi
testq %rdi, %rdi
je 0x27d8b
callq 0x136a0
xorl %r12d, %r12d
jmp 0x27de8
xorl %r15d, %r15d
pushq $0x58
popq %rbx
movslq 0x34(%rsp), %rax
cmpq %rax, %r15
jge 0x27dc6
movq 0x28(%rsp), %rax
movq (%rax,%r15,8), %r14
testq %r14, %r14
je 0x27db6
movq %r14, %rdi
callq 0x288ba
movq %r14, %rdi
movq %rbx, %rsi
callq 0x13500
incq %r15
jmp 0x27d96
xorl %r12d, %r12d
movq 0x10(%rsp), %rbx
testq %rbx, %rbx
je 0x27ddb
movq %rbx, %rdi
callq 0x136a0
leaq 0xb0(%rsp), %rdi
callq 0x28974
leaq 0x28(%rsp), %rdi
callq 0x9a81c
jmp 0x276c1
movq %rax, %r14
pushq $0x10
popq %rsi
movq %r12, %rdi
jmp 0x27e2b
jmp 0x27e71
movq %rax, %r14
movl $0xf8, %esi
movq %rbp, %rdi
jmp 0x27e2b
movq %rax, %r14
movl $0xd0, %esi
movq %r13, %rdi
jmp 0x27e2b
jmp 0x27e71
movq %rax, %r14
movl $0xc8, %esi
movq %rbx, %rdi
callq 0x13500
jmp 0x27e74
jmp 0x27e71
jmp 0x27e71
jmp 0x27e71
jmp 0x27e71
jmp 0x27e71
jmp 0x27e71
jmp 0x27e71
jmp 0x27e71
jmp 0x27e68
movq %rax, %r14
pushq $0x58
popq %rsi
movq %r13, %rdi
callq 0x13500
jmp 0x27e81
movq %rax, %r14
movq %r13, %rdi
movq %rbx, %rsi
callq 0x139f0
jmp 0x27e74
jmp 0x27e71
jmp 0x27e71
movq %rax, %r14
jmp 0x27e81
jmp 0x27e71
jmp 0x27e71
movq %rax, %r14
leaq 0xb0(%rsp), %rdi
callq 0x28974
leaq 0x28(%rsp), %rdi
callq 0x9a81c
movq %r14, %rdi
callq 0x138f0