// File: /workspace/github_cpp_makefile_star2_O3/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 0x9e17e
movq 0x130(%rbx), %rax
testq %rax, %rax
je 0x2a46b
movsd 0x50(%rax), %xmm3
movsd 0x58(%rax), %xmm0
movsd %xmm0, 0x20(%rsp)
movsd 0x68(%rax), %xmm1
movsd 0x70(%rax), %xmm5
movsd 0x78(%rax), %xmm4
movsd 0x60(%rax), %xmm0
movsd %xmm0, 0x18(%rsp)
movl 0xcc(%rax), %ebp
xorpd %xmm0, %xmm0
ucomisd %xmm0, %xmm1
movsd %xmm3, 0x8(%rsp)
jbe 0x2a478
movupd 0xf0(%rbx), %xmm2
movupd 0x78(%r14), %xmm0
subpd %xmm0, %xmm2
movapd 0xdf16e(%rip), %xmm3    # 0x109570
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 0xdfc0e(%rip), %xmm2    # 0x10a048
mulsd %xmm1, %xmm2
mulsd %xmm1, %xmm2
divsd %xmm2, %xmm0
movsd %xmm5, (%rsp)
callq 0x15af0
movsd (%rsp), %xmm5
movsd 0x10(%rsp), %xmm4
movsd 0x8(%rsp), %xmm3
movapd %xmm0, %xmm2
xorpd %xmm0, %xmm0
jmp 0x2a480
movsd 0xdebdd(%rip), %xmm2    # 0x109050
jmp 0x2a712
movsd 0xdebd0(%rip), %xmm2    # 0x109050
ucomisd %xmm0, %xmm5
jbe 0x2a4db
movsd 0x108(%rbx), %xmm0
subsd 0x68(%r14), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
je 0x2a4db
mulsd %xmm0, %xmm0
movsd 0xdfb8e(%rip), %xmm1    # 0x10a038
mulsd %xmm5, %xmm1
mulsd %xmm5, %xmm1
divsd %xmm1, %xmm0
movsd %xmm2, (%rsp)
movsd %xmm4, 0x10(%rsp)
callq 0x15af0
movsd 0x10(%rsp), %xmm4
movsd 0x8(%rsp), %xmm3
movsd (%rsp), %xmm2
mulsd %xmm0, %xmm2
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm4
jbe 0x2a53a
movl 0x110(%rbx), %eax
cmpl 0x90(%r14), %eax
je 0x2a53a
movsd 0x118(%rbx), %xmm0
minsd 0x98(%r14), %xmm0
mulsd %xmm0, %xmm0
movsd 0xdfb27(%rip), %xmm1    # 0x10a038
mulsd %xmm4, %xmm1
mulsd %xmm4, %xmm1
divsd %xmm1, %xmm0
movsd %xmm2, (%rsp)
callq 0x15af0
xorpd %xmm1, %xmm1
movsd 0x8(%rsp), %xmm3
movsd (%rsp), %xmm2
mulsd %xmm0, %xmm2
movsd 0x20(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x2a5b1
movsd %xmm2, (%rsp)
leaq 0x40(%rbx), %rdi
leaq 0x28(%rsp), %rsi
callq 0x29574
movq (%rbx), %rax
testq %rax, %rax
je 0x2a57f
testl %ebp, %ebp
setne %cl
movsd (%rax), %xmm1
xorpd %xmm2, %xmm2
ucomisd %xmm2, %xmm1
seta %al
andb %cl, %al
cmpb $0x1, %al
jne 0x2a57f
divsd %xmm1, %xmm0
mulsd %xmm0, %xmm0
movsd 0xdfabd(%rip), %xmm1    # 0x10a048
movsd 0x20(%rsp), %xmm2
mulsd %xmm2, %xmm1
mulsd %xmm2, %xmm1
divsd %xmm1, %xmm0
callq 0x15af0
movsd (%rsp), %xmm2
mulsd %xmm0, %xmm2
movsd 0x8(%rsp), %xmm3
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm3
jbe 0x2a601
leaq 0x80(%rbx), %rdi
leaq 0x28(%rsp), %rsi
movsd %xmm2, (%rsp)
callq 0x69830
mulsd %xmm0, %xmm0
movsd 0xdfa6b(%rip), %xmm1    # 0x10a048
movsd 0x8(%rsp), %xmm2
mulsd %xmm2, %xmm1
mulsd %xmm2, %xmm1
divsd %xmm1, %xmm0
callq 0x15af0
xorpd %xmm1, %xmm1
movsd (%rsp), %xmm2
mulsd %xmm0, %xmm2
movsd 0x18(%rsp), %xmm0
ucomisd %xmm1, %xmm0
jbe 0x2a712
movl 0x40(%rbx), %eax
cmpl $0x3, %eax
je 0x2a672
cmpl $0x2, %eax
jne 0x2a712
movsd %xmm2, (%rsp)
leaq 0xb0(%rbx), %rdi
addq $0x20, %r14
movq %r14, %rsi
callq 0x9ee8e
andpd 0xdef2e(%rip), %xmm0    # 0x109570
movsd 0xdea06(%rip), %xmm1    # 0x109050
ucomisd %xmm0, %xmm1
leaq 0xe9563(%rip), %rax     # 0x113bb8
movsd (%rax), %xmm2
jbe 0x2a6bc
movsd %xmm2, 0x8(%rsp)
callq 0x155b0
movsd 0x8(%rsp), %xmm2
subsd %xmm0, %xmm2
jmp 0x2a6bc
movsd %xmm2, (%rsp)
leaq 0xc8(%rbx), %rdi
addq $0x20, %r14
movq %r14, %rsi
callq 0x9ee8e
ucomisd 0xdf9be(%rip), %xmm0    # 0x10a050
jbe 0x2a6b1
xorpd %xmm2, %xmm2
movsd 0xde9b0(%rip), %xmm1    # 0x109050
ucomisd %xmm0, %xmm1
jbe 0x2a6bc
callq 0x155b0
movapd %xmm0, %xmm2
jmp 0x2a6bc
leaq 0xe94f0(%rip), %rax     # 0x113ba8
movsd (%rax), %xmm2
movq (%rbx), %rax
testq %rax, %rax
je 0x2a6e2
testl %ebp, %ebp
setne %cl
movsd (%rax), %xmm0
xorpd %xmm1, %xmm1
ucomisd %xmm1, %xmm0
seta %al
andb %cl, %al
cmpb $0x1, %al
jne 0x2a6e2
divsd %xmm0, %xmm2
mulsd %xmm2, %xmm2
movsd 0xdf95a(%rip), %xmm0    # 0x10a048
movsd 0x18(%rsp), %xmm1
mulsd %xmm1, %xmm0
mulsd %xmm1, %xmm0
divsd %xmm0, %xmm2
movapd %xmm2, %xmm0
callq 0x15af0
movsd (%rsp), %xmm2
mulsd %xmm0, %xmm2
movapd %xmm2, %xmm0
addq $0x40, %rsp
popq %rbx
popq %r14
popq %rbp
retq
nop