function [new_config, new_ctrm, new_norms]  = buddy140_km(sp_coords,Theta)

% This code represents the kinematic model created by 'Build_data_frame' as a function.

%%% INPUTS %%%
% sp_coords: a 4 x 1 vector for global position, R3, and rotation, [-pi,pi], in the spatial frame.
% Theta: a vector of N x 1 angles, where N is the number of rotation axes.
%%% Outputs %%%
% new_config: a point cloud from the surface of the kinematic model in the new configuration.
% new_ctrm: a new center of mass estimate for the kinematic model.

% new_norms: a new norm associated with the model point is calculated to reflect the new pose.

%%% Sampled meshes for each segment
I1  = [  0.044081  0.047957  0.046628  0.044177  0.046973  0.041779  0.040622  0.041175  0.035240  0.034748  0.037021  0.035800  0.036003  0.036224  0.036575  0.036652  0.036141  0.034849  0.024070  0.019157  0.008776  0.019864  0.008735  0.014492  0.020627  0.025182  0.029185  0.031698  0.035491  0.037868  0.041423  0.044743  0.047164  0.046583  0.050937  0.035276  0.028400  0.023336  0.042767  0.050300  0.044358  0.033453  0.035425  0.037871  0.031229  0.026803  0.020465  0.021162  0.026447  0.026127  0.031478  0.030293  0.030601  0.027489  0.028188  0.022225  0.022667  0.028554  0.022036  0.019128  0.017643  0.010965  0.008436 -0.010017 -0.005280 -0.010124 -0.017634 -0.005005 -0.014465 -0.006459 -0.006689 -0.012043  0.006231 -0.005899 -0.009183 -0.000917 -0.007002 -0.003068 -0.004746 -0.007794 -0.014313 -0.033063 -0.020626 -0.007614 -0.015627 -0.027616 -0.026054 -0.016569 -0.020587 -0.010154 -0.029006 -0.006167 -0.008660  0.011834  0.007152  0.010409 -0.010674  0.007439  0.016686  0.008565  0.007182  0.017701  0.004813  0.004389  0.019722 -0.000514 -0.000514  0.015410  0.018507  0.029138  0.030474  0.029094  0.020097  0.029223  0.033407  0.041578  0.033195  0.020169  0.003778  0.002473  0.019578  0.032496  0.033358  0.038842  0.039217  0.043870  0.047798  0.040843
        -0.044591 -0.053452 -0.054579 -0.056610 -0.061246 -0.051879 -0.046948 -0.057483 -0.052142 -0.038952 -0.035517 -0.030050 -0.029433 -0.028723 -0.028006 -0.027434 -0.028133 -0.030035 -0.022839 -0.146143 -0.131814 -0.128004 -0.112214 -0.101020 -0.104277 -0.100010 -0.100871 -0.105586 -0.106340 -0.091914 -0.100734 -0.084305 -0.082402 -0.076051 -0.079399 -0.126988 -0.132551 -0.133342 -0.111215 -0.060146 -0.073832 -0.090613 -0.084098 -0.078961 -0.073083 -0.084128 -0.079300 -0.065893 -0.060608 -0.052260 -0.059419 -0.038435 -0.024628  0.002083 -0.005913  0.009559 -0.002265 -0.010884 -0.008468 -0.011128 -0.007704 -0.008945 -0.011773 -0.141268 -0.133064 -0.128064 -0.119134 -0.111186 -0.106745 -0.104938 -0.086116 -0.080098  0.034544  0.031093  0.028047  0.015283  0.016604  0.008018  0.005097  0.002223  0.008441  0.050928  0.048055  0.038300  0.035074  0.038305  0.029129  0.018269  0.013852  0.023598  0.050214  0.008392  0.020207  0.001839  0.009656  0.012199  0.031330 -0.072535 -0.073269 -0.078413 -0.091371 -0.084140 -0.101091 -0.108233  0.024584 -0.093501 -0.155669  0.003049  0.006894 -0.010805 -0.016086 -0.011355 -0.050011 -0.065590 -0.062491 -0.078975 -0.116491 -0.120181 -0.117739 -0.141183 -0.019020 -0.021174 -0.042016 -0.039545 -0.039230 -0.065303 -0.058228 -0.038123
         0.098196  0.088529  0.076702  0.064263  0.069163  0.053836  0.033806  0.025398  0.010647  0.011326  0.023145  0.031630  0.044723  0.057794  0.070799  0.083925  0.097902  0.112618  0.126394  0.027385  0.009339  0.014616 -0.003687 -0.006803  0.003312  0.008084  0.019399  0.028817  0.037191  0.033729  0.046983  0.050505  0.060859  0.061382  0.069667  0.039209  0.031235  0.021235  0.051391  0.082492  0.045382  0.019611  0.016984  0.020137  0.004076 -0.001001 -0.014522 -0.015529 -0.006047 -0.007905  0.003483 -0.001109  0.002801  0.031719  0.046427  0.052936  0.071387  0.070989  0.092477  0.108496  0.103373  0.126183  0.141324  0.009837  0.003598 -0.002115 -0.017763 -0.016293 -0.025848 -0.022567 -0.034258 -0.039998  0.048803  0.066734  0.082614  0.093918  0.107945  0.120181  0.132881  0.148723  0.146738  0.068803  0.062174  0.058486  0.080697  0.096010  0.119336  0.128678  0.140871  0.097815  0.069181  0.128152  0.102828  0.093944  0.085990  0.070326  0.077881 -0.035153 -0.022163 -0.031444 -0.023182 -0.015827 -0.016507 -0.010625  0.043830 -0.026875  0.021991  0.080115  0.062831  0.052851  0.036722  0.026241 -0.019841 -0.000590  0.007094  0.031576  0.034304  0.011985 -0.003746  0.012405  0.128941  0.049074  0.007299  0.028769  0.046318  0.052605  0.074088  0.107786
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];
N1  = [  0.913454  0.913896  0.952672  0.992845  0.948412  1.026179  1.022757  1.014371  0.930147  0.964451  0.990119  0.963034  0.964533  0.967985  0.973606  0.959457  0.903525  0.812940  0.730408  0.679579  0.469647  0.660367  0.588720  0.682357  0.749162  0.897457  0.922334  0.920589  0.904846  0.908088  0.954342  1.019728  0.991626  0.995188  0.932287  0.858835  0.862462  0.866393  0.931421  0.939425  1.039938  0.868544  0.918487  0.970134  0.905175  0.916475  0.824232  0.893279  0.907677  0.916979  0.918667  0.985262  1.013185  1.023463  1.007946  0.838137  0.806069  0.889786  0.741838  0.723968  0.721967  0.714691  0.712132 -0.037850  0.192357  0.170151  0.151024  0.511803  0.308730  0.446248  0.327647  0.378462  0.591289  0.662593  0.681983  0.702811  0.694439  0.700636  0.682175  0.638835  0.487743 -0.020339  0.539665  0.611705  0.626342  0.205399  0.408507  0.582603  0.308328  0.688373  0.267818  0.685870  0.693181  0.715640  0.710877  0.707542  0.674297  0.580228  0.870182  0.605454  0.547536  0.773832  0.550121  0.565556  0.774198  0.453218  0.275997  0.730164  0.763504  0.988017  0.991051  1.009090  0.936962  0.912551  0.914322  0.984950  0.898600  0.723680  0.529259  0.335127  0.723615  0.962667  0.956591  1.005134  1.007676  1.032345  0.955041  0.889494
         0.445796  0.424511  0.343123  0.141804  0.182123  0.116383  0.135698  0.018921 -0.037295  0.080703  0.245810  0.343059  0.341013  0.331568  0.318466  0.357644  0.459945  0.575006  0.642786 -0.747249 -0.612166 -0.422516 -0.602033 -0.578580 -0.519930 -0.418013 -0.378347 -0.302469 -0.234600 -0.413261 -0.222147 -0.144664 -0.002045  0.046005  0.043143 -0.261805 -0.411832 -0.422939 -0.135390  0.357554 -0.057047 -0.491839 -0.390192 -0.283724 -0.187761 -0.300794 -0.274676 -0.101983 -0.107829 -0.168583 -0.074430 -0.076603  0.090912  0.087910  0.191322  0.426314  0.588884  0.491537  0.648627  0.655921  0.659714  0.658944  0.656113 -0.749524 -0.761561 -0.858046 -0.829364 -0.680905 -0.613269 -0.642896 -0.539539 -0.578045  0.589507  0.686437  0.706841  0.683150  0.686147  0.675904  0.684495  0.710052  0.827515  0.962668  0.739481  0.624073  0.756901  0.989785  0.817132  0.738072  0.832449  0.696167  0.934655  0.683319  0.689545  0.669670  0.677533  0.663187  0.712623 -0.400705 -0.219130 -0.466149 -0.621205 -0.413485 -0.658438 -0.646378  0.558774 -0.601718 -0.778220  0.661679  0.574751  0.271920  0.260682  0.187658 -0.215207 -0.101830 -0.071904 -0.282429 -0.191256 -0.397861 -0.628274 -0.690176  0.648627  0.343956  0.000241  0.203866  0.209776  0.076827  0.305258  0.489972
         0.037260 -0.058780 -0.067925 -0.182030 -0.288858  0.002410 -0.011473 -0.191515 -0.435357 -0.337005 -0.088479 -0.000407  0.020197  0.012937  0.026878  0.095971  0.194981  0.281442  0.367289 -0.422631 -0.736892 -0.694621 -0.654606 -0.577673 -0.541179 -0.363413 -0.334567 -0.384837 -0.440063 -0.339706 -0.342680 -0.163412 -0.257786 -0.230598 -0.386628 -0.511767 -0.444520 -0.431962 -0.406549 -0.104547 -0.047021 -0.356741 -0.338693 -0.278120 -0.468224 -0.402921 -0.576471 -0.503494 -0.476371 -0.447054 -0.457678 -0.295350 -0.142732  0.057597  0.080667  0.453690  0.263286  0.147487  0.316330  0.349838  0.345202  0.368454  0.383688 -0.783416 -0.748686 -0.661377 -0.701231 -0.655300 -0.825209 -0.733663 -0.860471 -0.814307  0.640177  0.418360  0.330664  0.336242  0.352264  0.362523  0.390869  0.433060  0.424330  0.479374  0.518248  0.581281  0.339235  0.296978  0.555463  0.479215  0.611735  0.342171  0.429264  0.384194  0.346565  0.336138  0.328295  0.370692  0.336059 -0.786297 -0.522433 -0.733856 -0.676859 -0.581334 -0.642609 -0.639505  0.425154 -0.758888 -0.710110  0.315343  0.412858  0.077734  0.010417  0.026680 -0.383255 -0.467943 -0.466087 -0.230447 -0.461159 -0.642207 -0.684352 -0.754373  0.370972  0.010824 -0.374611 -0.055082  0.037186  0.000487 -0.137544  0.137921
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];

