function [new_config, new_ctrm, new_norms]  = achilles151_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 18 x 1 angles about joint 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.000100 -0.007986 -0.008255  0.009863 -0.002744  0.017830  0.016325  0.019153  0.019918  0.026933  0.015715  0.025498  0.014546  0.030399  0.014845  0.020040  0.051156  0.036815  0.036676  0.042981  0.044634  0.054567  0.050633  0.075665  0.080772  0.076580  0.070397  0.067006  0.076430  0.072695  0.085241  0.083184  0.083621  0.073511  0.081868  0.089087  0.086821  0.088432  0.084902  0.106978  0.112588  0.108117  0.109881  0.112424  0.110652  0.105516  0.110738  0.112710  0.115728  0.116870  0.120455  0.123329  0.124618  0.126525  0.107365  0.117722  0.125934  0.123501  0.122698  0.120447  0.111778  0.111243  0.106835  0.097259  0.095841  0.088545  0.088386  0.098063  0.100598  0.098449  0.104833  0.094086  0.091013  0.101083  0.089830  0.090938  0.091352  0.087905  0.127107  0.124744  0.119870  0.122774  0.119939  0.115663  0.112966  0.099731  0.096844  0.101512  0.101503  0.092171  0.118364  0.116374  0.103362  0.064467  0.051076  0.052422  0.054641  0.058495  0.056042  0.060204  0.043841  0.047327  0.040156  0.066707  0.044383  0.044741  0.040418  0.034341  0.069765  0.069846  0.075598  0.076074  0.088386  0.088646  0.062108  0.067678  0.066571  0.068304  0.061175  0.057681  0.027526  0.031807  0.026682  0.024452 -0.000961  0.008517  0.008091 -0.009190
         0.133731  0.119433  0.119542 -0.089760 -0.096042 -0.107976 -0.110722 -0.124134 -0.141563  0.135084  0.134763  0.135184  0.126577  0.125030  0.118544  0.117330 -0.099249 -0.106844 -0.116259 -0.127743 -0.136596 -0.136576 -0.137906 -0.125858 -0.111616 -0.117072 -0.107337 -0.098979 -0.075949 -0.073922 -0.031044  0.069087  0.069506  0.078067  0.076393  0.077023  0.088429  0.094239  0.096922  0.077895  0.065647  0.060112  0.052080  0.032162  0.038474  0.028483  0.010252  0.003931 -0.006392  0.011109  0.013169  0.026609  0.031849  0.028438 -0.066941 -0.043379 -0.030816 -0.019315 -0.004620 -0.034240 -0.030616 -0.023529 -0.017767 -0.042055 -0.006140 -0.047825 -0.053794 -0.048561 -0.057729 -0.065963 -0.068131 -0.089813 -0.105372 -0.092283 -0.084120 -0.070876 -0.064958 -0.065525 -0.013446  0.054861  0.055129  0.039025  0.030306  0.021801 -0.013323  0.029197  0.045828  0.051861  0.050375  0.061472  0.043379  0.058752  0.080881  0.121262  0.126043  0.123232  0.119295  0.096693  0.091903  0.089454  0.102472  0.100519  0.113964  0.096124  0.129912  0.131178  0.133446  0.132992  0.110738  0.110691  0.105300  0.088044  0.065873 -0.030865 -0.092270 -0.106417 -0.121927 -0.131577 -0.134943 -0.112369 -0.103851  0.135096 -0.139944 -0.110753 -0.093367  0.120042  0.134414 -0.094347
         0.045501 -0.017294 -0.052194 -0.052926 -0.029248 -0.016033  0.014379  0.040340  0.095020  0.085597  0.045804  0.034306  0.006862 -0.006433 -0.049480 -0.056818 -0.047756 -0.010330  0.011814  0.032100  0.059365  0.074950  0.104097  0.068756  0.050033  0.043427  0.004292 -0.018832 -0.035463 -0.051800 -0.062556 -0.051399 -0.042425 -0.016194  0.002315  0.020654  0.035281  0.057659  0.087430  0.074258  0.057391  0.036839  0.027660  0.005457 -0.009485 -0.045584 -0.034901 -0.027293 -0.003095  0.013693  0.034481  0.038811  0.060133  0.087900  0.073238  0.066720  0.086009  0.061966  0.047899  0.063166  0.014171 -0.000350 -0.025734 -0.019656 -0.054027 -0.040585 -0.031181 -0.005407  0.020677  0.029435  0.064436  0.058763  0.077176  0.091037  0.039158  0.008304 -0.000851 -0.012305  0.085018  0.087340  0.060654  0.050618  0.028568  0.012052 -0.009738 -0.057290 -0.055804 -0.034876 -0.006685 -0.000208  0.036718  0.055196  0.086116  0.098958  0.060311  0.017279  0.013235 -0.017834 -0.038908 -0.053484 -0.066422 -0.040963 -0.024963  0.000754  0.015174  0.032104  0.064670 -0.001326  0.041239  0.076199  0.061597  0.008935 -0.038814 -0.057804 -0.038525 -0.004034  0.030699  0.082652  0.094290 -0.003602 -0.038326  0.052086  0.079015  0.009740 -0.059905 -0.016986  0.045746 -0.048629
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.132069 -0.049193 -0.040023  0.034966 -0.509833  0.083622 -0.049056 -0.235340  0.111539  0.004146 -0.026328  0.013613 -0.430128 -0.129772  0.204905  0.337592  0.548524  0.071321 -0.233094 -0.186542  0.114491  0.269025  0.311004  0.814207  0.969287  0.891456  0.765323  0.629029  0.941551  0.933745  0.866720  0.812375  0.728093  0.810699  0.844483  0.852583  0.849960  0.780055  0.789197  0.797958  0.907764  0.889343  0.905860  1.049363  1.000223  0.992913  1.035157  1.054500  1.094297  1.113622  1.041708  1.109230  1.119198  1.119703  0.945444  0.954536  1.080975  1.105053  1.080435  1.027571  1.025325  1.019846  0.986736  0.968893  0.944371  0.972768  0.961800  0.970937  0.970295  0.954309  1.061607  0.913706  0.923574  0.926449  0.975806  0.966577  0.964043  0.960625  1.081712  1.098197  1.002536  1.089060  1.000430  1.098994  1.044332  1.007427  0.967225  0.888826  0.855136  0.848655  0.992508  0.941835  0.733774  0.625901  0.659484  0.755240  0.791070  0.817886  0.743546  0.671054  0.637858  0.745782  0.585206  0.830124  0.621147  0.543332  0.395665 -0.051537  0.763978  0.775167  0.720303  0.839438  0.742506  0.933950  0.671633  0.675932  0.663454  0.577131  0.455007  0.329796 -0.140672  0.129430  0.057947 -0.225017  0.167156 -0.020630 -0.056929  0.193759
         1.121896  1.118583  1.119029 -1.088873 -0.723108 -1.101762 -1.069626 -1.003099 -1.135906  1.134822  1.133878  1.135095  0.908564  0.857454  1.100057  1.065275 -0.963770 -1.086935 -0.984307 -0.985061 -1.115611 -1.112137 -1.103153 -0.797636 -0.531472 -0.643236 -0.761000 -0.820909 -0.510532 -0.515642 -0.239886  0.738734  0.834057  0.722283  0.640297  0.638155  0.655124  0.813987  0.798554  0.797780  0.602843  0.595242  0.564191  0.376308  0.481348  0.110762 -0.065364 -0.138732 -0.171754  0.081672 -0.119370  0.109370  0.089687  0.063462 -0.427611 -0.467947 -0.174755 -0.144430 -0.133022 -0.348785 -0.383971 -0.384104 -0.389167 -0.461179 -0.192944 -0.441220 -0.474044 -0.472547 -0.499861 -0.540459 -0.338293 -0.552357 -0.634526 -0.607264 -0.532736 -0.497325 -0.490748 -0.489823 -0.061451  0.276568  0.467651  0.234457  0.088898  0.188852 -0.309823  0.052023  0.395226  0.654114  0.705431  0.676439  0.454423  0.552675  0.856776  0.948534  0.918089  0.799350  0.736522  0.667540  0.815690  0.879399  0.904787  0.801167  0.912599  0.657381  0.933872  0.998016  1.065225  0.967767  0.826276  0.810333  0.868523  0.649312  0.821676 -0.262385 -0.852275 -0.831695 -0.874184 -0.992401 -1.053269 -1.005628 -1.033044  1.130246 -1.131964 -1.013694 -1.078243  1.119130  1.132215 -1.070427
        -0.032351 -0.018088 -0.056269 -0.086726 -0.620558 -0.105810 -0.261717 -0.362963  0.041288  0.083284  0.047173  0.028183 -0.429900 -0.668173 -0.072016 -0.080482 -0.120051 -0.205862 -0.404978 -0.428686 -0.132074  0.027109  0.126545  0.011563 -0.135068 -0.199739 -0.295378 -0.422500 -0.285870 -0.303747 -0.650498 -0.192307 -0.053314 -0.220006 -0.314591 -0.299041 -0.275325 -0.002533 -0.020646  0.008589 -0.223883 -0.284601 -0.295079 -0.055430  0.102433 -0.499188 -0.408710 -0.331724 -0.125801 -0.025106 -0.331207 -0.106620 -0.026265 -0.023323 -0.336078 -0.278945 -0.173179 -0.082606 -0.209496 -0.216455 -0.187252 -0.211127 -0.322105 -0.273803 -0.549103 -0.292359 -0.277227 -0.246913 -0.198744 -0.176372 -0.043250 -0.279285 -0.086647 -0.140404 -0.078274 -0.218404 -0.239833 -0.253823 -0.208966  0.030459 -0.164570 -0.117006 -0.441861 -0.059745 -0.221033 -0.476297 -0.402729 -0.166889  0.047517 -0.222801 -0.221963 -0.218032  0.109944  0.078662  0.010289 -0.203878 -0.263717 -0.330018 -0.097815 -0.000112 -0.007820 -0.186747 -0.280114 -0.318911 -0.129706  0.031900 -0.010081 -0.545179 -0.036700 -0.037924  0.104563 -0.310837 -0.008999 -0.539322 -0.264072 -0.326524 -0.248300  0.091424  0.133952 -0.361432 -0.367429  0.064159 -0.043131 -0.340207 -0.018011 -0.048175  0.032894  0.029338
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.008851 -0.000928 -0.002374 -0.003283 -0.004691  0.015752  0.003750  0.003555  0.005071  0.022984  0.025227  0.026637  0.022169  0.020030  0.045276  0.051230  0.055696  0.049984  0.041033  0.069389  0.070222  0.072608  0.074470  0.062545  0.065156  0.056872  0.055447  0.063449  0.057863  0.058634  0.059125  0.061815  0.063716  0.064471  0.062833  0.042150  0.032085  0.037195  0.047366  0.050277  0.062378  0.049649  0.064179  0.053731  0.066915  0.058067  0.034467  0.033744  0.037318  0.037952  0.035290  0.036426  0.020852  0.028998  0.055306  0.044002  0.007718  0.012439  0.011896 -0.008209 -0.008416 -0.003093  0.006088 -0.009116
         0.119861  0.120216  0.123468  0.123716  0.123761 -0.077007 -0.088562 -0.089246 -0.089942  0.114013  0.110897  0.102212  0.104431  0.097840 -0.023040 -0.017702 -0.014727 -0.032628 -0.058647 -0.000677  0.012783  0.020014  0.031062  0.064406  0.069425  0.073711  0.067712  0.046325  0.053043  0.039082  0.014361  0.014598  0.014841  0.021691  0.032141  0.075000  0.089148  0.090653  0.085439  0.085240  0.038882  0.069837  0.057655  0.082080  0.024172 -0.023513 -0.072329 -0.064975 -0.046027 -0.037768 -0.042457 -0.037662 -0.074636 -0.067276 -0.029163 -0.031030  0.113745  0.112681  0.116227 -0.091915 -0.092284 -0.098026  0.119280 -0.101832
        -0.104428 -0.121057 -0.139786 -0.157087 -0.164822 -0.172454 -0.149674 -0.132363 -0.115742 -0.112841 -0.117763 -0.136259 -0.147417 -0.180286 -0.177978 -0.162410 -0.146536 -0.135397 -0.122690 -0.117564 -0.126499 -0.122406 -0.115698 -0.126311 -0.111643 -0.130008 -0.145155 -0.143793 -0.165214 -0.184229 -0.186196 -0.170277 -0.153778 -0.151812 -0.170826 -0.167562 -0.165963 -0.142250 -0.127921 -0.112947 -0.168187 -0.157555 -0.129439 -0.108665 -0.137791 -0.122248 -0.117667 -0.133953 -0.157373 -0.172218 -0.173210 -0.180595 -0.156885 -0.144674 -0.122681 -0.161986 -0.176473 -0.155444 -0.127780 -0.125732 -0.134140 -0.178294 -0.112002 -0.167284
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.011667  0.241144  0.438530  0.457733  0.402536  0.832146  0.516320  0.295784  0.274411  0.553219  0.658255  0.711186  0.713111  0.779424  0.888063  0.922747  0.949434  0.924806  0.889020  0.954387  0.990022  1.015246  1.022123  0.938824  0.846245  0.871461  0.897574  1.015604  0.954995  0.982731  1.029473  1.040343  1.049583  1.059027  1.049906  0.805980  0.789845  0.730951  0.706001  0.767214  1.037835  0.847540  0.971139  0.775740  1.024648  0.942309  0.739278  0.796015  0.903230  0.907799  0.906148  0.912490  0.811698  0.843355  0.924750  0.896087  0.662009  0.667667  0.446400  0.157064  0.258199  0.527730  0.117016  0.331608
         1.119034  1.083115  1.015521  1.007618  1.026660 -0.648923 -0.946073 -1.043147 -1.049964  0.939197  0.840834  0.806414  0.819129  0.672891 -0.530652 -0.487717 -0.416749 -0.456138 -0.507365 -0.346510 -0.157451 -0.051459  0.117272  0.430663  0.653781  0.571047  0.547371  0.249111  0.478142  0.186383 -0.055403 -0.142027 -0.114508 -0.015408  0.107664  0.660319  0.680060  0.793712  0.804995  0.773884  0.231026  0.603340  0.363854  0.762253 -0.005553 -0.415722 -0.709740 -0.644410 -0.490441 -0.497236 -0.499592 -0.507375 -0.672166 -0.617027 -0.441770 -0.508726  0.851163  0.863323  1.009694 -1.078131 -1.051197 -0.941999  1.110955 -1.041903
        -0.069316 -0.001780 -0.040565 -0.235701 -0.302443 -0.252528 -0.105553 -0.200734 -0.191992 -0.307579 -0.375575 -0.324655 -0.256069 -0.484650 -0.356977 -0.302213 -0.345559 -0.370611 -0.404773 -0.429297 -0.480037 -0.448483 -0.423139 -0.439345 -0.331707 -0.428508 -0.391631 -0.372437 -0.285402 -0.536857 -0.417619 -0.304253 -0.260249 -0.249191 -0.312183 -0.439529 -0.442771 -0.298491 -0.348010 -0.221443 -0.275722 -0.438170 -0.418687 -0.235425 -0.423909 -0.375802 -0.429033 -0.422395 -0.386924 -0.351818 -0.353863 -0.289593 -0.289251 -0.330677 -0.394382 -0.375892 -0.344154 -0.240371 -0.241460 -0.133568 -0.037173 -0.255337 -0.177404 -0.154093
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.129824 -0.134562 -0.120287 -0.123859 -0.137894 -0.129715 -0.135602 -0.135671 -0.129584 -0.137942 -0.121352 -0.111167 -0.094414 -0.087745 -0.096434 -0.108010 -0.097470 -0.109888 -0.104171 -0.108881 -0.088840 -0.100811 -0.088768 -0.092008 -0.103896 -0.102011 -0.051844 -0.042737 -0.034415 -0.039219 -0.040671 -0.031866 -0.047679 -0.047068 -0.045853 -0.041817 -0.040876 -0.030336 -0.025139 -0.036189 -0.015316 -0.021920 -0.034600 -0.019914 -0.019239 -0.017989 -0.018487 -0.004688  0.001480 -0.000588 -0.017742 -0.008813 -0.001083 -0.009814 -0.013412 -0.002214  0.006583  0.003218 -0.002896 -0.010123 -0.017430 -0.017795 -0.008434  0.013271  0.010761  0.019071  0.010060  0.007703  0.014852  0.009433  0.007759  0.010081  0.017866  0.003888  0.020148  0.016495  0.025711  0.024291  0.018358  0.020649  0.026826  0.029052  0.020350  0.033023  0.021113  0.023942  0.014223  0.007612  0.027448  0.028236  0.032143  0.037985  0.048776  0.033673  0.036184  0.043752  0.038217  0.034620  0.039882  0.035143  0.036800  0.039323  0.039747  0.051025  0.064415  0.065191  0.054513  0.037724  0.037074  0.043756  0.056518  0.072530  0.068432  0.053153  0.070208  0.070654  0.069758  0.069050  0.070257  0.068067  0.072171  0.080950  0.085008  0.072488  0.086433  0.075985  0.080440  0.076160  0.073909  0.070210  0.097367  0.088961  0.097187  0.088936  0.097812  0.098961  0.088738  0.102360  0.107515  0.105606  0.116073  0.113344  0.117333  0.120306  0.104479  0.121126  0.106111  0.116735  0.132927  0.140189  0.123218  0.137059  0.135622  0.129275  0.142758  0.141784  0.150171  0.158402  0.145033  0.131216  0.129737  0.125583  0.114481  0.096138  0.101927  0.097045  0.077697  0.068213  0.114597  0.097414  0.056780  0.052614  0.054370  0.062705  0.060669  0.063198  0.043222  0.080036  0.049290  0.059070  0.035967  0.063459  0.030330  0.061118  0.059262  0.028438  0.021773  0.052978  0.052149  0.001075  0.006118  0.003786  0.041488  0.018697  0.046619  0.044565  0.009637  0.006490  0.032093  0.021752  0.026024  0.021740 -0.004417 -0.016270 -0.014261 -0.001651 -0.007374 -0.025798 -0.015771 -0.003988 -0.008999  0.000674  0.006352 -0.068746 -0.058329 -0.058817 -0.059142 -0.050342 -0.043021 -0.013077 -0.042547 -0.020579 -0.060536 -0.067595 -0.077571 -0.029825 -0.065992 -0.057149 -0.024806 -0.066171 -0.065760 -0.064990 -0.078755 -0.090027 -0.027382 -0.059973 -0.083534 -0.115258 -0.076734 -0.086312 -0.120045 -0.076279 -0.108622 -0.148264 -0.152690 -0.159535 -0.153339 -0.159972 -0.151107 -0.150822 -0.151885 -0.117360 -0.142214 -0.112331 -0.114359 -0.143728
        -0.038650 -0.034519 -0.035448 -0.011096  0.012864  0.027667  0.031872  0.036713  0.049247  0.048353  0.055998  0.071694  0.067428  0.075615  0.057474  0.051129  0.036080  0.032005  0.010060 -0.003521  0.003899 -0.018536 -0.023097 -0.028760 -0.052740 -0.061203  0.104796  0.101373  0.095784  0.091187  0.072386  0.066627  0.063621  0.051304  0.040354  0.017767  0.001074 -0.005235 -0.015801 -0.018991 -0.032530 -0.041801 -0.052901 -0.054389 -0.069531  0.105024  0.103653  0.102923  0.101213  0.094984  0.077324  0.066459  0.075959  0.056495  0.062304  0.062304  0.054693  0.032755  0.019844  0.031050 -0.002041 -0.011191 -0.002396 -0.030256 -0.038500 -0.047585 -0.048126 -0.047320 -0.051999 -0.048301 -0.051267 -0.056121 -0.056798 -0.075709  0.090869  0.092009  0.084709  0.090524  0.103713  0.104924  0.102619  0.099192  0.083015  0.094236  0.064399  0.073755  0.006162  0.032092 -0.044450 -0.056275 -0.060858 -0.062376 -0.071362 -0.062594 -0.066337 -0.074980 -0.070870 -0.060793 -0.058730 -0.050061 -0.040439 -0.032250 -0.023497 -0.017566  0.023949  0.036696  0.064210  0.073926  0.075965  0.076846  0.078006  0.090158  0.103403  0.102566  0.108721  0.107374 -0.079728 -0.081460 -0.086183 -0.085941 -0.084314 -0.075505 -0.067723 -0.058374 -0.050403 -0.036878 -0.026616 -0.015151 -0.006480  0.004960  0.003157  0.029975  0.048768  0.058065  0.082117  0.096811  0.106194  0.103060 -0.089086 -0.085484 -0.074930 -0.064082 -0.057675 -0.050221 -0.041320 -0.033209 -0.024003 -0.001137 -0.019335  0.004977  0.022150  0.038796  0.058016  0.082404 -0.044281 -0.032586 -0.020736  0.002102  0.057696 -0.031268 -0.069023  0.084127  0.030442 -0.049123 -0.091239 -0.091477  0.099282  0.067029  0.006375 -0.038681 -0.078084 -0.081005 -0.078576 -0.080819 -0.077566 -0.074004 -0.066208  0.106491  0.063503  0.039833 -0.028691 -0.021708 -0.043471 -0.064175 -0.067387 -0.063573 -0.056878 -0.074511 -0.075372  0.005209  0.067823  0.070048  0.094713  0.092803  0.103717  0.108757  0.101183  0.099320  0.079347 -0.066375 -0.052107 -0.056449 -0.037652 -0.029544 -0.028985 -0.041175 -0.029840  0.022576  0.041095  0.002069  0.038653  0.084054  0.102762 -0.074015 -0.069764 -0.041025 -0.022429 -0.011408 -0.038750 -0.070410 -0.057307 -0.025593  0.007807  0.029522  0.032330  0.061432  0.059639  0.071967  0.078623  0.080340  0.086219  0.097732  0.089321  0.077612  0.103561  0.104077 -0.053615 -0.048631 -0.001987  0.025707  0.049553  0.065099  0.061983 -0.022677 -0.019174 -0.004313  0.017834  0.010439  0.035592  0.025223 -0.021267  0.061738  0.032664  0.013878 -0.021040 -0.026496
        -0.336532 -0.340469 -0.361003 -0.388006 -0.396351 -0.393159 -0.385660 -0.372505 -0.362492 -0.346101 -0.344733 -0.326677 -0.350287 -0.335865 -0.361857 -0.374563 -0.385909 -0.389283 -0.394563 -0.390629 -0.387446 -0.376217 -0.368811 -0.364947 -0.343401 -0.330226 -0.307638 -0.330972 -0.338365 -0.340467 -0.355909 -0.364959 -0.359413 -0.373961 -0.378337 -0.384100 -0.386662 -0.394434 -0.394842 -0.368677 -0.361856 -0.336621 -0.327661 -0.322945 -0.309061 -0.315325 -0.331688 -0.320967 -0.348105 -0.355618 -0.363659 -0.391045 -0.389018 -0.406893 -0.389851 -0.415685 -0.442756 -0.452425 -0.442541 -0.423548 -0.415652 -0.413656 -0.433243 -0.449843 -0.432912 -0.423919 -0.397534 -0.378608 -0.366552 -0.354824 -0.334721 -0.317093 -0.301582 -0.295541 -0.282934 -0.289926 -0.310332 -0.324040 -0.349073 -0.360691 -0.379921 -0.396128 -0.415764 -0.414182 -0.447784 -0.438620 -0.467672 -0.460045 -0.455401 -0.442137 -0.437804 -0.418129 -0.409347 -0.391644 -0.381069 -0.362023 -0.338949 -0.321735 -0.317860 -0.307625 -0.300819 -0.290985 -0.285546 -0.279612 -0.274417 -0.281827 -0.292093 -0.283204 -0.293143 -0.309188 -0.315696 -0.335904 -0.358277 -0.352578 -0.378546 -0.404836 -0.417178 -0.388706 -0.368189 -0.350732 -0.345338 -0.337928 -0.332534 -0.318032 -0.316156 -0.302129 -0.295259 -0.287460 -0.281737 -0.275905 -0.285531 -0.290727 -0.301690 -0.304235 -0.328294 -0.345596 -0.369715 -0.358121 -0.382407 -0.362236 -0.356128 -0.343500 -0.338920 -0.333203 -0.315420 -0.315740 -0.298715 -0.294397 -0.310433 -0.309417 -0.303133 -0.318179 -0.325894 -0.344785 -0.339860 -0.326685 -0.321534 -0.322488 -0.335127 -0.319382 -0.357204 -0.339641 -0.301738 -0.319185 -0.374332 -0.367374 -0.350133 -0.304896 -0.294007 -0.308847 -0.333592 -0.355093 -0.374106 -0.379619 -0.393464 -0.431546 -0.441895 -0.368421 -0.280190 -0.271918 -0.283757 -0.291608 -0.291659 -0.321191 -0.323438 -0.346215 -0.392504 -0.406577 -0.422210 -0.450181 -0.422228 -0.407041 -0.423277 -0.390296 -0.412318 -0.379829 -0.356650 -0.326449 -0.297006 -0.286943 -0.299435 -0.320528 -0.365075 -0.379959 -0.398873 -0.386256 -0.415408 -0.396376 -0.408334 -0.441886 -0.422972 -0.372160 -0.339734 -0.319244 -0.319999 -0.344753 -0.361843 -0.373646 -0.343419 -0.306781 -0.326135 -0.384147 -0.382143 -0.380645 -0.382080 -0.372166 -0.366404 -0.351852 -0.357016 -0.342138 -0.334990 -0.320087 -0.318140 -0.327416 -0.331355 -0.307302 -0.340825 -0.342865 -0.379747 -0.388063 -0.373833 -0.361153 -0.345831 -0.352606 -0.356472 -0.364481 -0.379129 -0.354502 -0.345263 -0.371642 -0.375127 -0.337206 -0.370715 -0.396895 -0.376656 -0.348714
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.825110 -0.820925 -0.320923 -0.010245 -0.498624 -0.270527 -0.444671 -0.651333 -0.654477 -0.699779 -0.491206 -0.492523 -0.470844 -0.317455 -0.316939 -0.105273  0.009627 -0.078265  0.193502  0.105806  0.217010  0.099663  0.146500  0.102745 -0.287084 -0.465541 -0.125510 -0.296121 -0.263600 -0.249856 -0.377303 -0.508123 -0.115357 -0.282970 -0.327387 -0.350392 -0.568824 -0.778767 -0.816843 -0.355985 -0.414807  0.247943  0.163277  0.029883  0.160890  0.061391 -0.061943  0.244084 -0.356370 -0.497681 -0.416032 -0.660545 -0.630618 -0.777197 -0.646035 -0.714760 -0.709445 -0.870165 -0.902276 -0.903825 -0.912596 -0.894489 -0.888278 -0.651783 -0.651055 -0.573640 -0.513607 -0.599176 -0.610475 -0.573103 -0.302190  0.051963  0.696983  0.384109  0.694522  0.663186  0.520595  0.398393  0.034137 -0.276060 -0.446219 -0.515561 -0.600078 -0.520213 -0.478680 -0.528992 -0.754460 -0.831366 -0.696594 -0.737052 -0.597968 -0.628198 -0.626389 -0.413656 -0.463981 -0.501194 -0.537461 -0.232763 -0.050147  0.376165  0.528953  0.678584  0.603202  0.588156  0.793158  0.760124  0.600296  0.667839  0.561161  0.291795  0.180539  0.109748  0.122799  0.184977  0.174986  0.059107  0.286248 -0.248330 -0.228718 -0.073074  0.147897  0.437885  0.410619  0.243342  0.349311  0.115428  0.113637  0.136477  0.272763  0.550087  0.466051  0.407508  0.425360  0.358612  0.206500  0.191364  0.188386  0.348360  0.113015  0.442463  0.478421  0.450363  0.470500  0.492169  0.458363  0.473881  0.431453  0.570470  0.600573  0.725895  0.584298  0.673297  0.724077  0.774081  0.424311  0.462398  0.607425  0.686023  0.819253  0.489143  0.413572  0.704990  0.529377  0.415085  0.153599  0.192351  0.027480  0.334489  0.556966  0.397069 -0.079940 -0.410590 -0.374484 -0.307292 -0.355052  0.079587 -0.109636  0.115487  0.759087  0.972323  0.931654  0.146569  0.896782  0.142908  0.130497 -0.598195 -0.426364 -0.505277 -0.467847 -0.833775 -0.596239 -0.689063 -0.477883 -0.618478 -0.237877 -0.115695 -0.440253  0.363805  0.549684  0.751407  0.624013 -0.160294 -0.513651 -0.629227 -0.743494 -0.600732 -0.724635 -0.855712 -0.877516 -0.865309 -0.895403 -0.567653  0.114186  0.056210  0.209003  0.144193  0.029823 -0.265723  0.045744  0.175831  0.164071 -0.690995 -0.097422  0.079398  0.168299 -0.521940 -0.070181 -0.162306 -0.319316 -0.430859 -0.456773 -0.515458 -0.587329 -0.419937 -0.095582 -0.199442  0.046435 -0.516224  0.164998  0.231063 -0.527286 -0.177133 -0.569667 -0.851123 -0.912617 -1.150904 -0.975242 -1.148129 -0.930830 -0.886301 -0.597474 -0.639035 -0.734815  0.115730 -0.029850 -0.812463
        -0.754437 -0.760479 -0.807628 -0.649987  0.188877  0.574285  0.933927  0.783355  0.847839  0.865554  0.943754  0.965270  0.827161  1.004518  0.909494  0.893183  0.541573  0.626593 -0.116980 -0.528109 -0.361494 -0.655574 -0.642522 -0.674179 -0.806368 -0.954445  1.101055  0.903831  0.647518  0.705333  0.567278  0.683612  0.847389  0.528939  0.304141  0.187415 -0.200966 -0.273085 -0.478595 -0.847350 -0.917311 -0.885823 -0.720413 -0.715406 -0.735178  1.101506  1.075145  1.071074  0.948638  0.786090  0.857970  0.676642  0.772200  0.616826  0.692142  0.726773  0.639481  0.181910  0.101839  0.179943 -0.015476 -0.196976 -0.064757 -0.522756 -0.729119 -0.845834 -0.877776 -0.836910 -0.832354 -0.852700 -0.973415 -0.983194 -0.785358 -0.702870  0.744805  0.834861  0.937008  0.924261  1.077322  1.052889  0.952216  0.905275  0.762317  0.810846  0.753938  0.760334 -0.168877  0.193520 -0.382004 -0.690942 -0.834172 -0.790667 -0.801603 -0.923738 -0.914797 -0.907333 -0.879560 -0.844784 -0.781954 -0.788346 -0.644026 -0.556458 -0.636284 -0.849135  0.296021  0.335389  0.769749  0.846506  0.917915  0.985935  0.931695  0.917757  1.015311  0.967293  1.098220  1.099574 -1.047063 -1.015151 -1.025183 -1.050724 -0.986364 -0.722105 -0.754393 -0.667498 -0.695099 -0.643108 -0.592180 -0.592531 -0.450007 -0.198675  0.079360  0.380984  0.469436  0.595262  0.815198  0.893722  1.095714  1.063231 -1.037947 -0.848830 -0.734012 -0.761612 -0.722668 -0.686480 -0.721804 -0.731451 -0.605103 -0.017459 -0.467722  0.079320  0.316148  0.389621  0.408142  0.816912 -0.638804 -0.746567 -0.413876  0.050935  0.506065 -0.714510 -0.761068  0.820472  0.360624 -0.717116 -1.083433 -1.063656  0.989973  0.765840  0.157523 -0.715531 -0.914155 -0.965354 -0.954499 -0.986654 -0.984576 -0.881102 -1.047296  1.087768  0.734926  0.240270 -0.326274 -0.649455 -0.542686 -0.678116 -0.679528 -0.842882 -0.911410 -0.886172 -0.923259 -0.059201  0.834972  0.711219  0.793293  0.810474  1.029935  1.088389  0.912792  1.017638  0.931018 -0.455498 -0.852534 -1.035258 -0.895742 -0.818120 -0.687623 -0.841610 -0.633888  0.164673  0.247010 -0.047539  0.230341  0.832206  1.082886 -0.762692 -0.733124 -0.684868 -0.713765 -0.696066 -0.752798 -0.756574 -0.708940 -0.756810 -0.205661  0.305981  0.386868  0.741799  0.713567  0.678562  0.868151  0.753312  0.854106  0.816194  0.758913  0.969769  1.044846  1.088099 -0.736110 -0.864991 -0.449745  0.264492  0.673845  0.779650  0.794458 -0.718607 -0.643500 -0.128299  0.440173  0.146353  0.639072  0.677477 -0.800607  0.801062  0.770398  0.051810 -0.724014 -0.768056
        -0.271521 -0.297023 -0.963899 -1.148868 -1.312263 -1.218617 -0.686947 -0.792764 -0.657005 -0.474635 -0.618770 -0.563510 -0.880481 -0.626334 -0.836661 -0.913948 -1.242068 -1.192691 -1.340741 -1.214472 -1.266619 -1.120524 -1.117784 -1.103530 -0.974658 -0.594733 -0.352817 -0.871211 -1.140277 -1.101030 -1.157009 -0.991464 -0.976769 -1.220256 -1.300917 -1.320049 -1.211557 -1.001152 -0.793624 -0.828625 -0.601792 -0.800085 -1.045485 -1.071662 -1.033263 -0.342221 -0.564743 -0.292757 -0.740297 -0.880282 -0.845279 -0.841515 -0.733887 -0.718587 -0.840509 -0.640982 -0.823975 -0.916057 -0.871951 -0.846784 -0.861183 -0.857380 -0.904398 -1.011223 -0.724537 -0.531124 -0.591064 -0.287808 -0.363055 -0.471423 -0.566183 -0.689626 -0.391028 -0.975320 -0.625845 -0.463020 -0.140990  0.082077 -0.121396 -0.476134 -0.613188 -0.627705 -0.807704 -0.838918 -0.971943 -0.910713 -1.082886 -0.979716 -1.056916 -0.549517 -0.367478 -0.257530 -0.304947 -0.633155 -0.554129 -0.463120 -0.218057  0.238502  0.366861  0.274303  0.326452  0.271644  0.268539 -0.138252  0.354002  0.372277  0.159941 -0.205264 -0.164878  0.025527  0.190105  0.224181  0.048499  0.132055 -0.278980 -0.528957 -0.549066 -0.554515 -0.538168 -0.128756  0.079601  0.336243  0.317436  0.456420  0.401662  0.492182  0.528777  0.526784  0.592186  0.577471  0.640895  0.589796  0.544089  0.494948  0.343106  0.251393 -0.265222 -0.225624 -0.698054  0.188970  0.302899  0.288854  0.319151  0.342735  0.326224  0.307174  0.447258  0.596590  0.451313  0.497689  0.534107  0.449526  0.402894 -0.133306  0.413314  0.295760  0.476188  0.525588  0.251723  0.317068  0.306506  0.009763  0.546108  0.353169 -0.487825 -0.153398  0.101695  0.359003  0.589998  0.363523  0.197719 -0.413136 -0.595111 -0.585935 -0.460617 -1.021736 -0.560646 -0.179114 -0.067171  0.082756  0.046680  0.482360 -0.284980  0.463911  0.464096 -0.343142 -0.655085 -0.234648 -0.525605 -0.996878 -0.642791 -0.736980 -0.915455 -0.671289 -0.659667 -0.500815 -0.729329 -0.156105 -0.214797 -0.849249 -0.340975 -0.226739 -0.431121 -0.429264 -0.584385 -0.366117 -0.762772 -0.935867 -0.872009 -0.947518 -0.844330 -0.714611 -0.173218 -1.033464 -1.018918 -1.082489 -1.078879 -1.069961 -1.037866 -1.009271 -1.055987 -0.510103 -1.358396 -1.330362 -1.284216 -0.915234 -1.122949 -1.139878 -0.895448 -0.985653 -0.842393 -0.850078 -0.859433 -0.635986 -0.662008 -0.417973 -1.060067 -0.758538 -1.240606 -1.305807 -1.040477 -1.053429 -0.846748 -0.205396 -0.175558 -0.321888 -0.761367 -0.425727 -0.512125 -0.555036 -0.815671 -0.762948 -0.694094 -1.369802 -1.082832 -0.295030
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.003872  0.019980  0.039358  0.026374  0.032159  0.034891  0.045243  0.044290  0.033680  0.054899  0.092652  0.072207  0.078639  0.076981  0.083737  0.090725  0.074797  0.110267  0.115035  0.111368  0.108476  0.133349  0.136153  0.137021  0.131285  0.140045  0.142382  0.144433  0.139953  0.132010  0.129274  0.136572  0.120432  0.109395  0.110128  0.116409  0.094800  0.095290  0.102572  0.120400  0.102221  0.138848  0.141485  0.146471  0.148237  0.140356  0.139779  0.136431  0.129482  0.149126  0.139621  0.096183  0.090528  0.098333  0.056310  0.059302  0.058995  0.091314  0.069003  0.061675  0.043312  0.045844  0.021067  0.007679
         0.115634 -0.141962  0.095696  0.107480  0.118380  0.124361 -0.142404 -0.142991 -0.137501 -0.132458 -0.124512 -0.139564 -0.137217  0.110506  0.103497  0.091754  0.086022  0.065630  0.074604  0.081943  0.081685  0.050918  0.049482  0.039398  0.006591  0.005087  0.022468  0.032411 -0.028065 -0.065524 -0.074889 -0.058733 -0.080579 -0.107976 -0.103160 -0.093991 -0.125362 -0.125715 -0.116656 -0.089114 -0.113009 -0.033584 -0.025642 -0.024367 -0.013578 -0.005951 -0.043650 -0.052597 -0.055099  0.004233  0.022105  0.091904  0.094606  0.086125  0.095726  0.105424  0.120100 -0.130113 -0.128731 -0.141293  0.122048  0.096618 -0.144860  0.104390
         0.230322  0.217157  0.256507  0.233866  0.195817  0.155823  0.167722  0.203005  0.243452  0.265257  0.235611  0.216153  0.182566  0.145834  0.176506  0.206024  0.254001  0.251556  0.215323  0.180650  0.144437  0.177781  0.212539  0.231451  0.257088  0.201304  0.180031  0.163235  0.151233  0.164804  0.208547  0.230286  0.258539  0.226369  0.186464  0.180355  0.145546  0.189623  0.224968  0.189060  0.239602  0.243304  0.210761  0.174446  0.165172  0.224688  0.226626  0.241347  0.146260  0.164365  0.218413  0.153143  0.195574  0.211582  0.238820  0.209780  0.158710  0.153569  0.264609  0.184786  0.157022  0.247159  0.169907  0.262064
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.222228 -0.002905  0.361584  0.279716  0.214687  0.294375  0.143665  0.097510  0.020775  0.205469  0.760713  0.300066  0.407748  0.725054  0.585057  0.567589  0.400876  0.767495  0.701476  0.804853  0.736042  1.025178  1.081474  1.083625  0.957926  1.133195  1.069956  1.114232  1.003043  1.006139  0.958426  1.097224  0.905090  0.931742  0.915174  0.927489  0.812618  0.859733  0.862799  0.953570  0.811090  1.106876  1.129736  1.125181  1.124280  1.140215  1.129257  1.099541  0.999078  1.144776  1.138777  0.801278  0.584809  0.588867  0.372334  0.337916  0.263101  0.713124  0.485283  0.207464  0.218873  0.371240  0.113561  0.255252
         1.045258 -1.125958  0.989528  1.037643  1.078426  1.053110 -1.136789 -1.140789 -1.120816 -1.092583 -0.816122 -1.111897 -1.079619  0.848829  0.943322  0.940836  0.971484  0.711141  0.871966  0.798776  0.857090  0.498334  0.372466  0.287333  0.224731  0.087215  0.223007  0.255267 -0.358670 -0.478611 -0.627068 -0.284886 -0.622467 -0.668131 -0.692223 -0.676908 -0.809053 -0.768935 -0.761150 -0.641872 -0.774707  0.032497  0.019815 -0.224542 -0.178833 -0.016099 -0.161699 -0.266368 -0.423709  0.038632  0.059688  0.794028  0.933096  0.940511  0.993977  1.039103  1.097636 -0.911177 -0.983765 -1.130258  1.093723  0.988389 -1.140094  1.032229
         0.527171  0.393874  0.568331  0.499600  0.407937  0.420573  0.128832  0.242584  0.424908  0.500816  0.510149  0.267633  0.122860 -0.040928  0.384769  0.433318  0.585104  0.640615  0.357805  0.108331  0.214486  0.244556  0.257802  0.437532  0.775817  0.284421  0.495292  0.262353 -0.230574 -0.090649  0.121342  0.391543  0.559651  0.326223  0.116418  0.131811  0.013999  0.233162  0.306716  0.172101  0.483860  0.485287  0.356686  0.129098  0.023640  0.211312  0.310280  0.404815 -0.182266  0.077769  0.201849  0.053832  0.424965  0.383047  0.544229  0.434763  0.211276  0.096192  0.573847  0.158441  0.315216  0.561581  0.139007  0.541029
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.065679 -0.067360 -0.053844 -0.058688 -0.052992 -0.051125 -0.048786 -0.043180 -0.044312 -0.050454 -0.054299 -0.047031 -0.052571 -0.033425 -0.035243 -0.027276 -0.015008 -0.014767 -0.029807 -0.019357 -0.027223 -0.036015 -0.025203 -0.027939 -0.007877 -0.001109 -0.005227 -0.003999 -0.010697 -0.016868 -0.016456 -0.018487 -0.008629  0.015259  0.009037  0.017640  0.012080  0.007992 -0.000293  0.015855  0.012048  0.009538  0.007727  0.020213  0.026757  0.026529  0.036255  0.043465  0.037656  0.021977  0.015545  0.001803  0.018512  0.032102  0.025598  0.026782  0.046457  0.045135  0.057550  0.040966  0.042690  0.056369  0.037146  0.041040  0.043903  0.053225  0.059823  0.068199  0.076368  0.071201  0.084847  0.072264  0.068131  0.074217  0.067147  0.081124  0.073280  0.103463  0.097042  0.104832  0.109236  0.108655  0.102062  0.086610  0.070059  0.053407  0.062062  0.070948  0.064974  0.055521  0.055137  0.048148  0.047184  0.034231  0.055017  0.107932  0.060634  0.053989  0.063130  0.060940  0.092446  0.027563  0.024510  0.026941  0.051341  0.063157  0.040213  0.046286  0.020911 -0.006044 -0.009020 -0.008355  0.028517  0.022838  0.003813  0.006019 -0.019919 -0.042731 -0.038798 -0.008082 -0.008098 -0.060944 -0.026939 -0.026682 -0.046161 -0.049482 -0.060528 -0.060718
         0.104703  0.083089 -0.110272 -0.127094 -0.129680 -0.126663 -0.122202  0.021660  0.040212  0.058051  0.080929  0.072373  0.088297  0.081788  0.093984 -0.122476 -0.123547 -0.122079 -0.124024 -0.117334 -0.116679 -0.114515 -0.103194 -0.096033 -0.003571  0.017682  0.025600  0.038676  0.055926  0.071744  0.078746  0.085811  0.088216 -0.123549 -0.120817 -0.115558 -0.105991 -0.102868 -0.098254 -0.086785 -0.078590 -0.072751 -0.065118 -0.055847 -0.020792 -0.005249 -0.003157 -0.003243  0.019462  0.020077  0.029520  0.032458  0.037379  0.052832  0.071472  0.076705 -0.113288 -0.104191 -0.095308 -0.093938 -0.080939 -0.073595 -0.068706 -0.060659 -0.052239 -0.041784 -0.045767 -0.035253 -0.051794 -0.035733 -0.044591 -0.072998 -0.066548 -0.090117 -0.082162  0.011796  0.003371 -0.015077 -0.020843 -0.029691 -0.053891 -0.034992 -0.059104 -0.032119 -0.019649 -0.017538 -0.029533 -0.011352 -0.006708  0.004337  0.031544  0.053341  0.066758  0.037303 -0.012525 -0.029306 -0.102710 -0.089959 -0.074457 -0.065776 -0.044252 -0.055968 -0.062779 -0.070704 -0.092853 -0.096136  0.055071 -0.012327 -0.052778 -0.075258 -0.081398 -0.092175 -0.077479 -0.109532  0.084314  0.054904 -0.108288 -0.119805 -0.128462 -0.122359 -0.123082  0.099664  0.064728  0.023182 -0.127205 -0.122083  0.059462  0.096892
         0.318235  0.364149  0.312792  0.336000  0.360611  0.383421  0.399917  0.416436  0.401174  0.387341  0.365098  0.375083  0.350893  0.363006  0.328174  0.313365  0.321735  0.339158  0.357714  0.365252  0.380437  0.397373  0.400651  0.414172  0.415730  0.402383  0.403662  0.395758  0.382834  0.366107  0.355648  0.340780  0.322598  0.315919  0.329173  0.336160  0.353592  0.359792  0.373010  0.369030  0.383631  0.395815  0.409998  0.405540  0.396323  0.380248  0.364024  0.356122  0.359100  0.376751  0.382082  0.395626  0.374610  0.348871  0.330347  0.317952  0.313031  0.324962  0.323285  0.343684  0.356371  0.352575  0.372623  0.378444  0.383935  0.385972  0.378995  0.379965  0.359245  0.377639  0.354343  0.335491  0.348504  0.314707  0.330059  0.313122  0.323464  0.313879  0.335967  0.330382  0.310572  0.320553  0.317856  0.356738  0.358170  0.367508  0.378693  0.338023  0.336620  0.340848  0.336706  0.331546  0.316908  0.357353  0.355455  0.323132  0.312808  0.333361  0.344156  0.357169  0.344246  0.395951  0.390971  0.377357  0.331856  0.317976  0.338603  0.364080  0.409734  0.414794  0.409295  0.390864  0.369479  0.342671  0.325355  0.372095  0.384006  0.398053  0.344500  0.329704  0.324182  0.326574  0.378121  0.414905  0.377912  0.324353  0.386863  0.335798
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.276888 -0.056528 -0.388706 -0.350146 -0.179230 -0.063606  0.215526 -0.089536 -0.053158  0.001092  0.030683  0.036252  0.097170  0.155774  0.222595 -0.201482 -0.000462  0.216386  0.364458  0.437960  0.554521  0.510854  0.612768  0.549956  0.614393  0.667131  0.540115  0.442071  0.297312  0.278578  0.353543  0.182330  0.228730  0.093612  0.149169  0.370486  0.446862  0.496085  0.587441  0.506211  0.611137  0.614458  0.643201  0.642108  0.605609  0.722347  0.761938  0.732604  0.781339  0.770715  0.748655  0.630879  0.708156  0.634650  0.441484  0.423960  0.554860  0.577764  0.593228  0.573925  0.504928  0.598376  0.466966  0.506762  0.524157  0.568640  0.529083  0.534717  0.738262  0.752136  0.814650  0.625030  0.633336  0.612602  0.604738  0.759857  0.701383  0.732515  0.607064  1.005215  0.930137  1.020036  0.829779  0.858834  0.400419  0.437251  0.417137  0.472333  0.615020  0.779015  0.793708  0.678074  0.538625  0.724243  0.487937  1.027398  0.525242  0.600960  0.627114  0.583313  0.837251  0.625048  0.599634  0.499831  0.621941  0.556240  0.650403  0.522047  0.660900  0.612172  0.610205  0.626696  0.485855  0.464707  0.334838  0.523978  0.580122  0.470471  0.125369  0.101258  0.054031  0.114429  0.137934  0.140142  0.098547 -0.281638 -0.055586  0.075397
         0.950078  0.906183 -0.909084 -1.001468 -1.119097 -1.108008 -0.909898  0.626620  0.673020  0.666674  0.914237  0.773420  0.983562  0.920898  0.985074 -1.106360 -1.122932 -1.080153 -1.028170 -0.960502 -0.851796 -0.833530 -0.723855 -0.784291  0.356564  0.198235  0.308860  0.601289  0.749406  0.843176  0.874745  1.031481  1.023692 -1.091498 -1.068765 -0.928578 -0.752636 -0.739064 -0.720257 -0.755006 -0.700273 -0.753430 -0.696356 -0.706137  0.589253  0.450102  0.347076  0.364204  0.117882  0.140650  0.266357  0.377495  0.409797  0.587693  0.880223  0.911760 -0.733745 -0.790395 -0.686024 -0.772417 -0.759080 -0.722065 -0.737578 -0.692726 -0.663982 -0.381231 -0.594247  0.301299 -0.583677  0.190704 -0.461828 -0.718622 -0.719056 -0.662531 -0.741932  0.243179  0.284287  0.626054  0.726916  0.059955 -0.357359 -0.174705 -0.497475  0.062088  0.843293  0.789494  0.593364  0.779661  0.608471  0.161582  0.254335  0.569987  0.794514  0.358166  0.781775  0.071492 -0.795653 -0.783492 -0.722293 -0.736895 -0.481703 -0.680945 -0.705663 -0.756624 -0.752126 -0.689604  0.599146  0.766585 -0.686064 -0.740865 -0.746200 -0.725761 -0.682272 -0.851910  0.973536  0.659821 -0.801263 -0.803933 -1.110832 -1.106394 -1.114714  1.033199  0.779525  0.593848 -1.109100 -0.910383  0.655970  1.016592
         0.728100  0.931949 -0.186975 -0.051972  0.432150  0.575269  0.956401  1.211342  1.175432  1.179124  0.911336  1.083318  0.770510  0.873005  0.701641  0.273067  0.353632  0.508455  0.522258  0.647967  0.728544  0.826268  0.856472  0.852736  1.110773  1.124087  1.192564  1.091811  1.034151  0.929670  0.834696  0.596478  0.584407  0.554531  0.615105  0.799299  0.980341  0.957304  0.890388  0.928523  0.888212  0.809032  0.854646  0.841852  0.937411  0.935669  0.956241  0.980678  1.020348  1.028559  1.019623  1.092198  0.995656  0.941204  0.746234  0.698654  0.910158  0.820372  0.926696  0.849273  0.927739  0.887098  0.979141  0.997794  1.012526  1.172817  1.071068  1.197946  0.887447  1.074097  0.895915  0.862387  0.853258  0.933159  0.855131  1.010104  1.049116  0.753475  0.761095  0.756145  0.794337  0.707677  0.845358  0.985064  0.740522  0.816243  1.075773  0.799747  0.901425  1.013031  0.973006  0.911435  0.795298  1.006145  0.781681  0.703160  0.864141  0.802227  0.856241  0.883216  0.848135  0.898360  0.896868  0.930429  0.821524  0.954109  0.914494  0.772672  0.844893  0.832855  0.827146  0.832769  1.021450  0.846286  0.641117  0.976905  0.783677  0.916312  0.255064  0.470137  0.437344  0.639236  1.057740  1.218963  0.500145 -0.245460  1.189455  0.704070
         1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  1.000000  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.999790 -0.020328 -0.002402]';
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.009912 -0.999163 -0.039697]';
q1a = [0.068930 -0.011052 0.120007]';
t1a = twist_matrix_fast(w1a,q1a,Theta(1));

