// File: /workspace/github_cpp_makefile_star2_O2/jingdao[P]PointCloudApp/pcd.cpp
// Function: PCD::segmentPlane(int, float, float)
PCD::Plane PCD::segmentPlane(int iter,float threshold,float inlierRatio) {
	Plane bestPlane = {0,0,0,0};
	int maxInliers = 0;
	int optimumInliers = inlierRatio * numPoints;
	int i;
	for (i=0;i<iter;i++) {
		//Pick 3 points
		int p0 = rand() % numPoints;
		int p1 = rand() % numPoints;
		int p2 = rand() % numPoints;
		float x[3] = {float_data[p0*4], float_data[p1*4], float_data[p2*4]};
		float y[3] = {float_data[p0*4+1], float_data[p1*4+1], float_data[p2*4+1]};
		float z[3] = {float_data[p0*4+2], float_data[p1*4+2], float_data[p2*4+2]};
		Plane currentPlane = {
			(y[1]-y[0])*(z[2]-z[0]) - (y[2]-y[0])*(z[1]-z[0]),
			(z[1]-z[0])*(x[2]-x[0]) - (z[2]-z[0])*(x[1]-x[0]),
			(x[1]-x[0])*(y[2]-y[0]) - (x[2]-x[0])*(y[1]-y[0]),
			0
		};
		currentPlane.d = -(currentPlane.a * x[0] + currentPlane.b * y[0] + currentPlane.c * z[0]);
		if (currentPlane.a == 0 && currentPlane.b == 0 && currentPlane.c ==0 )
			continue; //picked collinear points
		float distanceThreshold = threshold * sqrt(
			currentPlane.a * currentPlane.a + 
			currentPlane.b * currentPlane.b + 
			currentPlane.c * currentPlane.c
			);
		int numInliers = 0;
		for (int j=0;j<numPoints;j++) {
			if ( fabs( currentPlane.a * float_data[j * 4] +
				 currentPlane.b * float_data[j * 4 + 1] +
				 currentPlane.c * float_data[j * 4 + 2] +
				 currentPlane.d )
				 < distanceThreshold)	
				numInliers++;
		}
		if (numInliers > maxInliers) {
			maxInliers = numInliers;
			bestPlane = currentPlane;
			if (maxInliers > optimumInliers)
				break;
		}
//		printf("Current plane: %f %f %f %f %d\n",currentPlane.a,currentPlane.b,currentPlane.c,currentPlane.d,numInliers);
	}
	printf("RANSAC: %d iters\n",i);
	return bestPlane;
}