I2  = [  0.020749  0.021985  0.023727  0.027888 -0.008568 -0.007002 -0.011395 -0.019410 -0.006903 -0.005844 -0.001418 -0.003698 -0.001953 -0.004534 -0.035152 -0.034654 -0.040141 -0.037708 -0.045359 -0.030578 -0.035051 -0.027127 -0.030500 -0.048994 -0.021335 -0.021755 -0.015960 -0.017924 -0.024473 -0.030945  0.011242  0.011611  0.017344  0.007017  0.007175  0.007498  0.003249  0.000315  0.014511  0.014334  0.016129  0.019299  0.024260  0.021050  0.028192  0.027083  0.025690  0.030816  0.028874  0.030492  0.029796  0.029740  0.027857  0.027354  0.021889 -0.001683  0.009915  0.009156  0.025039 -0.013870 -0.021173 -0.005448  0.028764  0.019111
        -0.024972 -0.005988  0.008332  0.002163 -0.049962 -0.042475 -0.039756 -0.031977 -0.018654 -0.006171  0.086760  0.075774  0.063424  0.059128  0.073700  0.077515  0.095291  0.106584 -0.011383 -0.031031 -0.051531 -0.065677 -0.052101  0.118210  0.092384  0.083119  0.072834  0.070260  0.077027  0.094650  0.045546  0.072831  0.068591  0.002509 -0.005321 -0.012184 -0.034018 -0.038730 -0.033107 -0.016815 -0.003393  0.007137  0.014331  0.015069  0.056094  0.055654  0.054931  0.034276  0.042314  0.020931  0.021004  0.023802  0.023425  0.028801  0.019137 -0.025003 -0.011593  0.006086  0.041571  0.096836 -0.023743 -0.050041  0.015673 -0.022360
        -0.041786 -0.041780 -0.044697 -0.022470 -0.061952 -0.069065 -0.080580 -0.093617 -0.093091 -0.101404 -0.020003 -0.000861  0.011484  0.024812  0.048488  0.042288  0.018262  0.004670 -0.104999 -0.097353 -0.087916 -0.066156 -0.085734 -0.003215  0.003142  0.014469  0.020898  0.032513  0.030526  0.012902  0.017909 -0.019830 -0.025047 -0.088604 -0.074068 -0.069136 -0.062294 -0.062565 -0.047429 -0.055624 -0.056697 -0.057494 -0.051499 -0.067964 -0.049798 -0.035495 -0.021316 -0.022068 -0.009798 -0.008325 -0.035088 -0.042966 -0.055152 -0.066715 -0.073210 -0.079262 -0.065249 -0.087864  0.000375 -0.013544 -0.099278 -0.058902 -0.036362 -0.045418
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];
N2  = [  0.870070  0.927603  0.860200  1.023458  0.566630  0.665570  0.650401  0.354028  0.655580  0.651340  0.694679  0.635830  0.652956  0.692917  0.035519 -0.001529  0.124756  0.260901 -0.530302  0.056183  0.206403  0.600583  0.343858 -0.332444  0.651302  0.651947  0.674808  0.657758  0.567629  0.526917  0.698050  0.712031  0.759476  0.848849  0.842494  0.833972  0.751336  0.726687  0.823208  0.881339  0.913655  0.914298  0.861700  0.872242  0.999745  0.971613  0.917368  1.028105  0.945405  1.025258  1.019198  1.013009  0.948918  0.954885  0.862571  0.843008  0.845841  0.854005  0.812530  0.681067  0.515481  0.566108  1.003591  0.853658
        -0.229566 -0.056602 -0.366002 -0.067888 -0.649927 -0.627111 -0.631834 -0.503733 -0.434883 -0.534757  0.735220  0.660948  0.673369  0.725943  0.703338  1.013614  0.838419  0.847138 -0.554670 -0.530942 -0.627289 -0.687945 -0.659941  0.682908  0.642925  0.675518  0.723361  0.748180  0.814658  0.711326  0.687285  0.716067  0.695218 -0.474469 -0.381989 -0.286242 -0.444553 -0.497305 -0.382172 -0.176116 -0.242384 -0.379336 -0.459160 -0.455211  0.278300  0.369275  0.480502  0.105555  0.372344 -0.073894 -0.114087 -0.140787 -0.314587 -0.223440 -0.430625 -0.385559 -0.265857 -0.473374  0.502340  0.714992 -0.532477 -0.613509 -0.181289 -0.211123
        -0.528402 -0.462843 -0.444930 -0.085181 -0.618000 -0.522769 -0.540438 -0.892360 -0.715882 -0.638719  0.288134  0.497712  0.457647  0.287339  0.822155  0.392461  0.666775  0.606677 -0.790323 -0.959073 -0.869070 -0.533877 -0.786011  0.771880  0.497573  0.456270  0.336579  0.322133  0.355049  0.568330  0.359178  0.289460  0.212816 -0.341211 -0.474522 -0.560899 -0.583665 -0.574516 -0.520886 -0.527774 -0.427277 -0.280241 -0.324447 -0.301009  0.032113  0.061994  0.132957 -0.040324  0.216146  0.029732 -0.088322 -0.121022 -0.248528 -0.342506 -0.374819 -0.474857 -0.551642 -0.325241  0.409705  0.353805 -0.772474 -0.655415 -0.140852 -0.563008
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];

