mlib - multimedia libraries Code

#include <windows.h>
#include <direct.h>
#include <newutil.h>
#include <winutil.h>
#include <distortion.h>

#pragma warning(disable: 4018)

//#include <linear_fit.h>

const double mouse_gain = 0.04;
const double mouse_accel = 0.005;
const double cursor_radius = 0.2;
const color grid_color(1);
const color cursor_color(0.5, 0.25, 0.25);
const color drag_color(1, 0, 0);
const color text_color(0.5);
const color fit_color(0.25, 0.25, 0.5);
const string data_directory = "C:\\Programming\\graphics\\calibrate_screen\\data";
const string default_calib_file = data_directory + "\\calib.dat";

distortion::distortion()
{
	distortion_options opt;
	init(opt);
}

distortion::distortion(const distortion_options &opt)
{
	init(opt);
}

void distortion::init(const distortion_options &opt)
{
	_opt = opt;
	_grid0 = _grid = 0;
	make_grid(_grid0);
	make_grid(_grid);
	//_fit = false;
}

distortion::~distortion()
{
	free_grid(_grid0);
	free_grid(_grid);
}

void distortion::make_grid(v2 **&g)
{
	free_grid(g);
	g = new v2*[_opt.n_grid_x + 1];
	const double i0 = double(_opt.n_grid_x)/2, j0 = double(_opt.n_grid_y)/2;
	for(int i = 0; i <= _opt.n_grid_x; i++) {
		g[i] = new v2[_opt.n_grid_y + 1];
		for(int j = 0; j <= _opt.n_grid_y; j++)
			g[i][j] = _opt.grid_center +
				v2(_opt.cell_size(0)*(i - i0), _opt.cell_size(1)*(j - j0));
	}
}

void distortion::free_grid(v2 **&g)
{
	if(g) {
		for(int i = 0; i <= _opt.n_grid_x; i++)
			delete [] g[i];
		delete [] g;
		g = 0;
	}
}

void distortion::show_grid(ideo *pid)
{
	pid->set_color(grid_color);
	for(int i = 0; i <= _opt.n_grid_x; i++)
		for(int j = 0; j <= _opt.n_grid_y - 1; j++)
			pid->line(_grid[i][j], _grid[i][j + 1]);
	for(int j = 0; j <= _opt.n_grid_y; j++)
		for(int i = 0; i <= _opt.n_grid_x - 1; i++)
			pid->line(_grid[i][j], _grid[i + 1][j]);
}

void distortion::move_node(int i, int j, const v2 &disp)
{
	_grid[i][j] += disp;
}

void distortion::set_node(int i, int j, const v2 &pos)
{
	_grid[i][j] = pos;
}

/*
void distortion::fit()
{
	for(int d = 0; d < 2; d++) {
		vector<fit_data_t> data;
		for(int i = 0; i <= _opt.n_grid_x; i++)
			for(int j = 0; j <= _opt.n_grid_y; j++) {
				data.push_back(fit_data_t(Vd(_grid0[i][j](0), _grid0[i][j](1)),
					_grid[i][j](d)));
			}
		multidimensional_polynomial_fit(data, _opt.degree, _fit_coeff[d]);
	}
	_fit = true;
}

void distortion::show_fit(ideo *pid)
{
	if(!_fit) return;
	pid->set_color(fit_color);
	for(int i = 0; i <= _opt.n_grid_x; i++)
		for(int j = 0; j <= _opt.n_grid_y - 1; j++) {
			const v2 fr(
				multidimensional_polynomial(Vd(_grid0[i][j](0), _grid0[i][j](1)), _fit_coeff[0]),
				multidimensional_polynomial(Vd(_grid0[i][j](0), _grid0[i][j](1)), _fit_coeff[1]));
			const v2 to(
				multidimensional_polynomial(Vd(_grid0[i][j + 1](0), _grid0[i][j + 1](1)), _fit_coeff[0]),
				multidimensional_polynomial(Vd(_grid0[i][j + 1](0), _grid0[i][j + 1](1)), _fit_coeff[1]));
			pid->line(fr, to);
		}
	for(int j = 0; j <= _opt.n_grid_y; j++)
		for(int i = 0; i <= _opt.n_grid_x - 1; i++) {
			const v2 fr(
				multidimensional_polynomial(Vd(_grid0[i][j](0), _grid0[i][j](1)), _fit_coeff[0]),
				multidimensional_polynomial(Vd(_grid0[i][j](0), _grid0[i][j](1)), _fit_coeff[1]));
			const v2 to(
				multidimensional_polynomial(Vd(_grid0[i + 1][j](0), _grid0[i + 1][j](1)), _fit_coeff[0]),
				multidimensional_polynomial(Vd(_grid0[i + 1][j](0), _grid0[i + 1][j](1)), _fit_coeff[1]));
			pid->line(fr, to);
		}
}
*/

