function [f,fel,ffr,e] = JumperForce_JMS(r,v,t)
% Force and potential energy of a jumper
% Input:
%   r(3,1) : jumper coordinates, relative to bridge (m)
%   v(3,1) : jumper velocity (m/s)
%   t      : time (not used)
% Output:
%   f(3,1) : total jumper force (N)
%   e      : potential energy (J)
% J.M.Soler, May.2023

% Set data
m = 80;        % jumper mass (kg)
g = 9.8;       % gravitational acceleration (m/s^2)
rb = [0;-5;0]; % anchor point b (m)
rc = [0;+5;0]; % anchor point c (m)
lb = 24;       % equilibrium length of rope b (m)
lc = 25;       % equilibrium length of rope c (m)
kb1 = 50;      % elastic constant of rope b (N/m)
kc1 = 60;      % elastic constant of rope b (N/m)
kb2 = 5;       % inelastic constant of rope b (N/m^2)
kc2 = 10;      % inelastic constant of rope c (N/m^2)
beta = 40;     % friction constant (kg/s)

% Find enerfy and force
e = m*g*r(3);       % gravitational energy
fg = [ 0; 0; -m*g ]; % gravitational force
rbj = (r-rb);
rcj = (r-rc);
lbj = vecnorm(rbj);
lcj = vecnorm(rcj);
e = e + (1/2*kb1*(lbj-lb)^2 + 1/3*kb2*(lbj-lb)^3)*(lbj>lb) ... % elastic energy
      + (1/2*kc1*(lcj-lc)^2 + 1/3*kc2*(lcj-lc)^3)*(lcj>lc);
fel = - rbj/lbj*( kb1*(lbj-lb) + kb2*(lbj-lb)^2 )*(lbj>lb) ... % elastic force
      - rcj/lcj*( kc1*(lcj-lc) + kc2*(lcj-lc)^2 )*(lcj>lc);
ffr = - beta*v;                                                 % friction force
f = fg + fel + ffr;                                             % total force

end