I3  = [ -0.014143 -0.007853 -0.005741 -0.005047  0.002113  0.005379  0.007698 -0.061973 -0.066207 -0.074706 -0.059679 -0.067294 -0.059142 -0.063033 -0.065539 -0.058623 -0.070340 -0.060705 -0.060650 -0.060661 -0.066919 -0.061513 -0.091911 -0.085202 -0.093883 -0.085202 -0.093478 -0.088494 -0.100163 -0.090751 -0.093552 -0.100686 -0.095761 -0.102588 -0.087140 -0.117218 -0.114718 -0.116477 -0.105603 -0.111269 -0.109017 -0.099629 -0.101208 -0.114292 -0.116114 -0.107789 -0.085237 -0.098877 -0.101424 -0.083710 -0.040432 -0.040023 -0.041004 -0.035188 -0.040435 -0.036839 -0.035966 -0.042521 -0.035246 -0.035967 -0.030843 -0.035817 -0.042937 -0.032111 -0.030770 -0.029257 -0.022727 -0.029429 -0.035582 -0.031801 -0.032801 -0.018965 -0.027046 -0.039420 -0.006134 -0.003585  0.000594  0.010289  0.000743 -0.004720 -0.006030 -0.006969 -0.000695 -0.004063 -0.001820 -0.006324 -0.010610 -0.004312 -0.008062 -0.003475  0.003059 -0.004928  0.007901  0.004267  0.009194  0.006824  0.009259 -0.003316  0.010030 -0.005349  0.008913 -0.000224 -0.006087  0.009573  0.003827 -0.003843  0.022417  0.015849  0.023696  0.015205  0.022180  0.010837  0.014290  0.022291  0.035723  0.018259  0.033481  0.028528  0.024028  0.033523  0.030296  0.034329  0.031971  0.031079  0.036315  0.050606  0.041590  0.034632  0.050513  0.040711  0.034882  0.048099  0.043770  0.037417  0.049083  0.036940  0.048328  0.040985  0.036732  0.045764  0.040986  0.046230  0.054553  0.063769  0.049246  0.055419  0.048839  0.057098  0.049368  0.045759  0.070914  0.069143  0.075440  0.069940  0.077202  0.068599  0.075463  0.058936  0.072075  0.059777  0.076177  0.059267  0.057088  0.048421  0.041928  0.067869  0.052816  0.054311  0.021294  0.022892  0.027890  0.010129  0.054800  0.029197  0.021250  0.055228  0.029172  0.027504  0.026426  0.025431  0.020716  0.011061  0.010664  0.010259  0.014457  0.017781  0.015816  0.040963  0.028313  0.028424 -0.012121 -0.008241 -0.015731 -0.024731 -0.009716  0.016089  0.015094  0.015842 -0.010267 -0.017789 -0.022785 -0.015013 -0.017825 -0.016841 -0.013787 -0.010960 -0.022706 -0.029915 -0.017699 -0.026845 -0.012586 -0.012006 -0.011295 -0.016670 -0.010937 -0.013873 -0.007565 -0.012153 -0.013915 -0.028303 -0.013306 -0.028930 -0.010652 -0.028837 -0.007966 -0.024915  0.008202 -0.007003 -0.008560  0.015786  0.013983 -0.040942 -0.039562 -0.044963 -0.047069 -0.029106 -0.031529 -0.048353 -0.050267 -0.053308 -0.032441 -0.051102 -0.051166 -0.029002 -0.052245 -0.042883 -0.055205 -0.054880 -0.054606 -0.054920 -0.053784 -0.053442 -0.080583 -0.080676  0.008521 -0.003495
         0.070136  0.067811  0.077810  0.083800  0.082852  0.087862  0.088335  0.094895  0.117570  0.122570  0.123712  0.138440  0.142425  0.147725  0.154018  0.155862  0.158541  0.159659  0.163305  0.164835  0.166528  0.167922  0.168161  0.169053  0.170552  0.163171  0.163266  0.161587  0.155839  0.159783  0.144246  0.127957  0.137478  0.121902  0.114476  0.167569  0.154997  0.170092  0.168519  0.171115  0.170182  0.171291  0.170232  0.141352  0.170367  0.143346  0.158088  0.169915  0.168087  0.170089  0.176322  0.182392  0.177057  0.178931  0.176925  0.179452  0.181017  0.171325  0.180707  0.174263  0.184464  0.160851  0.160138  0.158132  0.149875  0.146852  0.134563  0.126603  0.122494  0.109505  0.103095  0.096591  0.084541  0.090618  0.094754  0.098849  0.091260  0.098101  0.115926  0.117100  0.130186  0.136728  0.149548  0.160716  0.172126  0.175805  0.182548  0.199203  0.206906  0.217461  0.224373  0.216023  0.225507  0.219994  0.223915  0.219395  0.215478  0.196681  0.205070  0.190736  0.183608  0.179371  0.162739  0.157526  0.144881  0.145987  0.109959  0.100235  0.101849  0.097137  0.100118  0.093625  0.092190  0.095124  0.096553  0.091239  0.091598  0.089421  0.089748  0.087264  0.088486  0.092131  0.097172  0.107262  0.204889  0.197376  0.193775  0.177713  0.171139  0.165876  0.150692  0.150761  0.133446  0.118970  0.126690  0.114462  0.117687  0.109394  0.103436  0.104711  0.098492  0.099234  0.099084  0.101295  0.097228  0.090829  0.093318  0.089126  0.086603  0.089954  0.160647  0.146745  0.145419  0.132154  0.132525  0.120832  0.128689  0.105968  0.109203  0.104066  0.104966  0.115159  0.129100  0.088396  0.090335  0.097639  0.104034  0.107231  0.121453  0.128617  0.139542  0.138141  0.152834  0.158842  0.164098  0.180342  0.186204  0.193806  0.208154  0.214515  0.218792  0.225817  0.223235  0.127229  0.103186  0.093484  0.091708  0.097392  0.101973  0.106572  0.156361  0.157781  0.162743  0.171169  0.153973  0.159450  0.185328  0.198113  0.196122  0.196126  0.193675  0.200829  0.198954  0.195966  0.185515  0.196083  0.179437  0.173163  0.170397  0.158145  0.169732  0.142600  0.154577  0.131727  0.123899  0.115043  0.108119  0.203892  0.204696  0.185728  0.204821  0.187984  0.203784  0.187395  0.199888  0.183032  0.224003  0.212952  0.159445  0.097847  0.091687  0.098198  0.114610  0.128049  0.138715  0.153945  0.169696  0.156841  0.161134  0.157279  0.184555  0.167729  0.170496  0.174596  0.174641  0.183649  0.167266  0.164468  0.162990  0.152188  0.144702  0.140662  0.116529  0.102801  0.091552  0.065078
        -0.175824 -0.173515 -0.170229 -0.172938 -0.162729 -0.158518 -0.146275 -0.174537 -0.176656 -0.176046 -0.178645 -0.177017 -0.175862 -0.172724 -0.164930 -0.160370 -0.153868 -0.153171 -0.142111 -0.136135 -0.123316 -0.117225 -0.126898 -0.120620 -0.137710 -0.155932 -0.169143 -0.167115 -0.175157 -0.171632 -0.175897 -0.174036 -0.175152 -0.173912 -0.174131 -0.149311 -0.173428 -0.166433 -0.169840 -0.153930 -0.147684 -0.140418 -0.129720 -0.173687 -0.154488 -0.174265 -0.170420 -0.156636 -0.168144 -0.117415 -0.101527 -0.093342 -0.114943 -0.115832 -0.129467 -0.121870 -0.142394 -0.153852 -0.155883 -0.169877 -0.163824 -0.183215 -0.170650 -0.191156 -0.198113 -0.201643 -0.209350 -0.195987 -0.189236 -0.185077 -0.181880 -0.183809 -0.177898 -0.176980 -0.185803 -0.193527 -0.182417 -0.210879 -0.219264 -0.215450 -0.224078 -0.226494 -0.235906 -0.228628 -0.224195 -0.215545 -0.204600 -0.205530 -0.190728 -0.185035 -0.176375 -0.163201 -0.163393 -0.149031 -0.149825 -0.138146 -0.133600 -0.126287 -0.126812 -0.125148 -0.123239 -0.123770 -0.118897 -0.124194 -0.114928 -0.103766 -0.121664 -0.134624 -0.147311 -0.148449 -0.150636 -0.162635 -0.182069 -0.176425 -0.177125 -0.192231 -0.193024 -0.201940 -0.214304 -0.212146 -0.218345 -0.226732 -0.229400 -0.237930 -0.126499 -0.125862 -0.125566 -0.124093 -0.125363 -0.123751 -0.124554 -0.124124 -0.126785 -0.125927 -0.128719 -0.127150 -0.130248 -0.134401 -0.152186 -0.151644 -0.171605 -0.170133 -0.171856 -0.167199 -0.177413 -0.200146 -0.189863 -0.210875 -0.219813 -0.232368 -0.127092 -0.128874 -0.131616 -0.136637 -0.140167 -0.141951 -0.141206 -0.152495 -0.154315 -0.158152 -0.166130 -0.138719 -0.131639 -0.207631 -0.197550 -0.184943 -0.156544 -0.146744 -0.118230 -0.120638 -0.125040 -0.114714 -0.124729 -0.124129 -0.124352 -0.125898 -0.124217 -0.125029 -0.130333 -0.135504 -0.140248 -0.155662 -0.143087 -0.235836 -0.220384 -0.213144 -0.204864 -0.175609 -0.151822 -0.133222 -0.103018 -0.114764 -0.107922 -0.104574 -0.098781 -0.125123 -0.122957 -0.124697 -0.130505 -0.143089 -0.156581 -0.149277 -0.167074 -0.183648 -0.198593 -0.196329 -0.190377 -0.183806 -0.203729 -0.198561 -0.210733 -0.223398 -0.221788 -0.215483 -0.215536 -0.202602 -0.202129 -0.182859 -0.169815 -0.170006 -0.156770 -0.157132 -0.144613 -0.143412 -0.130954 -0.122728 -0.156311 -0.183516 -0.222938 -0.216730 -0.186217 -0.178122 -0.183582 -0.185464 -0.184038 -0.198180 -0.183837 -0.166667 -0.160279 -0.161042 -0.143219 -0.135948 -0.123544 -0.107120 -0.101628 -0.088773 -0.129921 -0.142390 -0.148591 -0.169620 -0.176069 -0.178593 -0.174663 -0.174159 -0.144245 -0.171173
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];
N3  = [  0.156002  0.437374  0.681384  0.468618  0.946998  0.907670  0.983227 -0.133279 -0.221143 -0.202887 -0.296324 -0.130945 -0.201902 -0.034386  0.044125 -0.237512  0.052776 -0.285159 -0.220448 -0.210388 -0.066933 -0.387594  0.043545 -0.370332  0.145279  0.326078  0.335784  0.337153 -0.124234  0.255817 -0.170698 -0.137532 -0.203006 -0.168745 -0.121216 -0.594119 -0.273831 -0.534643  0.171876 -0.273068 -0.329470 -0.078179 -0.455199 -0.197785 -0.524120 -0.203848  0.278958  0.254455  0.246391  0.138069 -0.045570  0.115118 -0.342900 -0.190146 -0.705216 -0.455388 -0.744545 -0.787192 -0.832562 -0.849669 -0.821621 -0.823945 -0.798752 -0.807274 -0.804131 -0.804942 -0.837633 -0.644540 -0.538020 -0.423019 -0.355497 -0.257234 -0.144636 -0.217197 -0.228117 -0.481505 -0.073633 -0.543028 -0.556041 -0.578482 -0.580792 -0.642184 -0.753740 -0.788433 -0.798348 -0.780186 -0.761153 -0.838285 -0.857108 -0.784635 -0.648033 -0.704054 -0.525237 -0.641666 -0.544648 -0.278313  0.099222 -0.318986  0.100017 -0.199411 -0.057548  0.085263  0.511416  0.274476  0.606437  0.754703  0.595368  0.655124  0.409345  0.547058  0.395639  0.552581  0.195813  0.197218  0.114070  0.002635  0.057482  0.008596 -0.404125 -0.042248 -0.329238 -0.412982 -0.474385 -0.550661 -0.074393  0.172602  0.046617  0.138180  0.147595  0.125718 -0.050485  0.119194  0.178905  0.349975  0.362352  0.374628  0.456852  0.281445  0.171120  0.158475  0.109183  0.108037  0.133832  0.183404  0.109493  0.313801  0.144024  0.295688  0.072378 -0.357678  0.229215  0.353148  0.453719  0.474806  0.516944  0.571675  0.521124  0.220933  0.505364  0.171299  0.570469  0.554488  0.458582  0.135380  0.023735  0.351122  0.142210  0.237628  0.614903  0.541931  0.241754  0.600063  0.211224 -0.068212 -0.075174  0.083789  0.140301  0.125019 -0.108279 -0.139207  0.088701 -0.526204 -0.021331 -0.604846 -0.537810 -0.546943 -0.411961  0.104589  0.292662  0.344893  0.768844  0.614638  0.733323  0.582163  0.757798  0.052279  0.019466  0.125064 -0.655094 -0.663004 -0.720709 -0.741892 -0.815134 -0.820814 -0.773462 -0.824717 -0.790452 -0.829207 -0.763535 -0.795159 -0.760371 -0.726511 -0.779035 -0.692067 -0.613519 -0.650569 -0.608686 -0.856957 -0.844313 -0.817006 -0.795574 -0.725843 -0.709904 -0.644280 -0.624497 -0.129898 -0.542621 -0.839965 -0.781169 -0.538025  0.081112 -0.260653 -0.430063 -0.551203 -0.635619 -0.799209 -0.837579 -0.593775 -0.653985 -0.480393 -0.694570 -0.522388 -0.488413  0.327170 -0.316250 -0.078871 -0.376208 -0.394225 -0.397667 -0.343615 -0.375767 -0.423868 -0.170745 -0.099001  0.888102  0.742194
        -0.068692  0.076637 -0.068276 -0.476371 -0.092805 -0.324644 -0.127870  0.030684 -0.009923  0.014143  0.025176  0.328191  0.594335  0.751489  1.066240  1.044901  1.065495  1.020801  1.118301  1.127478  1.166042  1.099149  1.115790  1.072790  1.138485  1.045073  0.927629  0.976813  0.325021  0.806500  0.120741  0.095533  0.051263  0.063289  0.083940  0.978746  0.254768  1.052600  0.982838  1.147830  1.121480  1.170602  1.058645  0.140468  1.069928  0.103549  0.893656  1.098707  1.027071  1.130084  1.174132  0.981212  1.129476  1.135664  0.916810  1.084011  0.886610  0.807713  0.765978  0.684304  0.768879  0.565479  0.637608  0.486947  0.451818  0.327172 -0.091883 -0.271695 -0.242437 -0.274013 -0.229109 -0.389366 -0.125190 -0.084046 -0.664071 -0.637462 -0.820961 -0.584652 -0.411778 -0.429587 -0.283629 -0.149499  0.096324  0.452388  0.504161  0.510254  0.533302  0.431316  0.648215  0.829459  0.982040  0.922740  1.043181  0.972154  1.049455  1.009045  0.913049  0.587665  0.593105  0.428260  0.138202  0.240659  0.447525  0.341383  0.651929  0.787943 -0.420759 -0.656517 -0.781646 -0.741654 -0.802854 -0.741052 -0.883257 -0.862696 -0.864148 -0.902458 -0.875557 -0.885739 -0.740853 -0.906806 -0.761568 -0.554709 -0.482181 -0.481681  0.422504  0.335915  0.212414  0.255971  0.234401  0.222338  0.023001  0.071854  0.019359 -0.017470 -0.019995 -0.320739 -0.203102 -0.753587 -0.854069 -0.852849 -0.864067 -0.860539 -0.860295 -0.853741 -0.859799 -0.835044 -0.846658 -0.863858 -0.912438 -0.719394  0.082506 -0.128254 -0.229514 -0.284649 -0.319426 -0.324988 -0.319980 -0.816615 -0.655733 -0.826873 -0.659375 -0.395450 -0.154459 -0.887567 -0.883429 -0.824735 -0.844627 -0.810865  0.191049  0.302300  0.266149  0.639400  0.118868  0.045329  0.051192  0.185284  0.282538  0.374178  0.740033  0.891816  0.961434  1.055022  1.090954 -0.359268 -0.450293 -0.602196 -0.798303 -0.861940 -0.817055 -0.788948  0.777944  0.624733  0.714977  0.920196  0.779726  0.160197  0.192815  0.501214  0.743641  0.925034  0.908997  0.860933  0.799618  0.728832  0.497957  0.559771  0.553322  0.545791  0.544419  0.492931  0.522471  0.060435  0.420069 -0.234096 -0.382399 -0.466369 -0.529578  0.690468  0.759540  0.762417  0.816367  0.904407  0.857619  0.970325  0.787515  1.020137  1.039126  0.745182  0.491810 -0.515878 -0.904725 -0.110810 -0.209488 -0.069644  0.467075  0.489619  0.528865  0.865182  0.764686  0.893135  0.932416  1.025126  1.043651  1.050137  1.139075  1.144464  1.092910  1.074243  1.079753  0.924189  0.620851  0.637069  0.032243  0.092008 -0.382244  0.138630
        -1.151415 -1.068889 -0.881929 -0.852533 -0.439022 -0.283858 -0.186244 -1.169922 -1.156319 -1.161852 -1.145232 -1.156784 -1.056428 -0.969372 -0.559675 -0.581805 -0.556704 -0.609301 -0.392008 -0.361746 -0.092136 -0.279977  0.162324 -0.439932 -0.214562 -0.386359 -0.650270 -0.559837 -1.160448 -0.851079 -1.172639 -1.172831 -1.165639 -1.169998 -1.173084  0.189151 -1.155634 -0.381645 -0.679634 -0.013041  0.067798 -0.110121  0.162535 -1.170195  0.001466 -1.168845 -0.741648 -0.268454 -0.543872  0.053531 -0.167479 -0.674562 -0.073026  0.130437 -0.232595 -0.040697 -0.135029 -0.355041 -0.303345 -0.448708 -0.345839 -0.647037 -0.618719 -0.730603 -0.755555 -0.806454 -0.742877 -0.876426 -0.973061 -1.021654 -1.068169 -1.024686 -1.148560 -1.145426 -0.798100 -0.672525 -0.585335 -0.688046 -0.860762 -0.825310 -0.930056 -0.943833 -0.891718 -0.776068 -0.729463 -0.753393 -0.764651 -0.706142 -0.481189 -0.308510 -0.131430 -0.054692  0.053800 -0.018548 -0.041457  0.405131  0.577245  0.738285  0.790429  0.826651  0.873516  0.870682  0.688003  0.822391  0.501321  0.008034  0.502888  0.001960  0.118605 -0.031995  0.061900 -0.063502 -0.057358  0.051569  0.089175 -0.081230  0.060028  0.018660 -0.570376 -0.134145 -0.603239 -0.844399 -0.868114 -0.798933  0.843237  0.856952  0.874248  0.867449  0.867901  0.871029  0.863579  0.870220  0.857452  0.814122  0.809549  0.707456  0.724271  0.309945  0.103008  0.113642  0.090748  0.103755  0.098896  0.104084  0.106259  0.075528  0.137834 -0.024070 -0.182565 -0.659208  0.857202  0.789666  0.714747  0.677218  0.635956  0.598426  0.633444  0.197644  0.322269  0.189576  0.247945  0.564156  0.739221 -0.007794  0.029281  0.077723  0.146856  0.204672  0.683508  0.716281  0.843585  0.518309  0.862377  0.864621  0.864564  0.873681  0.864909  0.853724  0.705704  0.581543  0.525982 -0.001503  0.352936 -0.856292 -0.843812 -0.657121 -0.362622  0.099408  0.140592  0.179653 -0.041949  0.512912  0.258078  0.161203 -0.237906  0.874222  0.877005  0.821981  0.402806  0.085799 -0.121705  0.040224 -0.226241 -0.447623 -0.768921 -0.649683 -0.710738 -0.655271 -0.754942 -0.744100 -0.773221 -0.918186 -0.804960 -0.855805 -0.832428 -0.709139 -0.683789 -0.405456 -0.220683 -0.383026 -0.038159 -0.189544  0.144430 -0.052577  0.393053  0.414146  0.023047 -0.334863 -0.763871 -0.779433 -0.134663 -1.131035 -1.045248 -1.024891 -0.922813 -0.740642 -0.654242 -0.614739 -0.681095 -0.686508 -0.095373 -0.342730 -0.338991  0.219330 -0.114624  0.186072 -0.330269 -0.381458 -0.353175 -0.735898 -0.994365 -0.963682 -1.167017 -1.173933 -0.101176 -0.833394
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];

