├── .gitignore ├── README.md ├── Week00 ├── CoordG2L1.m ├── CoordL2G1.m ├── HW0.m ├── HWdemo.mat └── Syllabus.doc ├── Week01 ├── CoordFoot.m ├── CoordG2L.m ├── CoordL2G.m ├── CoordPelvis.m ├── CoordShank.m ├── CoordThigh.m ├── DataQ1.mat └── HW01.m ├── Week02 ├── COP.mp4 ├── DataQ1.mat ├── ForcePlate.m ├── ForcePlateN.m ├── HW02.m ├── fpCOP.m └── fpRV.m ├── Week03 ├── Ang2Rot.m ├── DataQ1.mat ├── EulerR.mat ├── HW03.m ├── JointAngOffset.m ├── Rot2Ang.m ├── Rot2AngFSOLVE.m ├── RotAngConvFix.m ├── RotAngConvert.m ├── RotFormula.m ├── rotang.mat ├── subcali.mat ├── x.jpg ├── y.jpg └── z.jpg ├── Week04 ├── 12RotSeq.jpg ├── Ang2LocalAngular.m ├── AngAcc.jpg ├── AngVel.jpg ├── DataQ1.mat ├── Derivative.m ├── HW04.m ├── PolyDer.m ├── PolyFit.m ├── PolyVal.m ├── Rot2LocalAngular.m └── 問答題.docx ├── Week05 ├── EulerP2Angular.m ├── EulerP2Rot.m ├── HW05.m ├── Rot2EulerP.m ├── Rot2LocalAngularEP.m ├── test.m └── unwrapEP.m ├── Week06 ├── HAtest.m ├── HW06.m ├── HelicalAxesCenter.m ├── HipROM.mat ├── KneeFE.mat ├── Prac3Prob3(SphereFit) │ ├── ShereFit_Average.fig │ ├── ShereFit_LLFC.fig │ ├── ShereFit_LMFC.fig │ └── ShereFit_LTRO.fig ├── Prac3Prob3(circumcenter) │ ├── circumcenter_LLFC.fig │ ├── circumcenter_LMFC.fig │ ├── circumcenter_LTRO.fig │ └── circumcenter_Total.fig ├── RV2Screw.m ├── Screw2RV.m ├── hw6prac3p3Circumcenter.m ├── hw6prac3p3SphereFit.m └── sphereFit.m ├── Week07 ├── BodyCOM_Dempster.m ├── EulerR.mat ├── Figure1.fig ├── Figure2.mp4 ├── HW07.m ├── SegCOM_PD.m ├── Walk.mat └── WholeBodyCOM.m ├── Week08 ├── 95.fig ├── HW08.m ├── QuietStance.mat └── SwayArea.m ├── Week09 ├── AngularMomentum.m ├── HW09.m ├── LLCOM2PD.m ├── LLInertia.m └── result │ ├── Ang1derMom.fig │ └── AngMom.fig ├── Week10 ├── HW10.m └── JointForceMoment.m ├── _config.yml ├── final ├── Ans │ ├── COP.fig │ ├── Inclination Angle.fig │ ├── Joint Angle.fig │ └── Joint_Moment.fig ├── DataSTS.mat ├── DataSTS.zip ├── final.m ├── final_a.m ├── final_b.m ├── final_cd.m ├── final_e1.m ├── final_e2.m ├── result │ ├── COP.fig │ ├── Inclination Angle.fig │ ├── Joint Angle.fig │ └── Joint_Moment.fig └── subcali.mat └── results ├── COP.gif ├── HW2.mp4 ├── hw10_1.jpg ├── hw10_2.jpg ├── hw3-1.png ├── hw3.png ├── hw4_12RotSeq.jpg ├── hw4_AngAcc.jpg ├── hw4_AngVel.jpg ├── hw5_AngAcc.jpg ├── hw5_AngVel.jpg ├── hw5_unwrapEP().jpg ├── hw6_1.jpg ├── hw6_n&phi.jpg ├── hw7.jpg ├── hw7_2.mp4 ├── hw8_COP.jpg ├── hw8_COP95.jpg ├── hw9_1der.jpg ├── hw9_AngMomentum.jpg ├── punisher.jpg └── walk.gif /.gitignore: -------------------------------------------------------------------------------- 1 | ##--------------------------------------------------- 2 | ## Remove autosaves generated by the Matlab editor 3 | ## We have git for backups! 4 | ##--------------------------------------------------- 5 | 6 | # Windows default autosave extension 7 | *.asv 8 | 9 | # Compiled MEX binaries (all platforms) 10 | *.mex* 11 | 12 | # Simulink Code Generation 13 | slprj/ 14 | 15 | # Session info 16 | octave-workspace 17 | 18 | # Simulink autosave extension 19 | *.autosave 20 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Human Motion Analysis Programming 2 | *Supervised by Prof. [Tung-Wu Lu](http://oemal.bme.ntu.edu.tw/professor/professorE.htm), Dept. of BioMedical Engineering, NTU* 3 | 4 | MATLAB programs, which use the data derived from a motion capture system and force plates, for human motion analysis, including motion tracking, evaluation of body balance, and derivation of joint forces and torques of the lower body, etc. 5 | 6 | ## Requirements 7 | 1. MATLAB 2015 (or newer versions) 8 | 2. [MTIMESX function](https://www.mathworks.com/matlabcentral/fileexchange/25977-mtimesx-fast-matrix-multiply-with-multi-dimensional-support)(Fast Matrix Multiply with Multi-Dimensional) 9 | 10 | ## Functions & Execution Results 11 | ### Week1: Transformation between Global & Local Coordinate 12 | The transformation between global & local coordinates of marker position on lower body segments. 13 | 14 | ### Week2: COP Tracking 15 | Derive the COP from data of two force plates and display the positions relative to the force plates. 16 | 17 |

