<s> from math import * <EOL> import cornu <EOL> def mod_2pi ( th ) : <EOL> u = th / ( <NUM_LIT:2> * pi ) <EOL> return <NUM_LIT:2> * pi * ( u - floor ( u + <NUM_LIT:0.5> ) ) <EOL> def compute_dth ( k0 , k1 ) : <EOL> if k1 < <NUM_LIT:0> : <EOL> return - compute_dth ( k0 , - k1 ) <EOL> elif k1 == <NUM_LIT:0> : <EOL> return <NUM_LIT:0> <EOL> sqrk1 = sqrt ( <NUM_LIT:2> * k1 ) <EOL> t0 = ( k0 - <NUM_LIT> * k1 ) / sqrk1 <EOL> t1 = ( k0 + <NUM_LIT> * k1 ) / sqrk1 <EOL> ( y0 , x0 ) = cornu . eval_cornu ( t0 ) <EOL> ( y1 , x1 ) = cornu . eval_cornu ( t1 ) <EOL> chord_th = atan2 ( y1 - y0 , x1 - x0 ) <EOL> return mod_2pi ( t1 * t1 - chord_th ) - mod_2pi ( chord_th - t0 * t0 ) <EOL> def compute_chord ( k0 , k1 ) : <EOL> if k1 == <NUM_LIT:0> : <EOL> if k0 == <NUM_LIT:0> : <EOL> return <NUM_LIT:1> <EOL> else : <EOL> return sin ( k0 * <NUM_LIT> ) / ( k0 * <NUM_LIT> ) <EOL> sqrk1 = sqrt ( <NUM_LIT:2> * abs ( k1 ) ) <EOL> t0 = ( k0 - <NUM_LIT> * k1 ) / sqrk1 <EOL> t1 = ( k0 + <NUM_LIT> * k1 ) / sqrk1 <EOL> ( y0 , x0 ) = cornu . eval_cornu ( t0 ) <EOL> ( y1 , x1 ) = cornu . eval_cornu ( t1 ) <EOL> return hypot ( y1 - y0 , x1 - x0 ) / abs ( t1 - t0 ) <EOL> def solve_clothoid ( th0 , th1 , verbose = False ) : <EOL> k0 = th0 + th1 <EOL> k1 = <NUM_LIT:6> * ( th1 - th0 ) <EOL> error = ( th1 - th0 ) - compute_dth ( k0 , k1 ) <EOL> if verbose : <EOL> print k0 , k1 , error <EOL> k1_old , error_old = k1 , error <EOL> k1 += <NUM_LIT:6> * error <EOL> error = ( th1 - th0 ) - compute_dth ( k0 , k1 ) <EOL> if verbose : <EOL> print k0 , k1 , error <EOL> for i in range ( <NUM_LIT:10> ) : <EOL> if abs ( error ) < <NUM_LIT> : break <EOL> k1_old , error_old , k1 = k1 , error , k1 + ( k1_old - k1 ) * error / ( error - error_old ) <EOL> error = ( th1 - th0 ) - compute_dth ( k0 , k1 ) <EOL> if verbose : <EOL> print k0 , k1 , error <EOL> return k0 , k1 <EOL> if __name__ == '<STR_LIT:__main__>' : <EOL> print solve_clothoid ( <NUM_LIT> , <NUM_LIT> , True ) </s>
