// File: /workspace/github_cpp_makefile_star2_O3/tomfunkhouser[P]gaps/apps/conf2sfl/conf2sfl.cpp
// Function: LoadSurfels(gaps::R3SurfelScene*, gaps::RNArray<gaps::R3SegmentationPoint*> const&, gaps::R3SurfelObject*, gaps::R3SurfelNode*, char const*, gaps::R2Grid const&, gaps::R3Point const&)
static R3SurfelBlock *
LoadSurfels(R3SurfelScene *scene, const RNArray<R3SegmentationPoint *>& points,
  R3SurfelObject *parent_object, R3SurfelNode *parent_node,
  const char *node_name, const R2Grid& depth_image, const R3Point& origin)
{
  // Get convenient variables
  R3SurfelTree *tree = scene->Tree();
  R3SurfelDatabase *database = tree->Database();
  int cx = depth_image.XResolution()/2;
  int cy = depth_image.YResolution()/2;
  double rx = cx;
  double ry = cy;
  
  // Allocate array of surfels
  int nsurfels = 0;
  R3Surfel *surfels = new R3Surfel [ points.NEntries() ];
  if (!surfels) {
    RNFail("Unable to allocate surfels for %s\n", node_name);
    return NULL;
  }

  // Load surfels into array
  for (int i = 0; i < points.NEntries(); i++) {
    R3SegmentationPoint *point = points.Kth(i);
        
    // Check depth
    if (RNIsNegativeOrZero(point->depth)) continue;
    if ((max_depth > 0) && (point->depth > max_depth)) continue;

    // Get/check grid index
    int ix, iy;
    if (point->data_index < 0) continue;
    depth_image.IndexToIndices(point->data_index, ix, iy);

    // Check pixel stride
    if (ix % pixel_stride != 0) continue;
    if (iy % pixel_stride != 0) continue;

    // Check if outside elipse centered in image
    if (omit_corners) {
      int dx = (ix - cx) / rx;
      int dy = (iy - cy) / ry;
      if (dx*dx + dy*dy > 1) continue;
    }

    // Set position
    float px = (float) (point->position.X() - origin.X());
    float py = (float) (point->position.Y() - origin.Y());
    float pz = (float) (point->position.Z() - origin.Z());
    surfels[nsurfels].SetPosition(px, py, pz);

    // Set normal
    float nx = (float) point->normal.X();
    float ny = (float) point->normal.Y();
    float nz = (float) point->normal.Z();
    surfels[nsurfels].SetNormal(nx, ny, nz);

    // Set tangent
    float tx = (float) point->tangent.X();
    float ty = (float) point->tangent.Y();
    float tz = (float) point->tangent.Z();
    surfels[nsurfels].SetTangent(tx, ty, tz);

    // Set radius
    surfels[nsurfels].SetRadius(0, (float) point->radius1);
    surfels[nsurfels].SetRadius(1, (float) point->radius2);

    // Set depth
    surfels[nsurfels].SetDepth(point->depth);
    
    // Set color
    surfels[nsurfels].SetColor(point->color);

    // Set identifier
    unsigned int identifier = database->NSurfels() + nsurfels + 1;    
    surfels[nsurfels].SetIdentifier(identifier);

    // Set boundary flags
    if (point->boundary == R3_SURFEL_BORDER_BOUNDARY_FLAG) surfels[nsurfels].SetBorderBoundary(TRUE);
    else if (point->boundary == R3_SURFEL_SILHOUETTE_BOUNDARY_FLAG) surfels[nsurfels].SetSilhouetteBoundary(TRUE);
    else if (point->boundary == R3_SURFEL_SHADOW_BOUNDARY_FLAG) surfels[nsurfels].SetShadowBoundary(TRUE);

    // Increment number of surfels
    nsurfels++;
  }

  // Create block from array of surfels
  R3SurfelBlock *block = new R3SurfelBlock(surfels, nsurfels, origin);
  if (!block) {
    RNFail("Unable to allocate block\n");
    delete [] surfels;
    return NULL;
  }

  // Delete array of surfels
  delete [] surfels;

  // Insert block into database
  block->UpdateProperties();
  database->InsertBlock(block);

  // Create node
  R3SurfelNode *node = NULL;
  if (block && parent_node) {
    // Create node
    node = new R3SurfelNode(node_name);
    if (!node) {
      RNFail("Unable to allocate node for %s\n", node_name);
      delete block;
      return NULL;
    }
            
    // Insert node into tree
    tree->InsertNode(node, parent_node);
    node->InsertBlock(block);
    node->UpdateProperties();
  }

  // Creat object
  R3SurfelObject *object = NULL;
  if (node && parent_object) {
    // Create object
    object = new R3SurfelObject(node_name);
    if (!object) {
      RNFail("Unable to allocate object for %s\n", node_name);
      delete block;
      delete node;
      return NULL;
    }

    // Insert object
    scene->InsertObject(object, parent_object);
    object->InsertNode(node);
    object->UpdateProperties();

    // Create PCA object property
    R3SurfelObjectProperty *pca = new R3SurfelObjectProperty(R3_SURFEL_OBJECT_PCA_PROPERTY, object);
    scene->InsertObjectProperty(pca);
  }

  // Release block
  database->ReleaseBlock(block);

  // Return block
  return block;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x68, %rsp
movq %r9, %r13
movq %r8, 0x10(%rsp)
movq %rcx, 0x38(%rsp)
movq %rdx, 0x40(%rsp)
movq %rsi, %rbp
movq 0xa0(%rsp), %rax
movq %rax, 0x18(%rsp)
movq %rdi, 0x48(%rsp)
movq 0x8(%rdi), %rax
movq %rax, 0x30(%rsp)
movq 0x10(%rax), %rbx
movsd 0xc0(%r9), %xmm0
movapd %xmm0, 0x20(%rsp)
movslq 0xc(%rsi), %r14
movl $0x30, %ecx
movq %r14, %rax
mulq %rcx
movq $-0x1, %r15
cmovnoq %rax, %r15
movq %r15, %rdi
callq 0x15040
movq %rax, %r12
testq %r14, %r14
je 0x1a8ae
movq %rbx, 0x8(%rsp)
movdqa 0x20(%rsp), %xmm1
movdqa %xmm1, %xmm0
psrld $0x1f, %xmm0
paddd %xmm1, %xmm0
psrad $0x1, %xmm0
movdqa %xmm0, 0x20(%rsp)
cvtdq2pd %xmm0, %xmm0
movapd %xmm0, 0x50(%rsp)
shlq $0x4, %r14
leaq (%r14,%r14,2), %r14
xorl %ebx, %ebx
leaq (%r12,%rbx), %rdi
callq 0x2f8b8
addq $0x30, %rbx
cmpq %rbx, %r14
jne 0x1a54b
movslq 0xc(%rbp), %rdi
testq %rdi, %rdi
jle 0x1a8b3
movq (%rbp), %r8
leaq 0x1547d3(%rip), %rax    # 0x16ed48
movsd (%rax), %xmm0
movsd 0x1548af(%rip), %xmm1   # 0x16ee30
movl 0xc8(%r13), %r9d
movl 0x1540b1(%rip), %r10d   # 0x16e640
movb 0x1548a2(%rip), %r11b   # 0x16ee38
movq 0x18(%rsp), %rax
movupd (%rax), %xmm2
movsd 0x10(%rax), %xmm3
xorl %r14d, %r14d
xorpd %xmm4, %xmm4
cmpltpd %xmm1, %xmm4
movapd 0xeeab8(%rip), %xmm5    # 0x109070
movapd 0xeeac0(%rip), %xmm6    # 0x109080
movsd 0xeea60(%rip), %xmm7    # 0x109028
movsd 0xeea37(%rip), %xmm8    # 0x109008
xorps %xmm9, %xmm9
movss 0xeead2(%rip), %xmm13   # 0x1090b0
movsd 0xeea59(%rip), %xmm14   # 0x109040
xorl %r13d, %r13d
movq 0x8(%rsp), %rbx
movq (%r8,%r14,8), %r15
movsd (%r15), %xmm15
ucomisd %xmm15, %xmm0
jae 0x1a8a0
movapd %xmm1, %xmm10
cmpltpd %xmm15, %xmm10
andpd %xmm4, %xmm10
movd %xmm10, %eax
testb $0x1, %al
jne 0x1a8a0
movl 0xd4(%r15), %eax
testl %eax, %eax
js 0x1a8a0
cltd
idivl %r9d
movl %edx, %ecx
movl %eax, %esi
movl %edx, %eax
cltd
idivl %r10d
testl %edx, %edx
jne 0x1a8a0
movl %esi, %eax
cltd
idivl %r10d
testl %edx, %edx
jne 0x1a8a0
testb $0x1, %r11b
je 0x1a6a3
movd %esi, %xmm10
movd %ecx, %xmm12
punpckldq %xmm10, %xmm12  # xmm12 = xmm12[0],xmm10[0],xmm12[1],xmm10[1]
psubd 0x20(%rsp), %xmm12
cvtdq2pd %xmm12, %xmm10
divpd 0x50(%rsp), %xmm10
cvttpd2dq %xmm10, %xmm10
pshufd $0xf5, %xmm10, %xmm12   # xmm12 = xmm10[1,1,3,3]
pmuludq %xmm10, %xmm10
pmuludq %xmm12, %xmm12
paddd %xmm10, %xmm12
movd %xmm12, %eax
cmpl $0x1, %eax
ja 0x1a8a0
movsd 0x18(%r15), %xmm10
subsd %xmm3, %xmm10
cvtsd2ss %xmm10, %xmm10
movslq %r13d, %rax
leaq (%rax,%rax,2), %rcx
shlq $0x4, %rcx
movupd 0x8(%r15), %xmm12
subpd %xmm2, %xmm12
cvtpd2ps %xmm12, %xmm12
movlpd %xmm12, (%r12,%rcx)
movss %xmm10, 0x8(%r12,%rcx)
xorps %xmm10, %xmm10
cvtsd2ss 0x30(%r15), %xmm10
movupd 0x20(%r15), %xmm12
cvtpd2ps %xmm12, %xmm12
cvtss2sd %xmm10, %xmm11
mulsd %xmm7, %xmm11
addsd %xmm8, %xmm11
cvttsd2si %xmm11, %edx
leaq (%r12,%rcx), %rax
cvtps2pd %xmm12, %xmm11
mulpd %xmm5, %xmm11
addpd %xmm6, %xmm11
cvttpd2dq %xmm11, %xmm11
pshuflw $0xe8, %xmm11, %xmm11   # xmm11 = xmm11[0,2,2,3,4,5,6,7]
movd %xmm11, 0x10(%r12,%rcx)
movw %dx, 0x14(%r12,%rcx)
ucomiss %xmm9, %xmm12
jne 0x1a74b
jp 0x1a74b
shufps $0x55, %xmm12, %xmm12   # xmm12 = xmm12[1,1,1,1]
ucomiss %xmm9, %xmm12
jne 0x1a74b
jp 0x1a74b
ucomiss %xmm9, %xmm10
jne 0x1a74b
jnp 0x1a74f
orb $0x40, 0x2f(%rax)
xorps %xmm10, %xmm10
cvtsd2ss 0x48(%r15), %xmm10
movupd 0x38(%r15), %xmm11
cvtpd2ps %xmm11, %xmm12
xorps %xmm11, %xmm11
cvtss2sd %xmm10, %xmm11
mulsd %xmm7, %xmm11
addsd %xmm8, %xmm11
cvttsd2si %xmm11, %ecx
cvtps2pd %xmm12, %xmm11
mulpd %xmm5, %xmm11
addpd %xmm6, %xmm11
cvttpd2dq %xmm11, %xmm11
pshuflw $0xe8, %xmm11, %xmm11   # xmm11 = xmm11[0,2,2,3,4,5,6,7]
movd %xmm11, 0x16(%rax)
movw %cx, 0x1a(%rax)
ucomiss %xmm9, %xmm12
jne 0x1a7bc
jp 0x1a7bc
shufps $0x55, %xmm12, %xmm12   # xmm12 = xmm12[1,1,1,1]
ucomiss %xmm9, %xmm12
jne 0x1a7bc
jp 0x1a7bc
ucomiss %xmm9, %xmm10
jne 0x1a7bc
jnp 0x1a7c0
orb $0x1, 0x2f(%rax)
movupd 0x50(%r15), %xmm10
cvtpd2ps %xmm10, %xmm10
cvtps2pd %xmm10, %xmm10
mulpd 0xee8b8(%rip), %xmm10   # 0x109090
addpd %xmm6, %xmm10
cvtpd2ps %xmm10, %xmm10
movaps 0xee8b6(%rip), %xmm11   # 0x1090a0
minps %xmm10, %xmm11
cvttps2dq %xmm11, %xmm10
pshuflw $0xe8, %xmm10, %xmm10   # xmm10 = xmm10[0,2,2,3,4,5,6,7]
xorps %xmm11, %xmm11
cvtsd2ss %xmm15, %xmm11
cvtss2sd %xmm11, %xmm11
mulsd 0xee820(%rip), %xmm11   # 0x109030
addsd %xmm8, %xmm11
cvtsd2ss %xmm11, %xmm11
movaps %xmm13, %xmm12
minss %xmm11, %xmm12
cvttss2si %xmm12, %ecx
movd %xmm10, 0x1c(%rax)
movw %cx, 0x28(%rax)
movsd 0x78(%r15), %xmm10
mulsd %xmm14, %xmm10
cvttsd2si %xmm10, %ecx
movb %cl, 0x2c(%rax)
movsd 0x80(%r15), %xmm10
mulsd %xmm14, %xmm10
cvttsd2si %xmm10, %ecx
movb %cl, 0x2d(%rax)
movsd 0x88(%r15), %xmm10
mulsd %xmm14, %xmm10
cvttsd2si %xmm10, %ecx
movb %cl, 0x2e(%rax)
incl %r13d
movl 0x50(%rbx), %ecx
addl %r13d, %ecx
movl %ecx, 0x20(%rax)
movl 0xa4(%r15), %ecx
cmpl $0x30, %ecx
je 0x1a89b
cmpl $0x20, %ecx
je 0x1a897
cmpl $0x10, %ecx
jne 0x1a8a0
movb $0x10, %cl
jmp 0x1a89d
movb $0x20, %cl
jmp 0x1a89d
movb $0x30, %cl
orb %cl, 0x2f(%rax)
incq %r14
cmpq %r14, %rdi
jne 0x1a5ef
jmp 0x1a8bb
xorl %r13d, %r13d
jmp 0x1a8bb
xorl %r13d, %r13d
movq 0x8(%rsp), %rbx
movl $0xc8, %edi
callq 0x15610
movq %rax, %rbp
xorpd %xmm0, %xmm0
movq %rax, %rdi
movq %r12, %rsi
movl %r13d, %edx
movq 0x18(%rsp), %rcx
callq 0x30a02
movq %r12, %rdi
callq 0x15810
movq %rbp, %rdi
callq 0x32758
movq (%rbx), %rax
movq %rbx, %rdi
movq %rbp, %rsi
callq *0x10(%rax)
movq 0x38(%rsp), %r12
testq %r12, %r12
movq 0x48(%rsp), %r13
movq 0x40(%rsp), %r14
je 0x1a9c2
movl $0xd0, %edi
callq 0x15610
movq %rax, %r15
movq %rax, %rdi
movq 0x10(%rsp), %rsi
callq 0x38348
movq 0x30(%rsp), %rdi
movq %r15, %rsi
movq %r12, %rdx
callq 0x3995e
movq (%r15), %rax
movq %r15, %rdi
movq %rbp, %rsi
callq *0x28(%rax)
movq %r15, %rdi
callq 0x39378
testq %r14, %r14
je 0x1a9c2
movl $0xf8, %edi
callq 0x15610
movq %rax, %r12
movq %rax, %rdi
movq 0x10(%rsp), %rsi
callq 0x3f5b0
movq (%r13), %rax
movq %r13, %rdi
movq %r12, %rsi
movq %r14, %rdx
callq *0x18(%rax)
movq %r12, %rdi
movq %r15, %rsi
callq 0x4081e
movq %r12, %rdi
callq 0x40d6c
movl $0x30, %edi
callq 0x15610
movq %rax, %r14
movq %rax, %rdi
movl $0x1, %esi
movq %r12, %rdx
xorl %ecx, %ecx
xorl %r8d, %r8d
callq 0x40e0c
movq (%r13), %rax
movq %r13, %rdi
movq %r14, %rsi
callq *0x40(%rax)
movl 0xb4(%rbp), %eax
cmpl $0x1, %eax
jne 0x1a9ed
movq (%rbx), %rax
movq 0x8(%rbx), %rdx
movl 0x28(%rbx), %ecx
movq %rbx, %rdi
movq %rbp, %rsi
callq *0x90(%rax)
testl %eax, %eax
je 0x1aa00
movl 0xb4(%rbp), %eax
decl %eax
movl %eax, 0xb4(%rbp)
jne 0x1aa00
testb $0x2, 0x89(%rbp)
jne 0x1aa0f
addq $0x68, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
movq (%rbx), %rax
movq %rbx, %rdi
movq %rbp, %rsi
callq *0x18(%rax)
movq %rbp, %rdi
callq 0x311a6
movl $0xc8, %esi
movq %rbp, %rdi
addq $0x68, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
jmp 0x15620
movq %rax, %rbx
movl $0x30, %esi
movq %r14, %rdi
jmp 0x1aa70
movq %rax, %rbx
movl $0xf8, %esi
movq %r12, %rdi
jmp 0x1aa70
movq %rax, %rbx
movl $0xd0, %esi
movq %r15, %rdi
jmp 0x1aa70
movq %rax, %rbx
movl $0xc8, %esi
movq %rbp, %rdi
callq 0x15620
jmp 0x1aa85
movq %rax, %rbx
movq %r12, %rdi
movq %r15, %rsi
callq 0x15c20
movq %rbx, %rdi
callq 0x15ad0
nop