// File: 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:
subq $0x168, %rsp            # imm = 0x168
movq %rdi, 0x158(%rsp)
movq %rsi, 0x150(%rsp)
movq %rdx, 0x148(%rsp)
movq 0x158(%rsp), %rax
movq %rax, 0x60(%rsp)
cmpq $0x0, 0x88(%rax)
jne 0x26e67
movq $0x0, 0x160(%rsp)
jmp 0x274ab
movq 0x60(%rsp), %rax
cmpq $0x0, 0x90(%rax)
jne 0x26e84
movq 0x60(%rsp), %rdi
movq (%rdi), %rax
callq *0xf8(%rax)
movq 0x60(%rsp), %rax
cmpq $0x0, 0x90(%rax)
jne 0x26ea4
movq $0x0, 0x160(%rsp)
jmp 0x274ab
movq 0x150(%rsp), %rdi
callq 0x1af20
movsd 0x13f16f(%rip), %xmm1   # 0x166028
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x144(%rsp)
movq 0x150(%rsp), %rdi
callq 0x1af70
movsd 0x13f14b(%rip), %xmm1   # 0x166028
addsd %xmm1, %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x140(%rsp)
cmpl $0x0, 0x144(%rsp)
jl 0x26f1c
movq 0x60(%rsp), %rax
movl 0x144(%rsp), %ecx
movl %ecx, 0x5c(%rsp)
movq 0x90(%rax), %rdi
callq 0x27940
movl %eax, %ecx
movl 0x5c(%rsp), %eax
cmpl %ecx, %eax
jl 0x26f2d
movq $0x0, 0x160(%rsp)
jmp 0x274ab
cmpl $0x0, 0x140(%rsp)
jl 0x26f5d
movq 0x60(%rsp), %rax
movl 0x140(%rsp), %ecx
movl %ecx, 0x58(%rsp)
movq 0x90(%rax), %rdi
callq 0x27960
movl %eax, %ecx
movl 0x58(%rsp), %eax
cmpl %ecx, %eax
jl 0x26f6e
movq $0x0, 0x160(%rsp)
jmp 0x274ab
movq 0x60(%rsp), %rax
movq 0x90(%rax), %rdi
movl 0x144(%rsp), %esi
movl 0x140(%rsp), %edx
callq 0x1a060
movsd %xmm0, 0x138(%rsp)
movsd 0x138(%rsp), %xmm0
movsd 0x13f079(%rip), %xmm1   # 0x166020
ucomisd %xmm1, %xmm0
jne 0x26fc0
jp 0x26fc0
movq $0x0, 0x160(%rsp)
jmp 0x274ab
movsd 0x13f060(%rip), %xmm0   # 0x166028
addsd 0x138(%rsp), %xmm0
cvttsd2si %xmm0, %eax
movl %eax, 0x134(%rsp)
cmpl $0x0, 0x134(%rsp)
jl 0x2700c
movq 0x60(%rsp), %rax
movl 0x134(%rsp), %ecx
movl %ecx, 0x54(%rsp)
movq 0x88(%rax), %rdi
callq 0x19f00
movl %eax, %ecx
movl 0x54(%rsp), %eax
cmpl %ecx, %eax
jl 0x2701d
movq $0x0, 0x160(%rsp)
jmp 0x274ab
movq 0x60(%rsp), %rax
movq 0x88(%rax), %rdi
movl 0x134(%rsp), %esi
callq 0x22bb0
movq %rax, 0x128(%rsp)
cmpq $0x0, 0x148(%rsp)
je 0x2749b
movq 0x60(%rsp), %rax
movq 0x88(%rax), %rdi
movq 0x128(%rsp), %rsi
xorl %edx, %edx
callq 0x22be0
movq %rax, %rcx
movq 0x60(%rsp), %rax
movq %rcx, 0x120(%rsp)
movq 0x88(%rax), %rdi
movq 0x128(%rsp), %rsi
movl $0x1, %edx
callq 0x22be0
movq %rax, %rcx
movq 0x60(%rsp), %rax
movq %rcx, 0x118(%rsp)
movq 0x88(%rax), %rdi
movq 0x128(%rsp), %rsi
movl $0x2, %edx
callq 0x22be0
movq %rax, %rcx
movq 0x60(%rsp), %rax
movq %rcx, 0x110(%rsp)
movq 0x88(%rax), %rdi
movq 0x120(%rsp), %rsi
callq 0x22b90
movq %rax, %rsi
leaq 0x100(%rsp), %rdi
callq 0xcfdb0
movq 0x60(%rsp), %rax
movq 0x88(%rax), %rdi
movq 0x118(%rsp), %rsi
callq 0x22b90
movq %rax, %rsi
leaq 0xf0(%rsp), %rdi
callq 0xcfdb0
movq 0x60(%rsp), %rax
movq 0x88(%rax), %rdi
movq 0x110(%rsp), %rsi
callq 0x22b90
movq %rax, %rsi
leaq 0xe0(%rsp), %rdi
callq 0xcfdb0
leaq 0xd0(%rsp), %rdi
leaq 0xf0(%rsp), %rsi
leaq 0x100(%rsp), %rdx
callq 0xd0470
leaq 0xc0(%rsp), %rdi
leaq 0xe0(%rsp), %rsi
leaq 0x100(%rsp), %rdx
callq 0xd0470
leaq 0xd0(%rsp), %rdi
xorl %esi, %esi
callq 0x27fe0
movsd (%rax), %xmm0
movsd %xmm0, 0x40(%rsp)
leaq 0xc0(%rsp), %rdi
movl $0x1, %esi
callq 0x27fe0
movsd (%rax), %xmm0
movsd %xmm0, 0x48(%rsp)
leaq 0xd0(%rsp), %rdi
movl $0x1, %esi
callq 0x27fe0
movsd (%rax), %xmm0
movsd %xmm0, 0x38(%rsp)
leaq 0xc0(%rsp), %rdi
xorl %esi, %esi
callq 0x27fe0
movsd 0x38(%rsp), %xmm1
movsd 0x40(%rsp), %xmm0
movsd 0x48(%rsp), %xmm2
mulsd (%rax), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0xb8(%rsp)
movsd 0xb8(%rsp), %xmm0
leaq 0x18ac99(%rip), %rax    # 0x1b1ec0
movsd (%rax), %xmm1
callq 0x1a370
cmpl $0x0, %eax
je 0x27275
movq 0x148(%rsp), %rax
xorps %xmm0, %xmm0
movsd %xmm0, (%rax)
movq 0x148(%rsp), %rax
xorps %xmm0, %xmm0
movsd %xmm0, 0x8(%rax)
movq 0x148(%rsp), %rax
xorps %xmm0, %xmm0
movsd %xmm0, 0x10(%rax)
movq $0x0, 0x160(%rsp)
jmp 0x274ab
leaq 0xa8(%rsp), %rdi
callq 0xcfd70
movq 0x60(%rsp), %rdx
movq 0x150(%rsp), %rdi
leaq 0xa8(%rsp), %rsi
callq 0x1a700
cmpl $0x0, %eax
jne 0x272b2
movq $0x0, 0x160(%rsp)
jmp 0x274ab
leaq 0x98(%rsp), %rdi
leaq 0xf0(%rsp), %rsi
leaq 0xa8(%rsp), %rdx
callq 0xd0470
leaq 0x88(%rsp), %rdi
leaq 0xe0(%rsp), %rsi
leaq 0xa8(%rsp), %rdx
callq 0xd0470
leaq 0x78(%rsp), %rdi
leaq 0x100(%rsp), %rsi
leaq 0xa8(%rsp), %rdx
callq 0xd0470
leaq 0x98(%rsp), %rdi
xorl %esi, %esi
callq 0x27fe0
movsd (%rax), %xmm0
movsd %xmm0, 0x10(%rsp)
leaq 0x88(%rsp), %rdi
movl $0x1, %esi
callq 0x27fe0
movsd (%rax), %xmm0
movsd %xmm0, 0x18(%rsp)
leaq 0x98(%rsp), %rdi
movl $0x1, %esi
callq 0x27fe0
movsd (%rax), %xmm0
movsd %xmm0, 0x8(%rsp)
leaq 0x88(%rsp), %rdi
xorl %esi, %esi
callq 0x27fe0
movsd 0x8(%rsp), %xmm1
movsd 0x10(%rsp), %xmm0
movsd 0x18(%rsp), %xmm2
mulsd (%rax), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x70(%rsp)
leaq 0x88(%rsp), %rdi
xorl %esi, %esi
callq 0x27fe0
movsd (%rax), %xmm0
movsd %xmm0, 0x28(%rsp)
leaq 0x78(%rsp), %rdi
movl $0x1, %esi
callq 0x27fe0
movsd (%rax), %xmm0
movsd %xmm0, 0x30(%rsp)
leaq 0x88(%rsp), %rdi
movl $0x1, %esi
callq 0x27fe0
movsd (%rax), %xmm0
movsd %xmm0, 0x20(%rsp)
leaq 0x78(%rsp), %rdi
xorl %esi, %esi
callq 0x27fe0
movsd 0x20(%rsp), %xmm1
movsd 0x28(%rsp), %xmm0
movsd 0x30(%rsp), %xmm2
mulsd (%rax), %xmm1
movq %xmm1, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm1
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x68(%rsp)
movsd 0x70(%rsp), %xmm0
divsd 0xb8(%rsp), %xmm0
movq 0x148(%rsp), %rax
movsd %xmm0, (%rax)
movsd 0x68(%rsp), %xmm0
divsd 0xb8(%rsp), %xmm0
movq 0x148(%rsp), %rax
movsd %xmm0, 0x8(%rax)
movq 0x148(%rsp), %rax
movsd 0x13ebb3(%rip), %xmm0   # 0x166030
subsd (%rax), %xmm0
movq 0x148(%rsp), %rax
subsd 0x8(%rax), %xmm0
movq 0x148(%rsp), %rax
movsd %xmm0, 0x10(%rax)
movq 0x128(%rsp), %rax
movq %rax, 0x160(%rsp)
movq 0x160(%rsp), %rax
addq $0x168, %rsp            # imm = 0x168
retq
nopl (%rax,%rax)