function [ need taumax cst] = step_length(x0,x1,h,Y,THR); % step_length checks if the current solution x1 respects the unactive % constraints. In that case, return [true 1 []]. % Otherwise, return false, the step length and the % reference on the first violated constraint. This function % corresponds roughly to Alg. 4 of the paper. %% Synopsis: % [ need taumax cst ] = step_length(x0,x1,h,Y) % [ need taumax cst ] = step_length(x0,x1,h,Y,THR) %% Input: % x0 current value of the primal optimum. % x1 next value of the primal optimum. % h "h" structure storing all the HQP data. % Y right basis of the HCOD. % THR is the threshold used to test the constraint violation. %% Output: % need is true iff at least one constraint needs to be activated at % the next step of the active search. % taumax is the step length. % cst is the constraint reference if one constraint needs to be % activated. In that case, cst = [ k c bound ], the three % arguments to be given to the "up" function. % % The function does not follows exactly Alg. 4 because of the double % bounds. It uses the subfunctions check_bound to factorize the code of % Alg. 4#6 and 4#10. Moreover, it uses a shortcut by setting a step of % length tau=1-EPS if a bound is not satisfied for x_0. % % Copyright Nicolas Mansard -- LAAS/CNRS % -- and Adrien Escande -- JRL/CNRS % -- cf. LICENSE.txt % % --- DEFAULT ARGUMENTS -------------------------------------------------------- nin = nargin; if nin==4 % Default argument for the HCOD threshold. THR=1e-8; nin=nin+1; end % --------------------------------------------------------------------- p=length(h); nh=size(Y,2); constants; taumax=1; cst=[]; for k=1:p massert( not(exist('hk')) ,'Error, hk should have been cleared.'); hk=h(k); mmax=hk.mmax; for r=setdiff(1:mmax,hk.active); b = hk.b(r,:); typ = hk.btype(r); Ax1 = hk.A(r,:)*x1; [ typ1 b1 ] = check_bound(Ax1,b,typ,THR); % Alg 4#10 if typ1~=Enone Ax0 = hk.A(r,:)*x0; typ0 = check_bound(Ax0,b,typ,THR); % Alg 4#6 if typ0==Enone if abs(Ax1-Ax0)<1e-8 % Alg 4#7 tau=1; else tau=(b1-Ax0)/(Ax1-Ax0); end else tau=1-THR; % Alg 4#9 end if tau=,<=<=). % THR is the threshold used to test the constraint violation. %% Output: % violtype if no bound is violated, return Enone, otherwise returns Einf or % Esup depending on the violation. % violval is the value of "bound" that is met. % % constants; violtype=Enone; violval=0; if (type==Einf | type==Edouble) & (valbound(2)+THR) violtype=Esup; violval=bound(2); end end