rot1 = [    0.8736    0.2915   -0.3897;
   -0.4011    0.8848   -0.2373;
    0.2756    0.3636    0.8898]

rot2 = [    0.9874   -0.1420   -0.0700;
    0.0700    0.7880   -0.6117;
    0.1420    0.5991    0.7880]

wpt = [200 200 200]

cpt = [  155.0812 49.2660 305.8148]