Delta Robot example
Dependencies: BufferedSerial Eigen
Fork of TCPSocket_Example by
DeltaKinematics.cpp@5:01e1e68309ae, 2018-10-15 (annotated)
- Committer:
- je310
- Date:
- Mon Oct 15 18:30:20 2018 +0000
- Revision:
- 5:01e1e68309ae
- Parent:
- 4:778bc352c47f
testing eigen;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
je310 | 4:778bc352c47f | 1 | // DeltaKinematics - kinematics computation for delta parallel robot arm |
je310 | 4:778bc352c47f | 2 | // Copyright(C) 2018 Szymon Szantula |
je310 | 4:778bc352c47f | 3 | // |
je310 | 4:778bc352c47f | 4 | // This program is free software : you can redistribute it and / or modify |
je310 | 4:778bc352c47f | 5 | // it under the terms of the GNU General Public License as published by |
je310 | 4:778bc352c47f | 6 | // the Free Software Foundation, either version 3 of the License, or |
je310 | 4:778bc352c47f | 7 | // (at your option) any later version. |
je310 | 4:778bc352c47f | 8 | // |
je310 | 4:778bc352c47f | 9 | // This program is distributed in the hope that it will be useful, |
je310 | 4:778bc352c47f | 10 | // but WITHOUT ANY WARRANTY; without even the implied warranty of |
je310 | 4:778bc352c47f | 11 | // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.See the |
je310 | 4:778bc352c47f | 12 | // GNU General Public License for more details. |
je310 | 4:778bc352c47f | 13 | // |
je310 | 4:778bc352c47f | 14 | // You should have received a copy of the GNU General Public License |
je310 | 4:778bc352c47f | 15 | // along with this program.If not, see <http://www.gnu.org/licenses/.> |
je310 | 4:778bc352c47f | 16 | |
je310 | 4:778bc352c47f | 17 | #include "DeltaKinematics.h" |
je310 | 4:778bc352c47f | 18 | |
je310 | 4:778bc352c47f | 19 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 20 | const RealDataType DeltaKinematics<RealDataType>::_SQRT3 = std::sqrt(3.0); |
je310 | 4:778bc352c47f | 21 | |
je310 | 4:778bc352c47f | 22 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 23 | const RealDataType DeltaKinematics<RealDataType>::_HSQRT3 = std::sqrt(3.0)/ 2.0; |
je310 | 4:778bc352c47f | 24 | |
je310 | 4:778bc352c47f | 25 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 26 | const RealDataType DeltaKinematics<RealDataType>::_DEG2RAD_FACTOR = M_PI / 180.0; |
je310 | 4:778bc352c47f | 27 | |
je310 | 4:778bc352c47f | 28 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 29 | const RealDataType DeltaKinematics<RealDataType>::_RAD2DEG_FACTOR = 180.0 / M_PI; |
je310 | 4:778bc352c47f | 30 | |
je310 | 4:778bc352c47f | 31 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 32 | const RealDataType DeltaKinematics<RealDataType>::_MROTZ120[3][3] = {{-0.5, 0.866025403784439, 0.0}, |
je310 | 4:778bc352c47f | 33 | {-0.866025403784439, -0.5, 0.0}, |
je310 | 4:778bc352c47f | 34 | {0.0, 0.0, 1.0}}; |
je310 | 4:778bc352c47f | 35 | |
je310 | 4:778bc352c47f | 36 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 37 | const RealDataType DeltaKinematics<RealDataType>::_ROTZ120[3][3] = {{-0.5, -0.866025403784439, 0.0}, |
je310 | 4:778bc352c47f | 38 | {0.866025403784439, -0.5, 0.0}, |
je310 | 4:778bc352c47f | 39 | {0.0, 0.0, 1.0}}; |
je310 | 4:778bc352c47f | 40 | |
je310 | 4:778bc352c47f | 41 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 42 | DeltaKinematics<RealDataType>::DeltaKinematics(DeltaGeometricDim dim) |
je310 | 4:778bc352c47f | 43 | { |
je310 | 4:778bc352c47f | 44 | _sb = dim.sb; |
je310 | 4:778bc352c47f | 45 | _sp = dim.sp; |
je310 | 4:778bc352c47f | 46 | _Ll = dim.L; |
je310 | 4:778bc352c47f | 47 | _l = dim.l; |
je310 | 4:778bc352c47f | 48 | _h = dim.h; |
je310 | 4:778bc352c47f | 49 | _max_neg_angle = dim.max_neg_angle; |
je310 | 4:778bc352c47f | 50 | _min_parallelogram_angle = dim.min_parallelogram_angle; |
je310 | 4:778bc352c47f | 51 | Initialise(); |
je310 | 4:778bc352c47f | 52 | } |
je310 | 4:778bc352c47f | 53 | |
je310 | 4:778bc352c47f | 54 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 55 | void DeltaKinematics<RealDataType>::Initialise() |
je310 | 4:778bc352c47f | 56 | { |
je310 | 4:778bc352c47f | 57 | _wb = _SQRT3 / 6 * _sb; |
je310 | 4:778bc352c47f | 58 | _ub = _SQRT3 / 3 * _sb; |
je310 | 4:778bc352c47f | 59 | _wp = _SQRT3 / 6 * _sp; |
je310 | 4:778bc352c47f | 60 | _up = _SQRT3 / 3 * _sp; |
je310 | 4:778bc352c47f | 61 | _B1[0] = 0; |
je310 | 4:778bc352c47f | 62 | _B1[1] = -_wb; |
je310 | 4:778bc352c47f | 63 | _B1[2] = 0; |
je310 | 4:778bc352c47f | 64 | _B2[0] = _HSQRT3 * _wb; |
je310 | 4:778bc352c47f | 65 | _B2[1] = _wb / 2; |
je310 | 4:778bc352c47f | 66 | _B2[2] = 0; |
je310 | 4:778bc352c47f | 67 | _B3[0] = -_HSQRT3 * _wb; |
je310 | 4:778bc352c47f | 68 | _B3[1] = _wb / 2; |
je310 | 4:778bc352c47f | 69 | _B3[2] = 0; |
je310 | 4:778bc352c47f | 70 | _Pp1[0] = 0; |
je310 | 4:778bc352c47f | 71 | _Pp1[1] = -_up; |
je310 | 4:778bc352c47f | 72 | _Pp1[2] = 0; |
je310 | 4:778bc352c47f | 73 | _Pp2[0] = _sp / 2; |
je310 | 4:778bc352c47f | 74 | _Pp2[1] = -_wp; |
je310 | 4:778bc352c47f | 75 | _Pp2[2] = 0; |
je310 | 4:778bc352c47f | 76 | _Pp3[0] = -_sp / 2; |
je310 | 4:778bc352c47f | 77 | _Pp3[1] = -_wp; |
je310 | 4:778bc352c47f | 78 | _Pp3[2] = 0; |
je310 | 4:778bc352c47f | 79 | _b1[0] = _sb / 2; |
je310 | 4:778bc352c47f | 80 | _b1[1] = -_wb; |
je310 | 4:778bc352c47f | 81 | _b1[2] = 0; |
je310 | 4:778bc352c47f | 82 | _b2[0] = 0; |
je310 | 4:778bc352c47f | 83 | _b2[1] = -_ub; |
je310 | 4:778bc352c47f | 84 | _b2[2] = 0; |
je310 | 4:778bc352c47f | 85 | _b3[0] = -_sb / 2; |
je310 | 4:778bc352c47f | 86 | _b3[1] = -_wb; |
je310 | 4:778bc352c47f | 87 | _b3[2] = 0; |
je310 | 4:778bc352c47f | 88 | } |
je310 | 4:778bc352c47f | 89 | |
je310 | 4:778bc352c47f | 90 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 91 | void DeltaKinematics<RealDataType>::RotateByMatrix(RealDataType *point, const RealDataType (*matrix)[3]) |
je310 | 4:778bc352c47f | 92 | { |
je310 | 4:778bc352c47f | 93 | RealDataType temp[3]; |
je310 | 4:778bc352c47f | 94 | for (int i = 0; i < 3; ++i) |
je310 | 4:778bc352c47f | 95 | { |
je310 | 4:778bc352c47f | 96 | temp[i] = matrix[i][0] * point[0] + matrix[i][1] * point[1] + matrix[i][2] * point[2]; |
je310 | 4:778bc352c47f | 97 | } |
je310 | 4:778bc352c47f | 98 | point[0] = temp[0]; |
je310 | 4:778bc352c47f | 99 | point[1] = temp[1]; |
je310 | 4:778bc352c47f | 100 | point[2] = temp[2]; |
je310 | 4:778bc352c47f | 101 | } |
je310 | 4:778bc352c47f | 102 | |
je310 | 4:778bc352c47f | 103 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 104 | int DeltaKinematics<RealDataType>::CalculateIpk(DeltaVector *v, int num) |
je310 | 4:778bc352c47f | 105 | { |
je310 | 4:778bc352c47f | 106 | RealDataType temp_p[3]; |
je310 | 4:778bc352c47f | 107 | RealDataType temp_phi; |
je310 | 4:778bc352c47f | 108 | int flag; |
je310 | 4:778bc352c47f | 109 | |
je310 | 4:778bc352c47f | 110 | for (int i = 0; i < num; ++i) |
je310 | 4:778bc352c47f | 111 | { |
je310 | 4:778bc352c47f | 112 | /* compute p1*/ |
je310 | 4:778bc352c47f | 113 | temp_p[0] = _Pp1[0] + v[i].x; |
je310 | 4:778bc352c47f | 114 | temp_p[1] = _Pp1[1] + v[i].y; |
je310 | 4:778bc352c47f | 115 | temp_p[2] = _Pp1[2] + v[i].z; |
je310 | 4:778bc352c47f | 116 | /* calculate angle */ |
je310 | 4:778bc352c47f | 117 | flag = CalculateAngle(_b1, temp_p, &temp_phi); |
je310 | 4:778bc352c47f | 118 | if (flag) |
je310 | 4:778bc352c47f | 119 | { |
je310 | 4:778bc352c47f | 120 | return 1; |
je310 | 4:778bc352c47f | 121 | } |
je310 | 4:778bc352c47f | 122 | else |
je310 | 4:778bc352c47f | 123 | { |
je310 | 4:778bc352c47f | 124 | v[i].phi1 = temp_phi; |
je310 | 4:778bc352c47f | 125 | } |
je310 | 4:778bc352c47f | 126 | |
je310 | 4:778bc352c47f | 127 | /* compute p2 and change reference system */ |
je310 | 4:778bc352c47f | 128 | temp_p[0] = v[i].x; |
je310 | 4:778bc352c47f | 129 | temp_p[1] = v[i].y; |
je310 | 4:778bc352c47f | 130 | temp_p[2] = v[i].z; |
je310 | 4:778bc352c47f | 131 | RotateByMatrix(temp_p, _MROTZ120); |
je310 | 4:778bc352c47f | 132 | temp_p[0] += _Pp1[0]; |
je310 | 4:778bc352c47f | 133 | temp_p[1] += _Pp1[1]; |
je310 | 4:778bc352c47f | 134 | temp_p[2] += _Pp1[2]; |
je310 | 4:778bc352c47f | 135 | /* check all conditions */ |
je310 | 4:778bc352c47f | 136 | flag = CalculateAngle(_b1, temp_p, &temp_phi); |
je310 | 4:778bc352c47f | 137 | if (flag) |
je310 | 4:778bc352c47f | 138 | { |
je310 | 4:778bc352c47f | 139 | return 1; |
je310 | 4:778bc352c47f | 140 | } |
je310 | 4:778bc352c47f | 141 | else |
je310 | 4:778bc352c47f | 142 | { |
je310 | 4:778bc352c47f | 143 | v[i].phi2 = temp_phi; |
je310 | 4:778bc352c47f | 144 | } |
je310 | 4:778bc352c47f | 145 | |
je310 | 4:778bc352c47f | 146 | /* compute p3 change reference system */ |
je310 | 4:778bc352c47f | 147 | temp_p[0] = v[i].x; |
je310 | 4:778bc352c47f | 148 | temp_p[1] = v[i].y; |
je310 | 4:778bc352c47f | 149 | temp_p[2] = v[i].z; |
je310 | 4:778bc352c47f | 150 | RotateByMatrix(temp_p, _ROTZ120); |
je310 | 4:778bc352c47f | 151 | temp_p[0] += _Pp1[0]; |
je310 | 4:778bc352c47f | 152 | temp_p[1] += _Pp1[1]; |
je310 | 4:778bc352c47f | 153 | temp_p[2] += _Pp1[2]; |
je310 | 4:778bc352c47f | 154 | /* check all conditions */ |
je310 | 4:778bc352c47f | 155 | flag = CalculateAngle(_b1, temp_p, &temp_phi); |
je310 | 4:778bc352c47f | 156 | if (flag) |
je310 | 4:778bc352c47f | 157 | { |
je310 | 4:778bc352c47f | 158 | return 1; |
je310 | 4:778bc352c47f | 159 | } |
je310 | 4:778bc352c47f | 160 | else |
je310 | 4:778bc352c47f | 161 | { |
je310 | 4:778bc352c47f | 162 | v[i].phi3 = temp_phi; |
je310 | 4:778bc352c47f | 163 | } |
je310 | 4:778bc352c47f | 164 | } |
je310 | 4:778bc352c47f | 165 | |
je310 | 4:778bc352c47f | 166 | return 0; |
je310 | 4:778bc352c47f | 167 | } |
je310 | 4:778bc352c47f | 168 | |
je310 | 4:778bc352c47f | 169 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 170 | int DeltaKinematics<RealDataType>::CalculateFpk(DeltaVector *v, int num) |
je310 | 4:778bc352c47f | 171 | { |
je310 | 4:778bc352c47f | 172 | _A1[0] = 0.0; |
je310 | 4:778bc352c47f | 173 | RealDataType hsp = _sp / 2.0; |
je310 | 4:778bc352c47f | 174 | int flag; |
je310 | 4:778bc352c47f | 175 | for (int i = 0; i < num; ++i) |
je310 | 4:778bc352c47f | 176 | { |
je310 | 4:778bc352c47f | 177 | _A1[1] = -_wb - _Ll * std::cos(_DEG2RAD_FACTOR * v[i].phi1) + _up; |
je310 | 4:778bc352c47f | 178 | _A1[2] = -_Ll * std::sin(_DEG2RAD_FACTOR * v[i].phi1); |
je310 | 4:778bc352c47f | 179 | _A2[0] = _HSQRT3 * (_wb + _Ll * std::cos(_DEG2RAD_FACTOR * v[i].phi2)) - hsp; |
je310 | 4:778bc352c47f | 180 | _A2[1] = 0.5 * (_wb + _Ll * std::cos(_DEG2RAD_FACTOR * v[i].phi2)) - _wp; |
je310 | 4:778bc352c47f | 181 | _A2[2] = -_Ll * std::sin(_DEG2RAD_FACTOR * v[i].phi2); |
je310 | 4:778bc352c47f | 182 | _A3[0] = -_HSQRT3 * (_wb + _Ll * std::cos(_DEG2RAD_FACTOR * v[i].phi3)) + hsp; |
je310 | 4:778bc352c47f | 183 | _A3[1] = 0.5 * (_wb + _Ll * std::cos(_DEG2RAD_FACTOR * v[i].phi3)) - _wp; |
je310 | 4:778bc352c47f | 184 | _A3[2] = -_Ll * std::sin(_DEG2RAD_FACTOR * v[i].phi3); |
je310 | 4:778bc352c47f | 185 | if (_A1[2] == _A2[2] && _A2[2] == _A3[2]) |
je310 | 4:778bc352c47f | 186 | { |
je310 | 4:778bc352c47f | 187 | flag = ThreeSpheresIntersectionB(&v[i]); |
je310 | 4:778bc352c47f | 188 | } |
je310 | 4:778bc352c47f | 189 | else |
je310 | 4:778bc352c47f | 190 | { |
je310 | 4:778bc352c47f | 191 | flag = ThreeSpheresIntersectionA(&v[i]); |
je310 | 4:778bc352c47f | 192 | } |
je310 | 4:778bc352c47f | 193 | if (flag) |
je310 | 4:778bc352c47f | 194 | { |
je310 | 4:778bc352c47f | 195 | return flag; |
je310 | 4:778bc352c47f | 196 | } |
je310 | 4:778bc352c47f | 197 | } |
je310 | 4:778bc352c47f | 198 | return 0; |
je310 | 4:778bc352c47f | 199 | } |
je310 | 4:778bc352c47f | 200 | |
je310 | 4:778bc352c47f | 201 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 202 | int DeltaKinematics<RealDataType>::ThreeSpheresIntersectionA(DeltaVector *v) |
je310 | 4:778bc352c47f | 203 | { |
je310 | 4:778bc352c47f | 204 | RealDataType a11, a12, a13, a21, a22, a23, b1, b2, a1, a2, a3, a4, a5, a6, a7, a, b, c, delta; |
je310 | 4:778bc352c47f | 205 | a11 = 2 * (_A3[0] - _A1[0]); |
je310 | 4:778bc352c47f | 206 | a12 = 2 * (_A3[1] - _A1[1]); |
je310 | 4:778bc352c47f | 207 | a13 = 2 * (_A3[2] - _A1[2]); |
je310 | 4:778bc352c47f | 208 | if (a13 == 0.0) |
je310 | 4:778bc352c47f | 209 | return 1; |
je310 | 4:778bc352c47f | 210 | a21 = 2 * (_A3[0] - _A2[0]); |
je310 | 4:778bc352c47f | 211 | a22 = 2 * (_A3[1] - _A2[1]); |
je310 | 4:778bc352c47f | 212 | a23 = 2 * (_A3[2] - _A2[2]); |
je310 | 4:778bc352c47f | 213 | if (a23 == 0.0) |
je310 | 4:778bc352c47f | 214 | return 1; |
je310 | 4:778bc352c47f | 215 | RealDataType A3_pow2 = _A3[0] * _A3[0] + _A3[1] * _A3[1] + _A3[2] * _A3[2]; |
je310 | 4:778bc352c47f | 216 | b1 = -_A1[0] * _A1[0] - _A1[1] * _A1[1] - _A1[2] * _A1[2] + A3_pow2; |
je310 | 4:778bc352c47f | 217 | b2 = -_A2[0] * _A2[0] - _A2[1] * _A2[1] - _A2[2] * _A2[2] + A3_pow2; |
je310 | 4:778bc352c47f | 218 | a1 = a11 / a13 - a21 / a23; |
je310 | 4:778bc352c47f | 219 | a2 = a12 / a13 - a22 / a23; |
je310 | 4:778bc352c47f | 220 | a3 = b2 / a23 - b1 / a13; |
je310 | 4:778bc352c47f | 221 | if (a1 == 0.0) |
je310 | 4:778bc352c47f | 222 | return 1; |
je310 | 4:778bc352c47f | 223 | a4 = -a2 / a1; |
je310 | 4:778bc352c47f | 224 | a5 = -a3 / a1; |
je310 | 4:778bc352c47f | 225 | a6 = (-a21 * a4 - a22) / a23; |
je310 | 4:778bc352c47f | 226 | a7 = (b2 - a21 * a5) / a23; |
je310 | 4:778bc352c47f | 227 | a = a4 * a4 + 1 + a6 * a6; |
je310 | 4:778bc352c47f | 228 | if (a == 0.0) |
je310 | 4:778bc352c47f | 229 | return 1; |
je310 | 4:778bc352c47f | 230 | b = 2 * a4 * (a5 - _A1[0]) - 2 * _A1[1] + 2 * a6 * (a7 - _A1[2]); |
je310 | 4:778bc352c47f | 231 | c = a5 * (a5 - 2 * _A1[0]) + a7 * (a7 - 2 * _A1[2]) + _A1[0] * _A1[0] + _A1[1] * _A1[1] + _A1[2] * _A1[2] - _l * _l; |
je310 | 4:778bc352c47f | 232 | if ((delta = b * b - 4 * a * c) < 0.0) |
je310 | 4:778bc352c47f | 233 | return 1; |
je310 | 4:778bc352c47f | 234 | DeltaVector pp1, pp2; |
je310 | 4:778bc352c47f | 235 | pp1.y = (-b + std::sqrt(delta)) / (2 * a); |
je310 | 4:778bc352c47f | 236 | pp1.x = a4 * pp1.y + a5; |
je310 | 4:778bc352c47f | 237 | pp1.z = a6 * pp1.y + a7; |
je310 | 4:778bc352c47f | 238 | pp2.y = (-b - std::sqrt(delta)) / (2 * a); |
je310 | 4:778bc352c47f | 239 | pp2.x = a4 * pp2.y + a5; |
je310 | 4:778bc352c47f | 240 | pp2.z = a6 * pp2.y + a7; |
je310 | 4:778bc352c47f | 241 | if (pp1.z < 0.0 && CalculateIpk(&pp1, 1) == 0) |
je310 | 4:778bc352c47f | 242 | { |
je310 | 4:778bc352c47f | 243 | v->x = pp1.x; |
je310 | 4:778bc352c47f | 244 | v->y = pp1.y; |
je310 | 4:778bc352c47f | 245 | v->z = pp1.z; |
je310 | 4:778bc352c47f | 246 | return 0; |
je310 | 4:778bc352c47f | 247 | } |
je310 | 4:778bc352c47f | 248 | else if (pp2.z < 0.0 && CalculateIpk(&pp2, 1) == 0) |
je310 | 4:778bc352c47f | 249 | { |
je310 | 4:778bc352c47f | 250 | v->x = pp2.x; |
je310 | 4:778bc352c47f | 251 | v->y = pp2.y; |
je310 | 4:778bc352c47f | 252 | v->z = pp2.z; |
je310 | 4:778bc352c47f | 253 | return 0; |
je310 | 4:778bc352c47f | 254 | } |
je310 | 4:778bc352c47f | 255 | else |
je310 | 4:778bc352c47f | 256 | { |
je310 | 4:778bc352c47f | 257 | return 1; |
je310 | 4:778bc352c47f | 258 | } |
je310 | 4:778bc352c47f | 259 | } |
je310 | 4:778bc352c47f | 260 | |
je310 | 4:778bc352c47f | 261 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 262 | int DeltaKinematics<RealDataType>::ThreeSpheresIntersectionB(DeltaVector *v) |
je310 | 4:778bc352c47f | 263 | { |
je310 | 4:778bc352c47f | 264 | RealDataType a, b, c, d, e, f, zn, B, C, delta; |
je310 | 4:778bc352c47f | 265 | zn = _A1[2]; |
je310 | 4:778bc352c47f | 266 | a = 2 * (_A3[0] - _A1[0]); |
je310 | 4:778bc352c47f | 267 | b = 2 * (_A3[1] - _A1[1]); |
je310 | 4:778bc352c47f | 268 | d = 2 * (_A3[0] - _A2[0]); |
je310 | 4:778bc352c47f | 269 | e = 2 * (_A3[1] - _A2[1]); |
je310 | 4:778bc352c47f | 270 | RealDataType A3_pow2 = _A3[0] * _A3[0] + _A3[1] * _A3[1]; |
je310 | 4:778bc352c47f | 271 | c = -_A1[0] * _A1[0] - _A1[1] * _A1[1] + A3_pow2; |
je310 | 4:778bc352c47f | 272 | f = -_A2[0] * _A2[0] - _A2[1] * _A2[1] + A3_pow2; |
je310 | 4:778bc352c47f | 273 | DeltaVector pp; |
je310 | 4:778bc352c47f | 274 | RealDataType denom = a * e - b * d; |
je310 | 4:778bc352c47f | 275 | if (denom == 0.0) |
je310 | 4:778bc352c47f | 276 | return 1; |
je310 | 4:778bc352c47f | 277 | pp.x = (c * e - b * f) / denom; |
je310 | 4:778bc352c47f | 278 | pp.y = (a * f - c * d) / denom; |
je310 | 4:778bc352c47f | 279 | B = -2*zn; |
je310 | 4:778bc352c47f | 280 | C = zn * zn - _l * _l + (pp.x - _A1[0]) * (pp.x - _A1[0]) + (pp.y - _A1[1]) * (pp.y - _A1[1]); |
je310 | 4:778bc352c47f | 281 | delta = B * B - 4 * C; |
je310 | 4:778bc352c47f | 282 | if (delta < 0.0) |
je310 | 4:778bc352c47f | 283 | return 1; |
je310 | 4:778bc352c47f | 284 | RealDataType z1, z2; |
je310 | 4:778bc352c47f | 285 | z1 = (-B + std::sqrt(delta)) / 2; |
je310 | 4:778bc352c47f | 286 | z2 = (-B - std::sqrt(delta)) / 2; |
je310 | 4:778bc352c47f | 287 | pp.z = z1; |
je310 | 4:778bc352c47f | 288 | if (pp.z < 0.0 && CalculateIpk(&pp, 1) == 0) |
je310 | 4:778bc352c47f | 289 | { |
je310 | 4:778bc352c47f | 290 | v->x = pp.x; |
je310 | 4:778bc352c47f | 291 | v->y = pp.y; |
je310 | 4:778bc352c47f | 292 | v->z = pp.z; |
je310 | 4:778bc352c47f | 293 | return 0; |
je310 | 4:778bc352c47f | 294 | } |
je310 | 4:778bc352c47f | 295 | pp.z = z2; |
je310 | 4:778bc352c47f | 296 | if (pp.z < 0.0 && CalculateIpk(&pp, 1) == 0) |
je310 | 4:778bc352c47f | 297 | { |
je310 | 4:778bc352c47f | 298 | v->x = pp.x; |
je310 | 4:778bc352c47f | 299 | v->y = pp.y; |
je310 | 4:778bc352c47f | 300 | v->z = pp.z; |
je310 | 4:778bc352c47f | 301 | return 0; |
je310 | 4:778bc352c47f | 302 | } |
je310 | 4:778bc352c47f | 303 | return 1; |
je310 | 4:778bc352c47f | 304 | } |
je310 | 4:778bc352c47f | 305 | |
je310 | 4:778bc352c47f | 306 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 307 | int DeltaKinematics<RealDataType>::CalculateAngle(const RealDataType *B, const RealDataType *P, RealDataType *phi) |
je310 | 4:778bc352c47f | 308 | { |
je310 | 4:778bc352c47f | 309 | /* length of projection of vector AP on yz plane */ |
je310 | 4:778bc352c47f | 310 | RealDataType lyz = _l * _l - P[0] * P[0]; |
je310 | 4:778bc352c47f | 311 | if (lyz <= 0.0) |
je310 | 4:778bc352c47f | 312 | return 1; |
je310 | 4:778bc352c47f | 313 | lyz = std::sqrt(lyz); |
je310 | 4:778bc352c47f | 314 | |
je310 | 4:778bc352c47f | 315 | /* check if gamma angle is correct */ |
je310 | 4:778bc352c47f | 316 | if (P[0] != 0.0) |
je310 | 4:778bc352c47f | 317 | { |
je310 | 4:778bc352c47f | 318 | if (_RAD2DEG_FACTOR * std::atan(lyz / std::abs(P[0])) < _min_parallelogram_angle) |
je310 | 4:778bc352c47f | 319 | return 1; |
je310 | 4:778bc352c47f | 320 | } |
je310 | 4:778bc352c47f | 321 | |
je310 | 4:778bc352c47f | 322 | /* compute vector BP -> both points B and P should be reduced to 2D points (yz) */ |
je310 | 4:778bc352c47f | 323 | RealDataType vector_bp[2]; |
je310 | 4:778bc352c47f | 324 | vector_bp[0] = P[1] - B[1]; |
je310 | 4:778bc352c47f | 325 | vector_bp[1] = P[2] - B[2]; |
je310 | 4:778bc352c47f | 326 | |
je310 | 4:778bc352c47f | 327 | /* check if vector points in right direction */ |
je310 | 4:778bc352c47f | 328 | if (vector_bp[1] >= 0) |
je310 | 4:778bc352c47f | 329 | return 1; |
je310 | 4:778bc352c47f | 330 | |
je310 | 4:778bc352c47f | 331 | /* check if the vector have right size */ |
je310 | 4:778bc352c47f | 332 | RealDataType d = std::sqrt(vector_bp[1] * vector_bp[1] + vector_bp[0] * vector_bp[0]); |
je310 | 4:778bc352c47f | 333 | |
je310 | 4:778bc352c47f | 334 | if (d >= lyz + _Ll || d <= std::abs(lyz - _Ll)) |
je310 | 4:778bc352c47f | 335 | return 1; |
je310 | 4:778bc352c47f | 336 | /* calculate alpha angle */ |
je310 | 4:778bc352c47f | 337 | RealDataType alpha = 180.0 + std::atan2(vector_bp[1], vector_bp[0]) * _RAD2DEG_FACTOR; |
je310 | 4:778bc352c47f | 338 | |
je310 | 4:778bc352c47f | 339 | /* calculate beta angle - cosine theorem */ |
je310 | 4:778bc352c47f | 340 | |
je310 | 4:778bc352c47f | 341 | RealDataType beta = _RAD2DEG_FACTOR * std::acos((_Ll * _Ll + d * d - lyz * lyz) / (2 * _Ll * d)); |
je310 | 4:778bc352c47f | 342 | |
je310 | 4:778bc352c47f | 343 | /* calculate phi angle and check max_neg_angle condition */ |
je310 | 4:778bc352c47f | 344 | *phi = alpha - beta; |
je310 | 4:778bc352c47f | 345 | |
je310 | 4:778bc352c47f | 346 | if (*phi < _max_neg_angle) |
je310 | 4:778bc352c47f | 347 | { |
je310 | 4:778bc352c47f | 348 | return 1; |
je310 | 4:778bc352c47f | 349 | } |
je310 | 4:778bc352c47f | 350 | |
je310 | 4:778bc352c47f | 351 | /* if everything is OK */ |
je310 | 4:778bc352c47f | 352 | return 0; |
je310 | 4:778bc352c47f | 353 | } |
je310 | 4:778bc352c47f | 354 | |
je310 | 4:778bc352c47f | 355 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 356 | void DeltaKinematics<RealDataType>::DeltaVector::Print() |
je310 | 4:778bc352c47f | 357 | { |
je310 | 4:778bc352c47f | 358 | #ifdef LIB_TEST_MODE |
je310 | 4:778bc352c47f | 359 | std::cout << "x = " << x << " " |
je310 | 4:778bc352c47f | 360 | << "y = " << y << " " |
je310 | 4:778bc352c47f | 361 | << "z = " << z << "\n" |
je310 | 4:778bc352c47f | 362 | << "phi1 = " << phi1 << " " |
je310 | 4:778bc352c47f | 363 | << "phi2 = " << phi2 << " " |
je310 | 4:778bc352c47f | 364 | << "phi3 = " << phi3 << "\n"; |
je310 | 4:778bc352c47f | 365 | #endif |
je310 | 4:778bc352c47f | 366 | } |
je310 | 4:778bc352c47f | 367 | template <typename RealDataType> |
je310 | 4:778bc352c47f | 368 | void DeltaKinematics<RealDataType>::DeltaVector::Clear() |
je310 | 4:778bc352c47f | 369 | { |
je310 | 4:778bc352c47f | 370 | x = y = z = phi1 = phi2 = phi3 = 0.0; |
je310 | 4:778bc352c47f | 371 | } |
je310 | 4:778bc352c47f | 372 | |
je310 | 4:778bc352c47f | 373 | /* Explicit instantations */ |
je310 | 4:778bc352c47f | 374 | |
je310 | 4:778bc352c47f | 375 | template class DeltaKinematics<float>; |
je310 | 4:778bc352c47f | 376 | template class DeltaKinematics<double>; |