// File: 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
movq %rsp, %rbp
subq $0x90, %rsp
movq %rdi, -0x18(%rbp)
movl %esi, -0x1c(%rbp)
movss %xmm0, -0x20(%rbp)
movss %xmm1, -0x24(%rbp)
movq -0x18(%rbp), %rax
movq %rax, -0x88(%rbp)
leaq -0x10(%rbp), %rdi
xorl %esi, %esi
movl $0x10, %edx
callq 0x2100
movq -0x88(%rbp), %rax
movl $0x0, -0x28(%rbp)
movss -0x24(%rbp), %xmm0
cvtsi2ssl 0x60(%rax), %xmm1
mulss %xmm1, %xmm0
cvttss2si %xmm0, %eax
movl %eax, -0x2c(%rbp)
movl $0x0, -0x30(%rbp)
movl -0x30(%rbp), %eax
cmpl -0x1c(%rbp), %eax
jge 0x987b
callq 0x2080
movq -0x88(%rbp), %rcx
cltd
idivl 0x60(%rcx)
movl %edx, -0x34(%rbp)
callq 0x2080
movq -0x88(%rbp), %rcx
cltd
idivl 0x60(%rcx)
movl %edx, -0x38(%rbp)
callq 0x2080
movq -0x88(%rbp), %rcx
cltd
idivl 0x60(%rcx)
movq -0x88(%rbp), %rax
movl %edx, -0x3c(%rbp)
movq 0x58(%rax), %rcx
movl -0x34(%rbp), %edx
shll $0x2, %edx
movslq %edx, %rdx
movss (%rcx,%rdx,4), %xmm0
movss %xmm0, -0x48(%rbp)
movq 0x58(%rax), %rcx
movl -0x38(%rbp), %edx
shll $0x2, %edx
movslq %edx, %rdx
movss (%rcx,%rdx,4), %xmm0
movss %xmm0, -0x44(%rbp)
movq 0x58(%rax), %rcx
movl -0x3c(%rbp), %edx
shll $0x2, %edx
movslq %edx, %rdx
movss (%rcx,%rdx,4), %xmm0
movss %xmm0, -0x40(%rbp)
movq 0x58(%rax), %rcx
movl -0x34(%rbp), %edx
shll $0x2, %edx
addl $0x1, %edx
movslq %edx, %rdx
movss (%rcx,%rdx,4), %xmm0
movss %xmm0, -0x54(%rbp)
movq 0x58(%rax), %rcx
movl -0x38(%rbp), %edx
shll $0x2, %edx
addl $0x1, %edx
movslq %edx, %rdx
movss (%rcx,%rdx,4), %xmm0
movss %xmm0, -0x50(%rbp)
movq 0x58(%rax), %rcx
movl -0x3c(%rbp), %edx
shll $0x2, %edx
addl $0x1, %edx
movslq %edx, %rdx
movss (%rcx,%rdx,4), %xmm0
movss %xmm0, -0x4c(%rbp)
movq 0x58(%rax), %rcx
movl -0x34(%rbp), %edx
shll $0x2, %edx
addl $0x2, %edx
movslq %edx, %rdx
movss (%rcx,%rdx,4), %xmm0
movss %xmm0, -0x60(%rbp)
movq 0x58(%rax), %rcx
movl -0x38(%rbp), %edx
shll $0x2, %edx
addl $0x2, %edx
movslq %edx, %rdx
movss (%rcx,%rdx,4), %xmm0
movss %xmm0, -0x5c(%rbp)
movq 0x58(%rax), %rax
movl -0x3c(%rbp), %ecx
shll $0x2, %ecx
addl $0x2, %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm0
movss %xmm0, -0x58(%rbp)
movss -0x50(%rbp), %xmm0
subss -0x54(%rbp), %xmm0
movss -0x58(%rbp), %xmm2
subss -0x60(%rbp), %xmm2
movss -0x4c(%rbp), %xmm1
subss -0x54(%rbp), %xmm1
movss -0x5c(%rbp), %xmm3
subss -0x60(%rbp), %xmm3
mulss %xmm3, %xmm1
movd %xmm1, %eax
xorl $0x80000000, %eax       # imm = 0x80000000
movd %eax, %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, -0x70(%rbp)
movss -0x5c(%rbp), %xmm0
subss -0x60(%rbp), %xmm0
movss -0x40(%rbp), %xmm2
subss -0x48(%rbp), %xmm2
movss -0x58(%rbp), %xmm1
subss -0x60(%rbp), %xmm1
movss -0x44(%rbp), %xmm3
subss -0x48(%rbp), %xmm3
mulss %xmm3, %xmm1
movd %xmm1, %eax
xorl $0x80000000, %eax       # imm = 0x80000000
movd %eax, %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, -0x6c(%rbp)
movss -0x44(%rbp), %xmm0
subss -0x48(%rbp), %xmm0
movss -0x4c(%rbp), %xmm2
subss -0x54(%rbp), %xmm2
movss -0x40(%rbp), %xmm1
subss -0x48(%rbp), %xmm1
movss -0x50(%rbp), %xmm3
subss -0x54(%rbp), %xmm3
mulss %xmm3, %xmm1
movd %xmm1, %eax
xorl $0x80000000, %eax       # imm = 0x80000000
movd %eax, %xmm1
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movss %xmm0, -0x68(%rbp)
xorps %xmm0, %xmm0
movss %xmm0, -0x64(%rbp)
movss -0x70(%rbp), %xmm1
movss -0x48(%rbp), %xmm2
movss -0x6c(%rbp), %xmm0
mulss -0x54(%rbp), %xmm0
mulss %xmm2, %xmm1
addss %xmm0, %xmm1
movss -0x68(%rbp), %xmm0
movss -0x60(%rbp), %xmm2
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
movd %xmm0, %eax
xorl $0x80000000, %eax       # imm = 0x80000000
movd %eax, %xmm0
movss %xmm0, -0x64(%rbp)
movss -0x70(%rbp), %xmm0
xorps %xmm1, %xmm1
ucomiss %xmm1, %xmm0
jne 0x973a
jp 0x973a
movss -0x6c(%rbp), %xmm0
xorps %xmm1, %xmm1
ucomiss %xmm1, %xmm0
jne 0x973a
jp 0x973a
movss -0x68(%rbp), %xmm0
xorps %xmm1, %xmm1
ucomiss %xmm1, %xmm0
jne 0x973a
jp 0x973a
jmp 0x986d
movss -0x20(%rbp), %xmm0
movss %xmm0, -0x8c(%rbp)
movss -0x70(%rbp), %xmm1
movss -0x70(%rbp), %xmm2
movss -0x6c(%rbp), %xmm0
mulss -0x6c(%rbp), %xmm0
mulss %xmm2, %xmm1
addss %xmm0, %xmm1
movss -0x68(%rbp), %xmm0
movss -0x68(%rbp), %xmm2
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
callq 0xae10
movaps %xmm0, %xmm1
movss -0x8c(%rbp), %xmm0
mulss %xmm1, %xmm0
movss %xmm0, -0x74(%rbp)
movl $0x0, -0x78(%rbp)
movl $0x0, -0x7c(%rbp)
movq -0x88(%rbp), %rcx
movl -0x7c(%rbp), %eax
cmpl 0x60(%rcx), %eax
jge 0x9841
movq -0x88(%rbp), %rax
movss -0x70(%rbp), %xmm1
movq 0x58(%rax), %rcx
movl -0x7c(%rbp), %edx
shll $0x2, %edx
movslq %edx, %rdx
movss (%rcx,%rdx,4), %xmm2
movss -0x6c(%rbp), %xmm0
movq 0x58(%rax), %rcx
movl -0x7c(%rbp), %edx
shll $0x2, %edx
addl $0x1, %edx
movslq %edx, %rdx
mulss (%rcx,%rdx,4), %xmm0
mulss %xmm2, %xmm1
addss %xmm0, %xmm1
movss -0x68(%rbp), %xmm0
movq 0x58(%rax), %rax
movl -0x7c(%rbp), %ecx
shll $0x2, %ecx
addl $0x2, %ecx
movslq %ecx, %rcx
movss (%rax,%rcx,4), %xmm2
mulss %xmm2, %xmm0
addss %xmm1, %xmm0
addss -0x64(%rbp), %xmm0
callq 0xad70
movaps %xmm0, %xmm1
movss -0x74(%rbp), %xmm0
ucomiss %xmm1, %xmm0
jbe 0x9831
movl -0x78(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x78(%rbp)
jmp 0x9833
movl -0x7c(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x7c(%rbp)
jmp 0x979c
movl -0x78(%rbp), %eax
cmpl -0x28(%rbp), %eax
jle 0x986b
movl -0x78(%rbp), %eax
movl %eax, -0x28(%rbp)
movq -0x70(%rbp), %rax
movq %rax, -0x10(%rbp)
movq -0x68(%rbp), %rax
movq %rax, -0x8(%rbp)
movl -0x28(%rbp), %eax
cmpl -0x2c(%rbp), %eax
jle 0x9869
jmp 0x987b
jmp 0x986b
jmp 0x986d
movl -0x30(%rbp), %eax
addl $0x1, %eax
movl %eax, -0x30(%rbp)
jmp 0x94c1
movl -0x30(%rbp), %esi
leaq 0x9ffd(%rip), %rdi      # 0x13882
xorl %eax, %eax
callq 0x2050
movsd -0x10(%rbp), %xmm0
movsd -0x8(%rbp), %xmm1
addq $0x90, %rsp
popq %rbp
retq
nop