function [o1,o2,o3] = mcI( i1, i2, i3 ) % mcI rigid-body inertia <--> mass, CoM and rotational inertia. % rbi=mcI(m,c,I) and [m,c,I]=mcI(rbi) convert between the spatial or planar % inertia matrix of a rigid body (rbi) and its mass, centre of mass and % rotational inertia about the centre of mass (m, c and I). In the spatial % case, c is 3x1, I is 3x3 and rbi is 6x6. In the planar case, c is 2x1, I % is a scalar and rbi is 3x3. In both cases, m is a scalar. When c is an % argument, it can be either a row or a column vector. If only one % argument is supplied then it is assumed to be rbi; and if it is a 6x6 % matrix then it is assumed to be spatial. Otherwise, three arguments must % be supplied, and if length(c)==3 then mcI calculates a spatial inertia % matrix. NOTE: (1) mcI(rbi) requires rbi to have nonzero mass; (2) if |c| % is much larger than the radius of gyration, or the dimensions of the % inertia ellipsoid, then extracting I from rbi is numerically % ill-conditioned. if nargin == 1 [o1,o2,o3] = rbi_to_mcI( i1 ); else o1 = mcI_to_rbi( i1, i2, i3 ); end function rbi = mcI_to_rbi( m, c, I ) if length(c) == 3 % spatial C = skew(c); rbi = [ I + m*C*C', m*C; m*C', m*eye(3) ]; else % planar rbi = [ I+m*dot(c,c), -m*c(2), m*c(1); -m*c(2), m, 0; m*c(1), 0, m ]; end function [m,c,I] = rbi_to_mcI( rbi ) if all(size(rbi)==[6 6]) % spatial m = rbi(6,6); mC = rbi(1:3,4:6); c = skew(mC)/m; I = rbi(1:3,1:3) - mC*mC'/m; else % planar m = rbi(3,3); c = [rbi(3,1);-rbi(2,1)]/m; I = rbi(1,1) - m*dot(c,c); end