I4  = [  0.036766  0.044953  0.035425  0.018361  0.014940  0.018792  0.011054  0.009987  0.011340  0.013531  0.065380  0.063198  0.056718  0.054265  0.062278  0.072378  0.073560  0.074249  0.073545  0.071364  0.076876  0.081416  0.078120  0.076986  0.080064  0.059544  0.062900  0.044610  0.050813  0.066161  0.074168  0.071536  0.080184  0.081020  0.078928  0.075305  0.072892  0.072526  0.022567  0.023511  0.038118  0.063932  0.039064  0.043099  0.024493  0.076116  0.076438  0.075309  0.080825  0.068043  0.074771  0.067597  0.027016  0.028172  0.045867  0.064794  0.005553 -0.005468 -0.003015  0.001904 -0.000358  0.023486 -0.001069  0.048935
        -0.070960 -0.061081 -0.047473 -0.041448 -0.047325 -0.068082 -0.223884 -0.207174 -0.194147 -0.187129 -0.078340 -0.074369 -0.071566 -0.085903 -0.105974 -0.100016 -0.105233 -0.112590 -0.110387 -0.132875 -0.133214 -0.141142 -0.129005 -0.126312 -0.128445 -0.195822 -0.187534 -0.179171 -0.159341 -0.160231 -0.159025 -0.175785 -0.173455 -0.165276 -0.160354 -0.176914 -0.150575 -0.140891 -0.171811 -0.185594 -0.164476 -0.173695 -0.192727 -0.203768 -0.220474 -0.116346 -0.123103 -0.121079 -0.146619 -0.084934 -0.115520 -0.087903 -0.086347 -0.069395 -0.095582 -0.076316 -0.027123 -0.046793 -0.208536 -0.070726 -0.192084 -0.212431 -0.051349 -0.069834
         0.191770  0.170440  0.161343  0.183442  0.193162  0.209908  0.114677  0.091351  0.075315  0.068715  0.126000  0.148300  0.165406  0.188744  0.196666  0.165543  0.158940  0.123876  0.116546  0.083662  0.097553  0.114478  0.130041  0.149440  0.169240  0.118743  0.111741  0.084610  0.074717  0.086870  0.099279  0.111710  0.140672  0.133747  0.114312  0.123470  0.085946  0.075320  0.062178  0.075478  0.068113  0.097026  0.095286  0.112402  0.115861  0.184679  0.152252  0.115774  0.108629  0.120004  0.129144  0.168529  0.220478  0.199762  0.205753  0.143906  0.183399  0.215139  0.091352  0.233826  0.068212  0.102500  0.215198  0.178406
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];
N4  = [  0.612714  0.731984  0.738152  0.673971  0.673259  0.643347  0.082626  0.152840  0.397378  0.499432  0.973503  0.885326  0.778236  0.636753  0.738179  1.041770  1.059417  1.063889  1.060746  0.833385  1.003473  1.068305  1.036698  1.052387  1.040682  0.772471  0.784026  0.455555  0.589160  0.678033  0.913151  0.866622  1.055319  1.067281  0.997849  0.937113  0.872780  0.736514  0.569174  0.509538  0.589226  0.718515  0.545932  0.352044  0.592760  0.968891  1.048260  1.037379  1.048316  0.997400  1.062205  0.935602  0.676403  0.662963  0.685504  0.923134  0.625475  0.562445  0.048321  0.619882  0.125435  0.409320  0.603747  0.692434
         0.480256  0.592337  0.618811  0.524123  0.457672  0.519100 -1.050853 -0.994315 -0.916791 -0.834280  0.339755  0.486743  0.573738  0.433832  0.395579  0.136456  0.061548  0.027444  0.013027  0.356178  0.108902 -0.011665  0.154113  0.081093  0.145007 -0.808142 -0.764059 -0.769169 -0.663174 -0.705199 -0.566355 -0.649128 -0.372859 -0.318615 -0.454131 -0.593463 -0.627986 -0.253176 -0.757911 -0.767635 -0.727332 -0.696365 -0.868779 -0.943048 -0.941580  0.280159  0.090249  0.135767 -0.231580  0.282132  0.042100  0.357589  0.488444  0.512475  0.444583  0.435020  0.601770  0.547598 -1.035967  0.499438 -0.988393 -0.940955  0.523001  0.545790
         0.795463  0.488297  0.410821  0.683747  0.751367  0.724835 -0.442996 -0.508650 -0.498061 -0.518734  0.103422  0.244529  0.416393  0.813710  0.736669  0.231577  0.175400  0.092216  0.015531 -0.340781 -0.190194  0.018113  0.161227  0.074769  0.119875 -0.223017 -0.272443 -0.610391 -0.600809 -0.486386 -0.261542 -0.267478  0.043983  0.072294 -0.148900 -0.165966 -0.277728 -0.663943 -0.535908 -0.576447 -0.547901 -0.449176 -0.439539 -0.485944 -0.280472  0.398541  0.052043  0.023851 -0.129580  0.080548  0.117734  0.387859  0.718386  0.708158  0.752639  0.186202  0.652643  0.784502 -0.467864  0.775137 -0.523453 -0.463526  0.766851  0.633287
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];

