// File: tomfunkhouser[P]gaps/pkgs/R3Utils/R3Segmentation.cpp
// Function: gaps::R3SegmentationPointAffinity(gaps::R3SegmentationPoint const*, gaps::R3SegmentationPoint const*, double, double, double, double, double, double, double)
static RNScalar
R3SegmentationPointAffinity(
  const R3SegmentationPoint *point1, const R3SegmentationPoint *point2,
  double max_distance,
  double max_primitive_distance,
  double max_normal_angle,
  double max_color_difference,
  double max_timestamp_difference,
  double max_category_difference,
  double min_affinity = 0)
{
  // Initialize affinity
  RNScalar affinity = 1.0;

  // Check category difference
  if (max_category_difference > 0) {
    if (point1->category_identifier != point2->category_identifier) {
      RNScalar max_d = max_category_difference;
      RNScalar d = (point1->category_confidence <  point2->category_confidence) ? point1->category_confidence : point2->category_confidence;
      if (d > max_d) return 0;
      affinity *= 1.0 - (d/max_d);
      // affinity *= exp(d*d / (-2.0*max_d*max_d));
      if (affinity < min_affinity) return 0;
    }
  }

  // Check timestamp difference
  if (max_timestamp_difference > 0) {
    RNScalar max_d = max_timestamp_difference;
    RNLength d = fabs(point1->timestamp - point2->timestamp);
    if (d > max_d) return 0;
    // affinity *= exp(d*d / (-2.0*max_d*max_d));
    affinity *= 1.0 - (d/max_d);
    if (affinity < min_affinity) return 0;
  }

  // Check distance
  if (max_distance > 0) {
    RNScalar max_dd = max_distance * max_distance;
    RNScalar dd = R3SquaredDistance(point1->position, point2->position);
    if (dd > max_dd) return 0;
    // affinity *= exp(dd / (-2.0 * max_dd));
    affinity *= 1.0 - (dd/max_dd);
    if (affinity < min_affinity) return 0;
  }

  // Check primitive distance 
  if (max_primitive_distance > 0) {
    RNScalar max_d = max_primitive_distance;
    RNLength d1 = R3Distance(R3Plane(point1->position, point1->normal), point2->position);
    if (d1 > max_d) return 0;
    RNLength d2 = R3Distance(R3Plane(point2->position, point2->normal), point1->position);
    if (d2 > max_d) return 0;
    // affinity *= exp(d1*d1 / (-2.0 * 0.25 * max_d * max_d));
    // affinity *= exp(d2*d2 / (-2.0 * 0.25 * max_d * max_d));
    affinity *= 1.0 - (d1/max_d);
    affinity *= 1.0 - (d2/max_d);
    if (affinity < min_affinity) return 0;
  }

  // Check normal angle
  if (max_normal_angle > 0) {
    RNScalar max_a = max_normal_angle;
    RNScalar dot = point1->normal.Dot(point2->normal);
    RNAngle a = (dot > -1) ? ((dot < 1) ? acos(dot) : 0) : RN_PI;
    if (a > max_a) return 0;
    // affinity *= exp(a*a / (-2.0 * 0.25 * max_a * max_a));
    affinity *= 1.0 - (a/max_a);
    if (affinity < min_affinity) return 0;
  }

  // Check color difference
  if (max_color_difference > 0) {
    RNScalar max_d = max_color_difference;
    RNLength d = 0;
    d += fabs(point1->color.R() - point2->color.R());
    d += fabs(point1->color.G() - point2->color.G());
    d += fabs(point1->color.B() - point2->color.B());
    if (d > max_d) return 0;
    // affinity *= exp(d*d / (-2.0*0.25*max_d*max_d));
    affinity *= 1.0 - (d/max_d);
    if (affinity < min_affinity) return 0;
  }

  // Just checking
  assert(affinity <= 1);
  assert(affinity >= 0);

  // Return affinity
  return affinity;
}

