// File: /workspace/github_cpp_makefile_star2_O3/tomfunkhouser[P]gaps/pkgs/RGBD/RGBDSurface.cpp
// Function: gaps::RGBDSurface::SearchMeshFaceIndex(gaps::R2Point const&, double*) const
R3MeshFace *RGBDSurface::
SearchMeshFaceIndex(const R2Point& texture_position, double *barycentrics) const
{
  // Check mesh
  if (!mesh) return NULL;

  // Update mesh face index
  if (!mesh_face_index) ((RGBDSurface *) this)->UpdateMeshFaceIndex();
  if (!mesh_face_index) return NULL;

  // Check texture coordinates
  int texture_ix = (int) (texture_position.X() + 0.5);
  int texture_iy = (int) (texture_position.Y() + 0.5);
  if ((texture_ix < 0) || (texture_ix >= mesh_face_index->XResolution())) return NULL;
  if ((texture_iy < 0) || (texture_iy >= mesh_face_index->YResolution())) return NULL;

  // Look up face index
  RNScalar face_index_value = mesh_face_index->GridValue(texture_ix, texture_iy);
  if (face_index_value == R2_GRID_UNKNOWN_VALUE) return NULL;
  int face_index = (int) (face_index_value + 0.5);

  // Get face
  if ((face_index < 0) || (face_index >= mesh->NFaces())) return NULL;
  R3MeshFace *face = mesh->Face(face_index);

  // Get barycentric coordinates
  if (barycentrics) {
    // Get useful variables
    R3MeshVertex *va = mesh->VertexOnFace(face, 0);
    R3MeshVertex *vb = mesh->VertexOnFace(face, 1);
    R3MeshVertex *vc = mesh->VertexOnFace(face, 2);
    R2Point ta = mesh->VertexTextureCoords(va);
    R2Point tb = mesh->VertexTextureCoords(vb);
    R2Point tc = mesh->VertexTextureCoords(vc);
    R2Vector vab = tb - ta;
    R2Vector vac = tc - ta;
    RNScalar areaABC = vab[0]*vac[1] - vab[1]*vac[0];
    if (RNIsZero(areaABC)) {
      barycentrics[0] = 0;
      barycentrics[1] = 0;
      barycentrics[2] = 0;
      return NULL;
    }

    // Get/check surface position
    R2Point surface_position;
    if (!RGBDTransformTextureToSurface(texture_position, surface_position, this)) return NULL;

    // Compute barycentric coordinates
    R2Vector vpb = tb - surface_position;
    R2Vector vpc = tc - surface_position;
    R2Vector vpa = ta - surface_position;
    RNScalar areaPBC = vpb[0]*vpc[1] - vpb[1]*vpc[0];
    RNScalar areaPCA = vpc[0]*vpa[1] - vpc[1]*vpa[0];
    barycentrics[0] = areaPBC / areaABC;
    barycentrics[1] = areaPCA / areaABC;
    barycentrics[2] = 1 - barycentrics[0] - barycentrics[1];

    // I guess it's OK if the barycentric coordinates are (barely) outside face (since face_index grid is pixelated)
    // if (RNIsNegative(barycentrics[0]) || RNIsGreater(barycentrics[0], 1.0)) return NULL; 
    // if (RNIsNegative(barycentrics[1]) || RNIsGreater(barycentrics[1], 1.0)) return NULL; 
    // if (RNIsNegative(barycentrics[2]) || RNIsGreater(barycentrics[2], 1.0)) return NULL; 
  }

  // Return face
  return face;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0xa8, %rsp
cmpq $0x0, 0x88(%rdi)
je 0x1c97a
movq %rdx, %rbx
movq %rsi, %r12
movq %rdi, %r15
movq 0x90(%rdi), %rax
testq %rax, %rax
jne 0x1c914
movq (%r15), %rax
movq %r15, %rdi
callq *0xf8(%rax)
movq 0x90(%r15), %rax
testq %rax, %rax
je 0x1c97a
movsd 0x9e704(%rip), %xmm0    # 0xbb020
movsd (%r12), %xmm1
addsd %xmm0, %xmm1
cvttsd2si %xmm1, %ecx
testl %ecx, %ecx
js 0x1c97a
addsd 0x8(%r12), %xmm0
cvttsd2si %xmm0, %edx
cmpl %ecx, 0xc0(%rax)
setle %sil
testl %edx, %edx
sets %dil
orb %sil, %dil
jne 0x1c97a
cmpl %edx, 0xc4(%rax)
jle 0x1c97a
movq 0xb8(%rax), %rsi
imull 0xc8(%rax), %edx
addl %ecx, %edx
movslq %edx, %rax
movsd (%rsi,%rax,8), %xmm0
ucomisd 0x9e6a2(%rip), %xmm0    # 0xbb018
jne 0x1c992
jp 0x1c992
xorl %r14d, %r14d
movq %r14, %rax
addq $0xa8, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
addsd 0x9e686(%rip), %xmm0    # 0xbb020
cvttsd2si %xmm0, %eax
testl %eax, %eax
js 0x1c97a
movq 0x88(%r15), %rcx
cmpl %eax, 0x34(%rcx)
jle 0x1c97a
movq 0x28(%rcx), %rcx
movl %eax, %eax
movq (%rcx,%rax,8), %r14
testq %rbx, %rbx
je 0x1c97d
movq 0x8(%r14), %rsi
movq 0x10(%r14), %rbp
movq 0x18(%r14), %r13
addq $0x48, %rsi
movq %rsp, %rdi
callq 0x6f0e0
addq $0x48, %rbp
leaq 0x20(%rsp), %rdi
movq %rbp, %rsi
callq 0x6f0e0
addq $0x48, %r13
leaq 0x10(%rsp), %rdi
movq %r13, %rsi
callq 0x6f0e0
leaq 0x98(%rsp), %r13
movq %r13, %rdi
leaq 0x20(%rsp), %rsi
movq %rsp, %rdx
callq 0x6f313
leaq 0x88(%rsp), %rbp
movq %rbp, %rdi
leaq 0x10(%rsp), %rsi
movq %rsp, %rdx
callq 0x6f313
movsd (%r13), %xmm2
movsd (%rbp), %xmm0
mulsd 0x8(%r13), %xmm0
mulsd 0x8(%rbp), %xmm2
subsd %xmm0, %xmm2
leaq 0xe8290(%rip), %rax     # 0x104cd8
movsd (%rax), %xmm0
movapd 0x9e5fc(%rip), %xmm1    # 0xbb050
xorpd %xmm0, %xmm1
ucomisd %xmm1, %xmm2
jb 0x1ca79
ucomisd %xmm2, %xmm0
jb 0x1ca79
xorpd %xmm0, %xmm0
movupd %xmm0, (%rbx)
movq $0x0, 0x10(%rbx)
jmp 0x1c97a
movapd %xmm2, 0x30(%rsp)
leaq 0x40(%rsp), %r13
movq %r13, %rdi
callq 0x6f0d4
movq %r12, %rdi
movq %r13, %rsi
movq %r15, %rdx
callq 0x15edc
testl %eax, %eax
je 0x1c97a
leaq 0x60(%rsp), %r15
leaq 0x20(%rsp), %rsi
leaq 0x40(%rsp), %r12
movq %r15, %rdi
movq %r12, %rdx
callq 0x6f313
leaq 0x50(%rsp), %r13
leaq 0x10(%rsp), %rsi
movq %r13, %rdi
movq %r12, %rdx
callq 0x6f313
leaq 0x78(%rsp), %rbp
movq %rsp, %rsi
movq %rbp, %rdi
movq %r12, %rdx
callq 0x6f313
movapd (%r15), %xmm0
movapd (%r13), %xmm1
movapd %xmm1, %xmm2
shufpd $0x1, %xmm1, %xmm2      # xmm2 = xmm2[1],xmm1[0]
movapd %xmm0, %xmm3
unpckhpd %xmm1, %xmm3            # xmm3 = xmm3[1],xmm1[1]
movhpd (%rbp), %xmm1           # xmm1 = xmm1[0],mem[0]
mulpd %xmm3, %xmm1
movhpd 0x8(%rbp), %xmm0        # xmm0 = xmm0[0],mem[0]
mulpd %xmm2, %xmm0
subpd %xmm1, %xmm0
movapd 0x30(%rsp), %xmm1
unpcklpd %xmm1, %xmm1            # xmm1 = xmm1[0,0]
divpd %xmm1, %xmm0
movupd %xmm0, (%rbx)
movsd 0x9e4f8(%rip), %xmm1    # 0xbb028
subsd %xmm0, %xmm1
unpckhpd %xmm0, %xmm0            # xmm0 = xmm0[1,1]
subsd %xmm0, %xmm1
movsd %xmm1, 0x10(%rbx)
jmp 0x1c97d