18 | 19 | ### Week3: Euler Angle & Fixed Angle 20 | 1. Represent the rotations of body segemnts with **Euler angle** and **fixed angle**. 21 | 2. Display the difference of Euler angle before and after **static calibration** of the individual. 22 | ![Static calibration](/results/hw3.png) 23 | 24 | ### Week4: Curve Fitting 25 | 1. Smoothen the data curve and display the **angular velocity** of the lower body. 26 | 2. Consider the right foot only, compare the results of **analytic solution of angular acceleration** & **1st derivation of angular velocity**. 27 | 3. Consider the right thigh only, compare the angular velocity derive from 12 sequences of Euler angle. 28 | The one with **Gimbal lock** during motion can be easily observed and avoided.   29 |   30 | 31 | ### Week5: Quaternions (Euler Parameters) 32 | 1. Write the function "unwrapEP.m" to eliminate the discontinuity of Quaternions data.   33 | 2. Compare angular velocity & angular acceleration derived from Euler angle and Quaternions(EP). 34 |   35 | 36 | ### Week6: Screw Axis (Helical Axis) 37 | 1. Compare the rotation axis and angle derived from the Screw axis and Quaternoins. 38 | 2. Determine the **joint center & rotation axis with least-square error** from several rotation axes derived during the motion. 39 |   40 | 41 | ### Week7: COM Tracking 42 | 1. Derive & compare the COM position of the whole body using Dempster's anthropometrical data with simplifying the body model as 7, 11, 12, & 13 segments. 43 | 2. Display the COM positions. (Yellow: markers' position, green: COM of body segments, purple: COM of the whole body). 44 | 45 | 46 | ### Week8: Evaluation of Body Balance 47 | Derive the COP data while the recipient is standing still. It determined the eclipse covering 95% of these COPs with **Principal Component Analysis (PCA)**. The area of the eclipse and the length of its axes can be an indicator for evaluating one's body balancing ability.   48 |

  49 | 50 | ### Week9: Angular Momentum 51 | Derive the angular momentum and 1st derivation of angular momentum during the motion. 52 | 53 | 54 | ### Week10: Joint Moment & Joint Force 55 | Derive the joint moment & joint force of angular momentum during the motion. The results are normalized by being divided by the weight of recipient. 56 |

  57 |

  58 | -------------------------------------------------------------------------------- /Week00/CoordG2L1.m: -------------------------------------------------------------------------------- 1 | function Pl = CoordG2L( Rg2l, Vg2l, Pg ) 2 | % the function converts the coordinate of points from global CS to local CS 3 | 4 | % Rg2l: the rotation matrix converting global to local [3x3] 5 | % Vg2l: the position vector pointing from global to local [1x3] 6 | % Pg: the coordinate of every marker in the global coordinate system [nframes x 3] 7 | % Pl: the coordinate of every marker in the local coordinate system [nframes x 3] 8 | n = length(Pg); % no. of frames 9 | Pl = (Pg - ones(n, 1) * Vg2l) * Rg2l; 10 | end 11 | 12 | -------------------------------------------------------------------------------- /Week00/CoordL2G1.m: -------------------------------------------------------------------------------- 1 | function Pg = CoordL2G( Rg2l, Vg2l, Pl ) 2 | % the function converts the coordinate of points from local CS to global CS 3 | 4 | % Rg2l: the rotation matrix converting global to local [3x3] 5 | % Vg2l: the position vector pointing from global to local [1x3] 6 | % Pg: the coordinate of every marker in the global coordinate system [nframes x 3] 7 | % Pl: the coordinate of every marker in the local coordinate system [nframes x 3] 8 | n = length(Pl); % no. of frames 9 | Pg = Pl*Rg2l.' + ones(n, 1)*Vg2l; 10 | end 11 | 12 | -------------------------------------------------------------------------------- /Week00/HW0.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week00/HW0.m -------------------------------------------------------------------------------- /Week00/HWdemo.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week00/HWdemo.mat -------------------------------------------------------------------------------- /Week00/Syllabus.doc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week00/Syllabus.doc -------------------------------------------------------------------------------- /Week01/CoordFoot.m: -------------------------------------------------------------------------------- 1 | function [Rg2f, Vg2f, Fcoord_local] = CoordFoot(Fcoord, side, MKstr) 2 | % the function converts the coordinate of points from global CS to 3 | % local CS on FOOT 4 | 5 | % Rg2l: the rotation matrix converting global to local [3 x 3 x nframes] 6 | % Vg2l: the position vector pointing from global to local [nframes x 3] 7 | % Fcoord: the coordinate of every marker in the global coordinate system [nframes x 3 x 3(markers)] 8 | % Fcoord_local: the coordinate of every marker in the local coordinate system [N markers x 3 x nframes] 9 | 10 | % Specify the sequence of input data & Validate the format of MKstr 11 | % & Validate side 12 | if side~='R' && side~='r' && side~='L' && side~='l' 13 | errordlg('Wrong input of side ! ''R'' or ''r'' for right side and ''L''or ''l'' for left side.','CoordFoot'); 14 | return 15 | end 16 | 17 | index = [find(strcmp(MKstr, 'LHEE')), find(strcmp(MKstr, 'LFOO')), find(strcmp(MKstr, 'LTOE'))]; 18 | if numel(index)~=3 || numel(unique(index))~=3 || ~isempty(find(index>3)) 19 | index = [find(strcmp(MKstr, 'RHEE')), find(strcmp(MKstr, 'RFOO')), find(strcmp(MKstr, 'RTOE'))]; 20 | if numel(index)~=3 || numel(unique(index))~=3 || ~isempty(find(index>3)) 21 | errordlg('Wrong spelling or repeating of marker string input (MKstr) !','CoordFoot'); 22 | return 23 | end 24 | if side~='R' && side~='r' 25 | errordlg('You pick wrong side! Choose another!','CoordFoot'); 26 | return 27 | end 28 | elseif side~='L' && side~='l' 29 | errordlg('You pick wrong side! Choose another!','CoordFoot'); 30 | return 31 | end 32 | 33 | % Validate the format of Pcoord 34 | sFc = size(Fcoord); 35 | if sFc(2)~=3 || sFc(3)~=3 36 | errordlg('Wrong format of global coordinate of markers input (Fcoord) !','CoordFoot'); 37 | return 38 | end 39 | 40 | % Assign the input variables 41 | Fcell = {Fcoord(:,:,index(1)), Fcoord(:,:,index(2)), Fcoord(:,:,index(3))}; 42 | [HEEp, FOOp,TOEp] = deal(Fcell{:}); 43 | 44 | % Decide position vectors 45 | Vg2f = HEEp; 46 | 47 | % Local coordinate systen 48 | vec = (FOOp + TOEp)/2 - HEEp; 49 | Xp = bsxfun(@rdivide, vec, sqrt(sum(abs(vec).^2,2))); 50 | switch side 51 | case {'R','r'} 52 | vec = FOOp - TOEp; 53 | case {'L','l'} 54 | vec = TOEp - FOOp; 55 | end 56 | vec = bsxfun(@cross, Xp, vec); 57 | Yp = bsxfun(@rdivide, vec, sqrt(sum(abs(vec).^2,2))); 58 | Zp = bsxfun(@cross, Xp, Yp); 59 | 60 | Rg2f = reshape((cat(2, Xp, Yp, Zp)).', 3, 3, []); 61 | 62 | Fcoord_local = permute(CoordG2L(Rg2f, Vg2f, Fcoord), [3 2 1]); 63 | 64 | end 65 | 66 | 67 | -------------------------------------------------------------------------------- /Week01/CoordG2L.m: -------------------------------------------------------------------------------- 1 | function [varargout] = CoordG2L( Rg2l, Vg2l, varargin ) 2 | % the function converts the coordinate of points from global CS to local CS 3 | 4 | % Rg2l: the rotation matrix converting global to local 5 | % [3 x 3 x nframes] or [3 x 3] 6 | 7 | % Vg2l: the position vector pointing from global to local [nframes x 3] 8 | 9 | % varargin: the coordinate of every marker in the global coordinate system 10 | % [nframes x 3] or [nframes x 3 x N markers] 11 | 12 | % varargout: the coordinate of every marker in the local coordinate system 13 | % [nframes x 3] or [nframes x 3 x N markers] 14 | 15 | %% Checking no. of input (minimum = 3) 16 | narginchk(3, nargin) 17 | 18 | %% Checking formats of rotation matrice and position vectors 19 | ir = size(Rg2l); iv = size(Vg2l); % size of rotation matrice and position vectors 20 | irl = ndims(Rg2l); ivl = ndims(Vg2l); 21 | 22 | if ~isequal(iv(2), 3) || ivl~=2 23 | errordlg('Wrong format of position vectors ( [nframes x 3] ) !!'); 24 | return 25 | end 26 | 27 | nframe = iv(1); % no. of frames 28 | 29 | if ~isequal(ir(1), ir(2), 3) 30 | errordlg('Wrong format of rotation matrice ( [3 x 3 x nframes] or [3 x 3] ) !!'); 31 | return 32 | end 33 | 34 | switch irl 35 | case 2 36 | nRg2l = repmat(Rg2l,1,1,nframe); % rotation matrice of n frames 37 | case 3 38 | nRg2l = Rg2l; 39 | if ir(3)~=nframe 40 | errordlg('Rotation matrice and position vectors have different no. of frames !!'); 41 | return 42 | end 43 | otherwise 44 | errordlg('Wrong format of rotation matrice ( [3 x 3 x nframes] or [3 x 3] ) !!'); 45 | return 46 | end 47 | 48 | 49 | %% Calculations for different syntax 50 | if length(varargin) == 1 % P_global has format of [nframes x 3 x N markers] 51 | P_global = varargin{:}; 52 | sPg = size(P_global); 53 | if sPg(1) ~= nframe 54 | errordlg('Markers and position vectors have different no. of frames !!'); 55 | return 56 | end 57 | if ndims(P_global)==3 58 | Nm = sPg(3);% no. of markers 59 | else 60 | Nm = 1; % only one marker 61 | end 62 | nargoutchk(1, 1) 63 | varargout{1} = permute(mtimesx(permute(P_global - repmat(Vg2l, 1, 1, Nm), [4 2 1 3]), repmat(nRg2l, 1, 1, 1, Nm)), [3 2 4 1]); 64 | 65 | elseif length(varargin) > 1 % P_global of each marker are input separately. 66 | nVarargs = length(varargin); 67 | nargoutchk(nVarargs, nVarargs) 68 | for k = 1:nVarargs 69 | varargout{k} = permute(mtimesx(permute(varargin{k} - Vg2l, [3 2 1]), nRg2l), [3 2 1]); 70 | end 71 | end 72 | 73 | end 74 | 75 | -------------------------------------------------------------------------------- /Week01/CoordL2G.m: -------------------------------------------------------------------------------- 1 | function [varargout] = CoordL2G( Rg2l, Vg2l, varargin ) 2 | % the function converts the coordinate of points from global CS to local CS 3 | 4 | % Rg2l: the rotation matrix converting global to local 5 | % [3 x 3 x nframes] or [3 x 3] 6 | 7 | % Vg2l: the position vector pointing from global to local [nframes x 3] 8 | 9 | % varargin: the coordinate of every marker in the global coordinate system 10 | %[nframes x 3] / [nframes x 3 x N markers] 11 | 12 | % varargout: the coordinate of every marker in the local coordinate system [nframes x 3] 13 | %[nframes x 3] / [nframes x 3 x N markers] 14 | 15 | %% Checking no. of input (minimum = 3) 16 | narginchk(3, nargin) 17 | 18 | %% Checking formats of rotation matrice and position vectors 19 | ir = size(Rg2l); iv = size(Vg2l); % size of rotation matrice and position vectors 20 | irl = ndims(Rg2l); ivl = ndims(Vg2l); 21 | 22 | if ~isequal(iv(2), 3) || ivl~=2 23 | errordlg('Wrong format of position vectors ( [nframes x 3] ) !!'); 24 | return 25 | end 26 | 27 | nframe = iv(1); % no. of frames 28 | 29 | if ~isequal(ir(1), ir(2), 3) 30 | errordlg('Wrong format of rotation matrice ( [3 x 3 x nframes] or [3 x 3] ) !!'); 31 | return 32 | end 33 | 34 | switch irl 35 | case 2 36 | nRg2l = repmat(Rg2l,1,1,nframe); % rotation matrice of n frames 37 | case 3 38 | nRg2l = Rg2l; 39 | if ir(3)~=nframe 40 | errordlg('Rotation matrice and position vectors have different no. of frames !!'); 41 | return 42 | end 43 | otherwise 44 | errordlg('Wrong format of rotation matrice ( [3 x 3 x nframes] or [3 x 3] ) !!'); 45 | return 46 | end 47 | 48 | 49 | %% Calculations for different syntax 50 | if length(varargin) == 1 % P_local has format of [nframes x 3 x N markers] 51 | P_local = varargin{:}; 52 | sPl = size(P_local); 53 | if sPl(1) ~= nframe 54 | errordlg('Markers and position vectors have different no. of frames !!'); 55 | return 56 | end 57 | if ndims(P_local)==3 58 | Nm = sPl(3);% no. of markers 59 | else 60 | Nm = 1; % only one marker 61 | end 62 | nargoutchk(1, 1) 63 | % varargout{1} = permute(pagefun(@mtimes, gpuArray(repmat(Rg2l, 1, 1, 1, Nm)), gpuArray(permute(P_local, [2 4 1 3]))), [3 1 4 2]) + repmat(Vg2l, 1, 1, Nm); 64 | varargout{1} = permute(mtimesx(repmat(Rg2l, 1, 1, 1, Nm),permute(P_local, [2 4 1 3])), [3 1 4 2]) + repmat(Vg2l, 1, 1, Nm); 65 | 66 | elseif length(varargin) > 1 % P_global of each marker are input separately. 67 | nVarargs = length(varargin); 68 | nargoutchk(nVarargs, nVarargs) 69 | for k = 1:nVarargs 70 | varargout{k} = permute(mtimesx(Rg2l, permute(varargin{k}, [2 3 1])), [3 1 2]) + Vg2l; 71 | end 72 | end 73 | 74 | end 75 | 76 | -------------------------------------------------------------------------------- /Week01/CoordPelvis.m: -------------------------------------------------------------------------------- 1 | function [Rg2p, Vg2p, Pcoord_local] = CoordPelvis(Pcoord, side, MKstr) 2 | % the function converts the coordinate of points from global CS to 3 | % local CS on PELVIS 4 | 5 | % Rg2l: the rotation matrix converting global to local [3 x 3 x nframes] 6 | % Vg2l: the position vector pointing from global to local [nframes x 3] 7 | % Pcoord: the coordinate of every marker in the global coordinate system [nframes x 3 x 3(markers)] 8 | % Pcoord_local: the coordinate of every marker in the local coordinate system [N markers x 3 x nframes] 9 | if side~='R' && side~='r' && side~='L' && side~='l' 10 | errordlg('Wrong input of side ! ''R'' or ''r'' for right side and ''L''or ''l'' for left side.','CoordPelvis'); 11 | return 12 | end 13 | 14 | % Specify the sequence of input data 15 | index = [find(strcmp(MKstr, 'RASI')), find(strcmp(MKstr, 'LASI')), find(strcmp(MKstr, 'RPSI'))]; 16 | 17 | if numel(index)~=3 || numel(unique(index))~=3 || ~isempty(find(index>4)) 18 | index = [find(strcmp(MKstr, 'RASI')), find(strcmp(MKstr, 'LASI')), find(strcmp(MKstr, 'LPSI'))]; 19 | if numel(index)~=3 || numel(unique(index))~=3 || ~isempty(find(index>4)) 20 | errordlg('Wrong spelling or repeating of marker string input (MKstr) !','CoordPelvis'); 21 | return 22 | end 23 | if side~='L' && side~='l' 24 | errordlg('You pick wrong side! Choose another!','CoordPelvis'); 25 | return 26 | end 27 | elseif side~='R' && side~='r' 28 | errordlg('You pick wrong side! Choose another!','CoordPelvis'); 29 | return 30 | end 31 | 32 | % Validate the format of Pcoord 33 | sPc = size(Pcoord); 34 | if sPc(2)~=3 || sPc(3)~=3 35 | errordlg('Wrong format of global coordinate of markers input (Pcoord) !','CoordPelvis'); 36 | return 37 | end 38 | 39 | % Assign the input variables 40 | Pcell = {Pcoord(:,:,index(1)), Pcoord(:,:,index(2)), Pcoord(:,:,index(3))}; 41 | [RASIp, LASIp, XPSIp] = deal(Pcell{:}); 42 | vec = RASIp - LASIp; 43 | Zp = bsxfun(@rdivide, vec, sqrt(sum(abs(vec).^2,2))); 44 | 45 | % Decide side 46 | switch side 47 | case {'R','r'} 48 | Vg2p = RASIp; 49 | vec = bsxfun(@cross, Zp, RASIp - XPSIp); 50 | Yp = bsxfun(@rdivide, vec, sqrt(sum(abs(vec).^2,2))); 51 | case {'L','l'} 52 | Vg2p = LASIp; 53 | vec = bsxfun(@cross, Zp, LASIp - XPSIp); 54 | Yp = bsxfun(@rdivide, vec, sqrt(sum(abs(vec).^2,2))); 55 | end 56 | 57 | Xp = bsxfun(@cross, Yp, Zp); 58 | 59 | Rg2p = reshape((cat(2, Xp, Yp, Zp)).', 3, 3, []); 60 | 61 | Pcoord_local = permute(CoordG2L(Rg2p, Vg2p, Pcoord), [3 2 1]); 62 | 63 | end 64 | 65 | -------------------------------------------------------------------------------- /Week01/CoordShank.m: -------------------------------------------------------------------------------- 1 | function [Rg2s, Vg2s, Scoord_local] = CoordShank(Scoord, side, MKstr) 2 | % the function converts the coordinate of points from global CS to 3 | % local CS on SHANK 4 | 5 | % Rg2l: the rotation matrix converting global to local [3 x 3 x nframes] 6 | % Vg2l: the position vector pointing from global to local [nframes x 3] 7 | % Scoord: the coordinate of every marker in the global coordinate system [nframes x 3 x 3(markers)] 8 | % Scoord_local: the coordinate of every marker in the local coordinate system [N markers x 3 x nframes] 9 | 10 | % Specify the sequence of input data & Validate the format of MKstr 11 | % & Validate side 12 | if side~='R' && side~='r' && side~='L' && side~='l' 13 | errordlg('Wrong input of side ! ''R'' or ''r'' for right side and ''L''or ''l'' for left side.','CoordShank'); 14 | return 15 | end 16 | 17 | index = [find(strcmp(MKstr, 'LTT')), find(strcmp(MKstr, 'LSHA')), find(strcmp(MKstr, 'LLMA')), find(strcmp(MKstr, 'LMMA'))]; 18 | if numel(index)~=4 || numel(unique(index))~=4 || ~isempty(find(index>4)) 19 | index = [find(strcmp(MKstr, 'RTT')), find(strcmp(MKstr, 'RSHA')), find(strcmp(MKstr, 'RLMA')), find(strcmp(MKstr, 'RMMA'))]; 20 | if numel(index)~=4 || numel(unique(index))~=4 || ~isempty(find(index>4)) 21 | errordlg('Wrong spelling or repeating of marker string input (MKstr) !','CoordShank'); 22 | return 23 | end 24 | if side~='R' && side~='r' 25 | errordlg('You pick wrong side! Choose another!','CoordShank'); 26 | return 27 | end 28 | elseif side~='L' && side~='l' 29 | errordlg('You pick wrong side! Choose another!','CoordShank'); 30 | return 31 | end 32 | 33 | % Validate the format of Pcoord 34 | sSc = size(Scoord); 35 | if sSc(2)~=3 || sSc(3)~=4 36 | errordlg('Wrong format of global coordinate of markers input (Scoord) !','CoordShank'); 37 | return 38 | end 39 | 40 | % Assign the input variables 41 | Scell = {Scoord(:,:,index(1)), Scoord(:,:,index(2)), Scoord(:,:,index(3)), Scoord(:,:,index(4))}; 42 | [TTp, SHAp, LMAp, MMAp] = deal(Scell{:}); 43 | 44 | % Decide position vectors 45 | Vg2s = TTp; 46 | 47 | % Local coordinate systen 48 | vec = bsxfun(@cross, SHAp - MMAp, LMAp - MMAp); 49 | Xp = bsxfun(@rdivide, vec, sqrt(sum(abs(vec).^2,2))); 50 | if side == 'L' || side == 'l' 51 | Xp=-Xp; 52 | end 53 | vec = bsxfun(@cross, Xp, TTp - (MMAp+LMAp)/2); 54 | Zp = bsxfun(@rdivide, vec, sqrt(sum(abs(vec).^2,2))); 55 | Yp = bsxfun(@cross, Zp, Xp); 56 | 57 | Rg2s = reshape((cat(2, Xp, Yp, Zp)).', 3, 3, []); 58 | 59 | Scoord_local = permute(CoordG2L(Rg2s, Vg2s, Scoord), [3 2 1]); 60 | 61 | end 62 | 63 | -------------------------------------------------------------------------------- /Week01/CoordThigh.m: -------------------------------------------------------------------------------- 1 | function [Rg2t, Vg2t, Tcoord_local] = CoordThigh(Tcoord, side, MKstr) 2 | % the function converts the coordinate of points from global CS to 3 | % local CS on THIGH 4 | 5 | % Rg2l: the rotation matrix converting global to local [3 x 3 x nframes] 6 | % Vg2l: the position vector pointing from global to local [nframes x 3] 7 | % Tcoord: the coordinate of every marker in the global coordinate system [nframes x 3 x 3(markers)] 8 | % Tcoord_local: the coordinate of every marker in the local coordinate system [N markers x 3 x nframes] 9 | 10 | % Specify the sequence of input data & Validate the format of MKstr 11 | % & Validate side 12 | if side~='R' && side~='r' && side~='L' && side~='l' 13 | errordlg('Wrong input of side ! ''R'' or ''r'' for right side and ''L''or ''l'' for left side.','CoordThigh'); 14 | return 15 | end 16 | 17 | index = [find(strcmp(MKstr, 'LTRO')), find(strcmp(MKstr, 'LLFC')), find(strcmp(MKstr, 'LMFC'))]; 18 | if numel(index)~=3 || numel(unique(index))~=3 || ~isempty(find(index>3)) 19 | index = [find(strcmp(MKstr, 'RTRO')), find(strcmp(MKstr, 'RLFC')), find(strcmp(MKstr, 'RMFC'))]; 20 | if numel(index)~=3 || numel(unique(index))~=3 || ~isempty(find(index>3)) 21 | errordlg('Wrong spelling or repeating of marker string input (MKstr) !','CoordThigh'); 22 | return 23 | end 24 | if side~='R' && side~='r' 25 | errordlg('You pick wrong side! Choose another!','CoordThigh'); 26 | return 27 | end 28 | elseif side~='L' && side~='l' 29 | errordlg('You pick wrong side! Choose another!','CoordThigh'); 30 | return 31 | end 32 | 33 | % Validate the format of Pcoord 34 | sTc = size(Tcoord); 35 | if sTc(2)~=3 || sTc(3)~=3 36 | errordlg('Wrong format of global coordinate of markers input (Tcoord) !','CoordThigh'); 37 | return 38 | end 39 | 40 | % Assign the input variables 41 | Tcell = {Tcoord(:,:,index(1)), Tcoord(:,:,index(2)), Tcoord(:,:,index(3))}; 42 | [TROp, LFCp, MFCp] = deal(Tcell{:}); 43 | 44 | % Decide position vectors 45 | Vg2t = TROp; 46 | 47 | % Local coordinate systen 48 | switch side 49 | case {'R','r'} 50 | vec = LFCp - MFCp; 51 | case {'L','l'} 52 | vec = MFCp - LFCp; 53 | end 54 | Zp = bsxfun(@rdivide, vec, sqrt(sum(abs(vec).^2,2))); 55 | vec = bsxfun(@cross, TROp - LFCp, Zp); 56 | Xp = bsxfun(@rdivide, vec, sqrt(sum(abs(vec).^2,2))); 57 | Yp = bsxfun(@cross, Zp, Xp); 58 | 59 | Rg2t = reshape((cat(2, Xp, Yp, Zp)).', 3, 3, []); 60 | 61 | Tcoord_local = permute(CoordG2L(Rg2t, Vg2t, Tcoord), [3 2 1]); 62 | 63 | end 64 | 65 | -------------------------------------------------------------------------------- /Week01/DataQ1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week01/DataQ1.mat -------------------------------------------------------------------------------- /Week01/HW01.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Computer Methods in Human Motion Analysis 2017 -- HW1 3 | % Matlab Version: MATLAB R2015b 4 | % Student: ����Ӥ@ ��µ� R05522625 5 | 6 | addpath(genpath(fileparts(cd))) % adding all hw directory to PATH. 7 | 8 | %% Initialization 9 | clc; 10 | clearvars; 11 | close all; 12 | 13 | %% Practice 1 14 | tic 15 | %load data 16 | load('DataQ1.mat'); 17 | 18 | Pcoordl = cat(3, LASI, LPSI, RASI); MKstrPl = { 'LASI', 'LPSI','RASI'}; 19 | Pcoordr = cat(3, LASI, RPSI, RASI); MKstrPr = { 'LASI', 'RPSI','RASI'}; 20 | Tcoordl = cat(3, LLFC, LTRO, LMFC); MKstrTl = {'LLFC', 'LTRO', 'LMFC'}; 21 | Tcoordr = cat(3, RTRO, RMFC, RLFC); MKstrTr = {'RTRO', 'RMFC', 'RLFC'}; 22 | Scoordl = cat(3, LTT, LLMA, LMMA, LSHA); MKstrSl = {'LTT', 'LLMA', 'LMMA', 'LSHA'}; 23 | Scoordr = cat(3, RTT, RSHA, RLMA, RMMA); MKstrSr = {'RTT', 'RSHA', 'RLMA', 'RMMA'}; 24 | Fcoordl = cat(3, LHEE, LTOE, LFOO); MKstrFl = {'LHEE', 'LTOE', 'LFOO'}; 25 | Fcoordr = cat(3, RFOO, RHEE,RTOE); MKstrFr = { 'RFOO', 'RHEE','RTOE'}; 26 | 27 | % Result 28 | [lRg2p, lVg2p, lPcoord_local] = CoordPelvis(Pcoordl, 'l', MKstrPl); 29 | [rRg2p, rVg2p, rPcoord_local] = CoordPelvis(Pcoordr, 'r', MKstrPr); 30 | [lRg2t, lVg2t, lTcoord_local] = CoordThigh(Tcoordl, 'l', MKstrTl); 31 | [rRg2t, rVg2t, rTcoord_local] = CoordThigh(Tcoordr, 'r', MKstrTr); 32 | [lRg2s, lVg2s, lScoord_local] = CoordShank(Scoordl, 'l', MKstrSl); 33 | [rRg2s, rVg2s, rScoord_local] = CoordShank(Scoordr, 'r', MKstrSr); 34 | [lRg2f, lVg2f, lFcoord_local] = CoordFoot(Fcoordl, 'l', MKstrFl); 35 | [rRg2f, rVg2f, rFcoord_local] = CoordFoot(Fcoordr, 'r', MKstrFr); 36 | 37 | % Elapsed time of Practice1 is 0.020202 seconds. 38 | toc 39 | %% Practice 2 40 | % Gather all difference between original and converted global coordinates 41 | % of every markers in the matrix "Result" 42 | tic 43 | Result = cat(3,... 44 | Pcoordl-CoordL2G(lRg2p, lVg2p, permute(lPcoord_local, [3 2 1])),... 45 | Pcoordr-CoordL2G(rRg2p, rVg2p, permute(rPcoord_local, [3 2 1])),... 46 | Tcoordl-CoordL2G(lRg2t, lVg2t, permute(lTcoord_local, [3 2 1])),... 47 | Tcoordr-CoordL2G(rRg2t, rVg2t, permute(rTcoord_local, [3 2 1])),... 48 | Scoordl-CoordL2G(lRg2s, lVg2s, permute(lScoord_local, [3 2 1])),... 49 | Scoordr-CoordL2G(rRg2s, rVg2s, permute(rScoord_local, [3 2 1])),... 50 | Fcoordl-CoordL2G(lRg2f, lVg2f, permute(lFcoord_local, [3 2 1])),... 51 | Fcoordr-CoordL2G(rRg2f, rVg2f, permute(rFcoord_local, [3 2 1]))); 52 | 53 | if ~isempty(find(abs(Result)>1.0e-12, 1)) 54 | errordlg('The result is not zero, WRONG conversion!!','Error!!'); 55 | return 56 | else 57 | msgbox('The result is zero, Run Sucessfully!') 58 | end 59 | toc 60 | % Elapsed time of Practice2 is 0.011175 seconds. -------------------------------------------------------------------------------- /Week02/COP.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week02/COP.mp4 -------------------------------------------------------------------------------- /Week02/DataQ1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week02/DataQ1.mat -------------------------------------------------------------------------------- /Week02/ForcePlate.m: -------------------------------------------------------------------------------- 1 | function [ gCOP, fpFg, Rg2fp, Vg2fp ] = ForcePlate( fpF_local, fpM_local, corners, Vfp2tc, Pz ) 2 | %% Input Arguments 3 | % fpF_local: The force measured by the force plate relative to the 4 | % local coordinate system on the force plate. 5 | % [nframes x 3] 6 | 7 | % fpM_local: The moment measured by the force plate relative to the 8 | % local coordinate system on the force plate. 9 | % [nframes x 3] 10 | 11 | % corners: The global coordinates of 4 corners on the force plate with 12 | % the sequence of 1st to 4th quadrant relative to the 13 | % local coordinate system of the force plate. [4 x 3] 14 | 15 | % Vfp2tc: The coordinate of top plate center relative to local C.S. on 16 | % the force plate. [1 x 3] 17 | 18 | % Pz: Z-coordinate of the plane containing COP relative to local C.S. 19 | % [1 x 1] 20 | 21 | %% Output Arguments 22 | % gCOP: 23 | 24 | % fpFg: 25 | 26 | % Rg2fp: The rotation matrix of force plate C.S. relative to the global 27 | % C.S. [3 x 3] 28 | 29 | % Vg2fp: The postion vector of the origin of force plate C.S. relative 30 | % to global C.S. [1 x 3] 31 | 32 | %% Function 33 | nframes = length(fpF_local); 34 | if nframes ~= length(fpM_local) 35 | errordlg('fpF_local and fpM_local have differnet no. of frames!!'); 36 | return 37 | end 38 | 39 | Vg2tc = sum(corners, 1)/4; 40 | 41 | vec = (corners(1,:) + corners(4,:) - corners(2,:) - corners(3,:))/2; 42 | Xp = (vec/norm(vec)).'; 43 | vec = (corners(1,:) + corners(2,:) - corners(4,:) - corners(3,:))/2; 44 | Yp = (vec/norm(vec)).'; 45 | Zp = bsxfun(@cross, Xp, Yp); 46 | 47 | Rg2fp = [ Xp, Yp, Zp ]; 48 | Vg2fp = Vg2tc - Vfp2tc * Rg2fp.'; 49 | 50 | Px = (Pz*fpF_local(:, 1) - fpM_local(:, 2)) ./ fpF_local(:, 3); 51 | Py = (Pz*fpF_local(:, 2) + fpM_local(:, 1)) ./ fpF_local(:, 3); 52 | 53 | COP_local = [Px Py repmat(Pz, nframes, 1)]; 54 | gCOP = CoordL2G( Rg2fp, repmat(Vg2fp,nframes,1), COP_local); 55 | 56 | fpFg = fpF_local*Rg2fp.'; 57 | 58 | 59 | 60 | end 61 | 62 | -------------------------------------------------------------------------------- /Week02/ForcePlateN.m: -------------------------------------------------------------------------------- 1 | function [ gCOP, nCOP, fpFg, Rg2fp, Vg2fp ] = ForcePlateN( fpF_local, fpM_local, corners, Vfp2tc, Pz ) 2 | %% Input Arguments 3 | % fpF_local: The force measured by the force plate relative to the 4 | % local coordinate system on the force plate. 5 | % [nframes x 3 x nfp] 6 | 7 | % fpM_local: The moment measured by the force plate relative to the 8 | % local coordinate system on the force plate. 9 | % [nframes x 3 x nfp] 10 | 11 | % corners: The global coordinates of 4 corners on the force plate with 12 | % the sequence of 1st to 4th quadrant relative to the 13 | % local coordinate system of the force plate. [4 x 3 x nfp] 14 | 15 | % Vfp2tc: The coordinate of top plate center relative to local C.S. on 16 | % the force plate. [nfp x 3] 17 | 18 | % Pz: Z-coordinate of the plane containing COP relative to local C.S. 19 | % [1 x nfp] 20 | 21 | %% Output Arguments 22 | % gCOP: Global coordinate of COP (combination) 23 | % [nframes x 3] 24 | 25 | % nCOP: Global coordinate of COP of each force plates 26 | % [nframes x 3 x nfp] 27 | 28 | % fpFg: [nframes x 3 x nfp] 29 | 30 | % Rg2fp: The rotation matrix of force plate C.S. relative to the global 31 | % C.S. [3 x 3 x nfp] 32 | 33 | % Vg2fp: The postion vector of the origin of force plate C.S. relative 34 | % to global C.S. [nfp x 3] 35 | 36 | %% 37 | nframes = size(fpF_local, 1); 38 | if nframes ~= size(fpM_local, 1) 39 | errordlg('fpF_local and fpM_local have differnet no. of frames!!'); 40 | return 41 | end 42 | if ~isequal(size(fpF_local,3), size(fpM_local,3), size(corners,3), size(Vfp2tc,1), size(Pz, 2)) 43 | errordlg('Different no. of force plates input!!'); 44 | return 45 | end 46 | 47 | Vg2tc = sum(corners, 1)/4; %[1 x 3 x nfp] 48 | 49 | vec = (corners(1,:,:) + corners(4,:,:) - corners(2,:,:) - corners(3,:,:))/2; 50 | Xp = permute(bsxfun(@rdivide, vec, sqrt(sum(abs(vec).^2,2))), [2 1 3]); 51 | vec = (corners(1,:,:) + corners(2,:,:) - corners(4,:,:) - corners(3,:,:))/2; 52 | Yp = permute(bsxfun(@rdivide, vec, sqrt(sum(abs(vec).^2,2))), [2 1 3]); 53 | Zp = bsxfun(@cross, Xp, Yp); 54 | Rg2fp = cat(2, Xp, Yp, Zp);%[3 x 3 x nfp] 55 | 56 | Vg2fp = Vg2tc - mtimesx( permute(Vfp2tc,[3 2 1]), permute(Rg2fp, [2 1 3]));%[1 x 3 x nfp] 57 | 58 | Px = bsxfun(@rdivide, (bsxfun(@times, permute(Pz, [1 3 2]), fpF_local(:, 1, :)) - fpM_local(:, 2, :)), fpF_local(:, 3, :));%nfr x 1 x nfp 59 | Py = bsxfun(@rdivide, (bsxfun(@times, permute(Pz, [1 3 2]), fpF_local(:, 2, :)) + fpM_local(:, 1, :)), fpF_local(:, 3, :)); 60 | 61 | Tz = fpM_local(:, 3, :) - Px.* fpF_local(:, 2, :) + Py .* fpF_local(:, 1, :) ;% [nframes x 1 x nfp] 62 | 63 | fpFg = mtimesx(fpF_local, permute(Rg2fp, [2 1 3])); % [nframes x 3 x nfp] 64 | Fg = sum(fpFg, 3);% [nframes x 3] 65 | 66 | COP_local = cat(2, Px, Py, repmat(permute(Pz, [1 3 2]), nframes, 1, 1));% [nframes x 3 x nfp] 67 | 68 | nCOP = repmat(Vg2fp, nframes, 1, 1) + mtimesx(COP_local, permute(Rg2fp, [2 1 3])); % [nframes x 3 x nfp] 69 | 70 | %moment method 1 71 | Mg1 = sum(bsxfun(@cross, repmat(Vg2fp, nframes, 1, 1), fpFg) + mtimesx(fpM_local, permute(Rg2fp, [2 1 3])), 3); 72 | 73 | 74 | %moment method 2 75 | Tz(isnan(Tz))=0; 76 | nCOPp=nCOP; 77 | nCOPp(isnan(nCOPp))=0; 78 | Mg2 = sum(bsxfun(@cross, nCOPp, fpFg) + mtimesx(Tz, permute(Rg2fp(:,3,:), [2 1 3])), 3); 79 | 80 | % Choosing gCOP by different calculation method 81 | % gCOP = [-Mg1(:, 2) ./ Fg(:, 3), Mg1(:, 1) ./ Fg(:, 3)]; 82 | gCOP = [-Mg2(:, 2) ./ Fg(:, 3), Mg2(:, 1) ./ Fg(:, 3)]; 83 | 84 | Vg2fp = permute(Vg2fp, [3 2 1]); 85 | end 86 | 87 | -------------------------------------------------------------------------------- /Week02/HW02.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week02/HW02.m -------------------------------------------------------------------------------- /Week02/fpCOP.m: -------------------------------------------------------------------------------- 1 | function COP_local = fpCOP( fpF_local, fpM_local, Pz ) 2 | %% Input Arguments 3 | % fpF_local: The force measured by the force plate relative to the 4 | % local coordinate system on the force plate. 5 | % [nframes x 3] 6 | 7 | % fpM_local: The moment measured by the force plate relative to the 8 | % local coordinate system on the force plate. 9 | % [nframes x 3] 10 | 11 | % Pz: Z-coordinate of the plane containing COP relative to local C.S. 12 | % [1 x 1] 13 | 14 | %% Output Arguments 15 | % COP_local: The coordinates of COP relative to the same C.S. as input 16 | % force and moment. 17 | % [nframes x 3] 18 | 19 | %% 20 | nframes = length(fpF_local); 21 | if nframes ~= length(fpM_local) 22 | errordlg('fpF_local and fpM_local have differnet no. of frames!!'); 23 | return 24 | end 25 | Px = (Pz*fpF_local(:, 1) - fpM_local(:, 2)) ./ fpF_local(:, 3); 26 | Py = (Pz*fpF_local(:, 2) + fpM_local(:, 1)) ./ fpF_local(:, 3); 27 | 28 | % 29 | hold on 30 | axis equal 31 | for i=1:nframes 32 | scatter(Px(i), Py(i)); 33 | pause(0.05) 34 | end 35 | % 36 | 37 | COP_local = [Px Py repmat(Pz, nframes, 1)]; 38 | 39 | end 40 | 41 | -------------------------------------------------------------------------------- /Week02/fpRV.m: -------------------------------------------------------------------------------- 1 | function [ Rg2fp, Vg2fp ] = fpRV( corners, Vfp2tc ) 2 | %% Input Arguments 3 | % corners: The global coordinates of 4 corners on the force plate with 4 | % the sequence of 1st to 4th quadrant relative to the 5 | % local coordinate system of the force plate. [4 x 3] 6 | 7 | % Vfp2tc: The coordinate of top plate center relative to local C.S. on 8 | % the force plate. [1 x 3] 9 | 10 | %% Output Arguments 11 | % Rg2fp: The rotation matrix of force plate C.S. relative to the global 12 | % C.S. [3 x 3] 13 | 14 | % Vg2fp: The postion vector of the origin of force plate C.S. relative 15 | % to global C.S. [1 x 3] 16 | 17 | %% Function 18 | Vg2tc = sum(corners, 1)/4; 19 | 20 | vec = (corners(1,:) + corners(4,:) - corners(2,:) - corners(3,:))/2; 21 | Xp = (vec/norm(vec)).'; 22 | vec = (corners(1,:) + corners(2,:) - corners(4,:) - corners(3,:))/2; 23 | Yp = (vec/norm(vec)).'; 24 | Zp = bsxfun(@cross, Xp, Yp); 25 | 26 | Rg2fp = [ Xp, Yp, Zp ]; 27 | Vg2fp = Vg2tc - Vfp2tc * Rg2fp.'; 28 | 29 | end 30 | 31 | -------------------------------------------------------------------------------- /Week03/Ang2Rot.m: -------------------------------------------------------------------------------- 1 | function Rot = Ang2Rot( theta, sequence ) 2 | % The function derives the rotation matrices from Euler angles 3 | 4 | % Rot: Rotation matrix [3 x 3 x nframes] 5 | % sequence: sequence of the Euler angles '1 x 3' (composed of 'x', 'y', 'z') 6 | 7 | % Validation of rotation angles 8 | if size(theta,2)~=3 || ndims(theta)~=2 9 | errordlg('Please enter the rotation angles in the form of [n x 3] array in degrees.', 'Rotation Angle Input Error');return 10 | end 11 | 12 | eval(['R',sequence,'=RotFormula(sequence);']); 13 | 14 | nframes = size(theta,1); 15 | Rot = zeros(3,3,nframes); 16 | 17 | for i = 1:nframes 18 | t1 = theta(i,1)/180*pi; t2 = theta(i,2)/180*pi;t3 = theta(i,3)/180*pi; 19 | Rot(:,:,i)=eval(eval(['R',sequence])); 20 | end 21 | 22 | end -------------------------------------------------------------------------------- /Week03/DataQ1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week03/DataQ1.mat -------------------------------------------------------------------------------- /Week03/EulerR.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week03/EulerR.mat -------------------------------------------------------------------------------- /Week03/HW03.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week03/HW03.m -------------------------------------------------------------------------------- /Week03/JointAngOffset.m: -------------------------------------------------------------------------------- 1 | function Rc = JointAngOffset( Rs, Rd ) 2 | % The function output rotation matrices ,represent angle between two body segments, 3 | % which have been offsetted by the static position (anatomical position). 4 | 5 | % Rc: output offsetted rotation matrices [3 x 3 x nframes] 6 | % Rs: static rotation matrices representing angle between two body segments (anatomical position) [3 x 3] 7 | % Rd: dynamic rotation matrices representing angle between two body segments [3 x 3 x nframes] 8 | 9 | if isequal(size(Rd,1), size(Rd,2), size(Rs,1), size(Rs,2), 3) && numel(Rs)==9 10 | nframes = size(Rd,3); 11 | else 12 | errordlg('Please enter the correct form of rotation matrice. (Rs[3x3], Rd[3x3xn])', 'JointAngOffset');return 13 | end 14 | 15 | Rc = mtimesx( repmat(Rs.', 1, 1, nframes),Rd); 16 | 17 | -------------------------------------------------------------------------------- /Week03/Rot2Ang.m: -------------------------------------------------------------------------------- 1 | function theta = Rot2Ang( Rot, sequence ) 2 | % The function derives the Euler angles from rotation matrices 3 | 4 | % Rot: Rotation matrix [3 x 3 x nframes] 5 | % sequence: sequence of the Euler angles '1 x 3' (composed of 'x', 'y', 'z') 6 | 7 | % Validation of rotation matrice (dimension) 8 | if ~isequal(3,size(Rot,1),size(Rot,2)) 9 | errordlg('Please enter the rotation matrice in the form of [3 x 3 x n] matrices.', 'Rotation Matrices Input Error');return 10 | end 11 | eval(['R',sequence,'=RotFormula(sequence);']) 12 | nframes = size(Rot,3); 13 | 14 | t1 = zeros(1,1,nframes);t2 = zeros(1,1,nframes);t3 = zeros(1,1,nframes); 15 | switch sequence 16 | case 'zxy' 17 | t3 = atan2d(-Rot(3,1,:), Rot(3,3,:)); 18 | t1 = atan2d(-Rot(1,2,:), Rot(2,2,:)); 19 | t2 = asind(Rot(3,2,:)); 20 | case 'yxz' 21 | t3 = atan2d(Rot(2,1,:), Rot(2,2,:)); 22 | t1 = atan2d(Rot(1,3,:), Rot(3,3,:)); 23 | t2 = asind(-Rot(2,3,:)); 24 | case 'xyz' 25 | t3 = atan2d(-Rot(1,2,:), Rot(1,1,:)); 26 | t1 = atan2d(-Rot(2,3,:), Rot(3,3,:)); 27 | t2 = asind(Rot(1,3,:)); 28 | case 'zyx' 29 | t3 = atan2d(Rot(3,2,:), Rot(3,3,:)); 30 | t1 = atan2d(Rot(2,1,:), Rot(1,1,:)); 31 | t2 = asind(-Rot(3,1,:)); 32 | case 'xyx' 33 | t3 = atan2d(Rot(1,2,:), Rot(1,3,:)); 34 | t2 = acosd(Rot(1,1,:)); 35 | t1 = atan2d(Rot(2,1,:), -Rot(3,1,:)); 36 | case 'xzx' 37 | t1 = atan2d(Rot(3,1,:), Rot(2,1,:)); 38 | t3 = atan2d(Rot(1,3,:), -Rot(1,2,:)); 39 | t2 = acosd(Rot(1,1,:)); 40 | case 'xzy' 41 | t1 = atan2d(Rot(3,2,:), Rot(2,2,:)); 42 | t3 = atan2d(Rot(1,3,:), Rot(1,1,:)); 43 | t2 = asind(-Rot(1,2,:)); 44 | case 'yzx' 45 | t1 = atan2d(-Rot(3,1,:), Rot(1,1,:)); 46 | t3 = atan2d(-Rot(2,3,:), Rot(2,2,:)); 47 | t2 = asind(Rot(2,1,:)); 48 | case 'yxy' 49 | t3 = atan2d(Rot(2,1,:), -Rot(2,3,:)); 50 | t1 = atan2d(Rot(1,2,:), Rot(3,2,:)); 51 | t2 = acosd(Rot(2,2,:)); 52 | case 'yzy' 53 | t1 = atan2d(Rot(3,2,:), -Rot(1,2,:)); 54 | t3 = atan2d(Rot(2,3,:), Rot(2,1,:)); 55 | t2 = acosd(Rot(2,2,:)); 56 | case 'zxz' 57 | t1 = atan2d(Rot(1,3,:), -Rot(2,3,:)); 58 | t3 = atan2d(Rot(3,1,:), Rot(3,2,:)); 59 | t2 = acosd(Rot(3,3,:)); 60 | case 'zyz' 61 | t1 = atan2d(Rot(2,3,:), Rot(1,3,:)); 62 | t3 = atan2d(Rot(3,2,:), -Rot(3,1,:)); 63 | t2 = acosd(Rot(3,3,:)); 64 | end 65 | the = [t1 t2 t3]; 66 | 67 | t1 = t1*pi/180;t2 = t2*pi/180;t3 = t3*pi/180; 68 | 69 | wi = []; 70 | R = eval(eval(['R',sequence])); 71 | 72 | for i = 1:nframes 73 | dif = R(:,:,i)-Rot(:,:,i); 74 | if ~isempty(dif(dif>10^-3)) 75 | wi = [wi,i]; 76 | end 77 | end 78 | if ~isempty(wi) 79 | switch sequence 80 | case 'zxy' 81 | t3 = atan2d(Rot(3,1,:), -Rot(3,3,:)); 82 | t1 = atan2d(Rot(1,2,:), -Rot(2,2,:)); 83 | t2 = 180 - asind(Rot(3,2,:)); 84 | case 'yxz' 85 | t3 = atan2d(-Rot(2,1,:), -Rot(2,2,:)); 86 | t1 = atan2d(-Rot(1,3,:), -Rot(3,3,:)); 87 | t2 = 180 - asind(-Rot(2,3,:)); 88 | case 'xyz' 89 | t3 = atan2d(Rot(1,2,:), -Rot(1,1,:)); 90 | t1 = atan2d(Rot(2,3,:), -Rot(3,3,:)); 91 | t2 = 180 - asind(Rot(1,3,:)); 92 | case 'zyx' 93 | t3 = atan2d(-Rot(3,2,:), -Rot(3,3,:)); 94 | t1 = atan2d(-Rot(2,1,:), -Rot(1,1,:)); 95 | t2 = 180 - asind(-Rot(3,1,:)); 96 | case 'xyx' 97 | t3 = atan2d(-Rot(1,2,:), -Rot(1,3,:)); 98 | t2 = -acosd(Rot(1,1,:)); 99 | t1 = atan2d(-Rot(2,1,:), Rot(3,1,:)); 100 | case 'xzx' 101 | t1 = atan2d(-Rot(3,1,:), -Rot(2,1,:)); 102 | t3 = atan2d(-Rot(1,3,:), Rot(1,2,:)); 103 | t2 = -acosd(Rot(1,1,:)); 104 | case 'xzy' 105 | t1 = atan2d(-Rot(3,2,:), -Rot(2,2,:)); 106 | t3 = atan2d(-Rot(1,3,:), -Rot(1,1,:)); 107 | t2 = 180 - asind(-Rot(1,2,:)); 108 | case 'yzx' 109 | t1 = atan2d(Rot(3,1,:), -Rot(1,1,:)); 110 | t3 = atan2d(Rot(2,3,:), -Rot(2,2,:)); 111 | t2 = 180 - asind(Rot(2,1,:)); 112 | case 'yxy' 113 | t3 = atan2d(-Rot(2,1,:), Rot(2,3,:)); 114 | t1 = atan2d(-Rot(1,2,:), -Rot(3,2,:)); 115 | t2 = -acosd(Rot(2,2,:)); 116 | case 'yzy' 117 | t1 = atan2d(-Rot(3,2,:), Rot(1,2,:)); 118 | t3 = atan2d(-Rot(2,3,:), -Rot(2,1,:)); 119 | t2 = -acosd(Rot(2,2,:)); 120 | case 'zxz' 121 | t1 = atan2d(-Rot(1,3,:), Rot(2,3,:)); 122 | t3 = atan2d(-Rot(3,1,:), -Rot(3,2,:)); 123 | t2 = -acosd(Rot(3,3,:)); 124 | case 'zyz' 125 | t1 = atan2d(-Rot(2,3,:), -Rot(1,3,:)); 126 | t3 = atan2d(-Rot(3,2,:), Rot(3,1,:)); 127 | t2 = -acosd(Rot(3,3,:)); 128 | end 129 | end 130 | the(:,:,wi)=[t1(:,:,wi) t2(:,:,wi) t3(:,:,wi)]; 131 | t1 = t1*pi/180;t2 = t2*pi/180;t3 = t3*pi/180; 132 | theta = permute(the, [ 3 2 1 ]); 133 | % 134 | % wi = []; 135 | % for i = 1:nframes 136 | % dif = R(:,:,i)-Rot(:,:,i); 137 | % if ~isempty(dif(dif>10^-6)) 138 | % wi = [wi,i]; 139 | % end 140 | % end 141 | % if ~isempty(wi) 142 | % disp('error') 143 | % length(wi) 144 | % end 145 | % -------------------------------------------------------------------------------- /Week03/Rot2AngFSOLVE.m: -------------------------------------------------------------------------------- 1 | function theta = Rot2AngFSOLVE( Rot, sequence ) 2 | % The function derives the Euler angles from rotation matrice 3 | % Rot: Rotation matrix [3 x 3 x nframes] 4 | % sequence: sequence of the Euler angles '1 x 3' (composed of 'x', 'y', 'z') 5 | 6 | % Validation of rotation matrice (dimension) 7 | if ~isequal(3,size(Rot,1),size(Rot,2)) 8 | errordlg('Please enter the rotation matrice in the form of [3 x 3 x n] matrice.', 'Rotation Matrice Input Error');return 9 | end 10 | 11 | nframes = size(Rot,3); 12 | theta = zeros(nframes,3); 13 | 14 | % Eliminate the error when at least one component of Rot is zero 15 | % Rot = fix(10^17*Rot)/10^17; 16 | Rot(abs(Rot)<10^-12) = 0; 17 | 18 | 19 | % Validation of rotation matrice (determinant) 20 | dmtmo = ones(1, nframes);% DeterMinanT Minus One 21 | for i=1:nframes 22 | dmtmo(i) = det(Rot(:,:,i))-1; 23 | end 24 | if ~isempty(dmtmo(dmtmo>10^-12)) 25 | text = ['Rotation matrice have wrong forms (determinant is not equal to 1). Matrice of frame no. = ('... 26 | ,num2str(find(abs(dmtmo)>10^-12)), ') have problems.']; 27 | errordlg(text, 'Rotation Matrice Error'); 28 | end 29 | 30 | % fsolve 31 | options = optimset('Display','off','Algorithm','levenberg-marquardt','MaxIter',400000,'TolFun',10^-12); 32 | for i = 1:nframes 33 | eqn = matlabFunction(genEq( Rot(:,:,i), sequence )); 34 | modeqn = @(t)eqn(t(1),t(2),t(3)); 35 | the = fsolve(modeqn,[pi/4;10^3;10^3],options); 36 | theta(i,:) = reshape(the*180/pi,1,[]); 37 | theta(i,:) = degcst(theta(i,:)); 38 | end 39 | 40 | % Generate equations for 'fsolve' 41 | function F = genEq( Rot, sequence ) 42 | eval(['R',sequence,'=RotFormula(sequence);']); 43 | l=reshape(eval(['R',sequence])-Rot, 1, []); 44 | for i=1:length(l) 45 | F(i)=l(i); 46 | end 47 | 48 | % Constraint of output degree: 0 <= theta < 360 49 | function F = degcst(X) 50 | for j = 1:3 51 | if X(1,j)>360 52 | X(1,j)=X(1,j)-floor(X(1,j)/360)*360; 53 | continue 54 | elseif X(1,j)<=0 55 | X(1,j)=X(1,j)-floor(X(1,j)/360)*360; 56 | continue 57 | end 58 | end 59 | F=X; 60 | 61 | -------------------------------------------------------------------------------- /Week03/RotAngConvFix.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week03/RotAngConvFix.m -------------------------------------------------------------------------------- /Week03/RotAngConvert.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week03/RotAngConvert.m -------------------------------------------------------------------------------- /Week03/RotFormula.m: -------------------------------------------------------------------------------- 1 | function R = RotFormula( sequence ) 2 | % This function returns a symbolic Euler rotation matrix. The input 3 | % argument, 'sequence', is a string only composed of character 'x', 'y', 4 | % and 'z'. The symbols of rotation angles are t1, t2, ...tn, where n is the 5 | % number of characters in the variable 'sequence'. 6 | % 7 | % E.g. 8 | % 9 | % >> R = RotFormula( 'xxxx' ) 10 | % 11 | % R = 12 | % [ 1, 0, 0] 13 | % [ 0, cos(t1 + t2 + t3 + t4), -sin(t1 + t2 + t3 + t4)] 14 | % [ 0, sin(t1 + t2 + t3 + t4), cos(t1 + t2 + t3 + t4)] 15 | % 16 | % >> R = RotFormula( '' ) 17 | % 18 | % R = 19 | % [ 1, 0, 0] 20 | % [ 0, 1, 0] 21 | % [ 0, 0, 1] 22 | % 23 | % >> R = RotFormula( 'xzy' ) 24 | % 25 | % R = 26 | % [ cos(t2)*cos(t3), -sin(t2), cos(t2)*sin(t3)] 27 | % [ sin(t1)*sin(t3) + cos(t1)*cos(t3)*sin(t2), cos(t1)*cos(t2), cos(t1)*sin(t2)*sin(t3) - cos(t3)*sin(t1)] 28 | % [ cos(t3)*sin(t1)*sin(t2) - cos(t1)*sin(t3), cos(t2)*sin(t1), cos(t1)*cos(t3) + sin(t1)*sin(t2)*sin(t3)] 29 | 30 | n = length(sequence); 31 | var = sym('t', [1 n]);% Create t1 t2 t3 32 | R = sym(eye(3)); 33 | for i = 1:n 34 | t = var(i); 35 | switch sequence(i) 36 | case '' 37 | break 38 | case 'x' 39 | R = R * [1 0 0; 0 cos(t) -sin(t); 0 sin(t) cos(t)]; 40 | case 'y' 41 | R = R * [cos(t) 0 sin(t); 0 1 0; -sin(t) 0 cos(t)]; 42 | case 'z' 43 | R = R * [cos(t) -sin(t) 0; sin(t) cos(t) 0; 0 0 1]; 44 | otherwise 45 | errordlg('Please enter the sequence only composed of ''x'', ''y'', and ''z''.', 'Sequence Error'); 46 | return 47 | end 48 | end 49 | R = simplify(R); 50 | end 51 | 52 | -------------------------------------------------------------------------------- /Week03/rotang.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week03/rotang.mat -------------------------------------------------------------------------------- /Week03/subcali.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week03/subcali.mat -------------------------------------------------------------------------------- /Week03/x.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week03/x.jpg -------------------------------------------------------------------------------- /Week03/y.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week03/y.jpg -------------------------------------------------------------------------------- /Week03/z.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week03/z.jpg -------------------------------------------------------------------------------- /Week04/12RotSeq.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week04/12RotSeq.jpg -------------------------------------------------------------------------------- /Week04/Ang2LocalAngular.m: -------------------------------------------------------------------------------- 1 | function [AngVel, AngAcc] = Ang2LocalAngular( theta, seq, smprate ) 2 | % Derive the angular velocity and angular acceleration from Euler angles. 3 | 4 | % theta: Euler angles [nframes x 3] 5 | % seq: rotation sequence of the Euler angles '1 x 3' (composed of 'x', 'y', 'z') 6 | % smprate: sampling rate(Hz) [1 x 1] 7 | % AngVel: angular velocity relative to local coordinate [nframes x 3] 8 | % AngAcc: angular acceleration relative to local coordinate [nframes x 3] 9 | 10 | if size(theta,2)~=3 || ndims(theta)~=2 11 | error('Euler angles matrix must in the form of [nframes x 3]') 12 | end 13 | n = size(theta,1); 14 | % time sequence 15 | TimeSeq = reshape(1/smprate:1/smprate:n/smprate,[],1);%nx1 16 | % wrapped theta in radian 17 | theRad = unwrap(theta/180*pi);%nx3 18 | % Global Angular Velocity 19 | GAV = permute([Derivative(TimeSeq, theRad(:,1), 1), Derivative(TimeSeq, theRad(:,2), 1), Derivative(TimeSeq, theRad(:,3), 1)], [2 3 1]);%3x1xn 20 | GAV1 = GAV(1,1,:); GAV2 = GAV(2,1,:); GAV3 = GAV(3,1,:); 21 | % Global Angular Acceleration 22 | GAA = permute([Derivative(TimeSeq, theRad(:,1), 2), Derivative(TimeSeq, theRad(:,2), 2), Derivative(TimeSeq, theRad(:,3), 2)], [2 3 1]); 23 | 24 | theRad = permute(theRad, [3 2 1]);%1x3xn 25 | tR1 = theRad(1,1,:); tR2 = theRad(1,2,:); tR3 = theRad(1,3,:); 26 | 27 | z=zeros(1,1,n); 28 | o=ones(1,1,n); 29 | switch seq 30 | case 'zxy' 31 | R1 = [-cos(tR2).*sin(tR3) cos(tR3) z; ... 32 | sin(tR2) z o; ... 33 | cos(tR2).*cos(tR3) sin(tR3) z];%3x3xn 34 | R2 = [GAV2.*sin(tR2).*sin(tR3)-GAV3.*cos(tR2).*cos(tR3) -GAV3.*sin(tR3) z;... 35 | GAV2.*cos(tR2) z z;... 36 | -GAV2.*sin(tR2).*cos(tR3)-GAV3.*cos(tR2).*sin(tR3) GAV3.*cos(tR3) z]; 37 | case 'yxz' 38 | R1 = [ cos(tR2).*sin(tR3) cos(tR3) z; ... 39 | cos(tR2).*cos(tR3) -sin(tR3) z; ... 40 | -sin(tR2) z o];%3x3xn 41 | R2 = [-GAV2.*sin(tR2).*sin(tR3)+GAV3.*cos(tR2).*cos(tR3) -GAV3.*sin(tR3) z;... 42 | -GAV2.*sin(tR2).*cos(tR3)-GAV3.*sin(tR3).*cos(tR2) -GAV3.*cos(tR3) z;... 43 | -GAV2.*cos(tR2) z z]; 44 | case 'xyz' 45 | R1 = [ cos(tR2).*cos(tR3) sin(tR3) z; ... 46 | -cos(tR2).*sin(tR3) cos(tR3) z; ... 47 | sin(tR2) z o];%3x3xn 48 | R2 = [-GAV2.*sin(tR2).*cos(tR3)-GAV3.*cos(tR2).*sin(tR3) GAV3.*cos(tR3) z;... 49 | GAV2.*sin(tR2).*sin(tR3)-GAV3.*cos(tR3).*cos(tR2) -GAV3.*sin(tR3) z;... 50 | GAV2.*cos(tR2) z z]; 51 | case 'zyx' 52 | R1 = [ -sin(tR2) z o; ... 53 | cos(tR2).*sin(tR3) cos(tR3) z; ... 54 | cos(tR2).*cos(tR3) -sin(tR3) z];%3x3xn 55 | R2 = [-GAV2.*cos(tR2) z z;... 56 | -GAV2.*sin(tR2).*sin(tR3)+GAV3.*cos(tR3).*cos(tR2) -GAV3.*sin(tR3) z;... 57 | -GAV2.*sin(tR2).*cos(tR3)-GAV3.*cos(tR2).*sin(tR3) -GAV3.*cos(tR3) z]; 58 | case 'xyx' 59 | R1 = [ cos(tR2) z o; ... 60 | sin(tR2).*sin(tR3) cos(tR3) z; ... 61 | sin(tR2).*cos(tR3) -sin(tR3) z];%3x3xn 62 | R2 = [-GAV2.*sin(tR2) z z;... 63 | GAV2.*cos(tR2).*sin(tR3)+GAV3.*cos(tR3).*sin(tR2) -GAV3.*sin(tR3) z;... 64 | -GAV3.*sin(tR2).*sin(tR3)+GAV2.*cos(tR2).*cos(tR3) -GAV3.*cos(tR3) z]; 65 | case 'xzx' 66 | R1 = [ cos(tR2) z o; ... 67 | -sin(tR2).*cos(tR3) sin(tR3) z; ... 68 | sin(tR2).*sin(tR3) cos(tR3) z];%3x3xn 69 | R2 = [-GAV2.*sin(tR2) z z;... 70 | -GAV2.*cos(tR2).*cos(tR3)+GAV3.*sin(tR3).*sin(tR2) GAV3.*cos(tR3) z;... 71 | GAV3.*sin(tR2).*cos(tR3)+GAV2.*cos(tR2).*sin(tR3) -GAV3.*sin(tR3) z]; 72 | case 'yzx' 73 | R1 = [ sin(tR2) z o; ... 74 | cos(tR2).*cos(tR3) sin(tR3) z; ... 75 | -cos(tR2).*sin(tR3) cos(tR3) z];%3x3xn 76 | R2 = [GAV2.*cos(tR2) z z;... 77 | -GAV2.*sin(tR2).*cos(tR3)-GAV3.*sin(tR3).*cos(tR2) GAV3.*cos(tR3) z;... 78 | -GAV3.*cos(tR2).*cos(tR3)+GAV2.*sin(tR2).*sin(tR3) -GAV3.*sin(tR3) z]; 79 | case 'yxy' 80 | R1 = [ sin(tR2).*sin(tR3) cos(tR3) z; ... 81 | cos(tR2) z o; ... 82 | -sin(tR2).*cos(tR3) sin(tR3) z];%3x3xn 83 | R2 = [GAV2.*cos(tR2).*sin(tR3)+GAV3.*sin(tR2).*cos(tR3) -GAV3.*sin(tR3) z;... 84 | -GAV2.*sin(tR2) z z;... 85 | -GAV2.*cos(tR2).*cos(tR3)+GAV3.*sin(tR2).*sin(tR3) GAV3.*cos(tR3) z]; 86 | case 'yzy' 87 | R1 = [ sin(tR2).*cos(tR3) -sin(tR3) z; ... 88 | cos(tR2) z o; ... 89 | sin(tR2).*sin(tR3) cos(tR3) z];%3x3xn 90 | R2 = [GAV2.*cos(tR2).*cos(tR3)-GAV3.*sin(tR2).*sin(tR3) -GAV3.*cos(tR3) z;... 91 | -GAV2.*sin(tR2) z z;... 92 | GAV2.*cos(tR2).*sin(tR3)+GAV3.*sin(tR2).*cos(tR3) -GAV3.*sin(tR3) z]; 93 | case 'xzy' 94 | R1 = [ cos(tR2).*cos(tR3) -sin(tR3) z; ... 95 | -sin(tR2) z o; ... 96 | cos(tR2).*sin(tR3) cos(tR3) z];%3x3xn 97 | R2 = [-GAV2.*sin(tR2).*cos(tR3)-GAV3.*cos(tR2).*sin(tR3) -GAV3.*cos(tR3) z;... 98 | -GAV2.*cos(tR2) z z;... 99 | -GAV2.*sin(tR2).*sin(tR3)+GAV3.*cos(tR2).*cos(tR3) -GAV3.*sin(tR3) z]; 100 | case 'zxz' 101 | R1 = [ sin(tR2).*sin(tR3) cos(tR3) z; ... 102 | sin(tR2).*cos(tR3) -sin(tR3) z; ... 103 | cos(tR2) z o];%3x3xn 104 | R2 = [GAV2.*cos(tR2).*sin(tR3)+GAV3.*sin(tR2).*cos(tR3) -GAV3.*sin(tR3) z;... 105 | GAV2.*cos(tR2).*cos(tR3)-GAV3.*sin(tR3).*sin(tR2) -GAV3.*cos(tR3) z;... 106 | -GAV2.*sin(tR2) z z]; 107 | case 'zyz' 108 | R1 = [ -sin(tR2).*cos(tR3) sin(tR3) z; ... 109 | sin(tR2).*sin(tR3) cos(tR3) z; ... 110 | cos(tR2) z o];%3x3xn 111 | R2 = [-GAV2.*cos(tR2).*cos(tR3)+GAV3.*sin(tR2).*sin(tR3) GAV3.*cos(tR3) z;... 112 | GAV2.*cos(tR2).*sin(tR3)+GAV3.*cos(tR3).*sin(tR2) -GAV3.*sin(tR3) z;... 113 | -GAV2.*sin(tR2) z z]; 114 | end 115 | AngVel = permute(mtimesx(R1, GAV),[3 1 2]);%3x1xn to nx3 116 | AngAcc = permute(mtimesx(R2, GAV)+mtimesx(R1, GAA),[3 1 2]); 117 | -------------------------------------------------------------------------------- /Week04/AngAcc.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week04/AngAcc.jpg -------------------------------------------------------------------------------- /Week04/AngVel.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week04/AngVel.jpg -------------------------------------------------------------------------------- /Week04/DataQ1.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week04/DataQ1.mat -------------------------------------------------------------------------------- /Week04/Derivative.m: -------------------------------------------------------------------------------- 1 | function dyi = Derivative( xi, yi, dorder ) 2 | % dorder-th derivative of data 3 | 4 | % xi: x-coordinates of data points [nframes x 1] / [1 x nframes] 5 | % yi: y-coordinates of data points [nframes x 1] / [1 x nframes] 6 | % dorder: order of derivative 7 | % dyi: derivative of yi 8 | 9 | if ~isequal(size(xi),size(yi)) 10 | error('x and y must be the same size.') 11 | end 12 | if ~isequal(size(xi,2),1) && ~isequal(size(xi,1),1) || ~isequal(ndims(xi),2) 13 | warning('x and y should be the format of [nframes x 1] or [1 x nframes]. Modified to [nframes x 1] when calculation automatically.') 14 | end 15 | xir = xi(:); yir = yi(:); 16 | 17 | % method 1 18 | % fpp = fit(xir,yir,'smoothingspline','SmoothingParam',1-10e-10); 19 | % fpp.p = fnder(fpp.p, dorder); 20 | % dyi = fpp(xir); 21 | % dyi = reshape(dyi,size(yi)); 22 | 23 | % method 2 24 | sp = csapi(xir, yir); 25 | sp = fnder(sp, dorder); 26 | dyi = fnval(sp, xir); 27 | 28 | % method 3 29 | % for i = 1:dorder 30 | % yir = diff([eps; yir(:)])./diff([eps; xir(:)]); 31 | % end 32 | % dyi = yir; 33 | 34 | end 35 | 36 | -------------------------------------------------------------------------------- /Week04/HW04.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week04/HW04.m -------------------------------------------------------------------------------- /Week04/PolyDer.m: -------------------------------------------------------------------------------- 1 | function dp = PolyDer( p, dorder ) 2 | % the function the coefficients of polynmial with dth-order of derivation. 3 | % p: coefficient of original polynomial (power down) [(n+1) x 1] 4 | % dorder: order of derivation [1 x 1] / [1 x N] 5 | % dp: coefficient of derivative polynomial (power down) [(n+1-dorder) x 1] 6 | 7 | % E.g. 8 | % >> PolyDer( 1:7, 0:7) 9 | % 10 | % ans = 11 | % 12 | % 1 2 3 4 5 6 7 13 | % 0 6 10 12 12 10 6 14 | % 0 0 30 40 36 24 10 15 | % 0 0 0 120 120 72 24 16 | % 0 0 0 0 360 240 72 17 | % 0 0 0 0 0 720 240 18 | % 0 0 0 0 0 0 720 19 | % 0 0 0 0 0 0 0 20 | 21 | 22 | if ~isequal(size(p,1),1) || ~isequal(ndims(p),2) 23 | warning('p should be the format of [1 x n]. Modified automatically.') 24 | p = reshape(p, 1, []); 25 | end 26 | 27 | if ~isequal(size(dorder,1), size(dorder,2), 1) 28 | d = dorder(:); 29 | dp = zeros(length(d), size(p,2)); 30 | for i = 1:length(d) 31 | dp(i,d(i)+1:size(p,2)) = derivation( p, d(i) ); 32 | end 33 | 34 | else 35 | dp = derivation( p, dorder ); 36 | end 37 | 38 | function np = derivation( p, d ) 39 | n = length(p); 40 | if d == 0 41 | np = p; 42 | return 43 | elseif d >= n 44 | np = 0; 45 | return 46 | end 47 | sq = fliplr(d:n-1); 48 | np = p(1:n-d); 49 | for i = 1:d 50 | np = np .* (sq - i + 1); 51 | end 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /Week04/PolyFit.m: -------------------------------------------------------------------------------- 1 | function p = PolyFit( xi, yi, n ) 2 | % the function derives a curve-fitting polynomial with least-square-error from data. 3 | % xi: x-coordinates of data points [nframes x 1] / [1 x nframes] 4 | % yi: y-coordinates of data points [nframes x 1] / [1 x nframes] 5 | % n: the highest power of the polynomial [1 x 1] 6 | % p: coefficient of polynomial (power down) [(n+1) x 1] 7 | 8 | if ~isequal(size(xi),size(yi)) 9 | error('x and y must be the same size.') 10 | end 11 | if ~isequal(size(xi,2),size(yi,2),1) || ~isequal(ndims(xi),ndims(yi),2) 12 | warning('x and y should be the format of [nframes x 1]. Modified automatically.') 13 | xi = xi(:); yi = yi(:); 14 | end 15 | 16 | % Construct Vandermonde matrix. 17 | V(:,n+1) = ones(length(xi),1); 18 | for j = n:-1:1 19 | V(:,j) = xi.*V(:,j+1); 20 | end 21 | 22 | % Solve least squares problem. 23 | % Method 1: 24 | p1 = (V.' * V) \ V.' *yi; p1 = p1.'; 25 | % Method 2: 26 | [Q,R] = qr(V,0); 27 | p2 = R\(Q'*yi); p2 = p2.'; 28 | 29 | % choose the one with smaller summation of least square error 30 | if sum((PolyVal(p1, xi)-yi).^2,1) < sum((PolyVal(p2, xi)-yi).^2,1) 31 | p = p1;c = 'r'; 32 | else 33 | p = p2;c = 'k'; 34 | end 35 | 36 | 37 | % close all 38 | % hold on 39 | % scatter(xi,yi) 40 | % x = linspace(max(xi),min(xi),length(xi)*10); 41 | % y = PolyVal(p1, x); 42 | % po = polyfit(xi,yi,n); 43 | % yo = PolyVal(po, x); 44 | % plot(x,y,c) 45 | % plot(x,yo,'b') 46 | % disp('p diff:') 47 | % norm(p1-po) 48 | % disp('P') 49 | % sum((polyval(p1, xi)-yi).^2,1) 50 | % disp('p') 51 | % sum((polyval(po, xi)-yi).^2,1) 52 | % sum((polyval(p1, xi)-yi).^2,1)-sum((polyval(po, xi)-yi).^2,1) 53 | 54 | end 55 | 56 | -------------------------------------------------------------------------------- /Week04/PolyVal.m: -------------------------------------------------------------------------------- 1 | function yi = PolyVal( p, xi ) 2 | % the function gives the predicted value of yi 3 | 4 | % xi: x-coordinates of data points [nframes x 1] 5 | % yi: prediction of y-coordinates of data points [nframes x 1] 6 | 7 | if ~isequal(size(xi,2),1) || ~isequal(ndims(xi),2) 8 | warning('x should be the format of [nframes x 1]. Modified automatically.') 9 | xi = xi(:); 10 | end 11 | if ~isequal(size(p,1),1) || ~isequal(ndims(p),2) 12 | warning('p should be the format of [1 x n]. Modified automatically.') 13 | p = reshape(p, 1, []); 14 | end 15 | n = length(p)-1; 16 | y(:,n+1) = ones(length(xi),1); 17 | for j = n:-1:1 18 | y(:,j) = xi.*y(:,j+1); 19 | end 20 | 21 | yi = sum(p.*y,2); 22 | 23 | end 24 | 25 | -------------------------------------------------------------------------------- /Week04/Rot2LocalAngular.m: -------------------------------------------------------------------------------- 1 | function [AngVel, AngAcc] = Rot2LocalAngular( Rg2l, smprate ) 2 | % Derive the angular velocity and angular acceleration from rotaion matrices. 3 | 4 | % Rg2l: the rotaiton matrices describes from global coordinate to local 5 | % coordinate [3 x 3 x nframes] 6 | % smprate: sampling rate(Hz) [1 x 1] 7 | % AngVel: angular velocity relative to local coordinate [nframes x 3] 8 | % AngAcc: angular acceleration relative to local coordinate [nframes x 3] 9 | 10 | % find the Ang. Vel. and Ang. Acc. derived from different rotation sequence 11 | sequence = {'xyx', 'xyz', 'xzy', 'xzx', 'yxy', 'yxz', 'yzy', 'yzx', 'zxy', 'zxz', 'zyx', 'zyz'}; 12 | nframes = size(Rg2l,3); 13 | AVp = zeros(nframes,3,12); AAp = zeros(nframes,3,12); 14 | 15 | for i = 1:12 16 | [AVp(:,:,i), AAp(:,:,i)] = Ang2LocalAngular( RotAngConvert(Rg2l,sequence{i}), sequence{i}, smprate ); 17 | end 18 | 19 | rAV = 100; rAA = 15;%might need modified 20 | thdAV1 = mean2(abs(AVp(:,1,:)))/rAV;thdAV2 = mean2(abs(AVp(:,2,:)))/rAV;thdAV3 = mean2(abs(AVp(:,3,:)))/rAV; 21 | thdAA1 = mean2(abs(AAp(:,1,:)))/rAA;thdAA2 = mean2(abs(AAp(:,2,:)))/rAA;thdAA3 = mean2(abs(AAp(:,3,:)))/rAA; 22 | [maxAV, ImaxAV] = max(abs(AVp),[],3); minAV = min(abs(AVp),[],3); 23 | [maxAA, ImaxAA] = max(abs(AAp),[],3); minAA = min(abs(AAp),[],3); 24 | 25 | Iglv = intersect(ImaxAV(abs(maxAV(:,1)-minAV(:,1))>thdAV1,1),intersect(ImaxAV(abs(maxAV(:,2)-minAV(:,2))>thdAV2,2),ImaxAV(abs(maxAV(:,3)-minAV(:,3))>thdAV3,3))); 26 | Igla = intersect(ImaxAA(abs(maxAA(:,1)-minAA(:,1))>thdAA1,1),intersect(ImaxAA(abs(maxAA(:,2)-minAA(:,2))>thdAA2,2),ImaxAA(abs(maxAA(:,3)-minAA(:,3))>thdAA3,3))); 27 | GLindex = intersect(Iglv,Igla); 28 | index = setdiff(1:12,GLindex); 29 | if ~isempty(GLindex) 30 | disp('Gimbal lock happend when sequence =') 31 | disp(string(sequence(GLindex))) 32 | end 33 | 34 | AngVel = mean(AVp(:,:,index),3); 35 | AngAcc = mean(AAp(:,:,index),3); 36 | 37 | end 38 | 39 | -------------------------------------------------------------------------------- /Week04/問答題.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week04/問答題.docx -------------------------------------------------------------------------------- /Week05/EulerP2Angular.m: -------------------------------------------------------------------------------- 1 | function [ AngVel, AngAcc ] = EulerP2Angular( P, smprate ) 2 | 3 | 4 | % P: Euler parameters(normalized quaternion) [nframes x 4 x N(segments)] 5 | % smprate: sampling rate [1 x 1] 6 | % AngVel: the xyz-coordinates of angular velocity relative to segment local 7 | % coordinate system [nframes x 3 x N(segments)] 8 | % AngAcc: the xyz-coordinates of angular acceleration relative to segment local 9 | % coordinate system [nframes x 3 x N(segments)] 10 | 11 | N = size(P, 3); nf = size(P, 1); 12 | TimeSeq = reshape(1/smprate:1/smprate:nf/smprate,[],1);%nx1 13 | AngVel = zeros(nf,3,N); AngAcc = zeros(nf,3,N); 14 | for i = 1:N 15 | Pseg = permute(P(:,:,i), [2 3 1]);%4x1xn 16 | % e0 = Pseg(1,:,:); e1 = Pseg(2,:,:); e2 = Pseg(3,:,:); e3 = Pseg(4,:,:); 17 | Eslash = Pseg(2,:,:).*repmat([0 0 0; 0 0 1; 0 -1 0],1,1,nf)+... 18 | Pseg(3,:,:).*repmat([0 0 -1; 0 0 0; 1 0 0],1,1,nf)+... 19 | Pseg(4,:,:).*repmat([0 1 0; -1 0 0; 0 0 0],1,1,nf); 20 | G = [-Pseg(2:4,:,:) Eslash + Pseg(1,:,:).*repmat(eye(3),1,1,nf)]; 21 | Pdir1 = permute([Derivative(TimeSeq, P(:,1,i), 1), Derivative(TimeSeq, P(:,2,i), 1),... 22 | Derivative(TimeSeq, P(:,3,i), 1), Derivative(TimeSeq, P(:,4,i), 1)], [2 3 1]);%4x1xn 23 | Pdir2 = permute([Derivative(TimeSeq, P(:,1,i), 2), Derivative(TimeSeq, P(:,2,i), 2),... 24 | Derivative(TimeSeq, P(:,3,i), 2), Derivative(TimeSeq, P(:,4,i), 2)], [2 3 1]);%4x1xn 25 | AngVel(:,:,i) = permute(2 * mtimesx(G, Pdir1),[3 1 2]);%n*3*1 26 | AngAcc(:,:,i) = permute(2 * mtimesx(G, Pdir2),[3 1 2]); 27 | end 28 | 29 | -------------------------------------------------------------------------------- /Week05/EulerP2Rot.m: -------------------------------------------------------------------------------- 1 | function R = EulerP2Rot( P ) 2 | 3 | 4 | % P: Euler parameters(normalized quaternion) [nframes x 4 x N(segments)] 5 | % R: rotation matrices [3 x 3 x nframes x N(segments)] 6 | nf = size(P, 1); 7 | N = size(P, 3); 8 | R = zeros(3,nf,3,N); 9 | e0 = P(:,1,:); e1 = P(:,2,:); e2 = P(:,3,:); e3 = P(:,4,:); 10 | R(1,:,1,:) = shiftdim(e0.^2+e1.^2-0.5,-1); 11 | R(2,:,1,:) = shiftdim(e1.*e2+e0.*e3,-1); 12 | R(3,:,1,:) = shiftdim(e1.*e3-e0.*e2,-1); 13 | R(1,:,2,:) = shiftdim(e1.*e2-e0.*e3,-1); 14 | R(2,:,2,:) = shiftdim(e0.^2+e2.^2-0.5,-1); 15 | R(3,:,2,:) = shiftdim(e2.*e3+e0.*e1,-1); 16 | R(1,:,3,:) = shiftdim(e1.*e3+e0.*e2,-1); 17 | R(2,:,3,:) = shiftdim(e2.*e3-e0.*e1,-1); 18 | R(3,:,3,:) = shiftdim(e0.^2+e3.^2-0.5,-1); 19 | R = permute(2*R, [1 3 2 4]); 20 | end 21 | 22 | -------------------------------------------------------------------------------- /Week05/HW05.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week05/HW05.m -------------------------------------------------------------------------------- /Week05/Rot2EulerP.m: -------------------------------------------------------------------------------- 1 | function [ P, phi, n ] = Rot2EulerP( R ) 2 | % 3 | 4 | % R: rotation matrices [3 x 3 x nframes x N(segments)] 5 | % P: Euler parameters(normalized quaternion) [nframes x 4 x N(segments)] 6 | % phi: the rotation angles rotate about the axis (radians) [nframes x N(segments)] 7 | % n: unit vectors of the rotation axis [nframes x 3 x N(segments)] 8 | nf = size(R, 3); 9 | N = size(R, 4); 10 | 11 | e0 = permute((R(1,1,:,:) + R(2,2,:,:) + R(3,3,:,:) +1).^0.5/2, [3 1 4 2]);%n*1*N 12 | phi = 2 * acos(e0); 13 | e1 = permute((R(3,2,:,:) - R(2,3,:,:)) / 4, [3 1 4 2]) ./ e0; 14 | e2 = permute((R(1,3,:,:) - R(3,1,:,:)) / 4, [3 1 4 2]) ./ e0; 15 | e3 = permute((R(2,1,:,:) - R(1,2,:,:)) / 4, [3 1 4 2]) ./ e0; 16 | 17 | if ~isempty(union(isfinite(e3),isnan(e3))) 18 | Ie0z = e0 == 0; 19 | phi(Ie0z) = pi; 20 | e1ate0z = permute((-(R(2,2,:,:) + R(3,3,:,:)/ 2).^0.5) , [3 1 4 2]); 21 | e2ate0z = permute(R(2,1,:,:) , [3 1 4 2])/2./e1ate0z; 22 | e3ate0z = permute(R(3,1,:,:) , [3 1 4 2])/2./e1ate0z; 23 | e1(Ie0z) = e1ate0z(Ie0z); 24 | e2(Ie0z) = e2ate0z(Ie0z); 25 | e3(Ie0z) = e3ate0z(Ie0z); 26 | if ~isempty(union(isfinite(e3),isnan(e3))) 27 | Ie1z = intersect(e1 == 0, Ie0z); 28 | e2ate1z = permute(((1- R(3,3,:,:)/ 2).^0.5) , [3 1 4 2]); 29 | e3ate1z = permute(R(3,2,:,:) , [3 1 4 2])/2./e2ate1z; 30 | e2(Ie1z) = e2ate1z(Ie1z); 31 | e3(Ie1z) = e3ate1z(Ie1z); 32 | if ~isempty(union(isfinite(e3),isnan(e3))) 33 | Ie2z = intersect(e2 == 0, Ie1z); 34 | e3(Ie2z) = 0; 35 | end 36 | end 37 | end 38 | P = cat(2, e0, e1, e2, e3); 39 | n = cat(2, e1./sin(phi/2), e2./sin(phi/2), e3./sin(phi/2)); 40 | phi = squeeze(phi); 41 | end 42 | 43 | -------------------------------------------------------------------------------- /Week05/Rot2LocalAngularEP.m: -------------------------------------------------------------------------------- 1 | function [ AngVel, AngAcc ] = Rot2LocalAngularEP( R, smprate ) 2 | 3 | 4 | % R: rotation matrices [3 x 3 x nframes x N(segments)] 5 | % smprate: sampling rate [1 x 1] 6 | % AngVel: the xyz-coordinates of angular velocity relative to segment local 7 | % coordinate system [nframes x 3 x N(segments)] 8 | % AngAcc: the xyz-coordinates of angular acceleration relative to segment local 9 | % coordinate system [nframes x 3 x N(segments)] 10 | 11 | [ P, phi, n ] = Rot2EulerP( R ); 12 | [ Pi, ~, ~ ] = unwrapEP( P, phi, n ); 13 | [ AngVel, AngAcc ] = EulerP2Angular( Pi, smprate ); 14 | end 15 | 16 | -------------------------------------------------------------------------------- /Week05/test.m: -------------------------------------------------------------------------------- 1 | close all 2 | phi = linspace(0,6*pi,180); 3 | phiL = length(phi); 4 | theta = [zeros(phiL,1) phi.' zeros(phiL,1)]; 5 | R = Ang2Rot(theta/pi*180,'xyz'); 6 | [P, phi, n] = Rot2EulerP(R); 7 | find(sign(diff(P,1,1))==-1); 8 | find(sign(diff(phi))==-1); 9 | find(sign(diff(n))==-1); 10 | 11 | i=1; 12 | subplot(3,2,1+1*(i-1)) 13 | %plot(1:phiL,Derivative(a(:),P(:,1,:),1)) 14 | plot(1:phiL,P) 15 | subplot(3,2,3+1*(i-1)) 16 | plot(1:phiL,phi) 17 | subplot(3,2,5+1*(i-1)) 18 | plot(1:phiL,n) 19 | % size(P) 20 | % size(phi) 21 | % size(n) 22 | [ P, phi, n ] = unwrapEP( P, phi, n ); 23 | % size(P) 24 | % size(phi) 25 | % size(n) 26 | i=2; 27 | subplot(3,2,1+1*(i-1)) 28 | %plot(1:phiL,Derivative(a(:),P(:,1,:),1)) 29 | plot(1:phiL,P) 30 | subplot(3,2,3+1*(i-1)) 31 | plot(1:phiL,phi) 32 | subplot(3,2,5+1*(i-1)) 33 | plot(1:phiL,n) 34 | 35 | 36 | -------------------------------------------------------------------------------- /Week05/unwrapEP.m: -------------------------------------------------------------------------------- 1 | function [ Pi, phii, ni ] = unwrapEP( P, phi, n ) 2 | 3 | 4 | % P: Euler parameters(normalized quaternion) [nframes x 4 x N(segments)] 5 | % phi: the rotation angles rotate about the axis (radians) [nframes x N(segments)] 6 | % n: unit vectors of the rotation axis [nframes x 3 x N(segments)] 7 | 8 | if size(P,2)~=4 9 | error('4') 10 | elseif size(n,2)~=3 11 | error('3') 12 | elseif ~isequal(size(P,1), size(phi,1), size(n,1)) 13 | error('nframes') 14 | else 15 | nf = size(P,1); 16 | end 17 | if ~isequal(size(P,3), size(phi,2), size(n,3)) 18 | error('nframes') 19 | else 20 | N = size(P,3); 21 | end 22 | 23 | n = permute(n, [1 3 2]);P = permute(P, [1 3 2]); 24 | 25 | difP = diff(P,1,1);%n-1 x N x 4 26 | normp = sqrt(sum(abs(difP).^2,3));%n-1 x N x 1 27 | Ip = zeros(nf,N); 28 | for i = 1:N 29 | I = normp(:,i) >= 1; 30 | I = cat(1, 0, I); 31 | im = (-1).^cumsum(I); 32 | Ip(:,i) = im; 33 | end 34 | 35 | phii=phi.*Ip;Pi=P.*Ip; 36 | %ni = P(:,:,2:4)./sin(phii/2); 37 | ni=n.*Ip; 38 | % phii = unwrap(phii); 39 | 40 | difn = diff(ni,1,1); 41 | normn = sqrt(sum(abs(difn).^2,3)); 42 | In = zeros(nf,N); 43 | for i = 1:N 44 | I = normn(:,i) >= 1; 45 | I = cat(1, 0, I); 46 | im = (-1).^cumsum(I); 47 | In(:,i) = im; 48 | end 49 | ni=ni.*In;phii=phii.*In; 50 | phii = unwrap(phii); 51 | 52 | 53 | Pi = ipermute(Pi, [1 3 2]); 54 | ni = ipermute(ni, [1 3 2]); 55 | 56 | 57 | end 58 | 59 | 60 | -------------------------------------------------------------------------------- /Week06/HAtest.m: -------------------------------------------------------------------------------- 1 | function HAtest 2 | n = 200; 3 | prinV = rand(1,3); 4 | prinV = prinV./norm(prinV); 5 | lineP = 100*round((rand(1,3)-0.5)*10); 6 | t = 50*rand(1); 7 | sphereO = lineP + t*prinV; 8 | r = (t/2)*rand(n,1); 9 | theta = (2*pi)*rand(n,1); 10 | phi = (2*pi)*rand(n,1); 11 | p = ones(n,1)*sphereO + [r.*cos(phi).*cos(theta) r.*cos(phi).*sin(theta) r.*sin(phi)]; 12 | noise = (max(r)/8)*(rand(n,3)-.5); 13 | noiseCenter = ones(n,1)*lineP + noise; 14 | p2 = 2*noiseCenter - p; 15 | u = p2 - p; 16 | [o,E,v] = HelicalAxesCenter(u,p); 17 | figure 18 | plot3(lineP(1),lineP(2),lineP(3),'ob','linewidth',2,'markersize',10),hold on 19 | plot3(o(1),o(2),o(3),'oc','linewidth',2,'markersize',10),legend('Theoritical position','Least-Square') 20 | h = plot3([p(:,1) p2(:,1)].',[p(:,2) p2(:,2)].',[p(:,3) p2(:,3)].'); 21 | set(h,'color',[1 .7 .7]) 22 | L = sqrt(sum(u.*u,2)); 23 | L = (mean(L)+3*std(L))/2; 24 | meanD = [o-L*v;o+L*v]; 25 | plot3(meanD(:,1),meanD(:,2),meanD(:,3),'g','linewidth',2) 26 | title({['Error: ' num2str(norm(o-lineP),'%0.2f')];['Residual error: ' num2str(E,'%0.2f')]}) 27 | cameratoolbar, axis equal 28 | end -------------------------------------------------------------------------------- /Week06/HW06.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week06/HW06.m -------------------------------------------------------------------------------- /Week06/HelicalAxesCenter.m: -------------------------------------------------------------------------------- 1 | function [ o, E, v ] = HelicalAxesCenter( u, p ) 2 | % Derive the principal axis and least-square intersection point from 3 | % multiple helical axes (vector and one point on the helical axes) 4 | 5 | % u: Vectors of helical axes [nframes x 3] 6 | % p: Points on helical axe s[nframes x 3] 7 | % o: Least-square intersection point[1 x 3] 8 | % E: Error from the least-square process [1 x 1] 9 | % v: The direction of principal axis [1 x 3] 10 | 11 | % Calculation of the position of intersection point 12 | u = u./ sqrt(sum(abs(u).^2,2)); 13 | A = size(u,1)*eye(3) - u.'*u;%3*3 14 | B = sum(p,1) - sum(sum(u.*p, 2).*u,1);%1*3 15 | o = reshape(A\reshape(B,[],1),1,[]);%1*3 16 | 17 | % Calculation of the vector of the principal axis 18 | U = cat(1, u, -u); 19 | M = U.'*U; 20 | [V,D]=eig(M); 21 | 22 | D = sum(D,1); 23 | [~,I]=max(D); 24 | v = V(:,I).'; 25 | 26 | d = p-repmat(o,size(u,1),1); 27 | vec = d - sum(d.* u, 2).*u; 28 | E = sum(sqrt(sum(abs(vec).^2,2)),1)/size(u,1); 29 | 30 | end 31 | 32 | -------------------------------------------------------------------------------- /Week06/HipROM.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week06/HipROM.mat -------------------------------------------------------------------------------- /Week06/KneeFE.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week06/KneeFE.mat -------------------------------------------------------------------------------- /Week06/Prac3Prob3(SphereFit)/ShereFit_Average.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week06/Prac3Prob3(SphereFit)/ShereFit_Average.fig -------------------------------------------------------------------------------- /Week06/Prac3Prob3(SphereFit)/ShereFit_LLFC.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week06/Prac3Prob3(SphereFit)/ShereFit_LLFC.fig -------------------------------------------------------------------------------- /Week06/Prac3Prob3(SphereFit)/ShereFit_LMFC.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week06/Prac3Prob3(SphereFit)/ShereFit_LMFC.fig -------------------------------------------------------------------------------- /Week06/Prac3Prob3(SphereFit)/ShereFit_LTRO.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week06/Prac3Prob3(SphereFit)/ShereFit_LTRO.fig -------------------------------------------------------------------------------- /Week06/Prac3Prob3(circumcenter)/circumcenter_LLFC.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week06/Prac3Prob3(circumcenter)/circumcenter_LLFC.fig -------------------------------------------------------------------------------- /Week06/Prac3Prob3(circumcenter)/circumcenter_LMFC.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week06/Prac3Prob3(circumcenter)/circumcenter_LMFC.fig -------------------------------------------------------------------------------- /Week06/Prac3Prob3(circumcenter)/circumcenter_LTRO.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week06/Prac3Prob3(circumcenter)/circumcenter_LTRO.fig -------------------------------------------------------------------------------- /Week06/Prac3Prob3(circumcenter)/circumcenter_Total.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week06/Prac3Prob3(circumcenter)/circumcenter_Total.fig -------------------------------------------------------------------------------- /Week06/RV2Screw.m: -------------------------------------------------------------------------------- 1 | function [ phi, n, t, s ] = RV2Screw( R, V, unwraplog ) 2 | % Transfer rotation matrix and position vector to screw axis 3 | 4 | % R: Rotation matrix [3 x 3 x nframes] 5 | % V: Position vector [nframes x 3] 6 | % unwraplog: Logical value for determining whether to unwrap [1 x 1] 7 | % phi: The rotation angles rotate about the axis (radians) [nframes x 1] 8 | % n: Unit vectors of the rotation axis [nframes x 3] 9 | % t: The value of translation along with the rotation axis [nframes x 1] 10 | % s: Radius of gyrationfrom the origin to the screw axis [nframes x 3] 11 | 12 | if nargin == 2 13 | unwraplog = 1; 14 | end 15 | % if nargin == 2 16 | % uwl = 1; 17 | % elseif nargin == 3 18 | % if unwraplog == 1 || unwraplog == 0 19 | % uwl = unwraplog; 20 | % else 21 | % error('A non-logical value is given to unwraplog!') 22 | % end 23 | % end 24 | 25 | V = permute(V, [2 3 1]);%3*1*n 26 | 27 | sinphin = 0.5*[R(3,2,:)-R(2,3,:); R(1,3,:)-R(3,1,:); R(2,1,:)-R(1,2,:)]; 28 | sinphi = 0.5*((R(3,2,:)-R(2,3,:)).^2 + (R(1,3,:)-R(3,1,:)).^2 + (R(2,1,:)-R(1,2,:)).^2).^0.5; 29 | cosphi = 0.5*(R(3,3,:)+R(2,2,:)+R(1,1,:)-1); 30 | phi = atan2(sinphi, cosphi); 31 | n = sinphin ./ sinphi; 32 | t = mtimesx(permute(n, [2 1 3]), V); 33 | s = 0.5*(V - t.*n) + 0.5*sinphi./(1-cosphi).*bsxfun(@cross, n, V); 34 | 35 | % Index of phi=0 36 | Ipz = phi<10^-6; 37 | if ~isempty(phi(Ipz)) 38 | disp('Ipz') 39 | n(:,:,Ipz) = bsxfun(@rdivide, V, sqrt(sum(abs(V).^2,1))); 40 | t(Ipz) = sqrt(sum(abs(V).^2,1)); 41 | s(:,:,Ipz) = [0;0;0]; 42 | end 43 | 44 | % Index of sin(phi) close to zero 45 | Issp = setdiff(abs(sinphi)<0.03, Ipz); 46 | if ~isempty(sinphi(Issp)) 47 | disp('Issp') 48 | vec = 0.5 * (R + permute(R,[2 1 3])) - cosphi .* repmat(eye(3),1,1,size(R,3)); 49 | n(:,:,Issp) = bsxfun(@rdivide, vec(:,1,Issp), sqrt(sum(abs(vec(:,1,Issp)).^2,1))); 50 | t(Issp) = mtimesx(permute(n(:,:,Issp), [2 1 3]), V(:,:,Issp)); 51 | s(:,:,Issp) = 0.5*(V(:,:,Issp) - t(Issp).*n(:,:,Issp)) + 0.5*sinphi(Issp)./(1-cosphi(Issp)).*bsxfun(@cross, n(:,:,Issp), V(:,:,Issp)); 52 | end 53 | phi = permute(phi,[3 1 2]); 54 | n = permute(n,[3 1 2]); 55 | t = permute(t,[3 1 2]); 56 | s = permute(s,[3 1 2]); 57 | if unwraplog == 1 58 | phi = unwrap(phi); 59 | elseif unwraplog ~= 0 60 | error('A non-logical value is given to unwraplog!') 61 | end 62 | 63 | 64 | end 65 | 66 | -------------------------------------------------------------------------------- /Week06/Screw2RV.m: -------------------------------------------------------------------------------- 1 | function [ R, V ] = Screw2RV( phi, n, t, s ) 2 | % Transfer screw axis to rotation matrix and position vector 3 | 4 | % phi: The rotation angles rotate about the axis (radians) [nframes x 1] 5 | % n: Unit vectors of the rotation axis [nframes x 3] 6 | % t: The value of translation along with the rotation axis [nframes x 1] 7 | % s: Radius of gyrationfrom the origin to the screw axis [nframes x 3] 8 | % R: Rotation matrix [3 x 3 x nframes] 9 | % V: Position vector [nframes x 3] 10 | phi = ipermute(phi,[3 1 2]); 11 | n = ipermute(n,[3 1 2]); 12 | t = ipermute(t,[3 1 2]); 13 | s = ipermute(s,[3 1 2]); 14 | V = t.*n + (1-cos(phi)).*(s-dot(s,n).*n) + sin(phi).*cross(s,n); 15 | nf = size(V,3); 16 | R = mtimesx(n,permute(n,[2 1 3])).*(1-cos(phi)) + cos(phi).*repmat(eye(3),1,1,nf)... 17 | + sin(phi).*[zeros(1,1,nf) -n(3,1,:) n(2,1,:); n(3,1,:) zeros(1,1,nf) -n(1,1,:); -n(2,1,:) n(1,1,:) zeros(1,1,nf)]; 18 | V = permute(V, [3 1 2]); 19 | 20 | end 21 | 22 | -------------------------------------------------------------------------------- /Week06/hw6prac3p3Circumcenter.m: -------------------------------------------------------------------------------- 1 | %Calculate by Circumcenter & Intersection Point Fitting 2 | load('HipROM.mat') 3 | [lRg2p, lVg2p, ~] = CoordPelvis(cat(3, LASI, LPSI, RASI), 'l', {'LASI', 'LPSI', 'RASI'}); 4 | [lRg2t, lVg2t, ~] = CoordThigh(cat(3, LLFC, LTRO, LMFC), 'l', {'LLFC', 'LTRO', 'LMFC'}); 5 | [~, lVg2s, ~] = CoordShank(cat(3, LTT, LLMA, LMMA, LSHA), 'l', {'LTT', 'LLMA', 'LMMA', 'LSHA'}); 6 | marker = {'LTRO','LLFC','LMFC'}; 7 | o = []; 8 | c = []; 9 | for i = 1:3 10 | T = marker{i}; 11 | Tar = eval(T); 12 | 13 | PT = CoordG2L( lRg2p, lVg2p, Tar ); 14 | nf = size(PT,1); 15 | S = floor(nf/3); 16 | PT = PT(1:S*3,:); 17 | 18 | DT = triangulation(reshape(1:S*3,[],3),PT); 19 | CC = circumcenter(DT,[1:floor(nf/3)]'); 20 | UU = cross(PT(1:S,:)-PT(S+1:2*S,:),PT(2*S+1:end,:)-PT(1:S,:)); 21 | [O, E, ~] = HelicalAxesCenter( UU, CC ); 22 | 23 | % figure(i) 24 | % view(3) 25 | % title([string(T),'center: '+string(O(1))+', '+string(O(2))+', '+string(O(3))]) 26 | % hold on 27 | % axis equal 28 | % plot3(PT(1:S,1),PT(1:S,2),PT(1:S,3),'r') 29 | % text(PT(1,1),PT(1,2),PT(1,3),'1','color','r') 30 | % plot3(PT(S:2*S,1),PT(S:2*S,2),PT(S:2*S,3),'g') 31 | % text(PT(S+1,1),PT(S+1,2),PT(S+1,3),'2','color','g') 32 | % plot3(PT(2*S:end,1),PT(2*S:end,2),PT(2*S:end,3),'b') 33 | % text(PT(2*S+1,1),PT(2*S+1,2),PT(2*S+1,3),'3','color','b') 34 | 35 | % scatter3(0,0,0,'k') 36 | % text(0,0,0,'origin') 37 | % scatter3(O(1),O(2),O(3)) 38 | % text(O(1),O(2),O(3),'center') 39 | % plot3(O(1),O(2),O(3)) 40 | 41 | % figure(4) 42 | % axis equal 43 | % hold on 44 | % plot3(PT(:,1),PT(:,2),PT(:,3)) 45 | % text(O(1),O(2),O(3),(marker{i})) 46 | o = cat(1, o, O); 47 | end 48 | % legend(marker) 49 | % scatter3(o(:,1),o(:,2),o(:,3)) 50 | o = sum(o,1)/3; 51 | % title(['average center: '+string(o(1))+', '+string(o(2))+', '+string(o(3))]) 52 | % 53 | % view(3) 54 | % scatter3(0,0,0,'k') 55 | % text(0,0,0,'origin') 56 | disp('Calculated by Circumcenter & Intersection Point Fitting:') 57 | disp('The estimate hip center joint is (' + string(o(1)) + ', ' + string(o(2)) + ', '+ string(o(3)) + ').' ) 58 | -------------------------------------------------------------------------------- /Week06/hw6prac3p3SphereFit.m: -------------------------------------------------------------------------------- 1 | % Calculate by Sphere fitting 2 | load('HipROM.mat') 3 | [lRg2p, lVg2p, ~] = CoordPelvis(cat(3, LASI, LPSI, RASI), 'l', {'LASI', 'LPSI', 'RASI'}); 4 | [lRg2t, lVg2t, ~] = CoordThigh(cat(3, LLFC, LTRO, LMFC), 'l', {'LLFC', 'LTRO', 'LMFC'}); 5 | [~, lVg2s, ~] = CoordShank(cat(3, LTT, LLMA, LMMA, LSHA), 'l', {'LTT', 'LLMA', 'LMMA', 'LSHA'}); 6 | marker = {'LTRO','LLFC','LMFC','Average Center','Origin'}; 7 | nf = size(lRg2t,3); 8 | Col = [(1:nf)/nf; -(-nf:-1)/nf; ones(1,nf)*1].'; 9 | c = []; 10 | for i = 1:3 11 | T = marker{i}; 12 | Tar = eval(T); 13 | 14 | PT = CoordG2L( lRg2p, lVg2p, Tar ); 15 | 16 | [C,Radius] = sphereFit(PT); 17 | 18 | % figure(i) 19 | % view(3) 20 | % title([string(T),'center: '+string(C(1))+', '+string(C(2))+', '+string(C(3))]) 21 | % hold on 22 | % grid on 23 | % axis equal 24 | % 25 | % scatter3(PT(:,1),PT(:,2),PT(:,3),repmat(3,nf,1),Col) 26 | % scatter3(0,0,0,'k') 27 | % text(0,0,0,'origin') 28 | % scatter3(C(1),C(2),C(3)) 29 | % text(C(1),C(2),C(3),'center') 30 | % plot3(C(1),C(2),C(3)) 31 | % 32 | % figure(4) 33 | % axis equal 34 | % hold on 35 | % plot3(PT(:,1),PT(:,2),PT(:,3)) 36 | c = cat(1, c, C); 37 | end 38 | % grid on 39 | c = sum(c,1)/3; 40 | % title(['average center: '+string(c(1))+', '+string(c(2))+', '+string(c(3))]) 41 | % scatter3(c(:,1),c(:,2),c(:,3)) 42 | % view(3) 43 | % scatter3(0,0,0,'k') 44 | % legend(marker) 45 | disp('Calculated by Sphere Fitting:') 46 | disp('The estimate hip center joint is (' + string(c(1)) + ', ' + string(c(2)) + ', '+ string(c(3)) + ').' ) 47 | -------------------------------------------------------------------------------- /Week06/sphereFit.m: -------------------------------------------------------------------------------- 1 | function [Center,Radius] = sphereFit(X) 2 | % this fits a sphere to a collection of data using a closed form for the 3 | % solution (opposed to using an array the size of the data set). 4 | % Minimizes Sum((x-xc)^2+(y-yc)^2+(z-zc)^2-r^2)^2 5 | % x,y,z are the data, xc,yc,zc are the sphere's center, and r is the radius 6 | 7 | % Assumes that points are not in a singular configuration, real numbers, ... 8 | % if you have coplanar data, use a circle fit with svd for determining the 9 | % plane, recommended Circle Fit (Pratt method), by Nikolai Chernov 10 | % http://www.mathworks.com/matlabcentral/fileexchange/22643 11 | 12 | % Input: 13 | % X: n x 3 matrix of cartesian data 14 | % Outputs: 15 | % Center: Center of sphere 16 | % Radius: Radius of sphere 17 | % Author: 18 | % Alan Jennings, University of Dayton 19 | 20 | A=[mean(X(:,1).*(X(:,1)-mean(X(:,1)))), ... 21 | 2*mean(X(:,1).*(X(:,2)-mean(X(:,2)))), ... 22 | 2*mean(X(:,1).*(X(:,3)-mean(X(:,3)))); ... 23 | 0, ... 24 | mean(X(:,2).*(X(:,2)-mean(X(:,2)))), ... 25 | 2*mean(X(:,2).*(X(:,3)-mean(X(:,3)))); ... 26 | 0, ... 27 | 0, ... 28 | mean(X(:,3).*(X(:,3)-mean(X(:,3))))]; 29 | A=A+A.'; 30 | B=[mean((X(:,1).^2+X(:,2).^2+X(:,3).^2).*(X(:,1)-mean(X(:,1))));... 31 | mean((X(:,1).^2+X(:,2).^2+X(:,3).^2).*(X(:,2)-mean(X(:,2))));... 32 | mean((X(:,1).^2+X(:,2).^2+X(:,3).^2).*(X(:,3)-mean(X(:,3))))]; 33 | Center=(A\B).'; 34 | Radius=sqrt(mean(sum([X(:,1)-Center(1),X(:,2)-Center(2),X(:,3)-Center(3)].^2,2))); 35 | -------------------------------------------------------------------------------- /Week07/BodyCOM_Dempster.m: -------------------------------------------------------------------------------- 1 | function [ bodyCOM, segCOM ] = BodyCOM_Dempster( CoordP, CoordD ) 2 | % Calculate the COM of each limb and whole body from anthropometric data 3 | 4 | % CoordP: Proximal coordinates of N limbs [nframes x 3 x N(limbs)], N = 7, 11, 12, 13 5 | % CoordD: Distal coordinates of N limbs [nframes x 3 x N(limbs)], N = 7, 11, 12, 13 6 | 7 | % bodyCOM: coordinate of COM of whole body [n x 3] 8 | % segCOM: coordinate of COM of each limb [nframes x 3 x N(limbs)], N = 7, 11, 12, 13 9 | 10 | Nl = size(CoordP, 3); 11 | ProxLL = CoordP(:,:,1:6); DistLL = CoordD(:,:,1:6); 12 | if Nl ~= 7 13 | ProxUL=CoordP(:,:,7:10); DistUL=CoordD(:,:,7:10); 14 | ProxUB=CoordP(:,:,11:end); DistUB=CoordD(:,:,11:end); 15 | else 16 | ProxUB=CoordP(:,:,7); DistUB=CoordD(:,:,7); 17 | end 18 | 19 | %% COM of limb 20 | % Lower limb 21 | w = reshape(repmat([.433 .433 .5],2,1),1,1,[]); 22 | COMLL = w .* DistLL + (1-w) .* ProxLL; 23 | % Upper limb 24 | if Nl ~= 7 25 | w = reshape(repmat([.436 .682],2,1),1,1,[]); 26 | COMUL = w .* DistUL + (1-w) .* ProxUL; 27 | else 28 | COMUL = []; 29 | end 30 | % Upper body 31 | switch Nl 32 | case 7 33 | COMUB = .626 * DistUB + .374 * ProxUB; 34 | case 11 35 | COMUB = .66 * DistUB + .34 * ProxUB; 36 | case 12 37 | w = reshape([.5 1],1,1,[]); 38 | COMUB = w .* DistUB + (1-w) .* ProxUB; 39 | case 13 40 | w = reshape([.105 .63 1],1,1,[]); 41 | COMUB = w .* DistUB + (1-w) .* ProxUB; 42 | end 43 | segCOM = cat(3, COMLL, COMUL, COMUB); 44 | 45 | %% COM of body 46 | switch Nl 47 | case 7 48 | w = reshape([.1 .1 .0465 .0465 .0145 .0145 .678],1,1,[]); 49 | case 11 50 | w = reshape([.1 .1 .0465 .0465 .0145 .0145 ... 51 | .028 .028 .022 .022 .578],1,1,[]); 52 | case 12 53 | w = reshape([.1 .1 .0465 .0465 .0145 .0145 ... 54 | .028 .028 .022 .022 .497 .081],1,1,[]); 55 | case 13 56 | w = reshape([.1 .1 .0465 .0465 .0145 .0145 ... 57 | .028 .028 .022 .022 .142 .355 .081],1,1,[]); 58 | end 59 | bodyCOM = sum(segCOM .* w, 3); 60 | 61 | end 62 | 63 | -------------------------------------------------------------------------------- /Week07/EulerR.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week07/EulerR.mat -------------------------------------------------------------------------------- /Week07/Figure1.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week07/Figure1.fig -------------------------------------------------------------------------------- /Week07/Figure2.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week07/Figure2.mp4 -------------------------------------------------------------------------------- /Week07/HW07.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week07/HW07.m -------------------------------------------------------------------------------- /Week07/SegCOM_PD.m: -------------------------------------------------------------------------------- 1 | function [ CoordP, CoordD, LLJC ] = SegCOM_PD( MKcoord, mklist, shoulderR, SegNum ) 2 | % Calculate the proximal and distal coordinates of each limb and joint 3 | % centers of lower limbs 4 | 5 | % MKcoord: Position of all markers on human body [nframes x 3 x N(markers)] 6 | % mklist: List of marker names with the sequence of MKcoord {1 x N} 7 | % shoulderR: Radius of left and right shoulders [1 x 2] 8 | % SegNum: Number of limbs for calculation [1 x 1] 9 | 10 | % CoordP: Proximal coordinates of N limbs [nframes x 3 x N(limbs)], N = 7, 11, 12, 13 11 | % CoordD: Distal coordinates of N limbs [nframes x 3 x N(limbs)], N = 7, 11, 12, 13 12 | % LLJC: Joint centers of lower limbs (left to right, hip to knee to ankle) 13 | % [nframes x 3 x 2(sides) x 3(joints)] 14 | 15 | %% Variables assigned 16 | seq = {'RASI','LASI','RPSI','LPSI','L4L5','LPET','RPET','RTRO','RLFC','RMFC','RTHI','RTT','RSHA',... 17 | 'RLMA','RMMA','RTIB','RHEE','RFOO','RTOE','RMTH','LTRO','LLFC','LMFC','LTHI','LTT','LSHA','LLMA',... 18 | 'LMMA','LTIB','LHEE','LFOO','LTOE','LMTH','C7T1','LBTO','LFRM','LHead','LRM','LSAP','LUM','LUPA',... 19 | 'LUS','LWRA','RBTO','RFRM','RHead','RRM','RSAP','RUM','RUPA','RUS','RWRA'}; 20 | for i = 1:length(seq) 21 | index = find(strcmp(mklist,seq(i))); 22 | if isempty(index) 23 | eval(string(seq{i})+'=[];') 24 | else 25 | eval(string(seq{i})+'=MKcoord(:,:,index);') 26 | end 27 | end 28 | %% Equations 29 | LKnee = (LLFC + LMFC)/2; 30 | RKnee = (RLFC + RMFC)/2; 31 | Zvector = (RASI + LASI + RPSI + LPSI)/4 - (LSAP + RSAP)/2; 32 | unitV = Zvector./sqrt(sum(Zvector.^2,2)); 33 | LGH = LSAP + shoulderR(1,1) * unitV; 34 | RGH = RSAP + shoulderR(1,2) * unitV; 35 | LElbow = (LRM + LUM)/2; 36 | RElbow = (RRM + RUM)/2; 37 | GT = (LTRO + RTRO) / 2; 38 | GH = (LGH+RGH)/2; 39 | EarCanal = (LHead + RHead)/2; 40 | %% Upper boddy 41 | switch SegNum 42 | case 7 43 | ProxUB = GT; 44 | DistUB = GH; 45 | case 11 46 | ProxUB = GT; 47 | DistUB = GH; 48 | case 12 49 | ProxUB = cat(3, GT, C7T1); 50 | DistUB = cat(3, GH, EarCanal); 51 | case 13 52 | ProxUB = cat(3, L4L5, C7T1, C7T1); 53 | DistUB = cat(3, GT, L4L5, EarCanal); 54 | otherwise 55 | error('Only 7, 11, 12, 13 can be assigned as number of limbs for calaulation!') 56 | end 57 | %% Upper limb 58 | if SegNum == 7 59 | ProxUL = []; 60 | DistUL = []; 61 | else 62 | ProxUL = cat(3, LGH, RGH, LElbow, RElbow); 63 | DistUL = cat(3, LElbow, RElbow, LUS, RUS); 64 | end 65 | %% Lower limb 66 | ProxLL = cat(3, LTRO, RTRO, LKnee, RKnee, LLMA, RLMA); 67 | DistLL = cat(3, LKnee, RKnee, LMMA, RMMA, LMTH, RMTH); 68 | %% Proximal & Distal 69 | CoordP = cat(3, ProxLL, ProxUL, ProxUB); 70 | CoordD = cat(3, DistLL, DistUL, DistUB); 71 | %% Joint center of lower limb 72 | InterASIS = sqrt(sum((RASI - LASI).^2, 2));%nx1 73 | lHipLoc = permute(InterASIS .* [-.19 -.3 -.36], [2 3 1]); 74 | rHipLoc = permute(InterASIS .* [-.19 -.3 .36], [2 3 1]); 75 | [rRg2p, ~, ~] = CoordPelvis(cat(3, LASI, RPSI, RASI), 'r', {'LASI', 'RPSI', 'RASI'}); 76 | [lRg2p, ~, ~] = CoordPelvis(cat(3, LASI, LPSI, RASI), 'l', {'LASI', 'LPSI', 'RASI'}); 77 | LLJC = cat(4, cat(3, (RASI + LASI)/2 + permute(mtimesx(lRg2p,lHipLoc), [3 1 2]), ...%left hip 78 | (RASI + LASI)/2 + permute(mtimesx(rRg2p,rHipLoc), [3 1 2]))... %right hip 79 | ,cat(3, (LLFC+LMFC)/2, (RLFC+RMFC)/2)...%knee 80 | ,cat(3, (LLMA+LMMA)/2, (RLMA+RMMA)/2));%ankle 81 | end 82 | 83 | -------------------------------------------------------------------------------- /Week07/Walk.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week07/Walk.mat -------------------------------------------------------------------------------- /Week07/WholeBodyCOM.m: -------------------------------------------------------------------------------- 1 | function [ bodyCOM, segCOM, CoordP, CoordD, LLJC ] = WholeBodyCOM( MKcoord, mklist, shoulderR, SegNum ) 2 | % Calculate the COM of each limb and whole body from anthropometric data, the proximal 3 | % and distal coordinates of each limb and joint centers of lower limbs. 4 | 5 | % MKcoord: Position of all markers on human body [nframes x 3 x N(markers)] 6 | % mklist: List of marker names with the sequence of MKcoord {1 x N} 7 | % shoulderR: Radius of left and right shoulders [1 x 2] 8 | % SegNum: Number of limbs for calculation [1 x 1] 9 | 10 | % bodyCOM: coordinate of COM of whole body [n x 3] 11 | % segCOM: coordinate of COM of each limb [nframes x 3 x N(limbs)], N = 7, 11, 12, 13 12 | % CoordP: Proximal coordinates of N limbs [nframes x 3 x N(limbs)], N = 7, 11, 12, 13 13 | % CoordD: Distal coordinates of N limbs [nframes x 3 x N(limbs)], N = 7, 11, 12, 13 14 | % LLJC: Joint centers of lower limbs (left to right, hip to knee to ankle) 15 | % [nframes x 3 x 2(sides) x 3(joints)] 16 | 17 | [ CoordP, CoordD, LLJC ] = SegCOM_PD( MKcoord, mklist, shoulderR, SegNum ); 18 | [ bodyCOM, segCOM ] = BodyCOM_Dempster( CoordP, CoordD ); 19 | end 20 | 21 | -------------------------------------------------------------------------------- /Week08/95.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week08/95.fig -------------------------------------------------------------------------------- /Week08/HW08.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week08/HW08.m -------------------------------------------------------------------------------- /Week08/QuietStance.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week08/QuietStance.mat -------------------------------------------------------------------------------- /Week08/SwayArea.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week08/SwayArea.m -------------------------------------------------------------------------------- /Week09/AngularMomentum.m: -------------------------------------------------------------------------------- 1 | function [ H, dH ] = AngularMomentum( I, AngVel, AngAcc ) 2 | % Calculate the angular momentum and 1st derivative of angular momentum. 3 | 4 | % I: Mass moment of Inertia of each limb [3 x 3 x N(segments)] 5 | % AngVel: Angular velocity of limb [nframes x 3 x N(segments)] 6 | % AngAcc: Angular velocity of limb [nframes x 3 x N(segments)] 7 | 8 | % H: Angular momentum of each limb [nframes x 3 x N(segments)] 9 | % dH: The first derivation of angular momnetum of each limb [nframes x 3 x N(segments)] 10 | 11 | nf = size(AngVel,1); 12 | AngVel = permute(AngVel, [2 4 3 1]);%3 x 1 x N x nf 13 | AngAcc = permute(AngAcc, [2 4 3 1]);%3 x 1 x N x nf 14 | I = repmat(I, 1,1,1,nf);%3 x 3 x N x nf 15 | H = mtimesx(I, AngVel);%3 x 1 x N x nf 16 | dH = mtimesx(I, AngAcc) + cross(AngVel, H); 17 | H = ipermute(H, [2 4 3 1]); 18 | dH = ipermute(dH, [2 4 3 1]); 19 | end 20 | 21 | -------------------------------------------------------------------------------- /Week09/HW09.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Computer Methods in Human Motion Analysis 2017 -- HW9 3 | 4 | % Matlab Version: MATLAB R2017a 5 | % Operating System Ubuntu (Linux) 6 | % Student: Wei-hsiang Wang 7 | % Department: Mechanical Engineering 8 | % Student ID: R05522625 9 | 10 | addpath(genpath(fileparts(cd))) % adding all hw directory to PATH. 11 | 12 | %% Initialization 13 | clc; 14 | clearvars; 15 | % close all; 16 | 17 | %% Practice 1 18 | disp('[Practice 1]') 19 | disp('See the funciton LLCOM2PD.m') 20 | open LLCOM2PD.m 21 | disp(' ') 22 | 23 | %% Practice 2 24 | disp('[Practice 2]') 25 | load('subcali.mat') 26 | 27 | InterASIS = sqrt(sum((RASI - LASI).^2, 2)); 28 | lHipLoc = InterASIS .* [-.19 -.3 -.36]; 29 | rHipLoc = InterASIS .* [-.19 -.3 .36]; 30 | [rRg2p, ~, ~] = CoordPelvis(cat(3, LASI, RPSI, RASI), 'r', {'LASI', 'RPSI', 'RASI'}); 31 | [lRg2p, ~, ~] = CoordPelvis(cat(3, LASI, LPSI, RASI), 'l', {'LASI', 'LPSI', 'RASI'}); 32 | lHip = (RASI + LASI)/2 + lHipLoc * lRg2p; 33 | rHip = (RASI + LASI)/2 + rHipLoc * rRg2p; 34 | lKnee = (LLFC+LMFC)/2; 35 | rKnee = (RLFC+RMFC)/2; 36 | lAnkle = (LLMA+LMMA)/2; 37 | rAnkle = (RLMA+RMMA)/2; 38 | 39 | CoordP = cat(3, lHip, rHip, lKnee, rKnee, lAnkle, rAnkle); 40 | CoordD = cat(3, CoordP(:,:,3:end), LBTO, RBTO); 41 | BW = 73.5; 42 | 43 | [ Ms, Is ] = LLInertia( BW, CoordP, CoordD ); 44 | Is = Is/1000000; 45 | disp('Mass & Inertia of each limb are saved') 46 | disp(' ') 47 | 48 | %% Practice 3 49 | disp('[Practice 3]') 50 | load('DataQ1.mat') 51 | smprate = 120; 52 | lRg2t = CoordThigh(cat(3, LLFC, LTRO, LMFC), 'l', {'LLFC', 'LTRO', 'LMFC'}); 53 | rRg2t = CoordThigh(cat(3, RTRO, RMFC, RLFC), 'r', {'RTRO', 'RMFC', 'RLFC'}); 54 | lRg2s = CoordShank(cat(3, LTT, LLMA, LMMA, LSHA), 'l', {'LTT', 'LLMA', 'LMMA', 'LSHA'}); 55 | rRg2s = CoordShank(cat(3, RTT, RSHA, RLMA, RMMA), 'r', {'RTT', 'RSHA', 'RLMA', 'RMMA'}); 56 | lRg2f = CoordFoot(cat(3, LHEE, LTOE, LFOO), 'l', {'LHEE', 'LTOE', 'LFOO'}); 57 | rRg2f = CoordFoot(cat(3, RFOO, RHEE, RTOE), 'r', {'RFOO', 'RHEE', 'RTOE'}); 58 | AllLimb = cat(4, lRg2t, rRg2t, lRg2s, rRg2s, lRg2f, rRg2f); 59 | [ AngVel, AngAcc ] = Rot2LocalAngularEP( AllLimb, smprate ); 60 | [ H, dH ] = AngularMomentum( Is, AngVel, AngAcc ); 61 | 62 | nframes = size(H, 1); 63 | data = H; 64 | tilname = {'Angular Momentum', '1st Derivative of Angular Momentum'}; 65 | lgd = {'Hx', 'Hy', 'Hz'}; 66 | for i = 1:2 67 | artiname = {'lThigh','rThigh','lShank','rShank','lFoot','rFoot'}; 68 | figure('Name',[char(tilname(i)),' of Lower Limbs'], 'NumberTitle','off','position',[-800+800*i 50 800 700]); 69 | ylbl = {'Ang. Momemtum (kg-m^2/s)','1st der. (kg-m^2/s^2)'}; 70 | for j = 1:6 71 | eval(['ax',num2str(j)]) = subplot(3,2,j); 72 | hold on 73 | title(artiname(j)) 74 | a = plot( 1:nframes, data(:,1,j), 'b'); 75 | b = plot( 1:nframes, data(:,2,j), 'g'); 76 | c = plot( 1:nframes, data(:,3,j), 'r'); 77 | ylabel(ylbl(i)) 78 | xlabel('frames') 79 | xlim([0 nframes]) 80 | if j == 2 81 | Leg = legend([a; b; c], lgd); 82 | Pos = get(Leg, 'Position'); 83 | set(Leg, 'Position', [Pos(1)+0.1, Pos(2)+0.09, Pos(3), Pos(4)]) 84 | end 85 | end 86 | lgd = {'dHx', 'dHy', 'dHz'}; 87 | data = dH; 88 | end 89 | disp('Fig. 1 & 2 plotted.') -------------------------------------------------------------------------------- /Week09/LLCOM2PD.m: -------------------------------------------------------------------------------- 1 | function [ rCOM2P, rCOM2D, segCOMAcc ] = LLCOM2PD( segCOM, LLJC, nCOP, smprate ) 2 | % Calculate the vectors pointing from COM to distal and proxinal points (ceter of joint) 3 | % of each limb and the acceleration of the COM. 4 | 5 | % segCOM: Coordinate of COM of each limb [nframes x 3 x 6(limbs)] 6 | % LLJC: Joint centers of lower limbs (left to right, hip to knee to ankle) 7 | % [nframes x 3 x 2(sides) x 3(joints)] 8 | % nCOP: The COP positon derived from each force plate relative to the 9 | % global coordinate. (Left foor first, and then right foot) 10 | % [nframes x 3 x 2 (feet)] 11 | % smprate: sampling rate. [1 x 1] 12 | 13 | % rCOM2P: The vectors pointing from COMs to proxinal points. (unit: m) 14 | % [nframes x 3 x 2(sides) x 3(segment)] 15 | % rCOM2D: The vectors pointing from COMs to distal points. (unit: m) 16 | % [nframes x 3 x 2(sides) x 3(segment)] 17 | % segCOMAcc: The acceleration of COM on each limb. (unit: m/sec^2) 18 | % [nframes x 3 x 2(sides) x 3(segment)] 19 | 20 | %% Testng whether the variables have same no. of frames. 21 | % disp(size(segCOM)) 22 | % disp(size(LLJC)) 23 | % disp(size(nCOP)) 24 | if ~isequal(size(segCOM,1),size(LLJC,1),size(nCOP,1)) 25 | error("The variables have different no. of frames!") 26 | else 27 | nframes = size(nCOP,1); 28 | end 29 | MsegCOM = permute(cat(4, segCOM(:,:,[1,3,5]), segCOM(:,:,[2,4,6])),[1 2 4 3]); 30 | rCOM2P = LLJC - MsegCOM; 31 | 32 | %% Distinguish the force plate correspong to left and right foot (voting by algo.) 33 | vote = 0; 34 | weight = [0.3 1]; 35 | % method 1: frame index of whose value is NaN (fist and last) 36 | COP1 = nCOP(:,1,1); COP2 = nCOP(:,1,2); 37 | iCOP1 = find(isnan(COP1)); iCOP2 = find(isnan(COP2)); 38 | FirstOcc = [min(iCOP1);min(iCOP2)]; 39 | LastOcc = [max(iCOP1);max(iCOP2)]; 40 | if ~isnan(FirstOcc) 41 | if (FirstOcc(2) norm(avgLAnk-avgCOP2)+norm(avgRAnk-avgCOP1) 69 | vote = vote + weight(2); 70 | else 71 | vote = vote - weight(2); 72 | end 73 | 74 | % Result 75 | if vote >= 0 76 | segD = cat(4, LLJC(:,:,:,2:3), cat(3,COP2,COP1)); 77 | else 78 | segD = cat(4, LLJC(:,:,:,2:3), cat(3,COP1,COP2)); 79 | end 80 | 81 | rCOM2D = segD - MsegCOM; 82 | 83 | %% Acceleration 84 | % time sequence 85 | TimeSeq = reshape(1/smprate:1/smprate:size(LLJC,1)/smprate,[],1);%nx1 86 | segCOMAcc = zeros(nframes, 3, 2, 3); 87 | for i = 1:3 % 4th dimen.: seg. 88 | for j = 1:2 % 3rd dimen.: side 89 | for k = 1:3 % 2nd dimen.: xyz 90 | segCOMAcc(:,k,j,i) = Derivative(TimeSeq, MsegCOM(:,k,j,i), 2); 91 | end 92 | end 93 | end 94 | 95 | end 96 | 97 | -------------------------------------------------------------------------------- /Week09/LLInertia.m: -------------------------------------------------------------------------------- 1 | function [ Ms, Is ] = LLInertia( BW, CoordP, CoordD ) 2 | % Calculate the mass and inertia of each limb 3 | % (shape of limb is assumed to be a line with mass) 4 | 5 | % BW:Body weight of the subject.(unit: kg) [1 x 1] 6 | % CoordP: Coordinates of the proximal point on each (lower) limb [1 x 3 x 6(limb)] 7 | % CoordD: Coordinates of the diatal point on each (lower) limb [1 x 3 x 6(limb)] 8 | % (sequence: left thigh. right thigh, left knee, right knee, left anke, right ankle) 9 | 10 | % Ms: Mass of each limb (unit: kg) [2(sides) x 3(limb)] 11 | % Is: Inertia of each limb (unit: kg-m^2) [3 x 3 x 6(limb)] 12 | 13 | M = BW * [.1 .1 .0465 .0465 .0145 .0145]; 14 | Ms = reshape(M, 2, 3); 15 | 16 | C = [.323 .323 .302 .302 .475 .475]; 17 | I = M .* (C.^2) .* reshape(sum( (((CoordD-CoordP)).^2), 2 ), 1, []); 18 | I = permute(I, [1 3 2]); 19 | 20 | Is = zeros(3, 3, 6); 21 | Is(1,1,1:4) = I(1,1,1:4); 22 | Is(2,2,5:6) = I(1,1,5:6); 23 | Is(3,3,:) = I; 24 | 25 | end 26 | 27 | -------------------------------------------------------------------------------- /Week09/result/Ang1derMom.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week09/result/Ang1derMom.fig -------------------------------------------------------------------------------- /Week09/result/AngMom.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/Week09/result/AngMom.fig -------------------------------------------------------------------------------- /Week10/HW10.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Computer Methods in Human Motion Analysis 2017 -- HW9 3 | 4 | % Matlab Version: MATLAB R2017a 5 | % Operating System Ubuntu (Linux) 6 | % Student: Wei-hsiang Wang 7 | % Department: Mechanical Engineering 8 | % Student ID: R05522625 9 | 10 | addpath(genpath(fileparts(cd))) % adding all hw directory to PATH. 11 | 12 | %% Initialization 13 | clc; 14 | clearvars; 15 | close all; 16 | 17 | %% variables 18 | smprate = 120; 19 | Mass = 73.5; %kg 20 | g = 9.81; 21 | LL = 0.85; %m 22 | load('subcali.mat') 23 | 24 | %% Ms 25 | InterASIS = sqrt(sum((RASI - LASI).^2, 2)); 26 | lHipLoc = InterASIS .* [-.19 -.3 -.36]; 27 | rHipLoc = InterASIS .* [-.19 -.3 .36]; 28 | [rRg2p, ~, ~] = CoordPelvis(cat(3, LASI, RPSI, RASI), 'r', {'LASI', 'RPSI', 'RASI'}); 29 | [lRg2p, ~, ~] = CoordPelvis(cat(3, LASI, LPSI, RASI), 'l', {'LASI', 'LPSI', 'RASI'}); 30 | lHip = (RASI + LASI)/2 + lHipLoc * lRg2p; 31 | rHip = (RASI + LASI)/2 + rHipLoc * rRg2p; 32 | lKnee = (LLFC+LMFC)/2; rKnee = (RLFC+RMFC)/2; 33 | lAnkle = (LLMA+LMMA)/2; rAnkle = (RLMA+RMMA)/2; 34 | CoordP = cat(3, lHip, rHip, lKnee, rKnee, lAnkle, rAnkle); 35 | CoordD = cat(3, CoordP(:,:,3:end), LBTO, RBTO); 36 | [ Ms, Is ] = LLInertia( Mass, CoordP, CoordD); 37 | Is = Is / 1000000; % kg mm^2 -> kg m^2 38 | Ms = Ms(:); 39 | 40 | %% dH 41 | load('DataQ1.mat') 42 | lRg2t = CoordThigh(cat(3, LLFC, LTRO, LMFC), 'l', {'LLFC', 'LTRO', 'LMFC'}); 43 | rRg2t = CoordThigh(cat(3, RTRO, RMFC, RLFC), 'r', {'RTRO', 'RMFC', 'RLFC'}); 44 | lRg2s = CoordShank(cat(3, LTT, LLMA, LMMA, LSHA), 'l', {'LTT', 'LLMA', 'LMMA', 'LSHA'}); 45 | rRg2s = CoordShank(cat(3, RTT, RSHA, RLMA, RMMA), 'r', {'RTT', 'RSHA', 'RLMA', 'RMMA'}); 46 | lRg2f = CoordFoot(cat(3, LHEE, LTOE, LFOO), 'l', {'LHEE', 'LTOE', 'LFOO'}); 47 | rRg2f = CoordFoot(cat(3, RFOO, RHEE, RTOE), 'r', {'RFOO', 'RHEE', 'RTOE'}); 48 | AllLimbRg2l = cat(4, lRg2t, rRg2t, lRg2s, rRg2s, lRg2f, rRg2f); 49 | [ AngVel, AngAcc ] = Rot2LocalAngularEP( AllLimbRg2l, smprate ); 50 | [ H, dH ] = AngularMomentum( Is, AngVel, AngAcc ); 51 | 52 | %% segmentCOMAcc, rCOM2P, rCOM2D 53 | % nCOP 54 | Fthd = 5; Mthd = 80; 55 | fpF_local1 = [AVGfilt(Fx1, Fthd) AVGfilt(Fy1, Fthd), AVGfilt(Fz1, Fthd)]; 56 | fpF_local2 = [AVGfilt(Fx2, Fthd) AVGfilt(Fy2, Fthd), AVGfilt(Fz2, Fthd)]; 57 | % fpF_local1 = [Fx1,Fy1,Fz1]; 58 | % fpF_local2 = [Fx2,Fy2,Fz2]; 59 | fpM_local1 = [AVGfilt(Mx1, Mthd) AVGfilt(My1, Mthd), AVGfilt(Mz1, Mthd)]; 60 | fpM_local2 = [AVGfilt(Mx2, Mthd) AVGfilt(My2, Mthd), AVGfilt(Mz2, Mthd)]; 61 | corners1 = [0, 508, 0;... 62 | 464, 508, 0;... 63 | 464, 0, 0;... 64 | 0, 0, 0]; 65 | 66 | corners2 = [464, 511, 0;... 67 | 0, 511, 0;... 68 | 0, 1019, 0;... 69 | 464, 1019, 0]; 70 | Vfp2tc1 = [ -0.156, 0.995, -43.574 ]; 71 | Vfp2tc2 = [ 0.195, 1.142, -41.737 ]; 72 | fpF_local = cat(3, fpF_local1, fpF_local2); 73 | fpM_local = cat(3, fpM_local1, fpM_local2); 74 | corners = cat(3, corners1, corners2); 75 | Vfp2tc = cat(1, Vfp2tc1, Vfp2tc2); 76 | Pz = Vfp2tc(:,3).'; 77 | [ gCOP1, fpFg1, Rg2fp1, Vg2fp1 ] = ForcePlate( fpF_local1, fpM_local1, corners1, Vfp2tc1, Pz(1) ); 78 | [ gCOP2, fpFg2, Rg2fp2, Vg2fp2 ] = ForcePlate( fpF_local2, fpM_local2, corners2, Vfp2tc2, Pz(2) ); 79 | [ gCOP, nCOP, fpFg, Rg2fp, Vg2fp ] = ForcePlateN( fpF_local, fpM_local, corners, Vfp2tc, Pz ); 80 | fpMg1 = fpM_local1*Rg2fp1.'; %fpMg1(isnan(fpMg1)) = 0; 81 | fpMg2 = fpM_local2*Rg2fp2.'; %fpMg2(isnan(fpMg2)) = 0; 82 | 83 | % COM of lower limb 84 | LKnee = (LLFC + LMFC)/2; 85 | RKnee = (RLFC + RMFC)/2; 86 | ProxLL = cat(3, LTRO, RTRO, LKnee, RKnee, LLMA, RLMA); 87 | DistLL = cat(3, LKnee, RKnee, LMMA, RMMA, LBTO, RBTO); 88 | w = reshape(repmat([.433 .433 .5],2,1),1,1,[]); 89 | COMLL = w .* DistLL + (1-w) .* ProxLL; 90 | 91 | % Joint center of lower limb 92 | InterASIS = sqrt(sum((RASI - LASI).^2, 2));%nx1 93 | lHipLoc = permute(InterASIS .* [-.19 -.3 -.36], [2 3 1]); 94 | rHipLoc = permute(InterASIS .* [-.19 -.3 .36], [2 3 1]); 95 | [rRg2p, ~, ~] = CoordPelvis(cat(3, LASI, RPSI, RASI), 'r', {'LASI', 'RPSI', 'RASI'}); 96 | [lRg2p, ~, ~] = CoordPelvis(cat(3, LASI, LPSI, RASI), 'l', {'LASI', 'LPSI', 'RASI'}); 97 | LLJC = cat(4, cat(3, (RASI + LASI)/2 + permute(mtimesx(lRg2p,lHipLoc), [3 1 2]), ...%left hip 98 | (RASI + LASI)/2 + permute(mtimesx(rRg2p,rHipLoc), [3 1 2]))... %right hip 99 | ,cat(3, (LLFC+LMFC)/2, (RLFC+RMFC)/2)...%knee 100 | ,cat(3, (LLMA+LMMA)/2, (RLMA+RMMA)/2));%ankle 101 | 102 | [ rCOM2P, rCOM2D, segCOMAcc ] = LLCOM2PD( COMLL, LLJC, nCOP, smprate ); 103 | rCOP2P = cat(3, rCOM2P(:,:,:,1), rCOM2P(:,:,:,2), rCOM2P(:,:,:,3)); 104 | rCOP2D = cat(3, rCOM2D(:,:,:,1), rCOM2D(:,:,:,2), rCOM2D(:,:,:,3)); 105 | segCOMAcc = cat(3, segCOMAcc(:,:,:,1), segCOMAcc(:,:,:,2), segCOMAcc(:,:,:,3)); 106 | 107 | %% Force & Moment of segments 108 | 109 | segCOMAcc = segCOMAcc/1000; % mm/sec^2 -> m/sec^2 110 | Fd_fp = cat(3, fpFg2, fpFg1); % N 111 | Md_fp = cat(3, fpMg2, fpMg1)/1000; % Nmm -> Nm %%%????????????????????????????????????????????? /1000 ?????????????????????????????????????????????? 112 | rCOM2P = rCOM2P/1000; % mm -> m 113 | rCOM2D = rCOM2D/1000; % mm -> m 114 | [ Fp_local_Ak, Mp_local_Ak, Fp_Ak, Mp_Ak ] = JointForceMoment( AllLimbRg2l(:,:,:,5:6), Ms(5:6), segCOMAcc(:,:,5:6), dH(:,:,5:6), rCOM2P(:,:,5:6), rCOM2D(:,:,5:6), Fd_fp, 0); 115 | [ Fp_local_Kn, Mp_local_Kn, Fp_Kn, Mp_Kn ] = JointForceMoment( AllLimbRg2l(:,:,:,3:4), Ms(3:4), segCOMAcc(:,:,3:4), dH(:,:,3:4), rCOM2P(:,:,3:4), rCOM2D(:,:,3:4), -Fp_Ak, -Mp_Ak ); 116 | [ Fp_local_Hp, Mp_local_Hp, Fp_Hp, Mp_Hp ] = JointForceMoment( AllLimbRg2l(:,:,:,1:2), Ms(1:2), segCOMAcc(:,:,1:2), dH(:,:,1:2), rCOM2P(:,:,1:2), rCOM2D(:,:,1:2), -Fp_Kn, -Mp_Kn ); 117 | Fp_local = cat(4, Fp_local_Hp, Fp_local_Kn, Fp_local_Ak); 118 | Mp_local = cat(4, Mp_local_Hp, Mp_local_Kn, Mp_local_Ak); 119 | 120 | %% plotting 121 | % F = figure('Name','Force Applied at the Lower Limb Joints', 'NumberTitle','off','position',[0 50 1680 750]); 122 | % M = figure('Name','Moment Applied at the Lower Limb Joints', 'NumberTitle','off','position',[0 50 1680 750]); 123 | dirname = {'Anterior(+)/Posteriro(-)','Superior(+)/Inferior(-)','Lateral(+)/Medial(-)'}; 124 | artiname = {'Hip','Knee','Ankle'}; 125 | %open Joint_Force.fig; 126 | for i = 1:3 127 | for j = 1:3 128 | figure(1) 129 | subplot(3,3,(i-1)*3+j) 130 | hold on 131 | if j == 3 132 | data = cat(3,-Fp_local(:,j,1,i), Fp_local(:,j,2,i))/Mass/g; 133 | else 134 | data = cat(3, Fp_local(:,j,1,i), Fp_local(:,j,2,i))/Mass/g; 135 | end 136 | if i == 1 137 | title(dirname(j)) 138 | end 139 | if j == 1 140 | ylabel([artiname{i},' (N/BW)']) 141 | end 142 | xlim([0 159]) 143 | ylim([min(data(:)) max(data(:))]) 144 | hold on 145 | a=plot(1:159,data(:,:,1,:),'b'); 146 | b=plot(1:159,data(:,:,2,:),'r'); 147 | legend([a; b], {'Left','Right'}); 148 | end 149 | end 150 | 151 | figure 152 | %open Joint_Moment.fig; 153 | dirname = {'Abd(+)/Adduction(-)','Internal(+)/External(-) rotation','Flex(+)/Extension(-)'}; 154 | for i = 1:3 155 | for j = 1:3 156 | subplot(3,3,(i-1)*3+j) 157 | hold on 158 | if j == 1 159 | data = cat(3, Mp_local(:,j,1,i), -Mp_local(:,j,2,i))/Mass/g/LL; 160 | end 161 | if j == 2 162 | data = cat(3, -Mp_local(:,j,1,i), Mp_local(:,j,2,i))/Mass/g/LL; 163 | 164 | end 165 | if j == 3 166 | data = cat(3, Mp_local(:,j,1,i), Mp_local(:,j,2,i))/Mass/g/LL; 167 | if i == 2 168 | data = cat(3, -Mp_local(:,j,1,i), -Mp_local(:,j,2,i))/Mass/g/LL; 169 | end 170 | end 171 | % if j >0 172 | % data = cat(3, Mp_local(:,j,1,i), Mp_local(:,j,2,i))/Mass/g/LL; 173 | % end 174 | if i == 1 175 | title(dirname(j)) 176 | end 177 | if j == 1 178 | ylabel([artiname{i},' (Nm/BW/LL)']) 179 | end 180 | xlim([0 159]) 181 | ylim([min(data(:)) max(data(:))]) 182 | hold on 183 | a=plot(1:159,data(:,:,1,:),'b'); 184 | b=plot(1:159,data(:,:,2,:),'r'); 185 | legend([a; b], {'Left','Right'}); 186 | end 187 | end 188 | % open Joint_Moment.fig; 189 | % close all 190 | function [ Xf ]= AVGfilt( X, thd) 191 | % Function: offset the data to eliminate the means of error and the data 192 | % with absolute values under the threshold. 193 | % thd : threshold 194 | 195 | s = size(X); 196 | Is = abs(X) 3 1 n N 47 | % permute(Rg2s,[2 1 3 4]) -> Rg2s.' 3 3 n N 3 1 n N -> n 3 N 48 | % Mp = permute( permute(mtimesx(permute(Rg2s,[1 2 3 4]),permute(dH,[2 4 1 3])),[3 1 4 2]) , [4 2 1 3]); %1x3xnxN 49 | % Mp = permute( - Md , [4 2 1 3]); %1x3xnxN 50 | % Mp = permute( - cross(rCOM2D,Fd), [4 2 1 3]); %1x3xnxN 51 | % Mp = permute( - cross(rCOM2P,Fp), [4 2 1 3]); %1x3xnxN 52 | % Mp = permute( permute(mtimesx(permute(Rg2s,[1 2 3 4]),permute(dH,[2 4 1 3])),[3 1 4 2]) - cross(rCOM2D,Fd)- cross(rCOM2P,Fp), [4 2 1 3]); %1x3xnxN 53 | % Mp = permute( - cross(rCOM2D,Fd)- cross(rCOM2P,Fp) -Md , [4 2 1 3]); %1x3xnxN 54 | Mp = permute( permute(mtimesx(permute(Rg2s,[2 1 3 4]),permute(dH,[2 4 1 3])),[3 1 4 2]) - Md - cross(rCOM2D,Fd) - cross(rCOM2P,Fp), [4 2 1 3]); %1x3xnxN 55 | 56 | % Mp = permute( permute(mtimesx(permute(dH,[4 2 1 3]),Rg2s),[3 2 4 1 ] ) - Md - cross(rCOM2D,Fd) - cross(rCOM2P,Fp), [4 2 1 3]); %1x3xnxN 57 | Mp_local = permute( mtimesx(Mp,Rg2s), [3 2 4 1]); 58 | Mp = ipermute(Mp, [4 2 1 3]); 59 | 60 | end 61 | 62 | -------------------------------------------------------------------------------- /_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-cayman -------------------------------------------------------------------------------- /final/Ans/COP.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/final/Ans/COP.fig -------------------------------------------------------------------------------- /final/Ans/Inclination Angle.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/final/Ans/Inclination Angle.fig -------------------------------------------------------------------------------- /final/Ans/Joint Angle.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/final/Ans/Joint Angle.fig -------------------------------------------------------------------------------- /final/Ans/Joint_Moment.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/final/Ans/Joint_Moment.fig -------------------------------------------------------------------------------- /final/DataSTS.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/final/DataSTS.mat -------------------------------------------------------------------------------- /final/DataSTS.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/final/DataSTS.zip -------------------------------------------------------------------------------- /final/final.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Computer Methods in Human Motion Analysis 2017 -- HW9 3 | 4 | % Matlab Version: MATLAB R2017a 5 | % Operating System Ubuntu (Linux) 6 | % Student: Wei-hsiang Wang 7 | % Department: Mechanical Engineering 8 | % Student ID: R05522625 9 | 10 | addpath(genpath(fileparts(cd))) % adding all hw directory to PATH. 11 | 12 | %% Initialization 13 | clc; 14 | clearvars; 15 | close all; 16 | 17 | %% variables 18 | load('subcali.mat') 19 | 20 | %% Ms 21 | InterASIS = sqrt(sum((RASI - LASI).^2, 2)); 22 | lHipLoc = InterASIS .* [-.19 -.3 -.36]; 23 | rHipLoc = InterASIS .* [-.19 -.3 .36]; 24 | [rRg2p, ~, ~] = CoordPelvis(cat(3, LASI, RPSI, RASI), 'r', {'LASI', 'RPSI', 'RASI'}); 25 | [lRg2p, ~, ~] = CoordPelvis(cat(3, LASI, LPSI, RASI), 'l', {'LASI', 'LPSI', 'RASI'}); 26 | lHip = (RASI + LASI)/2 + lHipLoc * lRg2p; 27 | rHip = (RASI + LASI)/2 + rHipLoc * rRg2p; 28 | lKnee = (LLFC+LMFC)/2; rKnee = (RLFC+RMFC)/2; 29 | lAnkle = (LLMA+LMMA)/2; rAnkle = (RLMA+RMMA)/2; 30 | CoordP = cat(3, lHip, rHip, lKnee, rKnee, lAnkle, rAnkle); 31 | CoordD = cat(3, CoordP(:,:,3:end), LBTO, RBTO); 32 | [ Ms, Is ] = LLInertia( Mass, CoordP, CoordD); 33 | Is = Is / 1000000; % kg mm^2 -> kg m^2 34 | Ms = Ms(:); 35 | 36 | %% dH 37 | load('DataQ1.mat') 38 | lRg2t = CoordThigh(cat(3, LLFC, LTRO, LMFC), 'l', {'LLFC', 'LTRO', 'LMFC'}); 39 | rRg2t = CoordThigh(cat(3, RTRO, RMFC, RLFC), 'r', {'RTRO', 'RMFC', 'RLFC'}); 40 | lRg2s = CoordShank(cat(3, LTT, LLMA, LMMA, LSHA), 'l', {'LTT', 'LLMA', 'LMMA', 'LSHA'}); 41 | rRg2s = CoordShank(cat(3, RTT, RSHA, RLMA, RMMA), 'r', {'RTT', 'RSHA', 'RLMA', 'RMMA'}); 42 | lRg2f = CoordFoot(cat(3, LHEE, LTOE, LFOO), 'l', {'LHEE', 'LTOE', 'LFOO'}); 43 | rRg2f = CoordFoot(cat(3, RFOO, RHEE, RTOE), 'r', {'RFOO', 'RHEE', 'RTOE'}); 44 | AllLimbRg2l = cat(4, lRg2t, rRg2t, lRg2s, rRg2s, lRg2f, rRg2f); 45 | [ AngVel, AngAcc ] = Rot2LocalAngularEP( AllLimbRg2l, smprate ); 46 | [ H, dH ] = AngularMomentum( Is, AngVel, AngAcc ); 47 | 48 | %% segmentCOMAcc, rCOM2P, rCOM2D 49 | % nCOP 50 | Fthd = 5; Mthd = 80; 51 | fpF_local1 = [AVGfilt(Fx1, Fthd) AVGfilt(Fy1, Fthd), AVGfilt(Fz1, Fthd)]; 52 | fpF_local2 = [AVGfilt(Fx2, Fthd) AVGfilt(Fy2, Fthd), AVGfilt(Fz2, Fthd)]; 53 | % fpF_local1 = [Fx1,Fy1,Fz1]; 54 | % fpF_local2 = [Fx2,Fy2,Fz2]; 55 | fpM_local1 = [AVGfilt(Mx1, Mthd) AVGfilt(My1, Mthd), AVGfilt(Mz1, Mthd)]; 56 | fpM_local2 = [AVGfilt(Mx2, Mthd) AVGfilt(My2, Mthd), AVGfilt(Mz2, Mthd)]; 57 | corners1 = [0, 508, 0;... 58 | 464, 508, 0;... 59 | 464, 0, 0;... 60 | 0, 0, 0]; 61 | 62 | corners2 = [464, 511, 0;... 63 | 0, 511, 0;... 64 | 0, 1019, 0;... 65 | 464, 1019, 0]; 66 | Vfp2tc1 = [ -0.156, 0.995, -43.574 ]; 67 | Vfp2tc2 = [ 0.195, 1.142, -41.737 ]; 68 | fpF_local = cat(3, fpF_local1, fpF_local2); 69 | fpM_local = cat(3, fpM_local1, fpM_local2); 70 | corners = cat(3, corners1, corners2); 71 | Vfp2tc = cat(1, Vfp2tc1, Vfp2tc2); 72 | Pz = Vfp2tc(:,3).'; 73 | [ gCOP1, fpFg1, Rg2fp1, Vg2fp1 ] = ForcePlate( fpF_local1, fpM_local1, corners1, Vfp2tc1, Pz(1) ); 74 | [ gCOP2, fpFg2, Rg2fp2, Vg2fp2 ] = ForcePlate( fpF_local2, fpM_local2, corners2, Vfp2tc2, Pz(2) ); 75 | [ gCOP, nCOP, fpFg, Rg2fp, Vg2fp ] = ForcePlateN( fpF_local, fpM_local, corners, Vfp2tc, Pz ); 76 | fpMg1 = fpM_local1*Rg2fp1.'; %fpMg1(isnan(fpMg1)) = 0; 77 | fpMg2 = fpM_local2*Rg2fp2.'; %fpMg2(isnan(fpMg2)) = 0; 78 | 79 | % COM of lower limb 80 | LKnee = (LLFC + LMFC)/2; 81 | RKnee = (RLFC + RMFC)/2; 82 | ProxLL = cat(3, LTRO, RTRO, LKnee, RKnee, LLMA, RLMA); 83 | DistLL = cat(3, LKnee, RKnee, LMMA, RMMA, LBTO, RBTO); 84 | w = reshape(repmat([.433 .433 .5],2,1),1,1,[]); 85 | COMLL = w .* DistLL + (1-w) .* ProxLL; 86 | 87 | % Joint center of lower limb 88 | InterASIS = sqrt(sum((RASI - LASI).^2, 2));%nx1 89 | lHipLoc = permute(InterASIS .* [-.19 -.3 -.36], [2 3 1]); 90 | rHipLoc = permute(InterASIS .* [-.19 -.3 .36], [2 3 1]); 91 | [rRg2p, ~, ~] = CoordPelvis(cat(3, LASI, RPSI, RASI), 'r', {'LASI', 'RPSI', 'RASI'}); 92 | [lRg2p, ~, ~] = CoordPelvis(cat(3, LASI, LPSI, RASI), 'l', {'LASI', 'LPSI', 'RASI'}); 93 | LLJC = cat(4, cat(3, (RASI + LASI)/2 + permute(mtimesx(lRg2p,lHipLoc), [3 1 2]), ...%left hip 94 | (RASI + LASI)/2 + permute(mtimesx(rRg2p,rHipLoc), [3 1 2]))... %right hip 95 | ,cat(3, (LLFC+LMFC)/2, (RLFC+RMFC)/2)...%knee 96 | ,cat(3, (LLMA+LMMA)/2, (RLMA+RMMA)/2));%ankle 97 | 98 | [ rCOM2P, rCOM2D, segCOMAcc ] = LLCOM2PD( COMLL, LLJC, nCOP, smprate ); 99 | rCOP2P = cat(3, rCOM2P(:,:,:,1), rCOM2P(:,:,:,2), rCOM2P(:,:,:,3)); 100 | rCOP2D = cat(3, rCOM2D(:,:,:,1), rCOM2D(:,:,:,2), rCOM2D(:,:,:,3)); 101 | segCOMAcc = cat(3, segCOMAcc(:,:,:,1), segCOMAcc(:,:,:,2), segCOMAcc(:,:,:,3)); 102 | 103 | %% Force & Moment of segments 104 | segCOMAcc = segCOMAcc/1000; % mm/sec^2 -> m/sec^2 105 | Fd_fp = cat(3, fpFg2, fpFg1); % N 106 | Md_fp = cat(3, fpMg2, fpMg1)/1000000; % Nmm -> Nm %%%????????????????????????????????????????????? /1000 ?????????????????????????????????????????????? 107 | rCOM2P = rCOM2P/1000; % mm -> m 108 | rCOM2D = rCOM2D/1000; % mm -> m 109 | [ Fp_local_Ak, Mp_local_Ak, Fp_Ak, Mp_Ak ] = JointForceMoment( AllLimbRg2l(:,:,:,5:6), Ms(5:6), segCOMAcc(:,:,5:6), dH(:,:,5:6), rCOM2P(:,:,5:6), rCOM2D(:,:,5:6), Fd_fp, Md_fp ); 110 | [ Fp_local_Kn, Mp_local_Kn, Fp_Kn, Mp_Kn ] = JointForceMoment( AllLimbRg2l(:,:,:,3:4), Ms(3:4), segCOMAcc(:,:,3:4), dH(:,:,3:4), rCOM2P(:,:,3:4), rCOM2D(:,:,3:4), -Fp_Ak, -Mp_Ak ); 111 | [ Fp_local_Hp, Mp_local_Hp, Fp_Hp, Mp_Hp ] = JointForceMoment( AllLimbRg2l(:,:,:,1:2), Ms(1:2), segCOMAcc(:,:,1:2), dH(:,:,1:2), rCOM2P(:,:,1:2), rCOM2D(:,:,1:2), -Fp_Kn, -Mp_Kn ); 112 | Fp_local = cat(4, Fp_local_Hp, Fp_local_Kn, Fp_local_Ak); 113 | Mp_local = cat(4, Mp_local_Hp, Mp_local_Kn, Mp_local_Ak); 114 | 115 | %% plotting 116 | % F = figure('Name','Force Applied at the Lower Limb Joints', 'NumberTitle','off','position',[0 50 1680 750]); 117 | % M = figure('Name','Moment Applied at the Lower Limb Joints', 'NumberTitle','off','position',[0 50 1680 750]); 118 | dirname = {'Anterior(+)/Posteriro(-)','Superior(+)/Inferior(-)','Lateral(+)/Medial(-)'}; 119 | artiname = {'Hip','Knee','Ankle'}; 120 | open Joint_Force.fig; 121 | for i = 1:3 122 | for j = 1:3 123 | figure(1) 124 | subplot(3,3,(i-1)*3+j) 125 | hold on 126 | if j == 3 127 | data = cat(3,-Fp_local(:,j,1,i), Fp_local(:,j,2,i))/Mass/g; 128 | else 129 | data = cat(3, Fp_local(:,j,1,i), Fp_local(:,j,2,i))/Mass/g; 130 | end 131 | if i == 1 132 | title(dirname(j)) 133 | end 134 | if j == 1 135 | ylabel([artiname{i},' (N/BW)']) 136 | end 137 | xlim([0 159]) 138 | ylim([min(data(:)) max(data(:))]) 139 | hold on 140 | a=plot(1:159,data(:,:,1,:),'b--'); 141 | b=plot(1:159,data(:,:,2,:),'r--'); 142 | legend([a; b], {'Left','Right'}); 143 | end 144 | end 145 | 146 | open Joint_Moment.fig; 147 | dirname = {'Abd(+)/Adduction(-)','Internal(+)/External(-) rotation','Flex(+)/Extension(-)'}; 148 | % figure 149 | for i = 1:3 150 | for j = 1:3 151 | subplot(3,3,(i-1)*3+j) 152 | hold on 153 | if j == 1 154 | data = cat(3, Mp_local(:,j,1,i), -Mp_local(:,j,2,i))/Mass/g/LL; 155 | end 156 | if j == 2 157 | data = cat(3, -Mp_local(:,j,1,i), Mp_local(:,j,2,i))/Mass/g/LL; 158 | 159 | end 160 | if j == 3 161 | data = cat(3, Mp_local(:,j,1,i), Mp_local(:,j,2,i))/Mass/g/LL; 162 | if i == 2 163 | data = cat(3, -Mp_local(:,j,1,i), -Mp_local(:,j,2,i))/Mass/g/LL; 164 | end 165 | end 166 | % if j >0 167 | % data = cat(3, Mp_local(:,j,1,i), Mp_local(:,j,2,i))/Mass/g/LL; 168 | % end 169 | if i == 1 170 | title(dirname(j)) 171 | end 172 | if j == 1 173 | ylabel([artiname{i},' (Nm/BW/LL)']) 174 | end 175 | xlim([0 159]) 176 | ylim([min(data(:)) max(data(:))]) 177 | hold on 178 | a=plot(1:159,data(:,:,1,:),'b--'); 179 | b=plot(1:159,data(:,:,2,:),'r--'); 180 | legend([a; b], {'Left','Right'}); 181 | end 182 | end 183 | % open Joint_Moment.fig; 184 | % close all 185 | function [ Xf ]= AVGfilt( X, thd) 186 | % Function: offset the data to eliminate the means of error and the data 187 | % with absolute values under the threshold. 188 | % thd : threshold 189 | 190 | s = size(X); 191 | Is = abs(X)-0.1 52 | continue 53 | else 54 | Time1 = j; 55 | break 56 | end 57 | end 58 | Time2 = find(bodyCOMVecX==min(bodyCOMVecX));%229 59 | fpForceSum = sum(sqrt(sum(fpF_local.^2,2)),3); 60 | Time3 = find(fpForceSum==max(fpForceSum));%267 61 | Time4 = nf; 62 | 63 | Time1s = 1; 64 | Time2s = (Time2-Time1)/(Time4-Time1)*100; 65 | Time3s = (Time3-Time1)/(Time4-Time1)*100; 66 | Time4s = 100; 67 | TimePlot = reshape(1:100,[],1); 68 | TimeSeq100 = reshape(linspace(Time1/smprate,Time4/smprate,100),[],1); 69 | 70 | bodyCOM100 = interp1(TimeSeq,bodyCOM,linspace(Time1,Time4,100)/smprate); 71 | gCOP100 = interp1(TimeSeq,gCOP,linspace(Time1,Time4,100)/smprate); 72 | gCOP100withZ = cat(2,gCOP100,ones(100,1)); %100*3 73 | 74 | vec = bodyCOM100-gCOP100withZ; 75 | theS = atan(vec(:,1)./vec(:,3)); 76 | theF = -atan(vec(:,2)./vec(:,3)); 77 | 78 | 79 | figure('Name','(d) The Vector pointing from COP to COM', 'NumberTitle','off','position',[100 50 1100 450]); 80 | 81 | subplot(2,1,1) 82 | hold on 83 | plot([Time1s,Time1s],[min(ran(:)) max(ran(:))],'k--',[Time2s,Time2s],[min(ran(:)) max(ran(:))],'k--',[Time4s,Time4s],[min(ran(:)) max(ran(:))],'k--',[Time3s,Time3s],[min(ran(:)) max(ran(:))],'k--') 84 | ylim([0 30]) 85 | title('The angle projected on the Sagittal Plane (deg)') 86 | plot(TimePlot, theS*180/pi) 87 | 88 | subplot(2,1,2) 89 | hold on 90 | plot([Time2s,Time2s],[-5 15],'k--',[Time3s,Time3s],[-5 15],'k--') 91 | ylim([-5 15]) 92 | title('The angle projected on the Frontal Plane (deg)') 93 | plot(TimePlot, theF*180/pi) -------------------------------------------------------------------------------- /final/final_e1.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Computer Methods in Human Motion Analysis 2017 -- HW9 3 | 4 | % Matlab Version: MATLAB R2017a 5 | % Operating System Ubuntu (Linux) 6 | % Student: Wei-hsiang Wang 7 | % Department: Mechanical Engineering 8 | % Student ID: R05522625 9 | 10 | addpath(genpath(fileparts(cd))) % adding all hw directory to PATH. 11 | 12 | %% Initialization 13 | clc; 14 | clearvars; 15 | % close all; 16 | 17 | %% 18 | seq = {'RASI','LASI','RPSI','LPSI','L4L5','LPET','RPET','RTRO','RLFC','RMFC','RTHI','RTT','RSHA',... 19 | 'RLMA','RMMA','RTIB','RHEE','RFOO','RTOE','RMTH','LTRO','LLFC','LMFC','LTHI','LTT','LSHA','LLMA',... 20 | 'LMMA','LTIB','LHEE','LFOO','LTOE','LMTH','C7T1','LBTO','LFRM','LHead','LRM','LSAP','LUM','LUPA',... 21 | 'LUS','LWRA','RBTO','RFRM','RHead','RRM','RSAP','RUM','RUPA','RUS','RWRA'}; 22 | load('subcali.mat') 23 | load('DataSTS.mat') 24 | nf = size(MKtraj,1); 25 | [ bodyCOM, segCOM, ~, ~, ~ ] = WholeBodyCOM( MKtraj, mklist, shoulderR, 7 ); 26 | TimeSeq = reshape(1/smprate:1/smprate:nf/smprate,[],1);%nx1 27 | bodyCOMVecX = Derivative( TimeSeq, bodyCOM(:,1), 1 ); 28 | for i = 1:nf 29 | if bodyCOMVecX(i)<-0.1 30 | continue 31 | else 32 | Time1 = i; 33 | break 34 | end 35 | end 36 | for j = Time1:nf 37 | if bodyCOMVecX(j)>-0.1 38 | continue 39 | else 40 | Time1 = j; 41 | break 42 | end 43 | end 44 | fpF_local1 = [Fx1,Fy1,Fz1]; 45 | fpF_local2 = [Fx2,Fy2,Fz2]; 46 | fpF_local = cat(3, fpF_local1, fpF_local2); 47 | Time2 = find(bodyCOMVecX==min(bodyCOMVecX));%229 48 | fpForceSum = sum(sqrt(sum(fpF_local.^2,2)),3); 49 | Time3 = find(fpForceSum==max(fpForceSum));%267 50 | Time4 = nf; 51 | 52 | Time1s = 1; 53 | Time2s = (Time2-Time1)/(Time4-Time1)*100; 54 | Time3s = (Time3-Time1)/(Time4-Time1)*100; 55 | Time4s = 100; 56 | TimePlot = reshape(1:100,[],1); 57 | TimeSeq100 = reshape(linspace(Time1/smprate,Time4/smprate,100),[],1); 58 | 59 | %% 60 | for i = 1:length(seq) 61 | index = find(strcmp(mklist,seq(i))); 62 | if ~isempty(index) 63 | eval(string(seq{i})+'=MKtraj(:,:,index);') 64 | end 65 | end 66 | [lRg2p, lVg2p, lPcoord_local] = CoordPelvis(cat(3, LASI, LPSI, RASI), 'l', {'LASI', 'LPSI', 'RASI'}); 67 | [rRg2p, rVg2p, rPcoord_local] = CoordPelvis(cat(3, LASI, RPSI, RASI), 'r', {'LASI', 'RPSI', 'RASI'}); 68 | [lRg2t, lVg2t, lTcoord_local] = CoordThigh(cat(3, LLFC, LTRO, LMFC), 'l', {'LLFC', 'LTRO', 'LMFC'}); 69 | [rRg2t, rVg2t, rTcoord_local] = CoordThigh(cat(3, RTRO, RMFC, RLFC), 'r', {'RTRO', 'RMFC', 'RLFC'}); 70 | [lRg2s, lVg2s, lScoord_local] = CoordShank(cat(3, LTT, LLMA, LMMA, LSHA), 'l', {'LTT', 'LLMA', 'LMMA', 'LSHA'}); 71 | [rRg2s, rVg2s, rScoord_local] = CoordShank(cat(3, RTT, RSHA, RLMA, RMMA), 'r', {'RTT', 'RSHA', 'RLMA', 'RMMA'}); 72 | [lRg2f, lVg2f, lFcoord_local] = CoordFoot(cat(3, LHEE, LTOE, LFOO), 'l', {'LHEE', 'LTOE', 'LFOO'}); 73 | [rRg2f, rVg2f, rFcoord_local] = CoordFoot(cat(3, RFOO, RHEE, RTOE), 'r', {'RFOO', 'RHEE', 'RTOE'}); 74 | 75 | lRp2t = mtimesx(lRg2p, 'T', lRg2t); lRt2p = mtimesx(lRg2t, 'T', lRg2p); 76 | rRp2t = mtimesx(rRg2p, 'T', rRg2t); rRt2p = mtimesx(rRg2t, 'T', rRg2p); 77 | lRt2s = mtimesx(lRg2t, 'T', lRg2s); lRs2t = mtimesx(lRg2s, 'T', lRg2t); 78 | rRt2s = mtimesx(rRg2t, 'T', rRg2s); rRs2t = mtimesx(rRg2s, 'T', rRg2t); 79 | lRs2f = mtimesx(lRg2s, 'T', lRg2f); lRf2s = mtimesx(lRg2f, 'T', lRg2s); 80 | rRs2f = mtimesx(rRg2s, 'T', rRg2f); rRf2s = mtimesx(rRg2f, 'T', rRg2s); 81 | 82 | thelpt = RotAngConvert(lRp2t,'xyz'); 83 | therpt = RotAngConvert(rRp2t,'xyz'); 84 | thelts = RotAngConvert(lRt2s,'xyz'); 85 | therts = RotAngConvert(rRt2s,'xyz'); 86 | thelsf = RotAngConvert(lRs2f,'xyz'); 87 | thersf = RotAngConvert(rRs2f,'xyz'); 88 | 89 | 90 | %% 91 | for i = 1:length(seq) 92 | index = find(strcmp(mklist,seq(i))); 93 | if ~isempty(index) 94 | eval(string(seq{i})+'=MKpose(:,:,index);') 95 | end 96 | end 97 | lRg2p = CoordPelvis(cat(3, LASI, LPSI, RASI), 'l', {'LASI', 'LPSI', 'RASI'}); 98 | rRg2p = CoordPelvis(cat(3, LASI, RPSI, RASI), 'r', {'LASI', 'RPSI', 'RASI'}); 99 | lRg2t = CoordThigh(cat(3, LLFC, LTRO, LMFC), 'l', {'LLFC', 'LTRO', 'LMFC'}); 100 | rRg2t = CoordThigh(cat(3, RTRO, RMFC, RLFC), 'r', {'RTRO', 'RMFC', 'RLFC'}); 101 | lRg2s = CoordShank(cat(3, LTT, LLMA, LMMA, LSHA), 'l', {'LTT', 'LLMA', 'LMMA', 'LSHA'}); 102 | rRg2s = CoordShank(cat(3, RTT, RSHA, RLMA, RMMA), 'r', {'RTT', 'RSHA', 'RLMA', 'RMMA'}); 103 | lRg2f = CoordFoot(cat(3, LHEE, LTOE, LFOO), 'l', {'LHEE', 'LTOE', 'LFOO'}); 104 | rRg2f = CoordFoot(cat(3, RFOO, RHEE, RTOE), 'r', {'RFOO', 'RHEE', 'RTOE'}); 105 | lRspt = lRg2p\lRg2t; rRspt = rRg2p\rRg2t; 106 | lRsts = lRg2t\lRg2s; rRsts = rRg2t\rRg2s; 107 | lRssf = lRg2s\lRg2f; rRssf = rRg2s\rRg2f; 108 | 109 | % Create Rc 110 | lRcpt = JointAngOffset( lRspt, lRp2t ); rRcpt = JointAngOffset( rRspt, rRp2t ); 111 | lRcts = JointAngOffset( lRsts, lRt2s ); rRcts = JointAngOffset( rRsts, rRt2s ); 112 | lRcsf = JointAngOffset( lRssf, lRs2f ); rRcsf = JointAngOffset( rRssf, rRs2f ); 113 | 114 | % Euler angle of Rc 115 | rcpt = RotAngConvert(rRcpt, 'xyz'); lcpt = RotAngConvert(lRcpt, 'xyz'); 116 | rcts = RotAngConvert(rRcts, 'xyz'); lcts = RotAngConvert(lRcts, 'xyz'); 117 | rcsf = RotAngConvert(rRcsf, 'xyz'); lcsf = RotAngConvert(lRcsf, 'xyz'); 118 | lcpt = interp1(TimeSeq,lcpt,linspace(Time1,Time4,100)/smprate); 119 | rcpt = interp1(TimeSeq,rcpt,linspace(Time1,Time4,100)/smprate); 120 | lcts = interp1(TimeSeq,lcts,linspace(Time1,Time4,100)/smprate); 121 | rcts = interp1(TimeSeq,rcts,linspace(Time1,Time4,100)/smprate); 122 | lcsf = interp1(TimeSeq,lcsf,linspace(Time1,Time4,100)/smprate); 123 | rcsf = interp1(TimeSeq,rcsf,linspace(Time1,Time4,100)/smprate); 124 | % 125 | dataRc = cat(3, lcpt, rcpt, lcts, rcts, lcsf, rcsf); 126 | figure('Name','Joint Angle', 'NumberTitle','off','position',[0 50 1680 750]); 127 | dirname = {'Abd(+)/Adduction(-)','Internal(+)/External(-) rotation','Flex(+)/Extension(-)'}; 128 | artiname = {'Hip','Knee','Ankle'}; 129 | for i = 1:3 130 | for j = 1:3 131 | subplot(3,3,3*(j-1)+i) 132 | hold on 133 | ran = cat(3,-dataRc(:,i,2*j-1), dataRc(:,i,2*j)); 134 | if i == 3 135 | ran = cat(3,dataRc(:,i,2*j-1), dataRc(:,i,2*j)); 136 | if j == 2 137 | ran = cat(3,-dataRc(:,i,2*j-1), -dataRc(:,i,2*j)); 138 | end 139 | end 140 | plot([Time1s,Time1s],[min(ran(:)) max(ran(:))],'k--',[Time2s,Time2s],[min(ran(:)) max(ran(:))],'k--',[Time4s,Time4s],[min(ran(:)) max(ran(:))],'k--',[Time3s,Time3s],[min(ran(:)) max(ran(:))],'k--') 141 | ylim([min(ran(:)) max(ran(:))]) 142 | plot(ran(:,:,1)) 143 | plot(ran(:,:,2)) 144 | if j == 1 145 | title(dirname(i)) 146 | end 147 | if i == 1 148 | ylabel([artiname(j),'(deg)']) 149 | end 150 | end 151 | end -------------------------------------------------------------------------------- /final/final_e2.m: -------------------------------------------------------------------------------- 1 | %% 2 | % Computer Methods in Human Motion Analysis 2017 -- HW9 3 | 4 | % Matlab Version: MATLAB R2017a 5 | % Operating System Ubuntu (Linux) 6 | % Student: Wei-hsiang Wang 7 | % Department: Mechanical Engineering 8 | % Student ID: R05522625 9 | 10 | addpath(genpath(fileparts(cd))) % adding all hw directory to PATH. 11 | 12 | %% Initialization 13 | clc; 14 | clearvars; 15 | close all; 16 | %% 17 | seq = {'RASI','LASI','RPSI','LPSI','L4L5','LPET','RPET','RTRO','RLFC','RMFC','RTHI','RTT','RSHA',... 18 | 'RLMA','RMMA','RTIB','RHEE','RFOO','RTOE','RMTH','LTRO','LLFC','LMFC','LTHI','LTT','LSHA','LLMA',... 19 | 'LMMA','LTIB','LHEE','LFOO','LTOE','LMTH','C7T1','LBTO','LFRM','LHead','LRM','LSAP','LUM','LUPA',... 20 | 'LUS','LWRA','RBTO','RFRM','RHead','RRM','RSAP','RUM','RUPA','RUS','RWRA'}; 21 | load('DataSTS.mat') 22 | load('subcali.mat') 23 | nf = size(MKtraj,1); 24 | TimeSeq = reshape(1/smprate:1/smprate:nf/smprate,[],1);%nx1 25 | 26 | fpF_local1 = [Fx1,Fy1,Fz1]; 27 | fpF_local2 = [Fx2,Fy2,Fz2]; 28 | fpF_local = cat(3, fpF_local1, fpF_local2); 29 | fpM_local1 = [Mx1,My1,Mz1]; 30 | fpM_local2 = [Mx2,My2,Mz2]; 31 | fpM_local = cat(3, fpM_local1, fpM_local2); 32 | 33 | corners2 = [508 464, 0;... 34 | 508, 0, 0;... 35 | 0, 0, 0;... 36 | 0, 464, 0]; 37 | corners1 = [508, -3, 0;... 38 | 508, -467, 0;... 39 | 0, -467, 0;... 40 | 0, -3, 0]; 41 | corners = cat(3, corners1, corners2); 42 | 43 | [ gCOP, nCOP, fpFg, Rg2fp, Vg2fp ] = ForcePlateN( fpF_local, fpM_local, corners, Vfp2c, Pz ); 44 | [ bodyCOM, segCOM, ~, ~, ~ ] = WholeBodyCOM( MKtraj, mklist, shoulderR, 7 ); 45 | 46 | %% 47 | bodyCOMVecX = Derivative( TimeSeq, bodyCOM(:,1), 1 ); 48 | for i = 1:nf 49 | if bodyCOMVecX(i)<-0.1 50 | continue 51 | else 52 | Time1 = i; 53 | break 54 | end 55 | end 56 | for j = Time1:nf 57 | if bodyCOMVecX(j)>-0.1 58 | continue 59 | else 60 | Time1 = j; 61 | break 62 | end 63 | end 64 | Time2 = find(bodyCOMVecX==min(bodyCOMVecX));%229 65 | fpForceSum = sum(sqrt(sum(fpF_local.^2,2)),3); 66 | Time3 = find(fpForceSum==max(fpForceSum));%267 67 | Time4 = nf; 68 | 69 | Time1s = 1; 70 | Time2s = (Time2-Time1)/(Time4-Time1)*100; 71 | Time3s = (Time3-Time1)/(Time4-Time1)*100; 72 | Time4s = 100; 73 | TimePlot = reshape(1:100,[],1); 74 | TimeSeq100 = reshape(linspace(Time1/smprate,Time4/smprate,100),[],1); 75 | 76 | bodyCOM100 = interp1(TimeSeq,bodyCOM,linspace(Time1,Time4,100)/smprate); 77 | gCOP100 = interp1(TimeSeq,gCOP,linspace(Time1,Time4,100)/smprate); 78 | gCOP100withZ = cat(2,gCOP100,ones(100,1)); %100*3 79 | 80 | %% StaticCali 81 | for i = 1:length(seq) 82 | index = find(strcmp(mklist,seq(i))); 83 | if ~isempty(index) 84 | eval(string(seq{i})+'=MKpose(:,:,index);') 85 | end 86 | end 87 | 88 | InterASIS = sqrt(sum((RASI - LASI).^2, 2)); 89 | lHipLoc = InterASIS .* [-.19 -.3 -.36]; 90 | rHipLoc = InterASIS .* [-.19 -.3 .36]; 91 | [rRg2p, ~, ~] = CoordPelvis(cat(3, LASI, RPSI, RASI), 'r', {'LASI', 'RPSI', 'RASI'}); 92 | [lRg2p, ~, ~] = CoordPelvis(cat(3, LASI, LPSI, RASI), 'l', {'LASI', 'LPSI', 'RASI'}); 93 | lHip = (RASI + LASI)/2 + lHipLoc * lRg2p; 94 | rHip = (RASI + LASI)/2 + rHipLoc * rRg2p; 95 | lKnee = (LLFC+LMFC)/2; rKnee = (RLFC+RMFC)/2; 96 | lAnkle = (LLMA+LMMA)/2; rAnkle = (RLMA+RMMA)/2; 97 | CoordP = cat(3, lHip, rHip, lKnee, rKnee, lAnkle, rAnkle); 98 | CoordD = cat(3, CoordP(:,:,3:end), LMTH, RMTH);%%%%% 99 | [ Ms, Is ] = LLInertia( BW, CoordP, CoordD); 100 | Is = Is / 1000000; % kg mm^2 -> kg m^2 101 | Ms = Ms(:); 102 | 103 | %% dH 104 | for i = 1:length(seq) 105 | index = find(strcmp(mklist,seq(i))); 106 | if ~isempty(index) 107 | eval(string(seq{i})+'=MKtraj(:,:,index);') 108 | end 109 | end 110 | lRg2t = CoordThigh(cat(3, LLFC, LTRO, LMFC), 'l', {'LLFC', 'LTRO', 'LMFC'}); 111 | rRg2t = CoordThigh(cat(3, RTRO, RMFC, RLFC), 'r', {'RTRO', 'RMFC', 'RLFC'}); 112 | lRg2s = CoordShank(cat(3, LTT, LLMA, LMMA, LSHA), 'l', {'LTT', 'LLMA', 'LMMA', 'LSHA'}); 113 | rRg2s = CoordShank(cat(3, RTT, RSHA, RLMA, RMMA), 'r', {'RTT', 'RSHA', 'RLMA', 'RMMA'}); 114 | lRg2f = CoordFoot(cat(3, LHEE, LTOE, LFOO), 'l', {'LHEE', 'LTOE', 'LFOO'}); 115 | rRg2f = CoordFoot(cat(3, RFOO, RHEE, RTOE), 'r', {'RFOO', 'RHEE', 'RTOE'}); 116 | AllLimbRg2l = cat(4, lRg2t, rRg2t, lRg2s, rRg2s, lRg2f, rRg2f); 117 | [ AngVel, AngAcc ] = Rot2LocalAngularEP( AllLimbRg2l, smprate ); 118 | [ H, dH ] = AngularMomentum( Is, AngVel, AngAcc );%380*3*6 119 | 120 | %% segmentCOMAcc, rCOM2P, rCOM2D 121 | fpMg1 = fpM_local1*Rg2fp(:,:,1).'; %fpMg1(isnan(fpMg1)) = 0; 122 | fpMg2 = fpM_local2*Rg2fp(:,:,2).'; %fpMg2(isnan(fpMg2)) = 0; 123 | 124 | % COM of lower limb 125 | LKnee = (LLFC + LMFC)/2; 126 | RKnee = (RLFC + RMFC)/2; 127 | ProxLL = cat(3, LTRO, RTRO, LKnee, RKnee, LLMA, RLMA); 128 | DistLL = cat(3, LKnee, RKnee, LMMA, RMMA, LMTH, RMTH); 129 | w = reshape(repmat([.433 .433 .5],2,1),1,1,[]); 130 | COMLL = w .* DistLL + (1-w) .* ProxLL;%380*3*6 131 | 132 | % Joint center of lower limb 133 | InterASIS = sqrt(sum((RASI - LASI).^2, 2));%nx1 134 | lHipLoc = permute(InterASIS .* [-.19 -.3 -.36], [2 3 1]); 135 | rHipLoc = permute(InterASIS .* [-.19 -.3 .36], [2 3 1]); 136 | [rRg2p, ~, ~] = CoordPelvis(cat(3, LASI, RPSI, RASI), 'r', {'LASI', 'RPSI', 'RASI'}); 137 | [lRg2p, ~, ~] = CoordPelvis(cat(3, LASI, LPSI, RASI), 'l', {'LASI', 'LPSI', 'RASI'}); 138 | LLJC = cat(4, cat(3, (RASI + LASI)/2 + permute(mtimesx(lRg2p,lHipLoc), [3 1 2]), ...%left hip 139 | (RASI + LASI)/2 + permute(mtimesx(rRg2p,rHipLoc), [3 1 2]))... %right hip 140 | ,cat(3, (LLFC+LMFC)/2, (RLFC+RMFC)/2)...%knee 141 | ,cat(3, (LLMA+LMMA)/2, (RLMA+RMMA)/2));%ankle 142 | 143 | [ rCOM2P, rCOM2D, segCOMAcc ] = LLCOM2PD( COMLL, LLJC, nCOP, smprate ); 144 | rCOP2P = cat(3, rCOM2P(:,:,:,1), rCOM2P(:,:,:,2), rCOM2P(:,:,:,3));%380*3*6 145 | rCOP2D = cat(3, rCOM2D(:,:,:,1), rCOM2D(:,:,:,2), rCOM2D(:,:,:,3));%380*3*6 146 | segCOMAcc = cat(3, segCOMAcc(:,:,:,1), segCOMAcc(:,:,:,2), segCOMAcc(:,:,:,3));%380*3*6 147 | 148 | %% Force & Moment of segments 149 | segCOMAcc = segCOMAcc/1000; % mm/sec^2 -> m/sec^2 150 | Fd_fp = cat(3, fpFg(:,:,1), fpFg(:,:,2));% N 151 | Md_fp = cat(3, fpMg1, fpMg2)/1000000; % Nmm -> Nm %%%????????????????????????????????????????????? /1000 ?????????????????????????????????????????????? 152 | rCOM2P = rCOM2P/1000; % mm -> m 153 | rCOM2D = rCOM2D/1000; % mm -> m 154 | [ Fp_local_Ak, Mp_local_Ak, Fp_Ak, Mp_Ak ] = JointForceMoment( AllLimbRg2l(:,:,:,5:6), Ms(5:6), segCOMAcc(:,:,5:6), dH(:,:,5:6), rCOM2P(:,:,5:6), rCOM2D(:,:,5:6), Fd_fp, Md_fp ); 155 | [ Fp_local_Kn, Mp_local_Kn, Fp_Kn, Mp_Kn ] = JointForceMoment( AllLimbRg2l(:,:,:,3:4), Ms(3:4), segCOMAcc(:,:,3:4), dH(:,:,3:4), rCOM2P(:,:,3:4), rCOM2D(:,:,3:4), -Fp_Ak, -Mp_Ak ); 156 | [ Fp_local_Hp, Mp_local_Hp, Fp_Hp, Mp_Hp ] = JointForceMoment( AllLimbRg2l(:,:,:,1:2), Ms(1:2), segCOMAcc(:,:,1:2), dH(:,:,1:2), rCOM2P(:,:,1:2), rCOM2D(:,:,1:2), -Fp_Kn, -Mp_Kn ); 157 | Fp_local_Hp = interp1(TimeSeq,Fp_local_Hp,linspace(Time1,Time4,100)/smprate); 158 | Fp_local_Kn = interp1(TimeSeq,Fp_local_Kn,linspace(Time1,Time4,100)/smprate); 159 | Fp_local_Ak = interp1(TimeSeq,Fp_local_Ak,linspace(Time1,Time4,100)/smprate); 160 | Fp_local = cat(4, Fp_local_Hp, Fp_local_Kn, Fp_local_Ak); 161 | Mp_local_Hp = interp1(TimeSeq,Mp_local_Hp,linspace(Time1,Time4,100)/smprate); 162 | Mp_local_Kn = interp1(TimeSeq,Mp_local_Kn,linspace(Time1,Time4,100)/smprate); 163 | Mp_local_Ak = interp1(TimeSeq,Mp_local_Ak,linspace(Time1,Time4,100)/smprate); 164 | Mp_local = cat(4, Mp_local_Hp, Mp_local_Kn, Mp_local_Ak); 165 | 166 | % %% plotting 167 | artiname = {'Hip','Knee','Ankle'}; 168 | % open Joint_Moment.fig; 169 | dirname = {'Abd(+)/Adduction(-)','Internal(+)/External(-) rotation','Flex(+)/Extension(-)'}; 170 | % figure 171 | g = 9.80665; 172 | for i = 1:3 173 | for j = 1:3 174 | subplot(3,3,(i-1)*3+j) 175 | hold on 176 | if j == 1 177 | data = cat(3, Mp_local(:,j,1,i), -Mp_local(:,j,2,i))/BW/g/LegLength; 178 | end 179 | if j == 2 180 | data = cat(3, -Mp_local(:,j,1,i), Mp_local(:,j,2,i))/BW/g/LegLength; 181 | 182 | end 183 | if j == 3 184 | data = cat(3, Mp_local(:,j,1,i), Mp_local(:,j,2,i))/BW/g/LegLength; 185 | if i == 2 186 | data = cat(3, -Mp_local(:,j,1,i), -Mp_local(:,j,2,i))/BW/g/LegLength; 187 | end 188 | end 189 | % if j >0 190 | % data = cat(3, Mp_local(:,j,1,i), Mp_local(:,j,2,i))/Mass/g/LL; 191 | % end 192 | if i == 1 193 | title(dirname(j)) 194 | end 195 | if j == 1 196 | ylabel([artiname{i},' (Nm/BW/LL)']) 197 | end 198 | xlim([0 100]) 199 | ylim([min(data(:)) max(data(:))]) 200 | hold on 201 | a=plot(1:100,data(:,:,1,:),'b'); 202 | b=plot(1:100,data(:,:,2,:),'r'); 203 | plot([Time1s,Time1s],[min(data(:)) max(data(:))],'k--',[Time2s,Time2s],[min(data(:)) max(data(:))],'k--',[Time4s,Time4s],[min(data(:)) max(data(:))],'k--',[Time3s,Time3s],[min(data(:)) max(data(:))],'k--') 204 | 205 | legend([a; b], {'Left','Right'}); 206 | end 207 | end 208 | % open Joint_Moment.fig; 209 | % % close all 210 | 211 | -------------------------------------------------------------------------------- /final/result/COP.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/final/result/COP.fig -------------------------------------------------------------------------------- /final/result/Inclination Angle.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/final/result/Inclination Angle.fig -------------------------------------------------------------------------------- /final/result/Joint Angle.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/final/result/Joint Angle.fig -------------------------------------------------------------------------------- /final/result/Joint_Moment.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/final/result/Joint_Moment.fig -------------------------------------------------------------------------------- /final/subcali.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/final/subcali.mat -------------------------------------------------------------------------------- /results/COP.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/COP.gif -------------------------------------------------------------------------------- /results/HW2.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/HW2.mp4 -------------------------------------------------------------------------------- /results/hw10_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw10_1.jpg -------------------------------------------------------------------------------- /results/hw10_2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw10_2.jpg -------------------------------------------------------------------------------- /results/hw3-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw3-1.png -------------------------------------------------------------------------------- /results/hw3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw3.png -------------------------------------------------------------------------------- /results/hw4_12RotSeq.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw4_12RotSeq.jpg -------------------------------------------------------------------------------- /results/hw4_AngAcc.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw4_AngAcc.jpg -------------------------------------------------------------------------------- /results/hw4_AngVel.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw4_AngVel.jpg -------------------------------------------------------------------------------- /results/hw5_AngAcc.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw5_AngAcc.jpg -------------------------------------------------------------------------------- /results/hw5_AngVel.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw5_AngVel.jpg -------------------------------------------------------------------------------- /results/hw5_unwrapEP().jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw5_unwrapEP().jpg -------------------------------------------------------------------------------- /results/hw6_1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw6_1.jpg -------------------------------------------------------------------------------- /results/hw6_n&phi.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw6_n&phi.jpg -------------------------------------------------------------------------------- /results/hw7.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw7.jpg -------------------------------------------------------------------------------- /results/hw7_2.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw7_2.mp4 -------------------------------------------------------------------------------- /results/hw8_COP.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw8_COP.jpg -------------------------------------------------------------------------------- /results/hw8_COP95.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw8_COP95.jpg -------------------------------------------------------------------------------- /results/hw9_1der.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw9_1der.jpg -------------------------------------------------------------------------------- /results/hw9_AngMomentum.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/hw9_AngMomentum.jpg -------------------------------------------------------------------------------- /results/punisher.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/punisher.jpg -------------------------------------------------------------------------------- /results/walk.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mattwang44/Human-Motion-Analysis-MATLAB/75acbcf2c33dd40dffe04a7fcaea52e6300eafbf/results/walk.gif --------------------------------------------------------------------------------