// Assembly:
subq $0x138, %rsp            # imm = 0x138
movq %rdi, 0x128(%rsp)
movq %rsi, 0x120(%rsp)
movsd %xmm0, 0x118(%rsp)
movsd %xmm1, 0x110(%rsp)
movsd %xmm2, 0x108(%rsp)
movsd %xmm3, 0x100(%rsp)
movsd %xmm4, 0xf8(%rsp)
movsd %xmm5, 0xf0(%rsp)
movsd %xmm6, 0xe8(%rsp)
movsd 0x1b9e62(%rip), %xmm0   # 0x1fd020
movsd %xmm0, 0xe0(%rsp)
movsd 0xf0(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x432f6
movq 0x128(%rsp), %rax
movl 0x90(%rax), %eax
movq 0x120(%rsp), %rcx
cmpl 0x90(%rcx), %eax
je 0x432f4
movsd 0xf0(%rsp), %xmm0
movsd %xmm0, 0xd8(%rsp)
movq 0x128(%rsp), %rax
movsd 0x98(%rax), %xmm1
movq 0x120(%rsp), %rax
movsd 0x98(%rax), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x4324f
movq 0x128(%rsp), %rax
movsd 0x98(%rax), %xmm0
movsd %xmm0, 0x28(%rsp)
jmp 0x43265
movq 0x120(%rsp), %rax
movsd 0x98(%rax), %xmm0
movsd %xmm0, 0x28(%rsp)
movsd 0x28(%rsp), %xmm0
movsd %xmm0, 0xd0(%rsp)
movsd 0xd0(%rsp), %xmm0
ucomisd 0xd8(%rsp), %xmm0
jbe 0x43299
xorps %xmm0, %xmm0
movsd %xmm0, 0x130(%rsp)
jmp 0x438d8
movsd 0xd0(%rsp), %xmm1
divsd 0xd8(%rsp), %xmm1
movsd 0x1b9d6d(%rip), %xmm0   # 0x1fd020
subsd %xmm1, %xmm0
mulsd 0xe0(%rsp), %xmm0
movsd %xmm0, 0xe0(%rsp)
movsd 0xe0(%rsp), %xmm1
movsd 0xe8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x432f2
xorps %xmm0, %xmm0
movsd %xmm0, 0x130(%rsp)
jmp 0x438d8
jmp 0x432f4
jmp 0x432f6
movsd 0xf8(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x433cc
movsd 0xf8(%rsp), %xmm0
movsd %xmm0, 0xc8(%rsp)
movq 0x128(%rsp), %rax
movsd 0x68(%rax), %xmm0
movq 0x120(%rsp), %rax
subsd 0x68(%rax), %xmm0
movaps 0x1b9d31(%rip), %xmm1   # 0x1fd070
pand %xmm1, %xmm0
movsd %xmm0, 0xc0(%rsp)
movsd 0xc0(%rsp), %xmm0
ucomisd 0xc8(%rsp), %xmm0
jbe 0x43371
xorps %xmm0, %xmm0
movsd %xmm0, 0x130(%rsp)
jmp 0x438d8
movsd 0xc0(%rsp), %xmm1
divsd 0xc8(%rsp), %xmm1
movsd 0x1b9c95(%rip), %xmm0   # 0x1fd020
subsd %xmm1, %xmm0
mulsd 0xe0(%rsp), %xmm0
movsd %xmm0, 0xe0(%rsp)
movsd 0xe0(%rsp), %xmm1
movsd 0xe8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x433ca
xorps %xmm0, %xmm0
movsd %xmm0, 0x130(%rsp)
jmp 0x438d8
jmp 0x433cc
movsd 0x118(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x434a3
movsd 0x118(%rsp), %xmm0
mulsd 0x118(%rsp), %xmm0
movsd %xmm0, 0xb8(%rsp)
movq 0x128(%rsp), %rdi
addq $0x8, %rdi
movq 0x120(%rsp), %rsi
addq $0x8, %rsi
callq 0xc0520
movsd %xmm0, 0xb0(%rsp)
movsd 0xb0(%rsp), %xmm0
ucomisd 0xb8(%rsp), %xmm0
jbe 0x43448
xorps %xmm0, %xmm0
movsd %xmm0, 0x130(%rsp)
jmp 0x438d8
movsd 0xb0(%rsp), %xmm1
divsd 0xb8(%rsp), %xmm1
movsd 0x1b9bbe(%rip), %xmm0   # 0x1fd020
subsd %xmm1, %xmm0
mulsd 0xe0(%rsp), %xmm0
movsd %xmm0, 0xe0(%rsp)
movsd 0xe0(%rsp), %xmm1
movsd 0xe8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x434a1
xorps %xmm0, %xmm0
movsd %xmm0, 0x130(%rsp)
jmp 0x438d8
jmp 0x434a3
movsd 0x110(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x4361f
movsd 0x110(%rsp), %xmm0
movsd %xmm0, 0xa8(%rsp)
movq 0x128(%rsp), %rsi
addq $0x8, %rsi
movq 0x128(%rsp), %rdx
addq $0x20, %rdx
leaq 0x80(%rsp), %rdi
callq 0x128530
movq 0x120(%rsp), %rsi
addq $0x8, %rsi
leaq 0x80(%rsp), %rdi
callq 0x474b0
movsd %xmm0, 0xa0(%rsp)
movsd 0xa0(%rsp), %xmm0
ucomisd 0xa8(%rsp), %xmm0
jbe 0x43537
xorps %xmm0, %xmm0
movsd %xmm0, 0x130(%rsp)
jmp 0x438d8
movq 0x120(%rsp), %rsi
addq $0x8, %rsi
movq 0x120(%rsp), %rdx
addq $0x20, %rdx
leaq 0x58(%rsp), %rdi
callq 0x128530
movq 0x128(%rsp), %rsi
addq $0x8, %rsi
leaq 0x58(%rsp), %rdi
callq 0x474b0
movsd %xmm0, 0x78(%rsp)
movsd 0x78(%rsp), %xmm0
ucomisd 0xa8(%rsp), %xmm0
jbe 0x43597
xorps %xmm0, %xmm0
movsd %xmm0, 0x130(%rsp)
jmp 0x438d8
movsd 0xa0(%rsp), %xmm1
divsd 0xa8(%rsp), %xmm1
movsd 0x1b9a6f(%rip), %xmm0   # 0x1fd020
subsd %xmm1, %xmm0
mulsd 0xe0(%rsp), %xmm0
movsd %xmm0, 0xe0(%rsp)
movsd 0x78(%rsp), %xmm1
divsd 0xa8(%rsp), %xmm1
movsd 0x1b9a42(%rip), %xmm0   # 0x1fd020
subsd %xmm1, %xmm0
mulsd 0xe0(%rsp), %xmm0
movsd %xmm0, 0xe0(%rsp)
movsd 0xe0(%rsp), %xmm1
movsd 0xe8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x4361d
xorps %xmm0, %xmm0
movsd %xmm0, 0x130(%rsp)
jmp 0x438d8
jmp 0x4361f
movsd 0x108(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x43748
movsd 0x108(%rsp), %xmm0
movsd %xmm0, 0x50(%rsp)
movq 0x128(%rsp), %rdi
addq $0x20, %rdi
movq 0x120(%rsp), %rsi
addq $0x20, %rsi
callq 0x12c4c0
movsd %xmm0, 0x48(%rsp)
movsd 0x48(%rsp), %xmm0
movsd 0x1baa33(%rip), %xmm1   # 0x1fe0a8
ucomisd %xmm1, %xmm0
jbe 0x436b7
movsd 0x1b999d(%rip), %xmm0   # 0x1fd020
ucomisd 0x48(%rsp), %xmm0
jbe 0x4369e
movsd 0x48(%rsp), %xmm0
callq 0x166c0
movsd %xmm0, 0x20(%rsp)
jmp 0x436a9
xorps %xmm0, %xmm0
movsd %xmm0, 0x20(%rsp)
jmp 0x436a9
movsd 0x20(%rsp), %xmm0
movsd %xmm0, 0x18(%rsp)
jmp 0x436c8
leaq 0x1c0082(%rip), %rax    # 0x203740
movsd (%rax), %xmm0
movsd %xmm0, 0x18(%rsp)
movsd 0x18(%rsp), %xmm0
movsd %xmm0, 0x40(%rsp)
movsd 0x40(%rsp), %xmm0
ucomisd 0x50(%rsp), %xmm0
jbe 0x436f3
xorps %xmm0, %xmm0
movsd %xmm0, 0x130(%rsp)
jmp 0x438d8
movsd 0x40(%rsp), %xmm1
divsd 0x50(%rsp), %xmm1
movsd 0x1b9919(%rip), %xmm0   # 0x1fd020
subsd %xmm1, %xmm0
mulsd 0xe0(%rsp), %xmm0
movsd %xmm0, 0xe0(%rsp)
movsd 0xe0(%rsp), %xmm1
movsd 0xe8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x43746
xorps %xmm0, %xmm0
movsd %xmm0, 0x130(%rsp)
jmp 0x438d8
jmp 0x43748
movsd 0x100(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x438c6
movsd 0x100(%rsp), %xmm0
movsd %xmm0, 0x38(%rsp)
xorps %xmm0, %xmm0
movsd %xmm0, 0x30(%rsp)
movq 0x128(%rsp), %rdi
addq $0x78, %rdi
callq 0x1f700
movsd %xmm0, (%rsp)
movq 0x120(%rsp), %rdi
addq $0x78, %rdi
callq 0x1f700
movaps %xmm0, %xmm1
movsd (%rsp), %xmm0
subsd %xmm1, %xmm0
movaps 0x1b98c0(%rip), %xmm1   # 0x1fd070
pand %xmm1, %xmm0
addsd 0x30(%rsp), %xmm0
movsd %xmm0, 0x30(%rsp)
movq 0x128(%rsp), %rdi
addq $0x78, %rdi
callq 0x1f710
movsd %xmm0, 0x8(%rsp)
movq 0x120(%rsp), %rdi
addq $0x78, %rdi
callq 0x1f710
movaps %xmm0, %xmm1
movsd 0x8(%rsp), %xmm0
subsd %xmm1, %xmm0
movaps 0x1b9874(%rip), %xmm1   # 0x1fd070
pand %xmm1, %xmm0
addsd 0x30(%rsp), %xmm0
movsd %xmm0, 0x30(%rsp)
movq 0x128(%rsp), %rdi
addq $0x78, %rdi
callq 0x1f720
movsd %xmm0, 0x10(%rsp)
movq 0x120(%rsp), %rdi
addq $0x78, %rdi
callq 0x1f720
movaps %xmm0, %xmm1
movsd 0x10(%rsp), %xmm0
subsd %xmm1, %xmm0
movaps 0x1b9828(%rip), %xmm1   # 0x1fd070
pand %xmm1, %xmm0
addsd 0x30(%rsp), %xmm0
movsd %xmm0, 0x30(%rsp)
movsd 0x30(%rsp), %xmm0
ucomisd 0x38(%rsp), %xmm0
jbe 0x43874
xorps %xmm0, %xmm0
movsd %xmm0, 0x130(%rsp)
jmp 0x438d8
movsd 0x30(%rsp), %xmm1
divsd 0x38(%rsp), %xmm1
movsd 0x1b9798(%rip), %xmm0   # 0x1fd020
subsd %xmm1, %xmm0
mulsd 0xe0(%rsp), %xmm0
movsd %xmm0, 0xe0(%rsp)
movsd 0xe0(%rsp), %xmm1
movsd 0xe8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x438c4
xorps %xmm0, %xmm0
movsd %xmm0, 0x130(%rsp)
jmp 0x438d8
jmp 0x438c6
movsd 0xe0(%rsp), %xmm0
movsd %xmm0, 0x130(%rsp)
movsd 0x130(%rsp), %xmm0
addq $0x138, %rsp            # imm = 0x138
retq
nopl (%rax)