// File: /workspace/github_cpp_makefile_star2_O1/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 $0x58, %rsp
movq %r9, %r13
movq %r8, 0x10(%rsp)
movq %rcx, 0x18(%rsp)
movq %rdx, 0x30(%rsp)
movq %rsi, %rbp
movq 0x90(%rsp), %rax
movq %rax, 0x20(%rsp)
movq %rdi, 0x38(%rsp)
movq 0x8(%rdi), %rax
movq %rax, 0x28(%rsp)
movq 0x10(%rax), %rax
movq %rax, 0x8(%rsp)
movq 0xc0(%r9), %xmm0
movdqa %xmm0, 0x40(%rsp)
movslq 0xc(%rsi), %r12
movl $0x30, %ecx
movq %r12, %rax
mulq %rcx
movq $-0x1, %r14
cmovnoq %rax, %r14
movq %r14, %rdi
callq 0x15040
movq %rax, %r15
testq %r12, %r12
je 0x1a5c0
shlq $0x4, %r12
leaq (%r12,%r12,2), %r12
xorl %ebx, %ebx
leaq (%r15,%rbx), %rdi
callq 0x2fd74
addq $0x30, %rbx
cmpq %rbx, %r12
jne 0x1a5ae
movslq 0xc(%rbp), %rdi
testq %rdi, %rdi
jle 0x1a927
movdqa 0x40(%rsp), %xmm1
movdqa %xmm1, %xmm0
psrld $0x1f, %xmm0
paddd %xmm1, %xmm0
psrad $0x1, %xmm0
cvtdq2pd %xmm0, %xmm1
movq (%rbp), %r8
leaq 0x156764(%rip), %rax    # 0x170d58
movsd (%rax), %xmm2
movsd 0x156840(%rip), %xmm3   # 0x170e40
movl 0xc8(%r13), %r9d
movl 0x156042(%rip), %r10d   # 0x170650
movb 0x156833(%rip), %r11b   # 0x170e48
movq 0x20(%rsp), %rax
movupd (%rax), %xmm4
movsd 0x10(%rax), %xmm5
xorl %r14d, %r14d
xorpd %xmm6, %xmm6
cmpltpd %xmm3, %xmm6
movapd 0xf0a48(%rip), %xmm8    # 0x10b080
movsd 0xf09e7(%rip), %xmm9    # 0x10b028
movsd 0xf09be(%rip), %xmm10   # 0x10b008
xorps %xmm11, %xmm11
xorl %r13d, %r13d
movsd 0xf09e6(%rip), %xmm12   # 0x10b040
movq 0x8(%rsp), %rbx
movq (%r8,%r14,8), %r12
movsd (%r12), %xmm14
ucomisd %xmm14, %xmm2
jae 0x1a919
movapd %xmm3, %xmm13
cmpltpd %xmm14, %xmm13
andpd %xmm6, %xmm13
movd %xmm13, %eax
testb $0x1, %al
jne 0x1a919
movl 0xd4(%r12), %eax
testl %eax, %eax
js 0x1a919
cltd
idivl %r9d
movl %edx, %ecx
movl %eax, %esi
movl %edx, %eax
cltd
idivl %r10d
testl %edx, %edx
jne 0x1a919
movl %esi, %eax
cltd
idivl %r10d
testl %edx, %edx
jne 0x1a919
testb $0x1, %r11b
je 0x1a711
movd %esi, %xmm13
movd %ecx, %xmm15
punpckldq %xmm13, %xmm15  # xmm15 = xmm15[0],xmm13[0],xmm15[1],xmm13[1]
psubd %xmm0, %xmm15
cvtdq2pd %xmm15, %xmm13
divpd %xmm1, %xmm13
cvttpd2dq %xmm13, %xmm13
pshufd $0xf5, %xmm13, %xmm15   # xmm15 = xmm13[1,1,3,3]
pmuludq %xmm13, %xmm13
pmuludq %xmm15, %xmm15
paddd %xmm13, %xmm15
movd %xmm15, %eax
cmpl $0x1, %eax
ja 0x1a919
movsd 0x18(%r12), %xmm13
subsd %xmm5, %xmm13
cvtsd2ss %xmm13, %xmm13
movslq %r13d, %rax
leaq (%rax,%rax,2), %rcx
shlq $0x4, %rcx
movupd 0x8(%r12), %xmm15
subpd %xmm4, %xmm15
cvtpd2ps %xmm15, %xmm15
movlpd %xmm15, (%r15,%rcx)
movss %xmm13, 0x8(%r15,%rcx)
xorps %xmm13, %xmm13
cvtsd2ss 0x30(%r12), %xmm13
movupd 0x20(%r12), %xmm15
cvtpd2ps %xmm15, %xmm15
xorps %xmm7, %xmm7
cvtss2sd %xmm13, %xmm7
mulsd %xmm9, %xmm7
addsd %xmm10, %xmm7
cvttsd2si %xmm7, %edx
leaq (%r15,%rcx), %rax
cvtps2pd %xmm15, %xmm7
mulpd 0xf08e8(%rip), %xmm7    # 0x10b070
addpd %xmm8, %xmm7
cvttpd2dq %xmm7, %xmm7
pshuflw $0xe8, %xmm7, %xmm7     # xmm7 = xmm7[0,2,2,3,4,5,6,7]
movd %xmm7, 0x10(%r15,%rcx)
movw %dx, 0x14(%r15,%rcx)
ucomiss %xmm11, %xmm15
jne 0x1a7c0
jp 0x1a7c0
shufps $0x55, %xmm15, %xmm15   # xmm15 = xmm15[1,1,1,1]
ucomiss %xmm11, %xmm15
jne 0x1a7c0
jp 0x1a7c0
ucomiss %xmm11, %xmm13
jne 0x1a7c0
jnp 0x1a7c4
orb $0x40, 0x2f(%rax)
xorps %xmm13, %xmm13
cvtsd2ss 0x48(%r12), %xmm13
movupd 0x38(%r12), %xmm7
cvtpd2ps %xmm7, %xmm15
xorps %xmm7, %xmm7
cvtss2sd %xmm13, %xmm7
mulsd %xmm9, %xmm7
addsd %xmm10, %xmm7
cvttsd2si %xmm7, %ecx
cvtps2pd %xmm15, %xmm7
mulpd 0xf0873(%rip), %xmm7    # 0x10b070
addpd %xmm8, %xmm7
cvttpd2dq %xmm7, %xmm7
pshuflw $0xe8, %xmm7, %xmm7     # xmm7 = xmm7[0,2,2,3,4,5,6,7]
movd %xmm7, 0x16(%rax)
movw %cx, 0x1a(%rax)
ucomiss %xmm11, %xmm15
jne 0x1a831
jp 0x1a831
shufps $0x55, %xmm15, %xmm15   # xmm15 = xmm15[1,1,1,1]
ucomiss %xmm11, %xmm15
jne 0x1a831
jp 0x1a831
ucomiss %xmm11, %xmm13
jne 0x1a831
jnp 0x1a835
orb $0x1, 0x2f(%rax)
movupd 0x50(%r12), %xmm7
cvtpd2ps %xmm7, %xmm7
cvtps2pd %xmm7, %xmm7
mulpd 0xf0845(%rip), %xmm7    # 0x10b090
addpd %xmm8, %xmm7
cvtpd2ps %xmm7, %xmm7
movaps 0xf0844(%rip), %xmm13   # 0x10b0a0
minps %xmm7, %xmm13
cvttps2dq %xmm13, %xmm7
pshuflw $0xe8, %xmm7, %xmm7     # xmm7 = xmm7[0,2,2,3,4,5,6,7]
xorps %xmm13, %xmm13
cvtsd2ss %xmm14, %xmm13
movd %xmm7, 0x1c(%rax)
xorps %xmm7, %xmm7
cvtss2sd %xmm13, %xmm7
mulsd 0xf07a8(%rip), %xmm7    # 0x10b030
addsd %xmm10, %xmm7
cvtsd2ss %xmm7, %xmm7
movss 0xf0816(%rip), %xmm13   # 0x10b0b0
minss %xmm7, %xmm13
cvttss2si %xmm13, %ecx
movw %cx, 0x28(%rax)
movsd 0x78(%r12), %xmm7
mulsd %xmm12, %xmm7
cvttsd2si %xmm7, %ecx
movb %cl, 0x2c(%rax)
movsd 0x80(%r12), %xmm7
mulsd %xmm12, %xmm7
cvttsd2si %xmm7, %ecx
movb %cl, 0x2d(%rax)
movsd 0x88(%r12), %xmm7
mulsd %xmm12, %xmm7
cvttsd2si %xmm7, %ecx
movb %cl, 0x2e(%rax)
movl 0x50(%rbx), %ecx
addl %r13d, %ecx
incl %ecx
movl %ecx, 0x20(%rax)
movl 0xa4(%r12), %ecx
cmpl $0x30, %ecx
je 0x1a911
cmpl $0x20, %ecx
je 0x1a90d
cmpl $0x10, %ecx
jne 0x1a916
movb $0x10, %cl
jmp 0x1a913
movb $0x20, %cl
jmp 0x1a913
movb $0x30, %cl
orb %cl, 0x2f(%rax)
incl %r13d
incq %r14
cmpq %r14, %rdi
jne 0x1a65f
jmp 0x1a92f
xorl %r13d, %r13d
movq 0x8(%rsp), %rbx
movl $0xc8, %edi
callq 0x15610
movq %rax, %rbp
pxor %xmm0, %xmm0
movq %rax, %rdi
movq %r15, %rsi
movl %r13d, %edx
movq 0x20(%rsp), %rcx
callq 0x30eb6
movq %r15, %rdi
callq 0x15820
movq %rbp, %rdi
callq 0x32b90
movq (%rbx), %rax
movq %rbx, %rdi
movq %rbp, %rsi
callq *0x10(%rax)
cmpq $0x0, 0x18(%rsp)
movq 0x38(%rsp), %r13
movq 0x30(%rsp), %r14
je 0x1a9c3
movl $0xd0, %edi
callq 0x15610
movq %rax, %r15
movq %rax, %rdi
movq 0x10(%rsp), %rsi
callq 0x38720
movq 0x28(%rsp), %rdi
movq %r15, %rsi
movq 0x18(%rsp), %rdx
callq 0x39d6a
movq (%r15), %rax
movq %r15, %rdi
movq %rbp, %rsi
callq *0x28(%rax)
movq %r15, %rdi
callq 0x397c6
jmp 0x1a9c6
xorl %r15d, %r15d
testq %r15, %r15
sete %al
testq %r14, %r14
sete %cl
orb %al, %cl
jne 0x1aa42
movl $0xf8, %edi
callq 0x15610
movq %rax, %r12
movq %rax, %rdi
movq 0x10(%rsp), %rsi
callq 0x3f9c4
movq (%r13), %rax
movq %r13, %rdi
movq %r12, %rsi
movq %r14, %rdx
callq *0x18(%rax)
movq %r12, %rdi
movq %r15, %rsi
callq 0x40c92
movq %r12, %rdi
callq 0x41204
movl $0x30, %edi
callq 0x15610
movq %rax, %r14
movq %rax, %rdi
movl $0x1, %esi
movq %r12, %rdx
xorl %ecx, %ecx
xorl %r8d, %r8d
callq 0x4129c
movq (%r13), %rax
movq %r13, %rdi
movq %r14, %rsi
callq *0x40(%rax)
cmpl $0x1, 0xb4(%rbp)
jne 0x1aa65
movq (%rbx), %rax
movq 0x8(%rbx), %rdx
movl 0x28(%rbx), %ecx
movq %rbx, %rdi
movq %rbp, %rsi
callq *0x90(%rax)
testl %eax, %eax
je 0x1aa76
decl 0xb4(%rbp)
jne 0x1aa76
testb $0x2, 0x89(%rbp)
jne 0x1aa85
addq $0x58, %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 0x3165a
movl $0xc8, %esi
movq %rbp, %rdi
addq $0x58, %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 0x1aae6
movq %rax, %rbx
movl $0xf8, %esi
movq %r12, %rdi
jmp 0x1aae6
movq %rax, %rbx
movl $0xd0, %esi
movq %r15, %rdi
jmp 0x1aae6
movq %rax, %rbx
movl $0xc8, %esi
movq %rbp, %rdi
callq 0x15620
jmp 0x1aafb
movq %rax, %rbx
movq %r15, %rdi
movq %r14, %rsi
callq 0x15c30
movq %rbx, %rdi
callq 0x15ae0
nop