// Assembly:
pushq %rbp
pushq %r15
pushq %r14
pushq %r13
pushq %r12
pushq %rbx
subq $0x38, %rsp
movss %xmm0, 0xc(%rsp)
movl %esi, %ebx
movq %rdi, %r14
cvtsi2ssl 0x60(%rdi), %xmm0
mulss %xmm1, %xmm0
cvttss2si %xmm0, %eax
movl %eax, 0x8(%rsp)
xorl %ebp, %ebp
testl %esi, %esi
cmovlel %ebp, %ebx
xorps %xmm0, %xmm0
movaps %xmm0, 0x20(%rsp)
xorl %r13d, %r13d
movaps %xmm0, 0x10(%rsp)
cmpl %ebx, %ebp
je 0x614d
callq 0x2080
cltd
idivl 0x60(%r14)
movl %edx, %r15d
callq 0x2080
cltd
idivl 0x60(%r14)
movl %edx, %r12d
callq 0x2080
xorps %xmm8, %xmm8
movl 0x60(%r14), %ecx
cltd
idivl %ecx
movq 0x58(%r14), %rax
leal (,%r15,4), %esi
movslq %esi, %rsi
movss (%rax,%rsi,4), %xmm2
leal (,%r12,4), %esi
movslq %esi, %rsi
movss (%rax,%rsi,4), %xmm1
leal (,%rdx,4), %esi
movslq %esi, %rsi
leal 0x1(,%r15,4), %edi
movslq %edi, %rdi
leal 0x1(,%r12,4), %r8d
movslq %r8d, %r8
leal 0x1(,%rdx,4), %r9d
movslq %r9d, %r9
movss (%rax,%r9,4), %xmm0
leal 0x2(,%rdx,4), %edx
movslq %edx, %rdx
subss %xmm2, %xmm1
movsd (%rax,%rdi,4), %xmm5
movsd (%rax,%r8,4), %xmm4
subss %xmm5, %xmm0
subps %xmm5, %xmm4
movss (%rax,%rsi,4), %xmm3
movss (%rax,%rdx,4), %xmm6
unpcklps %xmm3, %xmm6            # xmm6 = xmm6[0],xmm3[0],xmm6[1],xmm3[1]
movaps %xmm2, %xmm3
shufps $0xd4, %xmm5, %xmm3     # xmm3 = xmm3[0,1],xmm5[1,3]
shufps $0x52, %xmm5, %xmm3     # xmm3 = xmm3[2,0],xmm5[1,1]
subps %xmm3, %xmm6
movaps %xmm6, %xmm3
shufps $0x0, %xmm6, %xmm3      # xmm3 = xmm3[0,0],xmm6[0,0]
movss %xmm0, %xmm3            # xmm3 = xmm0[0],xmm3[1,2,3]
mulss %xmm1, %xmm0
movaps %xmm1, %xmm7
shufps $0xd4, %xmm4, %xmm7     # xmm7 = xmm7[0,1],xmm4[1,3]
shufps $0x52, %xmm4, %xmm7     # xmm7 = xmm7[2,0],xmm4[1,1]
mulps %xmm3, %xmm7
movaps %xmm4, %xmm1
mulps %xmm6, %xmm1
subps %xmm7, %xmm1
movaps %xmm1, %xmm3
shufps $0x55, %xmm1, %xmm3     # xmm3 = xmm3[1,1],xmm1[1,1]
shufps $0x55, %xmm6, %xmm6     # xmm6 = xmm6[1,1,1,1]
mulss %xmm4, %xmm6
subss %xmm6, %xmm0
ucomiss %xmm8, %xmm1
jne 0x6078
jp 0x6078
ucomiss %xmm8, %xmm3
jne 0x6078
jp 0x6078
ucomiss %xmm8, %xmm0
jne 0x6078
jnp 0x613a
movaps %xmm5, %xmm4
shufps $0x55, %xmm5, %xmm4     # xmm4 = xmm4[1,1],xmm5[1,1]
movaps %xmm3, %xmm6
mulss %xmm5, %xmm6
mulss %xmm1, %xmm2
addss %xmm6, %xmm2
mulss %xmm0, %xmm4
addss %xmm2, %xmm4
movaps %xmm4, %xmm2
xorps 0x4320(%rip), %xmm2     # 0xa3c0
movaps %xmm3, %xmm5
mulss %xmm3, %xmm5
movaps %xmm1, %xmm6
mulss %xmm1, %xmm6
addss %xmm5, %xmm6
movaps %xmm0, %xmm5
mulss %xmm0, %xmm5
addss %xmm6, %xmm5
sqrtss %xmm5, %xmm5
mulss 0xc(%rsp), %xmm5
xorl %esi, %esi
testl %ecx, %ecx
cmovlel %esi, %ecx
shlq $0x4, %rcx
xorl %edx, %edx
movaps 0x4364(%rip), %xmm8     # 0xa440
cmpq %rsi, %rcx
je 0x611f
movss 0x4(%rax,%rsi), %xmm6
mulss %xmm3, %xmm6
movss (%rax,%rsi), %xmm7
mulss %xmm1, %xmm7
addss %xmm6, %xmm7
movss 0x8(%rax,%rsi), %xmm6
mulss %xmm0, %xmm6
addss %xmm7, %xmm6
subss %xmm4, %xmm6
andps %xmm8, %xmm6
xorl %edi, %edi
ucomiss %xmm6, %xmm5
seta %dil
addl %edi, %edx
addq $0x10, %rsi
jmp 0x60dc
cmpl %r13d, %edx
jle 0x613a
unpcklps %xmm2, %xmm0            # xmm0 = xmm0[0],xmm2[0],xmm0[1],xmm2[1]
movaps %xmm0, 0x10(%rsp)
movaps %xmm1, 0x20(%rsp)
movl %edx, %r13d
cmpl 0x8(%rsp), %edx
jg 0x6141
incl %ebp
jmp 0x5f61
movl %ebp, %ebx
movaps %xmm1, 0x20(%rsp)
movaps %xmm0, 0x10(%rsp)
leaq 0x47a8(%rip), %rdi      # 0xa8fc
movl %ebx, %esi
xorl %eax, %eax
callq 0x2050
movaps 0x20(%rsp), %xmm0
movaps 0x10(%rsp), %xmm1
addq $0x38, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq