# 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';