I5  = [  0.021887  0.008175  0.020442  0.013529  0.007816  0.014667  0.016110  0.007467  0.021408  0.010618  0.005847  0.013221  0.005736  0.005274  0.013015  0.008414  0.021898  0.050897  0.052139  0.047758  0.051109  0.051454  0.055390  0.064743  0.069743  0.060442  0.077220  0.073662  0.070886  0.066325  0.063264  0.059020  0.053584  0.048693  0.059045  0.049328  0.058332  0.051033  0.060904  0.061913  0.053050  0.060874  0.049110  0.059900  0.049364  0.059250  0.051798  0.048987  0.064347  0.071150  0.069092  0.070371  0.067277  0.076087  0.074042  0.066298  0.077692  0.076781  0.074018  0.046919  0.039275  0.039094  0.034651  0.035530  0.038517  0.036253  0.026846  0.029046  0.024516  0.034945  0.035995  0.040888  0.035031  0.035772  0.036639  0.050313  0.029041  0.025154  0.033703  0.032811  0.031801  0.038440  0.041832  0.047391  0.042441  0.040842  0.035413  0.031810  0.027662  0.020076  0.024388  0.027742  0.062663  0.041427  0.060965  0.075646  0.059311  0.040202  0.026409  0.026136 -0.006325 -0.019695 -0.020631 -0.011138 -0.019084 -0.013613 -0.020653 -0.006747 -0.003052 -0.003874 -0.001694 -0.018191 -0.011352 -0.010673 -0.011941 -0.019129 -0.002850 -0.023972 -0.013765 -0.026168 -0.025361 -0.025892 -0.000052  0.001822  0.017093  0.003569  0.000704  0.001523
        -0.099844 -0.113554 -0.111980 -0.124090 -0.132107 -0.138763 -0.151683 -0.159743 -0.163331 -0.237585 -0.240421 -0.234444 -0.240142 -0.240011 -0.235336 -0.238436 -0.232658 -0.120594 -0.126819 -0.136664 -0.141399 -0.154123 -0.140296 -0.141269 -0.141858 -0.129774 -0.151022 -0.145845 -0.169273 -0.165921 -0.160286 -0.164682 -0.178660 -0.171889 -0.173118 -0.193143 -0.183344 -0.183111 -0.186592 -0.192841 -0.203264 -0.202342 -0.212372 -0.206062 -0.216767 -0.210499 -0.217875 -0.218289 -0.204092 -0.197692 -0.201391 -0.195640 -0.193063 -0.179824 -0.172760 -0.177977 -0.164657 -0.190031 -0.194107 -0.220339 -0.225623 -0.225430 -0.224835 -0.223695 -0.221679 -0.220345 -0.224743 -0.222789 -0.224965 -0.195465 -0.188781 -0.186054 -0.184312 -0.172892 -0.161102 -0.160812 -0.165740 -0.165451 -0.149505 -0.138621 -0.181778 -0.151252 -0.208517 -0.200174 -0.209602 -0.214697 -0.214836 -0.218521 -0.227790 -0.232296 -0.231448 -0.229592 -0.208396 -0.216058 -0.180008 -0.162001 -0.124559 -0.126129 -0.119842 -0.104609 -0.092840 -0.099713 -0.105322 -0.126029 -0.142043 -0.131926 -0.148323 -0.146889 -0.250930 -0.249978 -0.246547 -0.259446 -0.255530 -0.255953 -0.249155 -0.246279 -0.243041 -0.261303 -0.260302 -0.247604 -0.258507 -0.111837 -0.239762 -0.241627 -0.231030 -0.244409 -0.144522 -0.099383
         0.240578  0.255851  0.249038  0.256897  0.261680  0.260707  0.263368  0.267187  0.262480  0.194726  0.181784  0.181931  0.174917  0.154835  0.162324  0.146129  0.146239  0.229026  0.232121  0.238627  0.233872  0.228703  0.228504  0.215716  0.209010  0.222817  0.195138  0.203236  0.194922  0.208638  0.217091  0.222495  0.219711  0.227560  0.215626  0.214652  0.208054  0.219903  0.195278  0.186746  0.191663  0.182738  0.187686  0.181925  0.176152  0.173545  0.161334  0.142062  0.149308  0.155836  0.167831  0.173297  0.179444  0.174024  0.181802  0.194968  0.181107  0.161869  0.167779  0.141748  0.149208  0.153332  0.166032  0.172353  0.177865  0.186411  0.188125  0.195034  0.208412  0.235556  0.236123  0.229635  0.240294  0.241644  0.243235  0.229015  0.253948  0.259521  0.250107  0.250717  0.247188  0.244153  0.208574  0.208582  0.202629  0.194841  0.215056  0.208752  0.166900  0.158200  0.140025  0.150499  0.167956  0.190720  0.208278  0.189277  0.221201  0.242453  0.248813  0.240567  0.255110  0.264536  0.266272  0.267164  0.272210  0.268913  0.273893  0.269333  0.195076  0.181620  0.175466  0.172541  0.175619  0.179839  0.154492  0.146878  0.147895  0.173869  0.187882  0.148469  0.167886  0.269303  0.140757  0.153286  0.181884  0.194879  0.266935  0.253722
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];
N5  = [  0.574145  0.411728  0.505013  0.429215  0.376464  0.410374  0.375397  0.277882  0.563616  0.675799  0.692337  0.609539  0.650133  0.464742  0.525002  0.357180  0.558165  0.716012  0.734649  0.740072  0.800353  0.758342  0.849990  0.854797  0.875385  0.972189  0.991710  0.928277  0.940350  0.910382  0.866891  0.702417  0.759389  0.700931  0.829662  0.862778  0.840117  0.756630  0.859491  0.833792  0.787920  0.800965  0.623096  0.764966  0.575945  0.725299  0.599622  0.780048  0.803956  0.823436  0.845204  0.866278  0.832561  0.933435  0.944755  0.891336  0.983179  0.965075  0.907922  0.614153  0.446262  0.525360  0.425833  0.364086  0.383965  0.477589  0.558350  0.610498  0.708382  0.908212  0.903189  0.813650  0.905755  0.862537  0.825391  0.702704  0.863179  0.746391  0.757056  0.597473  0.916388  0.788031  0.823685  0.859851  0.790418  0.607650  0.797926  0.683656  0.435599  0.396570  0.613102  0.354954  0.760064  0.556577  0.855636  0.977682  0.841827  0.604283  0.512412  0.596387  0.452950  0.364869  0.334238  0.280150  0.267449  0.262713  0.275835  0.272429  0.733092  0.675807  0.608043  0.333612  0.653119  0.664098  0.309119  0.046169  0.332571  0.015835  0.577637 -0.018606 -0.041036  0.263196  0.196006  0.470778  0.587845  0.695353  0.284158  0.448659
         0.412523  0.204355  0.254387  0.144808  0.106613  0.091537  0.047187 -0.028207 -0.100582 -0.975530 -0.966868 -1.037164 -1.003093 -1.128166 -1.088977 -1.171846 -1.072452  0.386959  0.202867 -0.178813 -0.384650 -0.263434 -0.335832 -0.150908 -0.132432 -0.058624 -0.337084 -0.201150 -0.541911 -0.499301 -0.163632 -0.202557 -0.510551 -0.466986 -0.490296 -0.628818 -0.709775 -0.545912 -0.683199 -0.565884 -0.631628 -0.536836 -0.926481 -0.687034 -1.027636 -0.929576 -1.052476 -0.893114 -0.849930 -0.840214 -0.825319 -0.604541 -0.518667 -0.421375 -0.526163 -0.658736 -0.479030 -0.648766 -0.701158 -1.015299 -1.136437 -1.089176 -1.134948 -1.157841 -1.137807 -1.087526 -1.062683 -1.024447 -0.954027 -0.249573 -0.424048 -0.437239 -0.341644 -0.300552 -0.315453 -0.257087 -0.226602 -0.174611 -0.244706  0.002522 -0.250519 -0.365384 -0.802913 -0.723936 -0.808483 -0.979608 -0.859662 -0.976123 -1.124202 -1.158026 -1.039371 -1.169917 -0.923742 -1.011294 -0.677891 -0.450358  0.363679  0.185112  0.221345  0.377538  0.263601  0.169631  0.100476  0.066976  0.028585  0.028671  0.029556  0.002724 -0.927748 -0.967357 -1.019469 -1.139313 -0.981903 -0.975965 -1.118530 -1.092205 -1.143702 -1.182659 -1.054292 -1.093847 -1.161975  0.036196 -1.148072 -1.123212 -1.050542 -0.961027  0.003409  0.263736
         0.898218  1.113801  1.043372  1.125747  1.160074  1.149739  1.175159  1.220903  1.100378  0.080843  0.150076  0.175253  0.123359  0.163240  0.258066  0.061825  0.230876  0.776759  0.884414  0.958991  0.849876  0.927531  0.803289  0.828677  0.801338  0.627360  0.554426  0.719546  0.519225  0.628669  0.812215  0.987091  0.845560  0.925774  0.768390  0.599989  0.542242  0.828597  0.535337  0.701568  0.717468  0.766159  0.588422  0.703028  0.431501  0.371806  0.103629  0.041258 -0.040091  0.010126  0.259281  0.619764  0.734712  0.628564  0.523803  0.491925  0.466174  0.184149  0.385733 -0.073393  0.080065  0.285578  0.302670  0.311717  0.381275  0.417104  0.312056  0.333810  0.180211  0.719785  0.675013  0.812511  0.706219  0.789516  0.838253  0.980759  0.802136  0.952150  0.933991  1.063881  0.708469  0.870461  0.396727  0.464672  0.488761  0.500848  0.267728  0.242450  0.340202  0.193926  0.114026  0.057117  0.211693  0.510439  0.555570  0.510495  0.607587  1.007268  1.053420  0.905660  1.068753  1.147465  1.178256  1.204128  1.214964  1.216464  1.212218  1.217846  0.192103  0.028643 -0.000067 -0.146944 -0.000051  0.017766 -0.221151 -0.382410 -0.128332 -0.212808  0.047088 -0.384274 -0.260484  1.215091 -0.228747  0.099557  0.233282  0.106036  1.214443  1.071165
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000];

