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