// File: /workspace/github_cpp_makefile_star2_O1/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:
pushq %rbp
pushq %r14
pushq %rbx
subq $0x40, %rsp
movq %rsi, %r14
movq %rdi, %rbx
addq $0x8, %rsi
leaq 0x28(%rsp), %rdi
callq 0x9f4de
movq 0x130(%rbx), %rax
xorpd %xmm1, %xmm1
xorps %xmm2, %xmm2
testq %rax, %rax
je 0x2a6e8
movsd 0x50(%rax), %xmm2
testq %rax, %rax
je 0x2a6f2
movsd 0x68(%rax), %xmm1
xorpd %xmm4, %xmm4
xorpd %xmm5, %xmm5
testq %rax, %rax
je 0x2a704
movsd 0x70(%rax), %xmm5
testq %rax, %rax
je 0x2a70e
movsd 0x78(%rax), %xmm4
xorpd %xmm0, %xmm0
xorpd %xmm3, %xmm3
movsd %xmm3, 0x8(%rsp)
testq %rax, %rax
je 0x2a72c
movsd 0x58(%rax), %xmm3
movsd %xmm3, 0x8(%rsp)
testq %rax, %rax
je 0x2a736
movsd 0x60(%rax), %xmm0
testq %rax, %rax
je 0x2a748
cmpl $0x0, 0xcc(%rax)
setne %bpl
jmp 0x2a74a
xorl %ebp, %ebp
movsd %xmm2, 0x20(%rsp)
xorpd %xmm3, %xmm3
ucomisd %xmm3, %xmm1
movsd %xmm0, 0x18(%rsp)
jbe 0x2a7dd
movupd 0xf0(%rbx), %xmm2
movupd 0x78(%r14), %xmm0
subpd %xmm0, %xmm2
movapd 0xe0df6(%rip), %xmm3    # 0x10b570
andpd %xmm3, %xmm2
movsd %xmm4, 0x10(%rsp)
pshufd $0xee, %xmm2, %xmm4     # xmm4 = xmm2[2,3,2,3]
movsd 0x100(%rbx), %xmm0
subsd 0x88(%r14), %xmm0
addsd %xmm2, %xmm4
andpd %xmm3, %xmm0
addsd %xmm4, %xmm0
mulsd %xmm0, %xmm0
movsd 0xe1896(%rip), %xmm2    # 0x10c048
mulsd %xmm1, %xmm2
mulsd %xmm1, %xmm2
divsd %xmm2, %xmm0
movsd %xmm5, (%rsp)
callq 0x15b00
xorpd %xmm3, %xmm3
movsd (%rsp), %xmm5
movsd 0x10(%rsp), %xmm4
movapd %xmm0, %xmm2
jmp 0x2a7e5
movsd 0xe086b(%rip), %xmm2    # 0x10b050
ucomisd %xmm3, %xmm5
jbe 0x2a83a
movsd 0x108(%rbx), %xmm0
subsd 0x68(%r14), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
je 0x2a83a
mulsd %xmm0, %xmm0
movsd 0xe1829(%rip), %xmm1    # 0x10c038
mulsd %xmm5, %xmm1
mulsd %xmm5, %xmm1
divsd %xmm1, %xmm0
movsd %xmm2, (%rsp)
movsd %xmm4, 0x10(%rsp)
callq 0x15b00
movsd 0x10(%rsp), %xmm4
movsd (%rsp), %xmm2
mulsd %xmm0, %xmm2
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm4
jbe 0x2a893
movl 0x110(%rbx), %eax
cmpl 0x90(%r14), %eax
je 0x2a893
movsd 0x118(%rbx), %xmm0
minsd 0x98(%r14), %xmm0
mulsd %xmm0, %xmm0
movsd 0xe17c8(%rip), %xmm1    # 0x10c038
mulsd %xmm4, %xmm1
mulsd %xmm4, %xmm1
divsd %xmm1, %xmm0
movsd %xmm2, (%rsp)
callq 0x15b00
xorpd %xmm1, %xmm1
movsd (%rsp), %xmm2
mulsd %xmm0, %xmm2
movsd 0x8(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x2a900
movsd %xmm2, (%rsp)
leaq 0x40(%rbx), %rdi
leaq 0x28(%rsp), %rsi
callq 0x298b0
movq (%rbx), %rax
testq %rax, %rax
je 0x2a8d4
movsd (%rax), %xmm1
xorpd %xmm2, %xmm2
ucomisd %xmm2, %xmm1
seta %al
andb %bpl, %al
cmpb $0x1, %al
jne 0x2a8d4
divsd %xmm1, %xmm0
mulsd %xmm0, %xmm0
movsd 0xe1768(%rip), %xmm1    # 0x10c048
movsd 0x8(%rsp), %xmm2
mulsd %xmm2, %xmm1
mulsd %xmm2, %xmm1
divsd %xmm1, %xmm0
callq 0x15b00
movsd (%rsp), %xmm2
mulsd %xmm0, %xmm2
xorpd %xmm1, %xmm1
movsd 0x20(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x2a956
leaq 0x80(%rbx), %rdi
leaq 0x28(%rsp), %rsi
movsd %xmm2, (%rsp)
callq 0x6a618
mulsd %xmm0, %xmm0
movsd 0xe1716(%rip), %xmm1    # 0x10c048
movsd 0x20(%rsp), %xmm2
mulsd %xmm2, %xmm1
mulsd %xmm2, %xmm1
divsd %xmm1, %xmm0
callq 0x15b00
xorpd %xmm1, %xmm1
movsd (%rsp), %xmm2
mulsd %xmm0, %xmm2
movsd 0x18(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x2aa63
movl 0x40(%rbx), %eax
cmpl $0x3, %eax
je 0x2a9c7
cmpl $0x2, %eax
jne 0x2aa63
movsd %xmm2, (%rsp)
leaq 0xb0(%rbx), %rdi
addq $0x20, %r14
movq %r14, %rsi
callq 0xa01fe
andpd 0xe0bd9(%rip), %xmm0    # 0x10b570
movsd 0xe06b1(%rip), %xmm1    # 0x10b050
ucomisd %xmm0, %xmm1
leaq 0xeb20e(%rip), %rax     # 0x115bb8
movsd (%rax), %xmm2
jbe 0x2aa11
movsd %xmm2, 0x8(%rsp)
callq 0x155b0
movsd 0x8(%rsp), %xmm2
subsd %xmm0, %xmm2
jmp 0x2aa11
movsd %xmm2, (%rsp)
leaq 0xc8(%rbx), %rdi
addq $0x20, %r14
movq %r14, %rsi
callq 0xa01fe
ucomisd 0xe1669(%rip), %xmm0    # 0x10c050
jbe 0x2aa06
xorpd %xmm2, %xmm2
movsd 0xe065b(%rip), %xmm1    # 0x10b050
ucomisd %xmm0, %xmm1
jbe 0x2aa11
callq 0x155b0
movapd %xmm0, %xmm2
jmp 0x2aa11
leaq 0xeb19b(%rip), %rax     # 0x115ba8
movsd (%rax), %xmm2
movq (%rbx), %rax
testq %rax, %rax
je 0x2aa33
movsd (%rax), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
seta %al
andb %bpl, %al
cmpb $0x1, %al
jne 0x2aa33
divsd %xmm0, %xmm2
mulsd %xmm2, %xmm2
movsd 0xe1609(%rip), %xmm0    # 0x10c048
movsd 0x18(%rsp), %xmm1
mulsd %xmm1, %xmm0
mulsd %xmm1, %xmm0
divsd %xmm0, %xmm2
movapd %xmm2, %xmm0
callq 0x15b00
movsd (%rsp), %xmm2
mulsd %xmm0, %xmm2
movapd %xmm2, %xmm0
addq $0x40, %rsp
popq %rbx
popq %r14
popq %rbp
retq