w1b = [-0.244879 0.040916 -0.968690]';
q1b = [0.068930 -0.011052 0.120007]';
t1b = twist_matrix_fast(w1b,q1b,Theta(2));

w1c = [0.969503 0.000119 -0.245080]';
q1c = [0.068930 -0.011052 0.120007]';
t1c = twist_matrix_fast(w1c,q1c,Theta(3));

w2a = [-0.028984 -0.998972 -0.034868]';
q2a = [0.013719 -0.001827 -0.098396]';
t2a = twist_matrix_fast(w2a,q2a,Theta(4));

w2b = [-0.007188 0.035090 -0.999358]';
q2b = [0.013719 -0.001827 -0.098396]';
t2b = twist_matrix_fast(w2b,q2b,Theta(5));

w2c = [0.999554 -0.028715 -0.008198]';
q2c = [0.013719 -0.001827 -0.098396]';
t2c = twist_matrix_fast(w2c,q2c,Theta(6));

w3a = [0.207089 -0.977666 -0.035818]';
q3a = [0.012823 0.002548 -0.222986]';
t3a = twist_matrix_fast(w3a,q3a,Theta(7));

w3b = [0.375414 0.113222 -0.919916]';
q3b = [0.012823 0.002548 -0.222986]';
t3b = twist_matrix_fast(w3b,q3b,Theta(8));

