// File: tomfunkhouser[P]gaps/pkgs/R3Utils/R3Segmentation.cpp
// Function: gaps::R3SegmentationCluster::Affinity(gaps::R3SegmentationPoint*) const
RNScalar R3SegmentationCluster::
Affinity(R3SegmentationPoint *point) const
{
  // Initialize affinity
  RNScalar affinity = 1.0;

  // Get useful variables
  R3Point position = point->position;
  double max_cluster_diameter = (segmentation) ? segmentation->max_cluster_diameter : 0;
  double max_cluster_color_difference = (segmentation) ? segmentation->max_cluster_color_difference : 0;
  double max_cluster_timestamp_difference = (segmentation) ? segmentation->max_cluster_timestamp_difference : 0;
  double max_cluster_category_difference = (segmentation) ? segmentation->max_cluster_category_difference : 0;
  double max_cluster_primitive_distance = (segmentation) ? segmentation->max_cluster_primitive_distance : 0;
  double max_cluster_normal_angle = (segmentation) ? segmentation->max_cluster_normal_angle : 0;
  RNBoolean scale_tolerances_with_depth = (segmentation) ? segmentation->scale_tolerances_with_depth : FALSE;

  // Check color difference
  if (max_cluster_color_difference > 0) {
    RNLength color_difference = 0;
    color_difference += fabs(color.R() - point->color.R());
    color_difference += fabs(color.G() - point->color.G());
    color_difference += fabs(color.B() - point->color.B());
    RNScalar color_difference_affinity = exp(color_difference * color_difference / (-2.0 * 0.25 * max_cluster_color_difference * max_cluster_color_difference));
    affinity *= color_difference_affinity;
  }

  // Check timestamp difference
  if (max_cluster_timestamp_difference > 0) {
    RNLength timestamp_difference = fabs(timestamp - point->timestamp);
    if (timestamp_difference > 0) {
      RNScalar timestamp_difference_affinity = exp(timestamp_difference * timestamp_difference / (-2.0 * max_cluster_timestamp_difference * max_cluster_timestamp_difference));
      affinity *= timestamp_difference_affinity;
    }
  }

  // Check category difference
  if (max_cluster_category_difference > 0) {
    if (category_identifier != point->category_identifier) {
      RNScalar min_category_confidence = (category_confidence <  point->category_confidence) ? category_confidence : point->category_confidence;
      RNScalar category_difference_affinity = exp(min_category_confidence * min_category_confidence / (-2.0 * max_cluster_category_difference * max_cluster_category_difference));
      affinity *= category_difference_affinity;
    }
  }

  // Check primitive distance 
  if (max_cluster_primitive_distance > 0) {
    RNLength primitive_distance = primitive.Distance(position);
    if (seed_point && (seed_point->depth > 0) && scale_tolerances_with_depth) primitive_distance  /= seed_point->depth;
    RNScalar primitive_distance_affinity = exp(primitive_distance * primitive_distance / (-2.0 * 0.25 * max_cluster_primitive_distance * max_cluster_primitive_distance));
    affinity *= primitive_distance_affinity;
  }

  // Check centroid distance
  if (max_cluster_diameter > 0) {
    RNLength centroid_distance = R3Distance(primitive.centroid, position);
    RNScalar centroid_distance_affinity = exp(centroid_distance * centroid_distance / (-2.0 * 0.25 * max_cluster_diameter * max_cluster_diameter));
    affinity *= centroid_distance_affinity;
  }

  // Check normal angle
  if (max_cluster_normal_angle > 0) {
    if (primitive.primitive_type == R3_SEGMENTATION_LINE_PRIMITIVE_TYPE) {
      RNScalar dot = fabs(primitive.line.Vector().Dot(point->normal));
      RNAngle normal_angle = (dot < 1) ? RN_PI_OVER_TWO - acos(dot) : RN_PI_OVER_TWO;
      if (seed_point && (seed_point->depth > 0) && scale_tolerances_with_depth) normal_angle /= seed_point->depth;
      RNScalar normal_angle_affinity = exp(normal_angle * normal_angle / (-2.0 * 0.25 * max_cluster_normal_angle * max_cluster_normal_angle));
      affinity *= normal_angle_affinity;
    }
    else if (primitive.primitive_type == R3_SEGMENTATION_PLANE_PRIMITIVE_TYPE) {
      RNScalar dot = primitive.plane.Normal().Dot(point->normal);
      RNAngle normal_angle = (dot > -1) ? ((dot < 1) ? acos(dot) : 0) : RN_PI;
      if (seed_point && (seed_point->depth > 0) && scale_tolerances_with_depth) normal_angle /= seed_point->depth;
      RNScalar normal_angle_affinity = exp(normal_angle * normal_angle / (-2.0 * 0.25 * max_cluster_normal_angle * max_cluster_normal_angle));
      affinity *= normal_angle_affinity;
    }
  }

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

  // Return affinity
  return affinity;
}