// returns true if successful
bool distortion::calc(const v2 &phys, v2 &log)
{
	v2 disp(0);
	double sum_weights = 0;
	for(int i = 0; i <= _opt.n_grid_x; i++)
		for(int j = 0; j <= _opt.n_grid_y; j++) {
			double w;
			const double r = (phys - _grid0[i][j]).length2();
			if(r > 0) {
				w = pow(r, -_opt.calc_power/2);
				if(w > _opt.max_weight)
					w = _opt.max_weight;
			}
			else
				w = _opt.max_weight;
			disp += w*(_grid[i][j] - _grid0[i][j]);
			sum_weights += w;
		}
	if(sum_weights == 0) return false;
	log = phys + disp/sum_weights;
	return true;
}

void distortion::save(const string &fn, const string &comment, bool append)
{
	ensure_dir(data_directory);
	char old_dir[_MAX_PATH];
	getcwd(old_dir, _MAX_PATH);
	ensure_dir(data_directory, true);

	try {
		string my_comment = string("screen calibration ") + date_time();
		if(comment != "") my_comment += "\n" + comment;
		ofstream out((fn != "" ? fn.c_str() : default_calib_file.c_str()), append ? ios::app : ios::out);
		out << "# ";
		for(string::const_iterator it = my_comment.begin(); it != my_comment.end(); it++) {
			if(*it != '\n')
				out << *it;
			else
				out << endl << "# ";
		}
		out << endl;
		out << "grid " << _opt.n_grid_x << " " << _opt.n_grid_y << endl;
		for(int i = 0; i <= _opt.n_grid_x; i++)
			for(int j = 0; j <= _opt.n_grid_y; j++)
				out << _grid0[i][j](0) << '\t' << _grid0[i][j](1) << '\t'
					<< _grid[i][j](0) << '\t' << _grid[i][j](1) << endl;
		out.close();
	}
	catch(error err) {
		chdir(old_dir);
		throw(err);
	}
	chdir(old_dir);
}

void distortion::load(const string &fn)
{
	free_grid(_grid0);
	free_grid(_grid);
	_comment = "";

	ifstream in(fn != "" ? fn.c_str() : default_calib_file.c_str());
	string line;
	while(true) {
		getline(in, line);
		if(!in) throw(error("distortion", "unexpected end-of-file"));
		trim_space(line);
		if(line == "") continue;
		if(line[0] != '#') break;
		line.erase(0, 1);
		trim_space(line);
		if(_comment != "") _comment += "\n";
		_comment += line;
	}
	int n_grid_x, n_grid_y;
	if(sscanf(line.c_str(), "grid %d %d", &n_grid_x, &n_grid_y) != 2)
		throw(error("distortion", "format error"));
	_opt.n_grid_x = n_grid_x;
	_opt.n_grid_y = n_grid_y;
	make_grid(_grid0);
	make_grid(_grid);
	for(int i = 0; i <= _opt.n_grid_x; i++)
		for(int j = 0; j <= _opt.n_grid_y; j++) {
			if(!in) throw(error("distortion", "format error"));
			in >> _grid0[i][j](0) >> _grid0[i][j](1)
				>> _grid[i][j](0) >> _grid[i][j](1);
		}
	in.close();
}

void distortion::calibrate(ideo *pid)
{
	ideo_state state;
	pid->get_state(state);
	v2 cursor(_opt.grid_center);
	bool dragging = false, drag_all;
	int drag_i, drag_j;

	while(true) {
		if(pid->mouse_right_button()) break;

		const bool button = pid->mouse_button();
		if(button && !dragging) {
			dragging = true;
			drag_all = key_down(VK_SHIFT);
			double min_dist2 = DBL_MAX;
			for(int i = 0; i <= _opt.n_grid_x; i++)
				for(int j = 0; j <= _opt.n_grid_y; j++) {
					const double dist2 = (_grid[i][j] - cursor).length2();
					if(dist2 < min_dist2) {
						min_dist2 = dist2;
						drag_i = i;
						drag_j = j;
					}
				}
			cursor = _grid[drag_i][drag_j];
		}
		if(!button && dragging) {
			dragging = false;
		}

		const v2 disp = pid->mouse();
		const double dist = disp.length();
		if(dist > 0) {
			const v2 dir = disp.normalize();
			cursor += (mouse_gain*dist + mouse_accel*dist*dist)*dir;
			if(cursor(0) < state.rf.minx) cursor(0) = state.rf.minx;
			if(cursor(0) > state.rf.maxx) cursor(0) = state.rf.maxx;
			if(cursor(1) < state.rf.miny) cursor(1) = state.rf.miny;
			if(cursor(1) > state.rf.maxy) cursor(1) = state.rf.maxy;
		}
		if(dragging) set_node(drag_i, drag_j, cursor);
		pid->set_color(dragging ? drag_color : cursor_color);
		pid->disk(cursor, cursor_radius);

		v2 log;
		calc(cursor, log);
		pid->line(cursor, log);

		pid->set_color(text_color);
		pid->text_pixel(state.width, state.height, DT_RIGHT | DT_BOTTOM, 
			"right-click when done");

		show_grid(pid);

		pid->show();
	}
}