joshua Elsdon / Mbed OS DeltaRobotFirmware

Delta Robot example

Dependencies:   BufferedSerial Eigen

Fork of TCPSocket_Example by mbed_example

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?

UserRevisionLine numberNew 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>;