%%% Ordered transformation matrices
% Global rotation operator
w0a = [0.994421 0.094730 -0.046401]';
q0a = [0.0, 0.0, 0.0]';
t0b = twist_matrix_fast(w0a,q0a,sp_coords(4));

% Global translation operator
t0a = twist_matrix_fast(sp_coords(1:3),0,0);

w1a = [-0.095661 -0.789197 -0.606644]';
q1a = [0.035447 -0.093433 0.099346]';
t1a = twist_matrix_fast(w1a,q1a,Theta(1));

w1b = [-0.263141 0.607808 -0.749217]';
q1b = [0.035447 -0.093433 0.099346]';
t1b = twist_matrix_fast(w1b,q1b,Theta(2));

w1c = [0.960003 0.087962 -0.265814]';
q1c = [0.035447 -0.093433 0.099346]';
t1c = twist_matrix_fast(w1c,q1c,Theta(3));

w2a = [0.174284 -0.733861 -0.656562]';
q2a = [-0.001341 -0.008460 -0.005397]';
t2a = twist_matrix_fast(w2a,q2a,Theta(4));

w2b = [-0.123760 0.645154 -0.753962]';
q2b = [-0.001341 -0.008460 -0.005397]';
t2b = twist_matrix_fast(w2b,q2b,Theta(5));