w3c = [0.903426 0.177058 0.390477]';
q3c = [0.012823 0.002548 -0.222986]';
t3c = twist_matrix_fast(w3c,q3c,Theta(9));

w4a = [0.101648 -0.991553 -0.080557]';
q4a = [0.041231 0.011116 -0.292597]';
t4a = twist_matrix_fast(w4a,q4a,Theta(10));

w4b = [-0.907033 -0.059114 -0.416888]';
q4b = [0.041231 0.011116 -0.292597]';
t4b = twist_matrix_fast(w4b,q4b,Theta(11));

w4c = [0.408605 0.115444 -0.905381]';
q4c = [0.041231 0.011116 -0.292597]';
t4c = twist_matrix_fast(w4c,q4c,Theta(12));

w5a = [0.009912 0.999163 0.039697]';
q5a = [0.068930 -0.011052 0.120007]';
t5a = twist_matrix_fast(w5a,q5a,Theta(13));

w5b = [0.001871 -0.039717 0.999209]';
q5b = [0.068930 -0.011052 0.120007]';
t5b = twist_matrix_fast(w5b,q5b,Theta(14));

w5c = [0.999949 -0.009830 -0.002264]';
q5c = [0.068930 -0.011052 0.120007]';
t5c = twist_matrix_fast(w5c,q5c,Theta(15));

w6a = [-0.090612 0.995094 0.039724]';
q6a = [0.069218 -0.017163 0.273749]';
t6a = twist_matrix_fast(w6a,q6a,Theta(16));

w6b = [-0.682818 -0.091114 0.724884]';
q6b = [0.069218 -0.017163 0.273749]';
t6b = twist_matrix_fast(w6b,q6b,Theta(17));

w6c = [0.724947 0.038559 0.687724]';
q6c = [0.069218 -0.017163 0.273749]';
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
