// File: GouMinghao[P]graspnms/./graspnms.cpp
// Function: iou(double*, double*)
tuple_thresh iou(double *pg1, double *pg2)
{
    // 0: score
    // 1: width
    // 2: height
    // 3: depth
    // 4-12: rotation
    // 13-15: translation
    // 16: object id
    double sum_of_square = 0;
    for (int i = TRANSLATION_START_PLACE; i < TRANSLATION_START_PLACE + 3; i++)
    {
        sum_of_square += pow(
            *(pg1 + i) - *(pg2 + i),
            2.0);
    }
    double t = sqrt(sum_of_square);
    // translation
    Mat3x3 m1 = rotation_array_to_matrix(pg1 + ROTATION_START_PLACE);
    Mat3x3 m2 = rotation_array_to_matrix(pg2 + ROTATION_START_PLACE);
    // Mat3x3 m1 = viewpoint_params_to_matrix(*(pg1 + 5), *(pg1 + 6), *(pg1 + 7), *(pg1 + 8));
    // Mat3x3 m2 = viewpoint_params_to_matrix(*(pg2 + 5), *(pg2 + 6), *(pg2 + 7), *(pg2 + 8));
    double trace = m1.matmul(m2.T()).trace();
    trace = (trace > 3.0) ? 3.0 : trace;
    trace = (trace < -1.0) ? -1.0 : trace;
    double r = acos(0.5 * (trace - 1.0));
    // rotation
    return tuple_thresh(t, r);
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x1c0, %rsp            # imm = 0x1C0
movq %rdi, -0x18(%rbp)
movq %rsi, -0x20(%rbp)
xorps %xmm0, %xmm0
movsd %xmm0, -0x28(%rbp)
movl $0xd, -0x2c(%rbp)
cmpl $0x10, -0x2c(%rbp)
jge 0xcf94
movq -0x18(%rbp), %rax
movslq -0x2c(%rbp), %rcx
movsd (%rax,%rcx,8), %xmm0
movq -0x20(%rbp), %rax
movslq -0x2c(%rbp), %rcx
subsd (%rax,%rcx,8), %xmm0
movsd 0xf09e(%rip), %xmm1     # 0x1c018
callq 0xb5c0
addsd -0x28(%rbp), %xmm0
movsd %xmm0, -0x28(%rbp)
movl -0x2c(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x2c(%rbp)
jmp 0xcf52
movsd -0x28(%rbp), %xmm0
callq 0xb2d0
movsd %xmm0, -0x38(%rbp)
movq -0x18(%rbp), %rsi
addq $0x20, %rsi
leaq -0x80(%rbp), %rdi
callq 0xbcf0
movq -0x20(%rbp), %rsi
addq $0x20, %rsi
leaq -0xc8(%rbp), %rdi
callq 0xbcf0
leaq -0x160(%rbp), %rdi
leaq -0xc8(%rbp), %rsi
callq 0xbad0
movq -0x120(%rbp), %rcx
movq %rsp, %rax
movq %rcx, 0x40(%rax)
movups -0x160(%rbp), %xmm0
movups -0x150(%rbp), %xmm1
movups -0x140(%rbp), %xmm2
movups -0x130(%rbp), %xmm3
movups %xmm3, 0x30(%rax)
movups %xmm2, 0x20(%rax)
movups %xmm1, 0x10(%rax)
movups %xmm0, (%rax)
leaq -0x118(%rbp), %rdi
leaq -0x80(%rbp), %rsi
callq 0xbb60
leaq -0x118(%rbp), %rdi
callq 0xbab0
movsd %xmm0, -0xd0(%rbp)
movsd -0xd0(%rbp), %xmm0
movsd 0xefd8(%rip), %xmm1     # 0x1c020
ucomisd %xmm1, %xmm0
jbe 0xd060
movsd 0xefca(%rip), %xmm0     # 0x1c020
movsd %xmm0, -0x170(%rbp)
jmp 0xd070
movsd -0xd0(%rbp), %xmm0
movsd %xmm0, -0x170(%rbp)
movsd -0x170(%rbp), %xmm0
movsd %xmm0, -0xd0(%rbp)
movsd 0xefa0(%rip), %xmm0     # 0x1c028
ucomisd -0xd0(%rbp), %xmm0
jbe 0xd0a4
movsd 0xef8e(%rip), %xmm0     # 0x1c028
movsd %xmm0, -0x178(%rbp)
jmp 0xd0b4
movsd -0xd0(%rbp), %xmm0
movsd %xmm0, -0x178(%rbp)
movsd -0x178(%rbp), %xmm0
movsd %xmm0, -0xd0(%rbp)
movsd -0xd0(%rbp), %xmm0
movsd 0xef54(%rip), %xmm1     # 0x1c028
addsd %xmm1, %xmm0
movsd 0xef50(%rip), %xmm1     # 0x1c030
mulsd %xmm1, %xmm0
callq 0xb390
movsd %xmm0, -0x168(%rbp)
movsd -0x38(%rbp), %xmm0
movsd -0x168(%rbp), %xmm1
leaq -0x10(%rbp), %rdi
callq 0xc780
movsd -0x10(%rbp), %xmm0
movsd -0x8(%rbp), %xmm1
addq $0x1c0, %rsp            # imm = 0x1C0
popq %rbp
retq
nopw (%rax,%rax)