// File: /workspace/github_cpp_makefile_star2_O3/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 $0x48, %rsp
movss %xmm0, 0x10(%rsp)
testl %esi, %esi
jle 0x6d51
movl %esi, %ebp
movq %rdi, %r14
xorps %xmm0, %xmm0
cvtsi2ssl 0x60(%rdi), %xmm0
mulss %xmm1, %xmm0
cvttss2si %xmm0, %eax
movl %eax, 0xc(%rsp)
xorl %r13d, %r13d
xorps %xmm0, %xmm0
movaps %xmm0, 0x20(%rsp)
movaps %xmm0, 0x30(%rsp)
xorl %ebx, %ebx
callq 0x2080
cltd
idivl 0x60(%r14)
movl %edx, %r15d
callq 0x2080
cltd
idivl 0x60(%r14)
movl %edx, %r12d
callq 0x2080
movl 0x60(%r14), %ecx
cltd
idivl %ecx
movq 0x58(%r14), %rax
leal (,%r15,4), %esi
movslq %esi, %rsi
movss (%rax,%rsi,4), %xmm0
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), %xmm6
leal 0x2(,%rdx,4), %edx
movslq %edx, %rdx
subss %xmm0, %xmm1
movsd (%rax,%rdi,4), %xmm3
movsd (%rax,%r8,4), %xmm2
subss %xmm3, %xmm6
subps %xmm3, %xmm2
movss (%rax,%rsi,4), %xmm5
movss (%rax,%rdx,4), %xmm4
unpcklps %xmm5, %xmm4            # xmm4 = xmm4[0],xmm5[0],xmm4[1],xmm5[1]
movaps %xmm0, %xmm5
shufps $0xd4, %xmm3, %xmm5     # xmm5 = xmm5[0,1],xmm3[1,3]
shufps $0x52, %xmm3, %xmm5     # xmm5 = xmm5[2,0],xmm3[1,1]
subps %xmm5, %xmm4
movaps %xmm4, %xmm5
shufps $0x0, %xmm4, %xmm5      # xmm5 = xmm5[0,0],xmm4[0,0]
movss %xmm6, %xmm5            # xmm5 = xmm6[0],xmm5[1,2,3]
mulss %xmm1, %xmm6
shufps $0xd4, %xmm2, %xmm1     # xmm1 = xmm1[0,1],xmm2[1,3]
shufps $0x52, %xmm2, %xmm1     # xmm1 = xmm1[2,0],xmm2[1,1]
mulps %xmm5, %xmm1
xorps %xmm5, %xmm5
movaps %xmm2, %xmm7
mulps %xmm4, %xmm7
subps %xmm1, %xmm7
movaps %xmm7, %xmm1
shufps $0x55, %xmm7, %xmm1     # xmm1 = xmm1[1,1],xmm7[1,1]
shufps $0x55, %xmm4, %xmm4     # xmm4 = xmm4[1,1,1,1]
mulss %xmm2, %xmm4
subss %xmm4, %xmm6
ucomiss %xmm5, %xmm7
jne 0x6c7b
jp 0x6c7b
ucomiss %xmm5, %xmm1
jne 0x6c7b
jp 0x6c7b
ucomiss %xmm5, %xmm6
jne 0x6c7b
jnp 0x6d39
movaps %xmm3, %xmm2
shufps $0x55, %xmm3, %xmm2     # xmm2 = xmm2[1,1],xmm3[1,1]
movaps %xmm1, %xmm4
mulss %xmm3, %xmm4
mulss %xmm7, %xmm0
addss %xmm4, %xmm0
mulss %xmm6, %xmm2
addss %xmm0, %xmm2
testl %ecx, %ecx
jle 0x6d15
movaps %xmm1, %xmm0
mulss %xmm1, %xmm0
movaps %xmm7, %xmm3
mulss %xmm7, %xmm3
addss %xmm0, %xmm3
movaps %xmm6, %xmm0
mulss %xmm6, %xmm0
addss %xmm3, %xmm0
sqrtss %xmm0, %xmm0
mulss 0x10(%rsp), %xmm0
shlq $0x4, %rcx
xorl %esi, %esi
xorl %edx, %edx
movaps 0x47bd(%rip), %xmm5     # 0xb490
movss 0x4(%rax,%rsi), %xmm3
mulss %xmm1, %xmm3
movss (%rax,%rsi), %xmm4
mulss %xmm7, %xmm4
addss %xmm3, %xmm4
movss 0x8(%rax,%rsi), %xmm3
mulss %xmm6, %xmm3
addss %xmm4, %xmm3
subss %xmm2, %xmm3
andps %xmm5, %xmm3
xorl %edi, %edi
ucomiss %xmm3, %xmm0
seta %dil
addl %edi, %edx
addq $0x10, %rsi
cmpq %rsi, %rcx
jne 0x6cd3
jmp 0x6d17
xorl %edx, %edx
cmpl %r13d, %edx
jle 0x6d39
xorps 0x46dd(%rip), %xmm2     # 0xb400
unpcklps %xmm2, %xmm6            # xmm6 = xmm6[0],xmm2[0],xmm6[1],xmm2[1]
movaps %xmm6, 0x20(%rsp)
movaps %xmm7, 0x30(%rsp)
movl %edx, %r13d
cmpl 0xc(%rsp), %edx
jg 0x6d62
incl %ebx
cmpl %ebp, %ebx
jne 0x6b73
movl %ebp, %ebx
movaps 0x30(%rsp), %xmm0
movaps %xmm0, 0x10(%rsp)
jmp 0x6d6c
xorps %xmm0, %xmm0
movaps %xmm0, 0x10(%rsp)
xorl %ebx, %ebx
movaps %xmm0, 0x20(%rsp)
jmp 0x6d6c
movaps %xmm6, 0x20(%rsp)
movaps %xmm7, 0x10(%rsp)
leaq 0x4be1(%rip), %rdi      # 0xb954
movl %ebx, %esi
xorl %eax, %eax
callq 0x2050
movaps 0x10(%rsp), %xmm0
movaps 0x20(%rsp), %xmm1
addq $0x48, %rsp
popq %rbx
popq %r12
popq %r13
popq %r14
popq %r15
popq %rbp
retq
nop