transformation_platform/platform.cpp at master · MaximDementyev/transformation_platform · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
#include "stdafx.h"
#include "Header.h"

engine_position build(const platform *plat, const platform *constraints) {
	engine_position position;
	const auto kappa = -plat->ang.kappa;
	const auto chi = -plat->ang.chi;
	auto h = -plat->pos.z + L;
	if (fabs(plat->pos.z) > constraints->pos.z) {
		h = constraints->pos.z * sign(plat->pos.z) + L;
	}

	auto flag_solve = true;
	//A
	auto Ax = -2 * a*cos(chi)*sin(chi)*sin(kappa) + s * cos(kappa);
	auto Ay = -s * sin(kappa)*sin(chi) - a * (cos(chi)*(cos(kappa)*pow(sin(chi), 2) + pow(cos(chi), 2)) - sin(chi)*(-cos(chi)*cos(kappa)*sin(chi) + cos(chi)*sin(chi)));
	auto Az = s * sin(kappa)*cos(chi) - a * (cos(chi)*(-cos(chi)*cos(kappa)*sin(chi) + cos(chi)*sin(chi)) - sin(chi)*(pow(cos(chi), 2) * cos(kappa) + pow(sin(chi), 2)));
	//çàìåíà ñì ðàñ÷¸òû
	auto m = s - Ax;
	auto j = -a - Ay;
	auto k = h - Az;
	auto solve = accessory(m, j, k, 1);//ïðîâåðÿåì íàëè÷èå ðåøåíèÿ
	if (solve > bad_value + 1) position.x = solve;
	else flag_solve = false;

	if (flag_solve == true) {
		// B
		Ax = 2 * a*cos(chi)*sin(chi)*sin(kappa) + s * cos(kappa);
		Ay = -s * sin(kappa)*sin(chi) + a * (cos(chi)*(cos(kappa)*pow(sin(chi), 2) + pow(cos(chi), 2)) - sin(chi)*(-cos(chi)*cos(kappa)*sin(chi) + cos(chi)*sin(chi)));
		Az = s * sin(kappa)*cos(chi) + a * (cos(chi)*(-cos(chi)*cos(kappa)*sin(chi) + cos(chi)*sin(chi)) - sin(chi)*(pow(cos(chi), 2) * cos(kappa) + pow(sin(chi), 2)));
		//çàìåíà ñì ðàñ÷¸òû
		m = s - Ax;
		j = a - Ay;
		k = h - Az;
		solve = accessory(m, j, k, -1);//ïðîâåðÿåì íàëè÷èå ðåøåíèÿ
		if (solve > bad_value + 1) position.y = solve;
		else flag_solve = false;

		if (flag_solve == true) {
			// C
			Ax = -c * cos(kappa);
			Ay = c * sin(kappa)*sin(chi);
			Az = -c * sin(kappa)*cos(chi);
			//çàìåíà ñì ðàñ÷¸òû
			m = -Ay;
			j = -c - Ax;
			k = h - Az;
			solve = accessory(m, j, k, 1);//ïðîâåðÿåì íàëè÷èå ðåøåíèÿ
			if (solve > bad_value + 1) position.z = solve;
			else flag_solve = false;
		}
	}

	if (flag_solve == false) {
		//fprintf(log, "error build\n");
		//print_platform(log, &bd->now->plat);
	}
	return position;
}


double accessory(double m, double j, double k, int flag) {//âîçâðàùàåò 1 â ñëó÷àå ñóùåñòâîâàíèÿ õîòÿ áû îäíîãî ðåøåíèÿ;
														  //0 - â ñëó÷àå îòñóòñòâèÿ ðåøåíèÿ
	if (fabs(j) > 10e-9) {
		//ðåøåíèå êâàäðàòíîãî óðàâíåíèÿ
		const auto G = -(m*m + j * j + k * k + r * r - L * L) / (2 * r);
		const auto A = k * k + j * j;
		const auto B = -2 * G*k;
		const auto C = G * G - j * j;
		auto D = B * B - 4 * A*C;

		if (D < 0) {
			if (fabs(D) < 10e-5) D = 0.0;
			else return bad_value;//ðåøåíèé íåò
		}

		double x1 = (-B - sqrt(D)) / (2 * A);//ðåøåíèå x1
		if ((x1 >= -1) && (x1 <= 1)) {//ïðîâåðêà íà äîïóñòèìûå çíà÷åíèÿ
			if (flag*(-G + k * x1) / (j) >= -(1e-6)) //ïðîâåðêà óñëîâèÿ (*) - ñì ðàñ÷¸òû
				return x1;
		}
		double x2 = (-B + sqrt(D)) / (2 * A);//ðåøåíèå õ2
		if ((x2 >= -1) && (x2 <= 1)) {//ïðîâåðêà íà äîïóñòèìûå çíà÷åíèÿ
			if (flag*(-G + k * x2) / (j) >= -(1e-6))//ïðîâåðêà óñëîâèÿ (*) - ñì ðàñ÷¸òû
				return x2;
		}
	} else {// ðåøåíèå ëèíåéíîãî óðàâíåíèÿ
		const auto x0 = (-L * L + r * r + k * k + m * m) / (-2 * k*r);//ðåøåíèå
		if ((x0 >= -1) && (x0 <= 1))//ïðîâåðêà íà äîïóñòèìûå çíà÷åíèÿ
			return x0;
	}
	return bad_value;
}