w2c = [0.976887 0.212659 0.021618]';
q2c = [-0.001341 -0.008460 -0.005397]';
t2c = twist_matrix_fast(w2c,q2c,Theta(6));

w3a = [0.125691 -0.748258 -0.651392]';
q3a = [-0.015170 0.079055 -0.108595]';
t3a = twist_matrix_fast(w3a,q3a,Theta(7));

w3b = [0.444563 0.629465 -0.637289]';
q3b = [-0.015170 0.079055 -0.108595]';
t3b = twist_matrix_fast(w3b,q3b,Theta(8));

w3c = [0.886886 -0.209483 0.411765]';
q3c = [-0.015170 0.079055 -0.108595]';
t3c = twist_matrix_fast(w3c,q3c,Theta(9));

w4a = [-0.094593 -0.674492 -0.732198]';
q4a = [0.004702 0.107194 -0.137083]';
t4a = twist_matrix_fast(w4a,q4a,Theta(10));

w4b = [-0.914922 0.348802 -0.203113]';
q4b = [0.004702 0.107194 -0.137083]';
t4b = twist_matrix_fast(w4b,q4b,Theta(11));

w4c = [0.392390 0.650691 -0.650101]';
q4c = [0.004702 0.107194 -0.137083]';
t4c = twist_matrix_fast(w4c,q4c,Theta(12));

