// File: /workspace/github_cpp_makefile_star2_O1/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 $0x78, %rsp
movss %xmm0, 0x4(%rsp)
testl %esi, %esi
jle 0x6d02
movl %esi, %ebp
movq %rdi, %r14
xorps %xmm0, %xmm0
cvtsi2ssl 0x60(%rdi), %xmm0
mulss %xmm1, %xmm0
cvttss2si %xmm0, %eax
movl %eax, (%rsp)
xorl %r13d, %r13d
xorps %xmm0, %xmm0
movaps %xmm0, 0x20(%rsp)
movaps %xmm0, 0x10(%rsp)
xorl %ebx, %ebx
callq 0x2080
cltd
idivl 0x60(%r14)
movl %edx, %r15d
callq 0x2080
cltd
idivl 0x60(%r14)
movl %edx, %r12d
callq 0x2080
cltd
idivl 0x60(%r14)
movq 0x58(%r14), %rax
leal (,%r15,4), %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm9
leal (,%r12,4), %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm5
leal (,%rdx,4), %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm0
leal 0x1(,%r15,4), %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm10
leal 0x1(,%r12,4), %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm2
leal 0x1(,%rdx,4), %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm1
leal 0x2(,%r15,4), %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm6
leal 0x2(,%r12,4), %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm7
leal 0x2(,%rdx,4), %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm3
subss %xmm10, %xmm2
subss %xmm6, %xmm3
subss %xmm10, %xmm1
subss %xmm6, %xmm7
movaps %xmm7, %xmm4
mulss %xmm1, %xmm4
subss %xmm9, %xmm0
mulss %xmm0, %xmm7
mulss %xmm2, %xmm0
movaps %xmm2, %xmm8
mulss %xmm3, %xmm8
subss %xmm4, %xmm8
subss %xmm9, %xmm5
mulss %xmm5, %xmm3
subss %xmm3, %xmm7
xorps %xmm2, %xmm2
mulss %xmm1, %xmm5
subss %xmm0, %xmm5
ucomiss %xmm2, %xmm8
jne 0x6bc1
jp 0x6bc1
ucomiss %xmm2, %xmm7
jne 0x6bc1
jp 0x6bc1
ucomiss %xmm2, %xmm5
jne 0x6bc1
jnp 0x6cda
movaps %xmm7, %xmm0
mulss %xmm7, %xmm0
movaps %xmm8, %xmm1
mulss %xmm8, %xmm1
addss %xmm0, %xmm1
movaps %xmm5, %xmm0
mulss %xmm5, %xmm0
addss %xmm1, %xmm0
ucomiss %xmm2, %xmm0
jb 0x6beb
sqrtss %xmm0, %xmm0
jmp 0x6c36
movaps %xmm5, 0x60(%rsp)
movaps %xmm6, 0x50(%rsp)
movaps %xmm7, 0x40(%rsp)
movaps %xmm8, 0x30(%rsp)
movss %xmm9, 0xc(%rsp)
movss %xmm10, 0x8(%rsp)
callq 0x23b0
movss 0x8(%rsp), %xmm10
movss 0xc(%rsp), %xmm9
movaps 0x30(%rsp), %xmm8
movaps 0x40(%rsp), %xmm7
movaps 0x50(%rsp), %xmm6
movaps 0x60(%rsp), %xmm5
movaps 0x4783(%rip), %xmm3     # 0xb3c0
mulss %xmm7, %xmm10
mulss %xmm8, %xmm9
addss %xmm10, %xmm9
mulss %xmm5, %xmm6
addss %xmm9, %xmm6
movslq 0x60(%r14), %rcx
testq %rcx, %rcx
jle 0x6cb3
mulss 0x4(%rsp), %xmm0
movq 0x58(%r14), %rdx
shlq $0x4, %rcx
xorl %esi, %esi
xorl %eax, %eax
movss 0x4(%rdx,%rsi), %xmm1
mulss %xmm7, %xmm1
movss (%rdx,%rsi), %xmm2
mulss %xmm8, %xmm2
addss %xmm1, %xmm2
movss 0x8(%rdx,%rsi), %xmm1
mulss %xmm5, %xmm1
addss %xmm2, %xmm1
subss %xmm6, %xmm1
andps %xmm3, %xmm1
xorl %edi, %edi
ucomiss %xmm1, %xmm0
seta %dil
addl %edi, %eax
addq $0x10, %rsi
cmpq %rsi, %rcx
jne 0x6c70
jmp 0x6cb5
xorl %eax, %eax
cmpl %r13d, %eax
jle 0x6cda
xorps 0x46ef(%rip), %xmm6     # 0xb3b0
unpcklps %xmm7, %xmm8            # xmm8 = xmm8[0],xmm7[0],xmm8[1],xmm7[1]
unpcklps %xmm6, %xmm5            # xmm5 = xmm5[0],xmm6[0],xmm5[1],xmm6[1]
cmpl (%rsp), %eax
setg %cl
movl %eax, %r13d
movaps %xmm8, %xmm1
movaps %xmm5, %xmm0
jmp 0x6ce6
xorl %ecx, %ecx
movaps 0x20(%rsp), %xmm0
movaps 0x10(%rsp), %xmm1
testb %cl, %cl
movaps %xmm0, 0x20(%rsp)
movaps %xmm1, 0x10(%rsp)
jne 0x6d11
incl %ebx
cmpl %ebp, %ebx
jne 0x6aa4
movl %ebp, %ebx
jmp 0x6d11
xorps %xmm0, %xmm0
movaps %xmm0, 0x10(%rsp)
xorl %ebx, %ebx
movaps %xmm0, 0x20(%rsp)
leaq 0x4b6c(%rip), %rdi      # 0xb884
movl %ebx, %esi
xorl %eax, %eax
callq 0x2050
movaps 0x10(%rsp), %xmm0
movaps 0x20(%rsp), %xmm1
addq $0x78, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq