// File: /workspace/github_cpp_makefile_star2_O2/tomfunkhouser[P]gaps/pkgs/R3Utils/R3SurfelViewer.cpp
// Function: gaps::R3SurfelViewer::DrawSurfels(int) const
void R3SurfelViewer::
DrawSurfels(int color_scheme) const
{
  // Check surfel visibility
  if (!SurfelVisibility()) return;

  // Set default lighting
  glDisable(GL_LIGHTING);

  // Set point size
  if (color_scheme != R3_SURFEL_VIEWER_COLOR_BY_PICK_INDEX) glPointSize(surfel_size);
  else glPointSize(0.01 * Viewport().Width() + 1);

#if (R3_SURFEL_VIEWER_DRAW_METHOD == R3_SURFEL_VIEWER_DRAW_WITH_VBO)
  if (!shape_draw_flags[R3_SURFEL_DISC_DRAW_FLAG]) {
    // Draw with VBO
    DrawVBO(color_scheme);
    glPointSize(1);    
    return;
  }
#endif

  // Draw resident nodes
  switch (color_scheme) {
  case R3_SURFEL_VIEWER_COLOR_BY_PICK_INDEX:
    for (int i = 0; i < resident_nodes.NNodes(); i++) {
      R3SurfelNode *node = resident_nodes.Node(i);
      LoadColor(color_scheme, NULL, NULL, node, NULL, NULL);
      DrawSurfels(node);
    }
    break;

  case R3_SURFEL_VIEWER_COLOR_BY_CURRENT_LABEL:
    for (int i = 0; i < resident_nodes.NNodes(); i++) {
      R3SurfelNode *node = resident_nodes.Node(i);
      R3SurfelObject *object = node->Object(TRUE, TRUE);
      while (object && object->Parent() && (object->Parent() != scene->RootObject())) object = object->Parent();
      R3SurfelLabel *label = (object) ? object->CurrentLabel() : NULL;
      const RNRgb& rgb = (label) ? label->Color() : RNwhite_rgb;
      RNLoadRgb(rgb);
      DrawSurfels(node);
    }
    break;

  case R3_SURFEL_VIEWER_COLOR_BY_GROUND_TRUTH_LABEL:
    for (int i = 0; i < resident_nodes.NNodes(); i++) {
      R3SurfelNode *node = resident_nodes.Node(i);
      R3SurfelObject *object = node->Object(TRUE, TRUE);
      while (object && object->Parent() && (object->Parent() != scene->RootObject())) object = object->Parent();
      R3SurfelLabel *label = (object) ? object->GroundTruthLabel() : NULL;
      const RNRgb& rgb = (label) ? label->Color() : RNwhite_rgb;
      RNLoadRgb(rgb);
      DrawSurfels(node);
    }
    break;

  case R3_SURFEL_VIEWER_COLOR_BY_SCAN:
    // Draw with colors based on nodes
    for (int i = 0; i < resident_nodes.NNodes(); i++) {
      R3SurfelNode *node = resident_nodes.Node(i);
      R3SurfelScan *scan = node->Scan();
      int scan_index = (scan) ? scan->SceneIndex() : 0;
      LoadColor(scan_index);
      DrawSurfels(node);
    }
    break;
    
  case R3_SURFEL_VIEWER_COLOR_BY_OBJECT:
    // Draw with colors based on nodes
    for (int i = 0; i < resident_nodes.NNodes(); i++) {
      R3SurfelNode *node = resident_nodes.Node(i);
      R3SurfelObject *object = node->Object(TRUE, TRUE);
      while (object && object->Parent() && (object->Parent() != scene->RootObject())) object = object->Parent();
      int object_index = (object) ? object->SceneIndex() : 0;
      LoadColor(object_index);
      DrawSurfels(node);
    }
    break;
    
  case R3_SURFEL_VIEWER_COLOR_BY_NODE:
    // Draw with colors based on nodes
    for (int i = 0; i < resident_nodes.NNodes(); i++) {
      R3SurfelNode *node = resident_nodes.Node(i);
      LoadColor(i);
      DrawSurfels(node);
    }
    break;
    
  case R3_SURFEL_VIEWER_COLOR_BY_BLOCK:
    // Draw with colors based on blocks
    for (int i = 0; i < resident_nodes.NNodes(); i++) {
      R3SurfelNode *node = resident_nodes.Node(i);
      if (!NodeVisibility(node)) continue;
      for (int j = 0; j < node->NBlocks(); j++) {
        R3SurfelBlock *block = node->Block(j);
        LoadColor(block->DatabaseIndex());
        block->Draw(shape_draw_flags, subsampling_factor);
      }
    }
    break;
    
  case R3_SURFEL_VIEWER_COLOR_BY_OBJECT_ATTRIBUTES:
    // Draw with colors based on flags encoding object attributes
    for (int i = 0; i < resident_nodes.NNodes(); i++) {
      R3SurfelNode *node = resident_nodes.Node(i);
      if (!NodeVisibility(node)) continue;
      R3SurfelObject *object = node->Object(TRUE, TRUE);
      while (object && object->Parent() && (object->Parent() != scene->RootObject())) object = object->Parent();
      RNFlags flags = (object) ? object->Flags() : RNFlags(0);
      LoadColor((int) flags);
      DrawSurfels(node);
    }
    break;
    
  case R3_SURFEL_VIEWER_COLOR_BY_SHADING:
    // Draw with colors based on shading
    glEnable(GL_LIGHTING);
    RNLoadRgb(0.8, 0.8, 0.8);
    for (int i = 0; i < resident_nodes.NNodes(); i++) {
      R3SurfelNode *node = resident_nodes.Node(i);
      DrawSurfels(node, R3_SURFEL_NORMAL_DRAW_FLAG);
    }
    glDisable(GL_LIGHTING);
    break;
    
  case R3_SURFEL_VIEWER_COLOR_BY_NORMAL:
    // Draw with colors based on normals
    for (int i = 0; i < resident_nodes.NNodes(); i++) {
      R3SurfelNode *node = resident_nodes.Node(i);
      if (!NodeVisibility(node)) continue;
      for (int j = 0; j < node->NBlocks(); j++) {
        R3SurfelBlock *block = node->Block(j);
        const R3Point& block_origin = block->PositionOrigin();
        glPushMatrix();
        glTranslated(block_origin.X(), block_origin.Y(), block_origin.Z());
        RNGrfxBegin(RN_GRFX_POINTS);
        for (int k = 0; k < block->NSurfels(); k += subsampling_factor) {
          const R3Surfel *surfel = block->Surfel(k);
          float r = 0.5F + 0.5F * surfel->NX();
          float g = 0.5F + 0.5F * surfel->NY();
          float b = 0.5F + 0.5F * surfel->NZ();
          RNLoadRgb(r, g, b);
          R3LoadPoint(surfel->PositionPtr());
        }
        RNGrfxEnd();
        glPopMatrix();
      }
    }
    break;
    
  case R3_SURFEL_VIEWER_COLOR_BY_Z:
  case R3_SURFEL_VIEWER_COLOR_BY_ELEVATION:
  case R3_SURFEL_VIEWER_COLOR_BY_SURFEL_LABEL:
  case R3_SURFEL_VIEWER_COLOR_BY_CONFIDENCE:
    // Draw with colors based on surfel attributes
    for (int i = 0; i < resident_nodes.NNodes(); i++) {
      R3SurfelNode *node = resident_nodes.Node(i);
      if (!NodeVisibility(node)) continue;
      for (int j = 0; j < node->NBlocks(); j++) {
        R3SurfelBlock *block = node->Block(j);
        const R3Point& block_origin = block->PositionOrigin();
        glPushMatrix();
        glTranslated(block_origin.X(), block_origin.Y(), block_origin.Z());
        RNGrfxBegin(RN_GRFX_POINTS);
        for (int k = 0; k < block->NSurfels(); k += subsampling_factor) {
          const R3Surfel *surfel = block->Surfel(k);
          if (color_scheme == R3_SURFEL_VIEWER_COLOR_BY_Z) {
            double z = block_origin.Z() + surfel->Z();
            double dz = (z > center_point.Z()) ? z - center_point.Z() : 0;
            double value = 0.5 * sqrt(dz);
            LoadColor(value);
          }
          else if (color_scheme == R3_SURFEL_VIEWER_COLOR_BY_ELEVATION) {
            double elevation = surfel->Elevation();
            double value = (elevation > 0) ? 0.5 * sqrt(elevation) : 0;
            LoadColor(value);
          }
          else if (color_scheme == R3_SURFEL_VIEWER_COLOR_BY_SURFEL_LABEL) {
            int label_identifier = surfel->Attribute() & 0xFF;
            LoadColor(label_identifier);
          }
          else if (color_scheme == R3_SURFEL_VIEWER_COLOR_BY_CONFIDENCE) {
            double confidence = ((surfel->Attribute() >> 8) & 0xFF) / 255.0;
            LoadColor(confidence);
          }
          R3LoadPoint(surfel->PositionPtr());
        }
        RNGrfxEnd();
        glPopMatrix();
      }
    }
    break;
    
  default:
    // Draw with RGB surfel colors
    for (int i = 0; i < resident_nodes.NNodes(); i++) {
      R3SurfelNode *node = resident_nodes.Node(i);
      if (!NodeVisibility(node)) continue;
      DrawSurfels(node, R3_SURFEL_COLOR_DRAW_FLAG);
    }
    break;
  }

  // Reset point size
  glPointSize(1);    
}

