function [Fp, Mp] = inv2d(RCM, Rp, Rd, Fd, Md, aCM, alfa, m, I, fCM)
% 2-D inverse dynamics calculation for one segment

% By: Ton van den Bogert, University of Calgary
% Date: April 26, 1996
% Report problems to bogert@acs.ucalgary.ca

%
% input:
% RCM:		position(x,y) of center of mass of the segment
% Rp, Rd: 	position(x,y) of proximal and distal endpoint of segment
% Fd, Md:	force(x,y) and moment acting on this segment at distal endpoint
% aCM:		acceleration(x,y) of segment center of mass
% alfa:		angular acceleration
% m,I,fCM	mass, moment of inertia, relative position of CM
%
% output:
% Fp:		force(x,y) at proximal endpoint (force acting on proximal segment!)
% Mp:		moment at proximal endpoint

g = -9.80665;

% calculate Fp
Fp(:,1) = m*aCM(:,1) - Fd(:,1);		% horizontal
Fp(:,2) = m*aCM(:,2) - Fd(:,2) - m*g;	% vertical

% calculate Mp
Mp = I*alfa - Md - cross2d(Rd-RCM,Fd) - cross2d(Rp-RCM,Fp);

% invert, to get force on proximal segment (action = -reaction)
Fp = -Fp;
Mp = -Mp;