Advertisement
dhume878

TransformProof

Jun 24th, 2011
644
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. close all; clear all; clc;
  2. % Donald Hume
  3. % Program to calculate local rotation matrices given global
  4. % coordinates. Shell1-4 represent global marker positions on
  5. % rigid shell.
  6.  
  7. %% Thigh shell calcs
  8. % Global coords
  9. tShell1 = [382.636, 197.612, 708.68];
  10. tShell2 = [451.709, 212.126, 705.496];
  11. tShell3 = [379.714, 208.419, 600.203];
  12. tShell4 = [445.368, 219.232, 601.864];
  13.  
  14. % Origin
  15. tShellO = (tShell1 + tShell2 + tShell3 + tShell4)/4;
  16.  
  17. % k
  18. Vk = tShell1-tShell3;
  19. vMag = sqrt(Vk(1)^2 + Vk(2)^2 + Vk(3)^2);
  20. Vk = Vk / vMag;
  21.  
  22. % temp j
  23. vector1 = tShell2-tShell1;
  24.  
  25. % i
  26. Vi = cross(vector1,Vk);
  27. vMag = sqrt(Vi(1)^2 + Vi(2)^2 + Vi(3)^2);
  28. Vi = Vi/vMag;
  29.  
  30. % actual j
  31. Vj = cross(Vk,Vi);
  32. vMag = sqrt(Vj(1)^2 + Vj(2)^2 + Vj(3)^2);
  33. Vj = Vj/vMag;
  34.  
  35. % Assemble [R] for thigh shell
  36. tShell = [Vi(1) Vj(1) Vk(1); Vi(2) Vj(2) Vk(2); Vi(3) Vj(3) Vk(3)];
  37.  
  38. %% Shank shell calcs
  39. % Global coords
  40. sShell1 = [347.358, 203.705, 366.3];
  41. sShell2 = [407.198, 230.431, 349.611];
  42. sShell3 = [325.109, 211.878, 290.957];
  43. sShell4 = [383.857, 234.366, 278.143];
  44. sShellO = (sShell1 + sShell2 + sShell3 + sShell4)/4;
  45.  
  46. % k
  47. Vk = sShell1-sShell3;
  48. vMag = sqrt(Vk(1)^2 + Vk(2)^2 + Vk(3)^2);
  49. Vk = Vk / vMag;
  50.  
  51. % temp j
  52. vector1 = sShell2-sShell1;
  53.  
  54. % i
  55. Vi = cross(vector1,Vk);
  56. vMag = sqrt(Vi(1)^2 + Vi(2)^2 + Vi(3)^2);
  57. Vi = Vi/vMag;
  58.  
  59. % actual j
  60. Vj = cross(Vk,Vi);
  61. vMag = sqrt(Vj(1)^2 + Vj(2)^2 + Vj(3)^2);
  62. Vj = Vj/vMag;
  63.  
  64. % Assemble [R] for shank shell
  65. sShell = [Vi(1) Vj(1) Vk(1); Vi(2) Vj(2) Vk(2); Vi(3) Vj(3) Vk(3)];
  66.  
  67. %% Foot Shell
  68. % Global coords
  69. fShell1 = [346.167, 228.193, 95.0572];
  70. fShell2 = [369.64, 270.515, 109.185];
  71. fShell3 = [387.272,210.507,71.9638];
  72. fShell4 = [407.879,254.698,88.0399];
  73. fShellO = (fShell1 + fShell2 + fShell3 + fShell4)/4;
  74.  
  75. % k
  76. Vk = fShell1-fShell3;
  77. vMag = sqrt(Vk(1)^2 + Vk(2)^2 + Vk(3)^2);
  78. Vk = Vk / vMag;
  79.  
  80. % temp j
  81. vector1 = fShell4-fShell3;
  82.  
  83. % i
  84. Vi = cross(vector1,Vk);
  85. vMag = sqrt(Vi(1)^2 + Vi(2)^2 + Vi(3)^2);
  86. Vi = Vi/vMag;
  87.  
  88. % actual j
  89. Vj = cross(Vk,Vi);
  90. vMag = sqrt(Vj(1)^2 + Vj(2)^2 + Vj(3)^2);
  91. Vj = Vj/vMag;
  92.  
  93. % Construct [R] for foot shell
  94. fShell = [Vi(1) Vj(1) Vk(1); Vi(2) Vj(2) Vk(2); Vi(3) Vj(3) Vk(3)];
  95.  
  96. %% Calculate Position of JC
  97. % Knee
  98. TtoK = [-30.0134, -12.1481, -180.546]';
  99. tShell*TtoK + tShellO';
  100. % Ankle
  101. StoA = [-20.3624,12.5553,-194.15]';
  102. sShell*StoA + sShellO';
  103. % Hip
  104. TtoH = [-50.2285,0.478356,219.834]';
  105. tShell*TtoH + tShellO';
Advertisement
Advertisement
Advertisement
RAW Paste Data Copied
Advertisement