// Assembly:
cmpl $0x0, 0x2f0(%rdi)
je 0x2d267
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x18, %rsp
movl %esi, %r14d
movq %rdi, %rbx
movl $0xb50, %edi            # imm = 0xB50
callq 0x27220
cmpl $0xe, %r14d
jne 0x2d268
cvtsi2sdl 0x118(%rbx), %xmm0
mulsd 0xecdcb(%rip), %xmm0    # 0x11a028
addsd 0xecda3(%rip), %xmm0    # 0x11a008
jmp 0x2d270
retq
movsd 0x2e8(%rbx), %xmm0
cvtsd2ss %xmm0, %xmm0
callq 0x270a0
testb $0x10, 0x458(%rbx)
jne 0x2d2ac
movq (%rbx), %rax
movq %rbx, %rdi
movl %r14d, %esi
callq *0x370(%rax)
movss 0xecda7(%rip), %xmm0    # 0x11a040
addq $0x18, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
jmp 0x270a0
leal -0x1(%r14), %eax
cmpl $0xd, %eax
ja 0x2d8cd
leaq 0xed274(%rip), %rcx     # 0x11a534
movslq (%rcx,%rax,4), %rax
addq %rcx, %rax
jmpq *%rax
xorl %ecx, %ecx
movslq 0x24(%rbx), %rax
cmpq %rax, %rcx
jge 0x2d291
movq 0x18(%rbx), %rax
movq %rcx, 0x10(%rsp)
movq (%rax,%rcx,8), %r15
movq %rbx, %rdi
movq %r15, %rsi
callq 0x2c136
testl %eax, %eax
je 0x2d432
xorl %ebp, %ebp
movq %r15, 0x8(%rsp)
movslq 0x4c(%r15), %rax
cmpq %rax, %rbp
jge 0x2d432
movq 0x40(%r15), %rax
movq (%rax,%rbp,8), %r13
callq 0x27fe0
movsd 0x10(%r13), %xmm0
movsd 0x18(%r13), %xmm1
movsd 0x20(%r13), %xmm2
callq 0x279d0
xorl %r15d, %r15d
xorl %edi, %edi
callq 0x271a0
cmpl 0x8(%r13), %r15d
jge 0x2d41b
movslq %r15d, %rax
imulq $0x30, %rax, %r12
addq (%r13), %r12
cmpl $0xc, %r14d
je 0x2d3b7
cmpl $0xa, %r14d
je 0x2d38f
cmpl $0xb, %r14d
je 0x2d3a3
cmpl $0x2, %r14d
jne 0x2d407
cvtss2sd 0x8(%r12), %xmm1
addsd 0x20(%r13), %xmm1
movsd 0x2c0(%rbx), %xmm0
movapd %xmm1, %xmm2
subsd %xmm0, %xmm2
cmpltsd %xmm1, %xmm0
andpd %xmm2, %xmm0
jmp 0x2d3de
movzbl 0x24(%r12), %esi
movq (%rbx), %rax
movq %rbx, %rdi
callq *0x218(%rax)
jmp 0x2d407
movzbl 0x25(%r12), %eax
cvtsi2sd %eax, %xmm0
divsd 0xecfbb(%rip), %xmm0    # 0x11a370
jmp 0x2d3fb
movswl 0x2a(%r12), %eax
cvtsi2sd %eax, %xmm0
divsd 0xecfd7(%rip), %xmm0    # 0x11a3a0
cvtsd2ss %xmm0, %xmm1
xorpd %xmm0, %xmm0
ucomiss 0xed118(%rip), %xmm1    # 0x11a4f0
jbe 0x2d3fb
cvtss2sd %xmm1, %xmm0
ucomisd 0xecffa(%rip), %xmm0    # 0x11a3e0
jb 0x2d3ee
sqrtsd %xmm0, %xmm0
jmp 0x2d3f3
callq 0x27700
mulsd 0xecc15(%rip), %xmm0    # 0x11a010
movq (%rbx), %rax
movq %rbx, %rdi
callq *0x220(%rax)
movq %r12, %rdi
callq 0x27650
addl 0x48c(%rbx), %r15d
jmp 0x2d336
callq 0x27cc0
callq 0x28240
incq %rbp
movq 0x8(%rsp), %r15
jmp 0x2d2fb
movq 0x10(%rsp), %rcx
incq %rcx
jmp 0x2d2cb
xorl %r15d, %r15d
pushq $0x1
popq %rbp
leaq 0x17ad3c(%rip), %r12    # 0x1a8188
movslq 0x24(%rbx), %rax
cmpq %rax, %r15
jge 0x2d291
movq 0x18(%rbx), %rax
movq (%rax,%r15,8), %r14
movq %r14, %rdi
movl %ebp, %esi
movl %ebp, %edx
callq 0x3d228
movq 0x8(%rbx), %rcx
testq %rax, %rax
je 0x2d495
movq %rax, %rdi
movq 0x18(%rax), %rax
testq %rax, %rax
je 0x2d48b
movq 0x10(%rcx), %rdx
cmpq (%rdx), %rax
jne 0x2d471
callq 0x432fe
testq %rax, %rax
jne 0x2d49a
movq %r12, %rdi
jmp 0x2d4a1
movq %rax, %rdi
addq $0x70, %rdi
callq 0x32b06
movq (%rbx), %rax
movq %rbx, %rdi
movq %r14, %rsi
xorl %edx, %edx
callq *0x278(%rax)
incq %r15
jmp 0x2d44c
xorl %r15d, %r15d
pushq $0x1
popq %rbp
movslq 0x24(%rbx), %rax
cmpq %rax, %r15
jge 0x2d291
movq 0x18(%rbx), %rax
movq (%rax,%r15,8), %r14
movq %rbx, %rdi
movq %r14, %rsi
callq 0x2c136
testl %eax, %eax
je 0x2d537
movq %r14, %rdi
movl %ebp, %esi
movl %ebp, %edx
callq 0x3d228
movq 0x8(%rbx), %rcx
testq %rax, %rax
je 0x2d518
movq %rax, %rdx
movq 0x18(%rax), %rax
testq %rax, %rax
je 0x2d510
movq 0x10(%rcx), %rsi
cmpq (%rsi), %rax
jne 0x2d4f6
movl 0xe8(%rdx), %esi
jmp 0x2d51a
xorl %esi, %esi
movq (%rbx), %rax
movq %rbx, %rdi
callq *0x218(%rax)
movq (%rbx), %rax
movq %rbx, %rdi
movq %r14, %rsi
xorl %edx, %edx
callq *0x278(%rax)
incq %r15
jmp 0x2d4c2
xorl %r12d, %r12d
movslq 0x24(%rbx), %rax
cmpq %rax, %r12
jge 0x2d291
movq 0x18(%rbx), %rax
movq (%rax,%r12,8), %r14
movq %rbx, %rdi
movq %r14, %rsi
callq 0x2c136
testl %eax, %eax
je 0x2d5a4
xorl %r13d, %r13d
movslq 0x4c(%r14), %rax
cmpq %rax, %r13
jge 0x2d5a4
movq 0x40(%r14), %rax
movq (%rax,%r13,8), %r15
movl 0xa0(%r15), %esi
movq (%rbx), %rax
movq %rbx, %rdi
callq *0x218(%rax)
movq 0x458(%rbx), %rsi
movl 0x48c(%rbx), %edx
movq %r15, %rdi
callq 0x359ae
incq %r13
jmp 0x2d566
incq %r12
jmp 0x2d53f
xorl %r15d, %r15d
pushq $0x1
popq %rbp
movslq 0x24(%rbx), %rax
cmpq %rax, %r15
jge 0x2d291
movq 0x18(%rbx), %rax
movq (%rax,%r15,8), %r14
movq %r14, %rdi
movl %ebp, %esi
movl %ebp, %edx
callq 0x3d228
movq 0x8(%rbx), %rcx
testq %rax, %rax
je 0x2d5f3
movq %rax, %rdx
movq 0x18(%rax), %rax
testq %rax, %rax
je 0x2d5ee
movq 0x10(%rcx), %rsi
cmpq (%rsi), %rax
jne 0x2d5d4
movl 0x10(%rdx), %esi
jmp 0x2d5f5
xorl %esi, %esi
movq (%rbx), %rax
movq %rbx, %rdi
callq *0x218(%rax)
movq (%rbx), %rax
movq %rbx, %rdi
movq %r14, %rsi
xorl %edx, %edx
callq *0x278(%rax)
incq %r15
jmp 0x2d5af
xorl %ecx, %ecx
movslq 0x24(%rbx), %rax
cmpq %rax, %rcx
jge 0x2d291
movq 0x18(%rbx), %rax
movq %rcx, 0x8(%rsp)
movq (%rax,%rcx,8), %r14
movq %rbx, %rdi
movq %r14, %rsi
callq 0x2c136
testl %eax, %eax
je 0x2d70d
xorl %r13d, %r13d
movslq 0x4c(%r14), %rax
cmpq %rax, %r13
jge 0x2d70d
movq 0x40(%r14), %rax
movq (%rax,%r13,8), %rbp
callq 0x27fe0
movsd 0x10(%rbp), %xmm0
movsd 0x18(%rbp), %xmm1
movsd 0x20(%rbp), %xmm2
callq 0x279d0
xorl %r12d, %r12d
xorl %edi, %edi
callq 0x271a0
cmpl 0x8(%rbp), %r12d
jge 0x2d6fb
movq (%rbp), %rax
movslq %r12d, %rcx
imulq $0x30, %rcx, %rcx
leaq (%rax,%rcx), %r15
movswl 0x10(%rax,%rcx), %edx
cvtsi2ss %edx, %xmm0
movss 0xece39(%rip), %xmm3    # 0x11a4e0
mulss %xmm3, %xmm0
movswl 0x12(%rax,%rcx), %edx
cvtsi2ss %edx, %xmm1
movss 0xece30(%rip), %xmm4    # 0x11a4ec
mulss %xmm4, %xmm0
addss %xmm4, %xmm0
mulss %xmm3, %xmm1
mulss %xmm4, %xmm1
movswl 0x14(%rax,%rcx), %eax
cvtsi2ss %eax, %xmm2
addss %xmm4, %xmm1
mulss %xmm3, %xmm2
mulss %xmm4, %xmm2
addss %xmm4, %xmm2
callq 0x28390
movq %r15, %rdi
callq 0x27650
addl 0x48c(%rbx), %r12d
jmp 0x2d681
callq 0x27cc0
callq 0x28240
incq %r13
jmp 0x2d649
movq 0x8(%rsp), %rcx
incq %rcx
jmp 0x2d619
xorl %r15d, %r15d
pushq $0xe
popq %rbp
movslq 0x24(%rbx), %rax
cmpq %rax, %r15
jge 0x2d291
movq (%rbx), %rax
movq 0x18(%rbx), %rcx
movq (%rcx,%r15,8), %r14
andq $0x0, (%rsp)
movq %rbx, %rdi
movl %ebp, %esi
xorl %edx, %edx
xorl %ecx, %ecx
movq %r14, %r8
xorl %r9d, %r9d
callq *0x228(%rax)
movq (%rbx), %rax
movq %rbx, %rdi
movq %r14, %rsi
xorl %edx, %edx
callq *0x278(%rax)
incq %r15
jmp 0x2d720
xorl %r15d, %r15d
pushq $0x1
popq %rbp
movslq 0x24(%rbx), %rax
cmpq %rax, %r15
jge 0x2d291
movq 0x18(%rbx), %rax
movq (%rax,%r15,8), %r14
movq %r14, %rdi
movl %ebp, %esi
callq 0x3d2fe
testq %rax, %rax
je 0x2d797
movl 0x10(%rax), %esi
jmp 0x2d799
xorl %esi, %esi
movq (%rbx), %rax
movq %rbx, %rdi
callq *0x218(%rax)
movq (%rbx), %rax
movq %rbx, %rdi
movq %r14, %rsi
xorl %edx, %edx
callq *0x278(%rax)
incq %r15
jmp 0x2d76e
xorl %r15d, %r15d
pushq $0x1
popq %rbp
leaq 0x17a9c0(%rip), %r12    # 0x1a8188
movslq 0x24(%rbx), %rax
cmpq %rax, %r15
jge 0x2d291
movq 0x18(%rbx), %rax
movq (%rax,%r15,8), %r14
movq %r14, %rdi
movl %ebp, %esi
movl %ebp, %edx
callq 0x3d228
movq 0x8(%rbx), %rcx
testq %rax, %rax
je 0x2d811
movq %rax, %rdi
movq 0x18(%rax), %rax
testq %rax, %rax
je 0x2d807
movq 0x10(%rcx), %rdx
cmpq (%rdx), %rax
jne 0x2d7ed
callq 0x43340
testq %rax, %rax
jne 0x2d816
movq %r12, %rdi
jmp 0x2d81d
movq %rax, %rdi
addq $0x70, %rdi
callq 0x32b06
movq (%rbx), %rax
movq %rbx, %rdi
movq %r14, %rsi
xorl %edx, %edx
callq *0x278(%rax)
incq %r15
jmp 0x2d7c8
xorl %r14d, %r14d
movslq 0x24(%rbx), %rax
cmpq %rax, %r14
jge 0x2d291
movq (%rbx), %rax
movq 0x18(%rbx), %rcx
movq (%rcx,%r14,8), %r15
movq %rbx, %rdi
movl %r14d, %esi
callq *0x218(%rax)
movq (%rbx), %rax
movq %rbx, %rdi
movq %r15, %rsi
xorl %edx, %edx
callq *0x278(%rax)
incq %r14
jmp 0x2d83b
movl $0xb50, %edi            # imm = 0xB50
callq 0x27870
movsd 0xecb51(%rip), %xmm0    # 0x11a3d8
movaps %xmm0, %xmm1
movaps %xmm0, %xmm2
callq 0x27ef0
xorl %r15d, %r15d
pushq $0x2
popq %r14
movslq 0x24(%rbx), %rax
cmpq %rax, %r15
jge 0x2d8be
movq (%rbx), %rax
movq 0x18(%rbx), %rcx
movq (%rcx,%r15,8), %rsi
movq %rbx, %rdi
movq %r14, %rdx
callq *0x278(%rax)
incq %r15
jmp 0x2d899
movl $0xb50, %edi            # imm = 0xB50
callq 0x27220
jmp 0x2d291
xorl %r12d, %r12d
pushq $0x1
popq %r14
movslq 0x24(%rbx), %rax
cmpq %rax, %r12
jge 0x2d291
movq 0x18(%rbx), %rax
movq (%rax,%r12,8), %r15
movq %rbx, %rdi
movq %r15, %rsi
callq 0x2c136
testl %eax, %eax
je 0x2d90a
movq (%rbx), %rax
movq %rbx, %rdi
movq %r15, %rsi
movq %r14, %rdx
callq *0x278(%rax)
incq %r12
jmp 0x2d8d4
nop