w5a = [0.095661 0.789197 0.606644]';
q5a = [0.035447 -0.093433 0.099346]';
t5a = twist_matrix_fast(w5a,q5a,Theta(13));

w5b = [0.077190 -0.613485 0.785925]';
q5b = [0.035447 -0.093433 0.099346]';
t5b = twist_matrix_fast(w5b,q5b,Theta(14));

w5c = [0.992417 -0.028355 -0.119605]';
q5c = [0.035447 -0.093433 0.099346]';
t5c = twist_matrix_fast(w5c,q5c,Theta(15));

w6a = [0.003333 0.788431 0.615114]';
q6a = [0.043083 -0.154119 0.177089]';
t6a = twist_matrix_fast(w6a,q6a,Theta(16));

w6b = [-0.481004 -0.538019 0.692221]';
q6b = [0.043083 -0.154119 0.177089]';
t6b = twist_matrix_fast(w6b,q6b,Theta(17));

w6c = [0.876712 -0.298180 0.377446]';
q6c = [0.043083 -0.154119 0.177089]';
t6c = twist_matrix_fast(w6c,q6c,Theta(18));

%%% Construct kinematic chains of transformations
% Generate new_config and new_norms
U1 = t0a*t0b*t1a*t1b*t1c*I1;
V1 = t0a*t0b*t1a*t1b*t1c*N1;
V1 = V1-U1;

U2 = t0a*t0b*t1a*t1b*t1c*t2a*t2b*t2c*I2;
V2 = t0a*t0b*t1a*t1b*t1c*t2a*t2b*t2c*N2;
V2 = V2-U2;

U3 = t0a*t0b*t1a*t1b*t1c*t2a*t2b*t2c*t3a*t3b*t3c*t4a*t4b*t4c*I3;
V3 = t0a*t0b*t1a*t1b*t1c*t2a*t2b*t2c*t3a*t3b*t3c*t4a*t4b*t4c*N3;
V3 = V3-U3;

U4 = t0a*t0b*t5a*t5b*t5c*I4;
V4 = t0a*t0b*t5a*t5b*t5c*N4;
V4 = V4-U4;

U5 = t0a*t0b*t5a*t5b*t5c*t6a*t6b*t6c*I5;
V5 = t0a*t0b*t5a*t5b*t5c*t6a*t6b*t6c*N5;
V5 = V5-U5;

%%% Construct complete update
new_config = [U1(1:3,:) U2(1:3,:) U3(1:3,:) U4(1:3,:) U5(1:3,:)];
new_norms  = [V1(1:3,:) V2(1:3,:) V3(1:3,:) V4(1:3,:) V5(1:3,:)];

% Update center of mass estimate
new_ctrm = mean(new_config,2);

%%% Twist matrix function
function [twist] = twist_matrix_fast(w,q,theta)
% Overview:
% This code generates an exponential map matrix for rotation and translation
% using the Rodrigues' formula for the matrix exponential, from "A Mathematical 
% Introduction to Robotic Manipulation" by Murray, Li, and Sastry (1994) p. 28. 
% The equation for the exponential map for twists is from Ibid. 42.
%
% This code has very little error checking to increase speed.
%
% [twist] = twist_matrix_fast(w,q,theta)
% INPUTS %
% w: a 3d vector pointing: 1) in the direction of translation or 2) 
%      along the axis of rotation relative to the origin.
% q: a 3d point along the axis of rotation, providing the offset
% theta: the amount of rotation, in radians
%
% OUTPUTS %
% twist: the resulting matrix for left multiplication
%
% I treat 2 cases: 1) pure translation and 2) pure rotation
if numel(q) < 3 && (q == 0 || theta == 0)
    
    % we want column vectors
    if size(w,1)<size(w,2)
        w = w';
    end
    
    % 1) pure translation
	% pure translation in absolute frame
	twist = [eye(3) w;zeros(1,3) 1];
else
    % 2) pure rotation twist
    % correct for case where w is the origin
    if sum(w) == 0
        w = q;
        W = 0;
    else
        W = 1;
    end
    
    % Generate cross product operator so(3)
    w_hat = [0   -w(3) w(2);
             w(3) 0   -w(1);
            -w(2) w(1) 0];
    
    v = -w_hat*q;
    % Generate rotation matrix using the Rodrigues' equation pg. 28
    Id3x3  = eye(3);
    w_hat2 = w*w'-W*Id3x3;
    R      = Id3x3+w_hat*sin(theta)+w_hat2*(1-cos(theta));
    
    % Generate relative frame twist, pg. 42
    twist  = [R (Id3x3-R)*w_hat*v+w*w'*v*theta;
              zeros(1,3) 1];
end
