bht_simple_delay(t,tau)

function [p,dp,d2p] = bht_simple_delay(t,tau)
% BHT_SIMPLE_DELAY
% 
% Utility function
% 
% Syntax
% 
%     [p,dp,d2p] = BHT_SIMPLE_DELAY(t,tau)
% 
% Description
% 
%     Defines a smooth regular (C3) function of time, which grows from 0 to
%     1 when t goes from 0 to tau then stays constant and equal to 1 for 
%     t > tau.
%
%     o t is a vector of non negative time instants
%    
%     o tau is a real positive scalar
% 
%     p, dp, and d2d are respectively the function values, the function derivatives
%     values and the second function derivatives values taken of the vector of 
%     instants t.
%     It can be used to start smoothly the control of an hinge.


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                         %
%                      BIOHYDRODYNAMICS MATLAB TOOLBOX                    %
%                                                                         %
%                         A. MUNNIER and B. PINCON                        %
%                                                                         %
% [e-mail]        [e-mail]   %
% http://www.iecn.u-nancy.fr/~munnier  http://www.iecn.u-nancy.fr/~pincon %
%                                                                         %
%                                                                         %
%                      INSTITUT ELIE CARTAN, NANCY 1                      %
%                       http://www.iecn.u-nancy.fr/                       %
%                                                                         %
%                      INRIA Lorraine, Projet CORIDA                      %
%                    http://www.iecn.u-nancy.fr/~corida/                  %
%                                                                         %  
%                                                                         %  
%                                                                         %
%                                                                         %
%                            August 15th 2008                             %
%                                                                         %
%                                               GNU GPL v3 licence        %
%                                                                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%##########################################################################
if ( ~isreal(t) || min(t) < 0 )
    error('first arg (t) should be a vector or matrix of non negative reals')
end
if ~isscalar(tau) || ~isreal(tau) || tau <= 0
    error('second arg (tau) should be a positive scalar')
end

[p,dp,d2p] = delay(t/tau);
dp = dp/tau;
d2p = d2p/tau^2;

function [p,dp,d2p] = delay(t)
    p = ones(size(t));
    dp = zeros(size(t));
    d2p = zeros(size(t));
    ind = find(t < 1);
    tt = t(ind);
    p(ind) = 140*(tt.^4.*(0.25 + tt.*(-0.6 + tt.*(0.5 - tt/7))));
    dp(ind) = 140*(tt.^3).*(1-tt).^3;
    d2p(ind) = 420*(tt.^2).*(1-tt).^2.*(1-2*tt);