// Assembly:
subq $0x168, %rsp            # imm = 0x168
movq %rdi, 0x160(%rsp)
movq %rsi, 0x158(%rsp)
movq 0x160(%rsp), %rax
movq %rax, 0x78(%rsp)
movsd 0x1bed24(%rip), %xmm0   # 0x1fd020
movsd %xmm0, 0x150(%rsp)
movq 0x158(%rsp), %rsi
addq $0x8, %rsi
leaq 0x138(%rsp), %rdi
callq 0x12ad90
movq 0x78(%rsp), %rax
cmpq $0x0, 0x130(%rax)
je 0x3e346
movq 0x78(%rsp), %rax
movq 0x130(%rax), %rax
movsd 0x50(%rax), %xmm0
movsd %xmm0, 0x70(%rsp)
jmp 0x3e351
xorps %xmm0, %xmm0
movsd %xmm0, 0x70(%rsp)
jmp 0x3e351
movq 0x78(%rsp), %rax
movsd 0x70(%rsp), %xmm0
movsd %xmm0, 0x130(%rsp)
cmpq $0x0, 0x130(%rax)
je 0x3e388
movq 0x78(%rsp), %rax
movq 0x130(%rax), %rax
movsd 0x68(%rax), %xmm0
movsd %xmm0, 0x68(%rsp)
jmp 0x3e393
xorps %xmm0, %xmm0
movsd %xmm0, 0x68(%rsp)
jmp 0x3e393
movq 0x78(%rsp), %rax
movsd 0x68(%rsp), %xmm0
movsd %xmm0, 0x128(%rsp)
cmpq $0x0, 0x130(%rax)
je 0x3e3ca
movq 0x78(%rsp), %rax
movq 0x130(%rax), %rax
movsd 0x70(%rax), %xmm0
movsd %xmm0, 0x60(%rsp)
jmp 0x3e3d5
xorps %xmm0, %xmm0
movsd %xmm0, 0x60(%rsp)
jmp 0x3e3d5
movq 0x78(%rsp), %rax
movsd 0x60(%rsp), %xmm0
movsd %xmm0, 0x120(%rsp)
cmpq $0x0, 0x130(%rax)
je 0x3e40c
movq 0x78(%rsp), %rax
movq 0x130(%rax), %rax
movsd 0x78(%rax), %xmm0
movsd %xmm0, 0x58(%rsp)
jmp 0x3e417
xorps %xmm0, %xmm0
movsd %xmm0, 0x58(%rsp)
jmp 0x3e417
movq 0x78(%rsp), %rax
movsd 0x58(%rsp), %xmm0
movsd %xmm0, 0x118(%rsp)
cmpq $0x0, 0x130(%rax)
je 0x3e44e
movq 0x78(%rsp), %rax
movq 0x130(%rax), %rax
movsd 0x58(%rax), %xmm0
movsd %xmm0, 0x50(%rsp)
jmp 0x3e459
xorps %xmm0, %xmm0
movsd %xmm0, 0x50(%rsp)
jmp 0x3e459
movq 0x78(%rsp), %rax
movsd 0x50(%rsp), %xmm0
movsd %xmm0, 0x110(%rsp)
cmpq $0x0, 0x130(%rax)
je 0x3e490
movq 0x78(%rsp), %rax
movq 0x130(%rax), %rax
movsd 0x60(%rax), %xmm0
movsd %xmm0, 0x48(%rsp)
jmp 0x3e49b
xorps %xmm0, %xmm0
movsd %xmm0, 0x48(%rsp)
jmp 0x3e49b
movq 0x78(%rsp), %rax
movsd 0x48(%rsp), %xmm0
movsd %xmm0, 0x108(%rsp)
cmpq $0x0, 0x130(%rax)
je 0x3e4d1
movq 0x78(%rsp), %rax
movq 0x130(%rax), %rax
movl 0xcc(%rax), %eax
movl %eax, 0x44(%rsp)
jmp 0x3e4d9
xorl %eax, %eax
movl %eax, 0x44(%rsp)
jmp 0x3e4d9
movl 0x44(%rsp), %eax
movl %eax, 0x104(%rsp)
movsd 0x128(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x3e655
movq 0x78(%rsp), %rdi
xorps %xmm0, %xmm0
movsd %xmm0, 0xf8(%rsp)
addq $0xf0, %rdi
callq 0x1f700
movsd %xmm0, 0x28(%rsp)
movq 0x158(%rsp), %rdi
addq $0x78, %rdi
callq 0x1f700
movq 0x78(%rsp), %rdi
movaps %xmm0, %xmm1
movsd 0x28(%rsp), %xmm0
subsd %xmm1, %xmm0
movaps 0x1beb29(%rip), %xmm1   # 0x1fd070
pand %xmm1, %xmm0
addsd 0xf8(%rsp), %xmm0
movsd %xmm0, 0xf8(%rsp)
addq $0xf0, %rdi
callq 0x1f710
movsd %xmm0, 0x30(%rsp)
movq 0x158(%rsp), %rdi
addq $0x78, %rdi
callq 0x1f710
movq 0x78(%rsp), %rdi
movaps %xmm0, %xmm1
movsd 0x30(%rsp), %xmm0
subsd %xmm1, %xmm0
movaps 0x1bead7(%rip), %xmm1   # 0x1fd070
pand %xmm1, %xmm0
addsd 0xf8(%rsp), %xmm0
movsd %xmm0, 0xf8(%rsp)
addq $0xf0, %rdi
callq 0x1f720
movsd %xmm0, 0x38(%rsp)
movq 0x158(%rsp), %rdi
addq $0x78, %rdi
callq 0x1f720
movaps %xmm0, %xmm1
movsd 0x38(%rsp), %xmm0
subsd %xmm1, %xmm0
movaps 0x1bea8a(%rip), %xmm1   # 0x1fd070
pand %xmm1, %xmm0
addsd 0xf8(%rsp), %xmm0
movsd %xmm0, 0xf8(%rsp)
movsd 0xf8(%rsp), %xmm0
mulsd 0xf8(%rsp), %xmm0
movsd 0x1befda(%rip), %xmm1   # 0x1fd5f0
mulsd 0x128(%rsp), %xmm1
mulsd 0x128(%rsp), %xmm1
divsd %xmm1, %xmm0
callq 0x16ce0
movsd %xmm0, 0xf0(%rsp)
movsd 0xf0(%rsp), %xmm0
mulsd 0x150(%rsp), %xmm0
movsd %xmm0, 0x150(%rsp)
movsd 0x120(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x3e706
movq 0x78(%rsp), %rax
movsd 0x108(%rax), %xmm0
movq 0x158(%rsp), %rax
subsd 0x68(%rax), %xmm0
movaps 0x1be9e4(%rip), %xmm1   # 0x1fd070
pand %xmm1, %xmm0
movsd %xmm0, 0xe8(%rsp)
movsd 0xe8(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x3e704
movsd 0xe8(%rsp), %xmm0
mulsd 0xe8(%rsp), %xmm0
movsd 0x1bf9eb(%rip), %xmm1   # 0x1fe0b0
mulsd 0x120(%rsp), %xmm1
mulsd 0x120(%rsp), %xmm1
divsd %xmm1, %xmm0
callq 0x16ce0
movsd %xmm0, 0xe0(%rsp)
movsd 0xe0(%rsp), %xmm0
mulsd 0x150(%rsp), %xmm0
movsd %xmm0, 0x150(%rsp)
jmp 0x3e706
movsd 0x118(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x3e7f3
movq 0x78(%rsp), %rax
movl 0x110(%rax), %eax
movq 0x158(%rsp), %rcx
cmpl 0x90(%rcx), %eax
je 0x3e7f1
movq 0x78(%rsp), %rax
movsd 0x118(%rax), %xmm1
movq 0x158(%rsp), %rax
movsd 0x98(%rax), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x3e773
movq 0x78(%rsp), %rax
movsd 0x118(%rax), %xmm0
movsd %xmm0, 0x20(%rsp)
jmp 0x3e789
movq 0x158(%rsp), %rax
movsd 0x98(%rax), %xmm0
movsd %xmm0, 0x20(%rsp)
movsd 0x20(%rsp), %xmm0
movsd %xmm0, 0xd8(%rsp)
movsd 0xd8(%rsp), %xmm0
mulsd 0xd8(%rsp), %xmm0
movsd 0x1bf8fe(%rip), %xmm1   # 0x1fe0b0
mulsd 0x118(%rsp), %xmm1
mulsd 0x118(%rsp), %xmm1
divsd %xmm1, %xmm0
callq 0x16ce0
movsd %xmm0, 0xd0(%rsp)
movsd 0xd0(%rsp), %xmm0
mulsd 0x150(%rsp), %xmm0
movsd %xmm0, 0x150(%rsp)
jmp 0x3e7f3
movsd 0x110(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x3e8cd
movq 0x78(%rsp), %rdi
addq $0x40, %rdi
leaq 0x138(%rsp), %rsi
callq 0x3cac0
movq 0x78(%rsp), %rax
movsd %xmm0, 0xc8(%rsp)
cmpq $0x0, (%rax)
je 0x3e874
movq 0x78(%rsp), %rax
movq (%rax), %rax
movsd (%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x3e874
cmpl $0x0, 0x104(%rsp)
je 0x3e874
movq 0x78(%rsp), %rax
movq (%rax), %rax
movsd (%rax), %xmm1
movsd 0xc8(%rsp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, 0xc8(%rsp)
movsd 0xc8(%rsp), %xmm0
mulsd 0xc8(%rsp), %xmm0
movsd 0x1bed62(%rip), %xmm1   # 0x1fd5f0
mulsd 0x110(%rsp), %xmm1
mulsd 0x110(%rsp), %xmm1
divsd %xmm1, %xmm0
callq 0x16ce0
movsd %xmm0, 0xc0(%rsp)
movsd 0xc0(%rsp), %xmm0
mulsd 0x150(%rsp), %xmm0
movsd %xmm0, 0x150(%rsp)
movsd 0x130(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x3e95b
movq 0x78(%rsp), %rdi
addq $0x40, %rdi
addq $0x40, %rdi
leaq 0x138(%rsp), %rsi
callq 0xc0550
movsd %xmm0, 0xb8(%rsp)
movsd 0xb8(%rsp), %xmm0
mulsd 0xb8(%rsp), %xmm0
movsd 0x1becd4(%rip), %xmm1   # 0x1fd5f0
mulsd 0x130(%rsp), %xmm1
mulsd 0x130(%rsp), %xmm1
divsd %xmm1, %xmm0
callq 0x16ce0
movsd %xmm0, 0xb0(%rsp)
movsd 0xb0(%rsp), %xmm0
mulsd 0x150(%rsp), %xmm0
movsd %xmm0, 0x150(%rsp)
movsd 0x108(%rsp), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x3ec2b
movq 0x78(%rsp), %rax
cmpl $0x2, 0x40(%rax)
jne 0x3eacb
movq 0x78(%rsp), %rdi
addq $0x40, %rdi
addq $0x58, %rdi
callq 0x47560
movq %rax, %rdi
movq 0x158(%rsp), %rsi
addq $0x20, %rsi
callq 0x12c4c0
movaps 0x1be6c3(%rip), %xmm1   # 0x1fd070
pand %xmm1, %xmm0
movsd %xmm0, 0xa8(%rsp)
movsd 0x1be65e(%rip), %xmm0   # 0x1fd020
ucomisd 0xa8(%rsp), %xmm0
jbe 0x3ea01
leaq 0x1c4d7c(%rip), %rax    # 0x203750
movsd (%rax), %xmm0
movsd %xmm0, 0x10(%rsp)
movsd 0xa8(%rsp), %xmm0
callq 0x166c0
movaps %xmm0, %xmm1
movsd 0x10(%rsp), %xmm0
subsd %xmm1, %xmm0
movsd %xmm0, 0x18(%rsp)
jmp 0x3ea12
leaq 0x1c4d48(%rip), %rax    # 0x203750
movsd (%rax), %xmm0
movsd %xmm0, 0x18(%rsp)
movq 0x78(%rsp), %rax
movsd 0x18(%rsp), %xmm0
movsd %xmm0, 0xa0(%rsp)
cmpq $0x0, (%rax)
je 0x3ea6d
movq 0x78(%rsp), %rax
movq (%rax), %rax
movsd (%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x3ea6d
cmpl $0x0, 0x104(%rsp)
je 0x3ea6d
movq 0x78(%rsp), %rax
movq (%rax), %rax
movsd (%rax), %xmm1
movsd 0xa0(%rsp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, 0xa0(%rsp)
movsd 0xa0(%rsp), %xmm0
mulsd 0xa0(%rsp), %xmm0
movsd 0x1beb69(%rip), %xmm1   # 0x1fd5f0
mulsd 0x108(%rsp), %xmm1
mulsd 0x108(%rsp), %xmm1
divsd %xmm1, %xmm0
callq 0x16ce0
movsd %xmm0, 0x98(%rsp)
movsd 0x98(%rsp), %xmm0
mulsd 0x150(%rsp), %xmm0
movsd %xmm0, 0x150(%rsp)
jmp 0x3ec29
movq 0x78(%rsp), %rax
cmpl $0x3, 0x40(%rax)
jne 0x3ec27
movq 0x78(%rsp), %rdi
addq $0x40, %rdi
addq $0x88, %rdi
callq 0x2dca0
movq %rax, %rdi
movq 0x158(%rsp), %rsi
addq $0x20, %rsi
callq 0x12c4c0
movsd %xmm0, 0x90(%rsp)
movsd 0x90(%rsp), %xmm0
movsd 0x1bf58b(%rip), %xmm1   # 0x1fe0a8
ucomisd %xmm1, %xmm0
jbe 0x3eb64
movsd 0x1be4f5(%rip), %xmm0   # 0x1fd020
ucomisd 0x90(%rsp), %xmm0
jbe 0x3eb4c
movsd 0x90(%rsp), %xmm0
callq 0x166c0
movsd %xmm0, 0x8(%rsp)
jmp 0x3eb57
xorps %xmm0, %xmm0
movsd %xmm0, 0x8(%rsp)
jmp 0x3eb57
movsd 0x8(%rsp), %xmm0
movsd %xmm0, (%rsp)
jmp 0x3eb74
leaq 0x1c4bd5(%rip), %rax    # 0x203740
movsd (%rax), %xmm0
movsd %xmm0, (%rsp)
movq 0x78(%rsp), %rax
movsd (%rsp), %xmm0
movsd %xmm0, 0x88(%rsp)
cmpq $0x0, (%rax)
je 0x3ebce
movq 0x78(%rsp), %rax
movq (%rax), %rax
movsd (%rax), %xmm0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x3ebce
cmpl $0x0, 0x104(%rsp)
je 0x3ebce
movq 0x78(%rsp), %rax
movq (%rax), %rax
movsd (%rax), %xmm1
movsd 0x88(%rsp), %xmm0
divsd %xmm1, %xmm0
movsd %xmm0, 0x88(%rsp)
movsd 0x88(%rsp), %xmm0
mulsd 0x88(%rsp), %xmm0
movsd 0x1bea08(%rip), %xmm1   # 0x1fd5f0
mulsd 0x108(%rsp), %xmm1
mulsd 0x108(%rsp), %xmm1
divsd %xmm1, %xmm0
callq 0x16ce0
movsd %xmm0, 0x80(%rsp)
movsd 0x80(%rsp), %xmm0
mulsd 0x150(%rsp), %xmm0
movsd %xmm0, 0x150(%rsp)
jmp 0x3ec29
jmp 0x3ec2b
movsd 0x150(%rsp), %xmm0
addq $0x168, %rsp            # imm = 0x168
retq
nopl (%rax)