server/line_fit.cpp

#include <algorithm>
#include <math.h>
#include "line_fit.h"
#include "line.h"

fit_item::fit_item()
	:x(0)
	,y(0)
	,xx(0)
	,xy(0)
{
}

fit_item&fit_item::set(double x,double y)
{
	this->x=x;
	this->y=y;

	xx=x*x;
	xy=x*y;

	return *this;
}

fit_batch::fit_batch()
{
	tool.reserve(8);
}

fit_batch::~fit_batch()
{
	std::for_each(tool.begin(),tool.end(),[](card_fit*f){
		delete f;
	});
}

void fit_batch::add_tool(double max_x_span,int min_point)
{
	tool.push_back(new card_fit(*this,max_x_span,min_point));
}

void fit_batch::add_tool(double max_x_span,int min_point,int max_point)
{
	tool.push_back(new card_fit_lastn(*this,max_x_span,min_point,max_point));
}


void fit_batch::log()
{
	printf("epos-bpos:%d, xo:%f, yo:%f\n",epos - bpos, xo, yo);
	std::for_each(tool.begin(),tool.end(),[](card_fit*f){
		f->log();
	});
}

void fit_batch::reset_data()
{
	xo=yo=0;
	clear();
	std::for_each(tool.begin(),tool.end(),[](card_fit*f){
		f->reset();
	});
}

double fit_batch::add(double x,double y)
{
	if(x==0 && y==0)
		return 0;
	
	if(empty())
	{
		xo=x; yo=y;
		std::for_each(tool.begin(),tool.end(),[this](card_fit*f){
				f->xo=xo;
				f->yo=yo;
		});
	}

	grow().set(x-xo,y-yo);
	std::for_each(tool.begin(),tool.end(),[](card_fit*f){
			f->add_last();
			f->fit();
	});
	
	return 0;
}
#if 0
void fit_batch::replace(double x,double y)
{
	if(empty())
		return;

	std::for_each(tool.begin(),tool.end(),[](card_fit*f){
		f->remove_last();
	});

	rat(0).set(x-xo,y-yo);

	std::for_each(tool.begin(),tool.end(),[](card_fit*f){
		f->add_last();
		f->fit();
	});
}
#endif

double fit_batch::x(int index)const
{
	return (*this)(index).x + xo;
}

double fit_batch::y(int index)const
{
	return (*this)(index).y + yo;
}

fit_result::fit_result()
	:k(0)
	,kb(0)
	,ke(0)
	,ka(0)
	,num_point(0)
	,min_point(0)
	,max_x_span(0)
{
}

fit_result::fit_result(double _max_x_span,int min_point_)
	:k(0)
	,kb(0)
	,ke(0)
	,ka(0)
	,num_point(0)
	,min_point(min_point_)
	,max_x_span(_max_x_span)
{
}
void fit_result::reset()
{
	k=kb=ke=ka=0;
	xo=yo=0;
	num_point=0;
}

card_fit::~card_fit()
{

}

card_fit::card_fit(const fit_batch&d_,double _max_x_span,int min_point_)
	:fit_result(_max_x_span,min_point_)
	,d(d_)
{
}

void card_fit::dec_(const fit_item&i)
{
	A -= i.xx;
	B -= i.x;
	C -= i.xy;
	D -= i.y;
}

void card_fit::add_(const fit_item&i)
{
	A += i.xx;
	B += i.x;
	C += i.xy;
	D += i.y;
}


void card_fit::add_last()
{
	++num_point;
	add_(d(0));
}
#if 0
void card_fit::remove_last()
{
	--num_point;
	dec_(d(0));
}
#endif

void card_fit::reset()
{
	fit_result::reset();
	A=B=C=D=0;
}

double fit_result::testk(double x,double y)const
{
	return k * (x-xo) + kb - (y-yo);
}

double fit_result::differ_k(double x,double y)const
{
	return fabs(k*x+kb-y);
}

bool card_fit::check_x_span()
{
	int i=size()-1;
	for(;i>0;i--)
	{
		double off=d(0).x-d(i).x;
		if(off>max_x_span+0.5)
		{
			--num_point;
			dec_(d(i));
			continue;
		}

		break;
	}

	return num_point>=min_point;
}

bool card_fit::fit()
{
	if(!check_x_span())
		return false;
	
	int count=size();
	double temp = count*A - B*B;

	kb = (A*D - B*C) / temp;
	k = (count*C - B*D) / temp;

	double sum=0;
	for (int i=0; i<count; i++)
		sum+=differ_k(d(i).x,d(i).y);

	ke = sum / count;

	return true;
}