Нахождение преобразования облака точек

source.cpp

#define _USE_MATH_DEFINES
#include <math.h>
#include <iostream>
#include <fstream>
#include <vector>
#include <tuple>
#include <algorithm>
#include <unordered_map>
#include <functional>
#include <ctime>

using namespace std;

typedef pair<double, double> pdd;
typedef pair<int, int> pii;
typedef vector<pdd> vpdd;
typedef vector<pii> vpii;
typedef tuple<double, double, unsigned> coords;
typedef vector<coords> vCoords;
typedef unsigned long long ull;

ull getCoordsHash(const coords& c) {
	hash<long long> h;

	ull hashAngle = h(llround(get<0>(c) * 1e7));
	ull hashDist2 = h(llround(get<1>(c) * 1e7));

	return hashDist2 + 0x9e3779b9 + (hashAngle << 6) + (hashAngle >> 2);
}

bool compareCoords(const coords& c1, const coords& c2) {
	double dAngle = get<0>(c1) - get<0>(c2);
	double dDist2 = get<1>(c1) - get<1>(c2);
	bool equal_angle = abs(dAngle) < 1e-7;
	return !equal_angle && dAngle < 0 || equal_angle && dDist2 < 0;
}
 
pdd getCenter(const vpdd &ps) {
	pdd center{ 0, 0 };
	for (const pdd &p : ps) {
		center.first += p.first;
		center.second += p.second;
	}
	center.first /= ps.size();
	center.second /= ps.size();
	return center;
}

//	origin - исходные декартовы координаты 
//	center - среднее значение облака точек
//	Зполнение вектора coords:
//		get<0>(coords[i]) - угол между (origin[i] - center) и get<0>(coords[i-1])
//		get<1>(coords[i]) - квадрат длины вектора (origin[i] - center)
//		get<2>(coords[i]) - номер точки в исходном массиве 
void fillCoords(const vpdd &origin, const pdd &center, vCoords &coords) {
	for (auto i = 0u; i < origin.size(); ++i) {
		double dx = origin[i].first - center.first;
		double dy = origin[i].second - center.second;
		double dist2 = dx * dx + dy * dy;
		double angle = atan2(dy, dx) + M_PI;
		get<0>(coords[i]) = angle;
		get<1>(coords[i]) = dist2;
		get<2>(coords[i]) = i;
	}

	sort(coords.begin(), coords.end(), compareCoords);

	double last_angle = get<0>(coords.back());
	for (int i = coords.size() - 1; i > 0; --i) {
		get<0>(coords[i]) -= get<0>(coords[i - 1]);
		if (get<0>(coords[i]) < 0) {
			get<0>(coords[i]) += M_PI + M_PI;
		}
	}
	get<0>(coords[0]) -= last_angle;
	if (get<0>(coords[0]) < 0) {
		get<0>(coords[0]) += M_PI + M_PI;
	}
}

struct PolyHash {
	ull max_power;
	static const ull p = (ull)1e9 + 123;

	PolyHash(int n) {
		max_power = 1;
		ull a = p;
		while (n) {
			if (n & 1)
				max_power *= a;
			a *= a;
			n >>= 1;
		} // max_power = p ^ n
	};

	ull operator() (const vCoords &coords) const {
		ull hash = 0;
		for (auto &c : coords) {
			hash = hash * p + getCoordsHash(c);
		}
		return hash;
	}

	// Пересчет хеша при циклическом сдвиге влево
	ull cycle(ull hash, const coords& c) const {
		ull h = getCoordsHash(c);
		return hash * p + h - max_power * h;
	}
};

bool compareLinear(const vCoords &coords1, const vCoords &coords2, int shift) {
	auto n = coords1.size();
	for (auto i = 0u; i < n; ++i) {
		double dAngle = get<0>(coords1[i]) - get<0>(coords2[(i - shift) % n]);
		double dDist2 = get<1>(coords1[i]) - get<1>(coords2[(i - shift) % n]);
		if (abs(dAngle) > 1e-7 || abs(dDist2) > 1e-7) {
			return false;
		}
	}
	return true;
};

// Возвращает число shift --- минимальный циклический сдвиг
//		массива coords2, после которого coords2 будет совпадать с 
//		coord1 (compareLinear(coords1, coords2, shift) == true)
int findCycleShift(const vCoords &coords1, const vCoords &coords2) {
	PolyHash ph(coords1.size());
	ull hash1 = ph(coords1);
	ull hash2 = ph(coords2);

	int shift = 0;
	while (hash1 != hash2 || compareLinear(coords1, coords2, shift) == false) {
		hash2 = ph.cycle(hash2, coords2[-shift]);
		shift--;
		if (0u-shift >= coords2.size())
			break;
	}

	return shift;
}

// Сопоставляет два набора точек.
// Возвращает вектор match такой, что
//		точке ps1[i] соответствует точка ps2[match[i]]
vector<unsigned> matchPoints(const vpdd &ps1, const vpdd &ps2) {
	auto n = ps1.size();
	vector<unsigned> match(n);

	pdd c1 = getCenter(ps1);
	pdd c2 = getCenter(ps2);

	vCoords coords1(n), coords2(n);
	fillCoords(ps1, c1, coords1);
	fillCoords(ps2, c2, coords2);
	
	int shift = findCycleShift(coords1, coords2);

	for (auto i = 0u; i < n; ++i) {
		match[get<2>(coords1[i])] = get<2>(coords2[(i - shift) % n]);
	}

	return match;
}

pair<double, pdd> getTransformation(const vpdd &ps1, const vpdd &ps2) {
	auto match = matchPoints(ps1, ps2);

	unsigned n1 = rand() % ps1.size();
	unsigned n2 = (n1 + (rand() % (ps1.size() - 1) + 1)) % ps1.size();
	pdd x1 = ps1[n1], x2 = ps1[n2], y1 = ps2[match[n1]], y2 = ps2[match[n2]];
	pdd vx = { x2.first - x1.first, x2.second - x1.second };
	pdd vy = { y2.first - y1.first, y2.second - y1.second };

	double angle = atan2(vy.second, vy.first) - atan2(vx.second, vx.first);
	while (angle < 0)
		angle += M_PI;

	pdd xr = { x1.first*cos(angle) - x1.second*sin(angle), x1.first*sin(angle) + x1.second*cos(angle) };
	xr = { y1.first - xr.first, y1.second - xr.second };

	return{ angle , xr };
}

int main() { 
					// Тест на случайных вершинах (cначала поворот, затем сдвиг)
	int n = 15153;			// Число вершин				
	double angle_deg = 17.123,	// Угол поворота (в градусах)
		   dx = 1.7722,		// Смещение по х
		   dy = 2.25456;	// Смещение по y

	vpdd points1, points2;
	for (int i = 0; i < n; ++i) {
		points1.push_back({ rand() % 1000 / 100., rand() % 1000 / 100. });
		double x = points1[i].first*cos(angle_deg *M_PI / 180.) - points1[i].second*sin(angle_deg * M_PI / 180.) + dx;
		double y = points1[i].first*sin(angle_deg * M_PI / 180.) + points1[i].second*cos(angle_deg * M_PI / 180.) + dy;
		points2.push_back({ x,y });
	}
	srand((unsigned)time(0));
	random_shuffle(points2.begin(), points2.end());

	auto tr = getTransformation(points1, points2); // <---
	
	cout << "Transformation:\n\trotate by " << tr.first / M_PI * 180
		<< " degrees, \n\tthen shift by vector ("
		<< tr.second.first << "; " << tr.second.second << ")\n";
	return 0;
}