#include #include #include typedef double Mat44Type[4][4]; #define SQR(a) ((a)*(a)) static double Tool_Point_Offset[4] = { -18.0, 0.0, -158.0, 0.0 }; static int npivot = 0; static double *ppivot = NULL; #define SIMULA_PIVOT #ifdef SIMULA_PIVOT static double PENN[] = { 205.193, 125.407, 151.311, 0.310492 ,0.950093, 0.008710 ,-0.023754 ,0.0, 205.082, 126.428, 150.513, 0.308154 ,0.950459, 0.023992 ,-0.029005 ,0.0, 202.285, 132.118, 146.935, 0.283977 ,0.958192, -0.002855 ,-0.031029 ,0.0, 206.492, 143.896, 141.909, 0.250345 ,0.966654, 0.043841 ,-0.024236 ,0.0, 222.480, 156.550, 139.601, 0.214389 ,0.974325, 0.060709 ,0.026022 ,0.0, 244.578, 168.611, 143.574, 0.189606 ,0.972267, 0.090813 ,0.101029 ,0.0, 260.446, 170.663, 150.600, 0.190124 ,0.964902, 0.089175 ,0.156453 ,0.0, 285.098, 169.819, 168.498, 0.205854 ,0.942194, 0.080134 ,0.251222 ,0.0, 304.862, 168.167, 190.118, 0.232918 ,0.908440, 0.086714 ,0.335815 ,0.0, 305.529, 146.175, 203.653, 0.301781 ,0.885945, 0.047623 ,0.348448 ,0.0, 300.344, 126.999, 213.944, 0.380827 ,0.861946, 0.046301 ,0.331202 ,0.0, 291.863, 111.493, 218.697, 0.445505 ,0.844680, 0.054774 ,0.291259 ,0.0, 278.195, 99.485 ,217.913 ,0.484470 ,0.840810, 0.053605 ,0.234674 ,0.0, 254.711, 84.158 ,215.869 ,0.527888 ,0.835885, 0.045745 ,0.141552 ,0.0, 242.173, 83.858 ,205.012 ,0.512000 ,0.852665, 0.047961 ,0.089876 ,0.0, 227.372, 83.416 ,196.895 ,0.499168 ,0.864427, 0.044049 ,0.036780 ,0.0, 213.862, 92.300 ,181.128 ,0.448829 ,0.892726, 0.035477 ,-0.005543 ,0.0, 208.705, 104.964, 166.790, 0.394783 ,0.916619, 0.052799 ,-0.028148 ,0.0, 200.571, 118.514, 154.915, 0.339347 ,0.937654, 0.051158 ,-0.052287 ,0.0, 189.482, 131.686, 146.445, 0.286444 ,0.954674, 0.018037 ,-0.076545 ,0.0, 194.934, 144.300, 140.316, 0.239166 ,0.969182, -0.020952 ,-0.052402 ,0.0, 211.300, 160.023, 136.425, 0.179905 ,0.981717, -0.058866 ,0.006178 ,0.0, 239.909, 171.726, 141.139, 0.120179 ,0.979282, -0.123522 ,0.105313 ,0.0, 257.783, 175.577, 147.691, 0.097234 ,0.972024, -0.134297 ,0.165418 ,0.0, 268.543, 172.585, 155.565, 0.094562 ,0.960298, -0.157060 ,0.209250 ,0.0, 265.625, 173.545, 153.345, 0.108467 ,0.967763, -0.115095 ,0.195174 ,0.0, 239.915, 167.139, 142.146, 0.160171 ,0.981472, -0.033442 ,0.097803 ,0.0, 222.056, 159.906, 138.714, 0.192744 ,0.980538, 0.005947 ,0.033549 ,0.0, 211.803, 151.348, 139.690, 0.220707 ,0.975175, 0.008837 ,-0.000504 ,0.0, 209.858, 147.187, 140.971, 0.237989 ,0.970547, 0.030576 ,-0.010416 ,0.0, 211.300, 134.060, 147.558, 0.285218 ,0.957468, 0.038215 ,-0.008917 ,0.0, 218.934, 117.008, 159.771, 0.356435 ,0.930881, 0.078584 ,0.003014 ,0.0, 224.512, 116.959, 161.419, 0.362451 ,0.927088, 0.092608 ,0.017137 ,0.0, 230.900, 114.603, 165.305, 0.387418 ,0.907589, 0.159694 ,0.016773 ,0.0, 248.884, 121.882, 168.153, 0.382382 ,0.902755, 0.180705 ,0.075294 ,0.0, 261.152, 135.329, 166.009, 0.342332 ,0.914546, 0.172984 ,0.127264 ,0.0, 261.569, 151.395, 158.379, 0.281568 ,0.935054, 0.161312 ,0.142099 ,0.0, 253.284, 169.930, 147.068, 0.204909 ,0.960081, 0.142733 ,0.125386 ,0.0, 250.829, 180.770, 143.567, 0.161541 ,0.970578, 0.127677 ,0.123494 ,0.0, 246.667, 191.384, 140.179, 0.123175 ,0.978117, 0.121810 ,0.113861 ,0.0, 256.614, 204.201, 144.125, 0.091524 ,0.973429, 0.141363 ,0.153909 ,0.0, 256.123, 189.648, 144.851, 0.133636 ,0.972871, 0.119079 ,0.146136 ,0.0, 250.601, 165.127, 147.223, 0.205732 ,0.966302, 0.096562 ,0.119644 ,0.0, 234.912, 141.601, 149.771, 0.271708 ,0.957825, 0.066087 ,0.063956 ,0.0, 224.925, 126.152, 155.199, 0.318189 ,0.946515, 0.035857 ,0.036019 ,0.0, 225.314, 115.921, 162.623, 0.354805 ,0.933493, 0.026970 ,0.040203 ,0.0, 212.080, 101.199, 171.041, 0.402295 ,0.915321, -0.006868 ,0.004683 ,0.0, 198.168, 88.504 ,181.929 ,0.447168 ,0.892528 , -0.051059 , -0.024948 ,0.0, 213.653, 88.382 ,185.340 ,0.453159 ,0.890306 , -0.034450 , 0.022761 ,0.0, 236.142, 86.103 ,197.840 ,0.485069 ,0.870031 , 0.011162 , 0.085483 ,0.0, 248.040, 84.260 ,208.860 ,0.529943 ,0.837736 , 0.096496 , 0.087367 ,0.0, 257.563, 83.180 ,219.917 ,0.553947 ,0.817175 , 0.100471 , 0.121888 ,0.0, 261.845, 88.265 ,215.902 ,0.534869 ,0.826715 , 0.103883 ,0.138697 ,0.0, 275.546, 103.204, 208.003, 0.471405 ,0.853561, 0.085258 ,0.204044 ,0.0, 285.000, 108.057, 213.858, 0.447116 ,0.853138, 0.047614 ,0.263654 ,0.0, 296.598, 118.703, 217.564, 0.402299 ,0.854723, 0.024883 ,0.326520 ,0.0, 295.031, 122.303, 209.579, 0.367179 ,0.872840, 0.000385 ,0.321056 ,0.0, 302.682, 134.942, 207.825, 0.296730 ,0.883103, -0.037616 ,0.361168 ,0.0, 313.894, 150.197, 213.755, 0.225477 ,0.878560, -0.058830 ,0.416488 ,0.0, 316.260, 158.340, 211.787, 0.171365 ,0.883338, -0.089318 ,0.426652 ,0.0, 316.511, 167.910, 207.154, 0.066791 ,0.882879, -0.183682 ,0.426513 ,0.0, 317.683, 181.661, 204.298, 0.000739 ,0.884780, -0.203962 ,0.418598 ,0.0, 316.182, 183.522, 200.564, 0.000319 ,0.892102, -0.197583 ,0.406013 ,0.0, 305.678, 186.633, 185.247, -0.002808 ,0.914505, -0.201898 ,0.349823 ,0.0, 295.231, 200.922, 172.261, -0.039871 ,0.934377, -0.197721 ,0.293170 ,0.0, 296.422, 219.845, 174.025, -0.093948 ,0.937474, -0.168954 ,0.288864 ,0.0, 291.272, 216.797, 168.802, -0.074663 ,0.945757, -0.162269 ,0.270827 ,0.0, 280.915, 217.763, 160.102, -0.059801 ,0.960937, -0.138695 ,0.231222 ,0.0, 270.922, 221.102, 153.587, -0.054319 ,0.972359, -0.113077 ,0.196055 ,0.0, 261.393, 222.536, 147.655, -0.054398 ,0.979385, -0.109178 ,0.159798 ,0.0, 250.717, 214.694, 141.359, -0.015119 ,0.988451, -0.082158 ,0.125305 ,0.0, 241.732, 216.097, 137.726, -0.010987 ,0.993278, -0.061168 ,0.095876 ,0.0, 233.733, 216.737, 134.744, -0.009419 ,0.995912, -0.054291 ,0.068804 ,0.0, 214.199, 195.495, 130.040, 0.059680 ,0.995107, -0.076293 ,0.008203 ,0.0, 196.155, 187.490, 128.498, 0.094514 ,0.993486, -0.034134 ,-0.050475 ,0.0, 188.643, 182.734, 128.912, 0.110711 ,0.990272, -0.037966 ,-0.072176 ,0.0, 177.463, 177.653, 129.931, 0.128680 ,0.985261, -0.014205 ,-0.110443 ,0.0, 177.968, 167.839, 132.270, 0.160711 ,0.980623, -0.030644 ,-0.105617 ,0.0, 174.069, 161.331, 134.295, 0.182098 ,0.975455, -0.037662 ,-0.116948 ,0.0, 171.851, 150.571, 138.297, 0.217020 ,0.966949, -0.050942 ,-0.121916 ,0.0, 167.042, 144.244, 141.444, 0.238815 ,0.959441, -0.065572 ,-0.133356 ,0.0, 162.840, 137.188, 145.306, 0.264035 ,0.949916, -0.084579 ,-0.142801 ,0.0, 192.383, 134.195, 145.055, 0.268715 ,0.959362, -0.068676 ,-0.049839 ,0.0, 206.064, 131.586, 147.599, 0.276857 ,0.958732, -0.061228 ,-0.006536 ,0.0, 214.047, 122.049, 154.914, 0.309516 ,0.947819, -0.069735 ,0.025423 ,0.0, 221.765, 113.572, 162.884, 0.340387 ,0.934911, -0.079563 ,0.058276 ,0.0, 235.160, 114.564, 166.957, 0.339105 ,0.932742, -0.066410 ,0.101202 ,0.0, 231.355, 116.715, 163.798, 0.332658 ,0.937040, -0.061355 ,0.085134 ,0.0, 224.451, 134.166, 150.297, 0.265657 ,0.959461, -0.072122 ,0.057519 ,0.0, 220.490, 142.885, 144.868, 0.233360 ,0.968114, -0.077802 ,0.042736 ,0.0, 222.157, 149.381, 142.400, 0.210622 ,0.973107, -0.078539 ,0.046793 ,0.0, 220.476, 158.040, 138.984, 0.177201 ,0.978098, -0.099025 ,0.041697 ,0.0, 222.930, 169.059, 136.433, 0.140039 ,0.984241, -0.097077 ,0.045899 ,0.0, 234.592, 166.179, 140.307, 0.146352 ,0.980527, -0.098048 ,0.085276 ,0.0, 252.389, 177.070, 144.608, 0.104360 ,0.978802, -0.101554 ,0.142970 ,0.0, 258.528, 186.276, 146.699, 0.067054 ,0.976915, -0.118779 ,0.163648 ,0.0, 252.267, 163.634, 148.568, 0.154352 ,0.972458, -0.090652 ,0.148047 ,0.0, 237.323, 143.758, 149.959, 0.218123 ,0.962710, -0.116516 ,0.108461 ,0.0, 224.927, 139.836, 147.460, 0.232809 ,0.962134, -0.122536 ,0.068322 ,0.0, 203.789, 123.773, 151.927, 0.295860 ,0.947605, -0.118777 ,0.003534 ,0.0, 197.177, 114.988, 157.456, 0.332131 ,0.936113, -0.113370 ,-0.013836 ,0.0, 204.120, 103.891, 167.186, 0.381010 ,0.922281, -0.062147 ,-0.003598 ,0.0, 206.172, 97.585 ,173.571 ,0.415167 ,0.909422 , -0.012350 ,-0.013232 ,0.0, 210.831, 93.557 ,178.741 ,0.440592 ,0.896871 , 0.032168 ,-0.014900 ,0.0, 222.731, 95.794 ,179.734 ,0.448129 ,0.887465 , 0.105920 ,-0.002025 ,0.0, 235.059, 97.590 ,182.686 ,0.460135 ,0.873282 , 0.157831 ,0.019538 ,0.0, 249.905, 105.653, 182.003, 0.455749 ,0.860413, 0.221721 ,0.049379 ,0.0, 258.759, 109.855, 184.561, 0.456207 ,0.855238, 0.231494 ,0.080764 ,0.0, 255.452, 111.991, 180.185, 0.432512 ,0.877229, 0.188149 ,0.087897 ,0.0, 247.899, 117.226, 171.043, 0.387242 ,0.908612, 0.130107 ,0.085051 ,0.0, 251.457, 128.371, 164.893, 0.344274 ,0.925300, 0.117891 ,0.105732 ,0.0, 255.851, 138.351, 161.605, 0.299310 ,0.941224, 0.077362 ,0.135884 ,0.0, 256.546, 142.443, 159.739, 0.272908 ,0.949743, 0.038596 ,0.147298 ,0.0, 253.840, 146.668, 155.755, 0.244761 ,0.958748, 0.003516 ,0.143429 ,0.0, 252.821, 155.062, 151.619, 0.207323 ,0.967366, -0.018823 ,0.142764 ,0.0, 250.163, 156.250, 150.206, 0.199057 ,0.969865, -0.032092 ,0.135872 ,0.0, 246.505, 153.491, 149.384, 0.208965 ,0.969676, -0.027660 ,0.122130 ,0.0, 243.969, 154.227, 148.000, 0.209529 ,0.971158, -0.014427 ,0.111100 ,0.0, 244.870, 160.972, 146.076, 0.183938 ,0.975703, -0.025525 ,0.114915 ,0.0, 247.896, 166.357, 145.703, 0.162004 ,0.977652, -0.038981 ,0.127069 ,0.0, 243.754, 161.990, 144.937, 0.172682 ,0.977113, -0.049203 ,0.112503 ,0.0, 245.514, 177.295, 142.022, 0.136561 ,0.983926, 0.003587 ,0.113569 ,0.0, 244.229, 181.447, 140.524, 0.123061 ,0.986184, 0.004208 ,0.109118 ,0.0, 226.662, 179.916, 134.458, 0.128366 ,0.990244, 0.019872 ,0.046591 ,0.0, 215.979, 177.763, 132.342, 0.136675 ,0.989807, 0.034350 ,0.009995 ,0.0, 206.927, 161.792, 135.057, 0.188410 ,0.981022, 0.037408 ,-0.020663 ,0.0, 206.456, 158.614, 136.081, 0.202799 ,0.977081, 0.057859 ,-0.024065 ,0.0, 218.451, 153.990, 139.920, 0.219879 ,0.973926, 0.052107 ,0.014808 ,0.0, 226.124, 147.020, 144.471, 0.249821 ,0.965007, 0.069687 ,0.035285 ,0.0, 214.126, 136.167, 146.683, 0.281549 ,0.957444, 0.060413 ,-0.004762 ,0.0, 216.456, 131.145, 149.989, 0.300719 ,0.951372, 0.063381 ,0.001089 ,0.0, 215.413, 112.697, 162.256, 0.371387 ,0.925052, 0.076327 ,-0.009934 ,0.0, 202.388, 101.992, 168.570, 0.404749 ,0.910169, 0.066121 ,-0.054999 ,0.0, 198.262, 108.121, 162.661, 0.379511 ,0.919678, 0.070203 ,-0.069061 ,0.0, 193.416, 127.132, 148.896, 0.306004 ,0.946821, 0.059874 ,-0.077408 ,0.0, 192.555, 147.685, 138.655, 0.231222 ,0.970422, 0.016363 ,-0.066020 ,0.0, 183.541, 154.432, 136.000, 0.205759 ,0.974065, -0.001173 ,-0.092649 ,0.0, 202.012, 150.264, 138.250, 0.219213 ,0.974972, -0.014894 ,-0.031168 ,0.0, 238.891, 162.015, 143.496, 0.185054 ,0.978122, -0.008975 ,0.093113 ,0.0, 257.951, 194.745, 145.257, 0.062744 ,0.984573, -0.043219 ,0.156189 ,0.0, 272.489, 195.124, 153.667, 0.057909 ,0.974453, -0.052771 ,0.210060 ,0.0, 282.343, 209.506, 160.256, 0.012640 ,0.968387, -0.036894 ,0.245903 ,0.0, 280.890, 199.457, 159.851, 0.049825 ,0.968048, -0.032593 ,0.242691 ,0.0, 283.922, 172.579, 165.988, 0.133251 ,0.954743, -0.056533 ,0.259163 ,0.0, 298.105, 171.754, 180.554, 0.154205 ,0.934454, -0.026051 ,0.319561 ,0.0, 293.612, 168.030, 176.930, 0.169476 ,0.938508, -0.020287 ,0.299521 ,0.0, 270.353, 153.913, 163.015, 0.218058 ,0.953295, -0.013096 ,0.207882 ,0.0, 255.931, 136.099, 162.604, 0.274886 ,0.947772, -0.026253 ,0.159208 ,0.0, 251.665, 121.054, 170.957, 0.333588 ,0.930856, -0.018070 ,0.146951 ,0.0, 246.833, 109.558, 177.747, 0.381971 ,0.914809, -0.008396 ,0.129390 ,0.0, 239.033, 109.029, 173.881, 0.378958 ,0.919675, -0.010201 ,0.100677 ,0.0, 222.020, 112.251, 164.196, 0.353185 ,0.933013, -0.045337 ,0.049808 ,0.0, 213.051, 114.220, 160.599, 0.343929 ,0.937150, -0.051671 ,0.021137 ,0.0, 202.270, 105.982, 164.963, 0.371873 ,0.925364, -0.071572 ,-0.007879 ,0.0, 199.475, 93.739 ,176.430 ,0.418570 ,0.903580, -0.089573 ,-0.004905 ,0.0, 207.614, 101.898, 170.007, 0.389408 ,0.918257, -0.067887 ,0.012396 ,0.0, 222.556, 122.338, 156.643, 0.316825 ,0.946772, -0.031776 ,0.043452 ,0.0, 223.389, 147.654, 143.496, 0.227122 ,0.972210, -0.030430 ,0.043891 ,0.0, 217.356, 160.708, 137.119, 0.185322 ,0.982308, -0.013311 ,0.018090 ,0.0, 201.752, 162.891, 134.080, 0.178834 ,0.983159, -0.001771 ,-0.032550 ,0.0, 188.600, 155.119, 135.887, 0.203656 ,0.976093, -0.011271 ,-0.073577 ,0.0, 182.959, 140.481, 141.899, 0.252447 ,0.963079, -0.025404 ,-0.088431 ,0.0, 182.179, 118.884, 154.048, 0.328538 ,0.939510, -0.017489 ,-0.093788 ,0.0, 184.747, 100.312, 168.717, 0.400235 ,0.911947, -0.009445 ,-0.088065 ,0.0, 189.791, 96.491 ,172.884 ,0.418140 ,0.904954 , 0.007761 ,-0.076103 ,0.0, 204.170, 96.197 ,174.261 ,0.426309 ,0.902274 , 0.045105 ,-0.043035 ,0.0, 228.727, 106.698, 171.305, 0.409460 ,0.905282, 0.110216 ,0.020582 ,0.0, 236.659, 116.305, 166.164, 0.379802 ,0.914833, 0.127819 ,0.046884 ,0.0, 247.212, 132.985, 159.723, 0.339591 ,0.918927, 0.184909 ,0.075731 ,0.0, 255.755, 151.582, 154.704, 0.291083 ,0.924453, 0.219501 ,0.110089 ,0.0, 258.030, 169.594, 149.535, 0.234242 ,0.936510, 0.225176 ,0.130518 ,0.0, 256.697, 179.325, 146.540, 0.191861 ,0.951518, 0.196238 ,0.137051 ,0.0, 262.010, 200.531, 147.231, 0.108526 ,0.968221, 0.144928 ,0.171988 ,0.0, 263.358, 208.197, 147.485, 0.035966 ,0.983521, 0.009141 ,0.176102 ,0.0, 266.709, 216.351, 150.153, -0.019176 ,0.980209, -0.066188 ,0.184956 ,0.0, 258.292, 209.419, 144.841, -0.038679 ,0.968201, -0.198496 ,0.145955 ,0.0, 243.118, 201.922, 137.821, -0.005782 ,0.972680, -0.209378 ,0.098234 ,0.0, 219.378, 180.792, 132.940, 0.063214 ,0.957525, -0.278140 ,0.039701 ,0.0, 203.790, 170.380, 132.381, 0.118640 ,0.965321, -0.232085 ,-0.003582 ,0.0, 198.898, 162.701, 133.958, 0.148920 ,0.964051, -0.218924 ,-0.015522 ,0.0, 193.365, 153.273, 136.531, 0.183803 ,0.958472, -0.215517 ,-0.028337 ,0.0, 193.520, 148.686, 138.436, 0.202133 ,0.959998, -0.190448 ,-0.027733 ,0.0, 186.668, 144.070, 140.046, 0.224036 ,0.957936, -0.170877 ,-0.051948 ,0.0, 230.050, 141.703, 148.144, 0.235381 ,0.965097, -0.083454 ,0.076852 ,0.0, 256.548, 145.464, 158.031, 0.239656 ,0.957381, -0.024236 ,0.158275 ,0.0, 274.725, 151.707, 167.329, 0.242864 ,0.944741, 0.024866 ,0.218052 ,0.0, 263.197, 144.107, 162.806, 0.258928 ,0.949146, 0.007821 ,0.178099 ,0.0, 239.679, 132.931, 156.302, 0.279981 ,0.953885, -0.031633 ,0.102320 ,0.0, 231.171, 133.357, 152.869, 0.274023 ,0.957546, -0.045455 ,0.075112 ,0.0, 238.672, 140.044, 152.036, 0.256582 ,0.961228, -0.025947 ,0.095808 ,0.0, 236.331, 132.812, 155.298, 0.275975 ,0.955043, -0.048452 ,0.095560 ,0.0, 228.056, 132.157, 152.788, 0.276337 ,0.956569, -0.058890 ,0.069965 ,0.0, 248.128, 151.525, 150.769, 0.224333 ,0.966494, -0.001053 ,0.123230 ,0.0, 229.191, 139.174, 149.033, 0.255071 ,0.963664, -0.039541 ,0.066476 ,0.0, 230.371, 139.362, 149.760, 0.254282 ,0.963506, -0.040180 ,0.071339 ,0.0, 223.199, 129.144, 152.933, 0.287140 ,0.954603, -0.057695 ,0.052804 ,0.0, 210.523, 124.410, 152.717, 0.303469 ,0.951026, -0.054392 ,0.009458 ,0.0, 187.125, 107.459, 162.675, 0.359843 ,0.922379, -0.133880 ,-0.038317 ,0.0, 190.255, 114.836, 156.987, 0.328736 ,0.932554, -0.145315 ,-0.028088 ,0.0, 199.889, 135.857, 144.835, 0.248943 ,0.955829, -0.155417 ,-0.004250 ,0.0, 203.814, 155.760, 136.442, 0.178205 ,0.971142, -0.157389 ,-0.005692 ,0.0, 206.226, 177.153, 131.223, 0.114825 ,0.986253, -0.117030 ,-0.010100 ,0.0, 201.475, 176.045, 131.009, 0.119997 ,0.985536, -0.115595 ,-0.025273 ,0.0, 184.381, 153.578, 136.297, 0.200662 ,0.971917, -0.094265 ,-0.076673 ,0.0, 176.662, 143.230, 140.809, 0.238829 ,0.960786, -0.101772 ,-0.096026 ,0.0, 189.895, 134.885, 144.721, 0.269635 ,0.959935, -0.039914 ,-0.063074 ,0.0, 205.156, 107.079, 164.650, 0.376784 ,0.925946, -0.010014 ,-0.018116 ,0.0, 226.719, 101.881, 174.831, 0.411190 ,0.909864, 0.030871 ,0.040944 ,0.0, 229.530, 91.839 ,186.853 ,0.456013 ,0.887894, 0.024800 ,0.053653 ,0.0, 234.946, 95.365 ,185.142 ,0.450465 ,0.888763, 0.057462 ,0.059652 ,0.0, 247.580, 95.877 ,192.312 ,0.474760 ,0.868744, 0.113103 ,0.082609 ,0.0, 255.021, 90.193 ,205.597 ,0.520995 ,0.836188, 0.142554 ,0.092925 ,0.0, 258.085, 85.289 ,216.363 ,0.562467 ,0.803486, 0.176118 ,0.081496 ,0.0, 245.190, 79.796 ,214.582 ,0.565691 ,0.807635, 0.160831 ,0.038943 ,0.0, 231.145, 90.761 ,189.307 ,0.486296 ,0.861801, 0.143369 ,0.007981 ,0.0, 241.501, 142.985, 151.790, 0.278477 ,0.951982, 0.094668 ,0.082346 ,0.0, 243.741, 167.013, 143.858, 0.193199 ,0.972129, 0.086071 ,0.099398 ,0.0, 258.135, 153.000, 155.695, 0.216340 ,0.962391, -0.017864 ,0.162465 ,0.0, 270.609, 156.907, 161.940, 0.173333 ,0.954630, -0.098744 ,0.220833 ,0.0, 272.922, 153.604, 165.045, 0.108975 ,0.923653, -0.266913 ,0.251937 ,0.0, 271.493, 155.840, 162.997, 0.023100 ,0.866008, -0.430602 ,0.252474 ,0.0, 268.242, 169.124, 155.934, -0.032391 ,0.852332, -0.474246 ,0.217454 ,0.0, 264.732, 177.462, 151.463, -0.069502 ,0.832469, -0.516527 ,0.187055 ,0.0, 264.538, 182.853, 150.448, -0.060957 ,0.865402, -0.462631 ,0.181233 ,0.0, 263.070, 182.308, 149.782, 0.029838 ,0.950805, -0.247222 ,0.183429 ,0.0, 257.715, 185.604, 146.095, 0.112026 ,0.981283, 0.009371 ,0.155431 ,0.0, 248.874, 188.155, 141.188, 0.142939 ,0.970025, 0.156846 ,0.117175 ,0.0, 239.788, 194.310, 137.160, 0.149512 ,0.943070, 0.283346 ,0.088067 ,0.0, 236.285, 198.256, 135.543, 0.154604 ,0.907965, 0.381169 ,0.077985 ,0.0, 233.284, 196.347, 134.798, 0.162541 ,0.896241, 0.406937 ,0.066073 ,0.0, 233.877, 180.206, 136.889, 0.192912 ,0.929485, 0.309189 ,0.053492 ,0.0, 230.829, 145.530, 146.402, 0.288316 ,0.927334, 0.236833 ,0.019784 ,0.0, 223.865, 119.727, 159.205, 0.378175 ,0.880493, 0.283060 ,-0.037661 ,0.0, 245.854, 118.738, 168.484, 0.399955 ,0.887452, 0.223284 ,0.048310 ,0.0, 264.633, 119.455, 180.789, 0.400518 ,0.894070, 0.137778 ,0.144468 ,0.0, 273.824, 113.311, 194.679, 0.466083 ,0.845888, 0.214100 ,0.145126 ,0.0, 278.708, 105.672, 208.187, 0.528590 ,0.794814, 0.269043 ,0.127269 ,0.0, 278.755, 103.584, 211.176, 0.534651 ,0.795694, 0.250807 ,0.133942 ,0.0, 283.842, 110.445, 209.390, 0.481365 ,0.836628, 0.158513 ,0.206892 ,0.0, 288.607, 120.467, 202.916, 0.400365 ,0.874084, 0.064891 ,0.266647 ,0.0, 285.159, 131.774, 189.114, 0.288989 ,0.911220, -0.056788 ,0.287292 ,0.0, 277.401, 152.188, 168.787, 0.168417 ,0.941277, -0.142409 ,0.255026 ,0.0, 269.601, 168.372, 157.084, 0.088533 ,0.948930, -0.207828 ,0.219457 ,0.0, 261.033, 172.161, 150.515, 0.061745 ,0.946326, -0.256153 ,0.186038 ,0.0, 229.217, 148.539, 144.406, 0.212356 ,0.971272, -0.078512 ,0.070094 ,0.0, 204.697, 133.929, 146.227, 0.265034 ,0.960251, -0.085497 ,-0.004514 ,0.0, 192.861, 126.341, 149.455, 0.295977 ,0.951062, -0.075342 ,-0.042792 ,0.0, 194.113, 130.520, 146.942, 0.281960 ,0.956309, -0.060060 ,-0.045034 ,0.0, 203.770, 146.785, 140.046, 0.232545 ,0.972104, -0.008565 ,-0.024743 ,0.0, 213.984, 165.569, 135.062, 0.174917 ,0.984145, 0.020257 ,0.004259 ,0.0, 219.752, 188.217, 131.802, 0.103831 ,0.993392, 0.036935 ,0.025508 ,0.0, 219.753, 204.882, 130.459, 0.048197 ,0.997735, 0.034635 ,0.025326 ,0.0, 212.677, 204.391, 129.287, 0.052338 ,0.997219, 0.049801 ,0.004332 ,0.0, 203.479, 180.791, 130.427, 0.122207 ,0.991793, 0.016870 ,-0.028015 ,0.0, 190.954, 163.956, 133.014, 0.173569 ,0.982247, -0.014841 ,-0.066774 ,0.0, 166.255, 152.640, 138.082, 0.211386 ,0.965912, -0.046754 ,-0.140522 ,0.0, 180.229, 152.753, 136.743, 0.211170 ,0.972016, -0.018839 ,-0.099745 ,0.0, 214.867, 152.221, 139.661, 0.219786 ,0.975109, 0.020547 ,0.006410 ,0.0, 254.596, 164.081, 149.684, 0.200168 ,0.967903, 0.056713 ,0.139280 ,0.0, 266.188, 176.683, 152.928, 0.168684 ,0.965544, 0.077984 ,0.181367 ,0.0, 265.116, 167.282, 154.019, 0.198135 ,0.961838, 0.073118 ,0.173100 ,0.0, 257.691, 120.967, 174.178, 0.357161 ,0.920709, 0.048945 ,0.147999 ,0.0, 252.531, 98.872 ,191.788 ,0.450204 ,0.882034, 0.055984 ,0.125730 ,0.0, 244.056, 82.984 ,207.031 ,0.512543 ,0.851558, 0.030128 ,0.105023 ,0.0, 254.783, 105.883, 186.325, 0.424477 ,0.893265, 0.063281 ,0.132801 ,0.0, 267.800, 136.263, 170.049, 0.318974 ,0.927240, 0.089361 ,0.173703 ,0.0, 278.985, 166.176, 164.707, 0.221782 ,0.943830, 0.099628 ,0.222988 ,0.0, 284.234, 191.837, 162.670, 0.133376 ,0.953701, 0.097409 ,0.250855 ,0.0, 277.639, 177.796, 160.076, 0.166848 ,0.957677, 0.068011 ,0.223791 ,0.0, 257.933, 157.332, 153.897, 0.233001 ,0.957810, 0.081387 ,0.146199 ,0.0, 231.757, 143.531, 147.856, 0.261531 ,0.961767, 0.055578 ,0.056863 ,0.0, 208.088, 131.501, 148.126, 0.290291 ,0.956323, 0.021409 ,-0.016712 ,0.0, 202.108, 133.994, 145.942, 0.277594 ,0.959925, 0.005111 ,-0.032888 ,0.0, 217.572, 151.379, 140.897, 0.215022 ,0.975731, -0.028460 ,0.024625 ,0.0, 249.513, 182.962, 142.594, 0.097024 ,0.984364, -0.065503 ,0.130187 ,0.0, 260.513, 185.399, 147.689, 0.093918 ,0.980048, -0.046001 ,0.168051 ,0.0, 213.190, 147.707, 141.288, 0.222474 ,0.973272, -0.051896 ,0.013348 ,0.0, 197.043, 141.555, 141.699, 0.243108 ,0.967073, -0.064030 ,-0.035342 ,0.0, 214.149, 113.379, 161.303, 0.353404 ,0.935177, -0.009793 ,0.011231 ,0.0, 218.664, 87.153 ,188.238 ,0.466441 ,0.883960, 0.000977 ,0.025689 ,0.0, 231.915, 95.797 ,183.349 ,0.439153 ,0.895660, 0.022158 ,0.063631 ,0.0, 251.751, 107.941, 181.482, 0.411210 ,0.901393, 0.061621 ,0.119764 ,0.0, 263.506, 124.160, 176.065, 0.366530 ,0.912081, 0.096461 ,0.155429 ,0.0, 270.475, 142.252, 169.433, 0.310471 ,0.925622, 0.116231 ,0.181641 ,0.0, 268.797, 159.017, 159.660, 0.248636 ,0.943705, 0.124558 ,0.178137 ,0.0, 259.714, 165.057, 151.892, 0.218431 ,0.957119, 0.117055 ,0.148851 ,0.0, 253.109, 166.087, 147.941, 0.208296 ,0.963330, 0.110663 ,0.126890 ,0.0, 233.199, 153.108, 144.139, 0.234759 ,0.966526, 0.082426 ,0.059264 ,0.0, 219.012, 141.099, 145.511, 0.263228 ,0.963462, 0.042948 ,0.016676 ,0.0, 223.191, 144.378, 144.899, 0.254668 ,0.965021, 0.051814 ,0.028449 ,0.0, 230.041, 153.198, 143.366, 0.233449 ,0.967286, 0.084836 ,0.048790 ,0.0, 237.652, 160.045, 143.725, 0.214758 ,0.969634, 0.086083 ,0.076731 ,0.0, 243.980, 163.258, 144.582, 0.205896 ,0.969721, 0.087690 ,0.096205 ,0.0, 229.957, 147.876, 145.110, 0.234841 ,0.970090, 0.006357 ,0.058767 ,0.0, 225.853, 144.029, 145.591, 0.241104 ,0.968757, -0.023898 ,0.050612 ,0.0, 243.554, 158.162, 146.481, 0.204058 ,0.972949, 0.011920 ,0.106007 ,0.0, 262.263, 171.994, 151.352, 0.174495 ,0.968638, 0.053345 ,0.167617 ,0.0, 273.967, 184.674, 156.264, 0.144361 ,0.963273, 0.076861 ,0.211827 ,0.0, 272.951, 181.722, 156.023, 0.152627 ,0.963317, 0.072298 ,0.207623 ,0.0 }; #endif static inline double SQRd(double x) {return x*x;} static void Mat44FromQuaternion(Mat44Type t, double *a) //Quaternion { double x, y, z, w, Nq; double X,Y,Z, s; double wX, wY, wZ, xX, xY, xZ, yY, yZ, zZ; x = a[1]; y = a[2]; z = a[3]; w = a[0]; Nq = SQR(w) + SQR(x) + SQR(y) + SQR(z); if (Nq > 0.0000001) s = 2.0/Nq; else s = 0.0; X = x*s; Y = y*s; Z = z*s; wX = w*X; wY = w*Y; wZ = w*Z; xX = x*X; xY = x*Y; xZ = x*Z; yY = y*Y; yZ = y*Z; zZ = z*Z; t[0][0] = 1.0-(yY+zZ); t[0][1] = xY-wZ; t[0][2] = xZ+wY; t[1][0] = xY+wZ; t[1][1] = 1.0-(xX+zZ); t[1][2] = yZ-wX; t[2][0] = xZ-wY; t[2][1] = yZ+wX; t[2][2] = 1.0-(xX+yY); } static void ComputeToolPoint(double *t, double *q, double *pp) { Mat44Type mat; double poffset[4], *v; Mat44FromQuaternion(mat, q); //Rotation Matix from Quaternions v = Tool_Point_Offset; poffset[0] = mat[0][0]*v[0] + mat[0][1]*v[1] + mat[0][2]*v[2]; poffset[1] = mat[1][0]*v[0] + mat[1][1]*v[1] + mat[1][2]*v[2]; poffset[2] = mat[2][0]*v[0] + mat[2][1]*v[1] + mat[2][2]*v[2]; pp[0] = t[0] + poffset[0]; pp[1] = t[1] + poffset[1]; pp[2] = t[2] + poffset[2]; } static int fobj_offset(int m, double *a, double *fun) //this is objective funct of an optimization { int n = npivot; double *p = ppivot; int i; double cen[3*n]; double x=0.0,y=0.0,z=0.0; double err=0.0; Tool_Point_Offset[0] = a[0]; Tool_Point_Offset[1] = a[1]; Tool_Point_Offset[2] = a[2]; // printf("befor: %lf %lf %lf\n", Tool_Point_Offset[0], Tool_Point_Offset[1], Tool_Point_Offset[1]); #pragma omp parallel for reduction(+ : x,y,z) num_threads (2) for (i=0; i