## this is not a function file (-*- octave -*-)
## Dirk Bonne
1;

##############################
## rotation matrix around a vector v for angle a
## adapted from:
##    http://www.iue.tuwien.ac.at/diss/bauer/diss/node99.html
##    http://mathworld.wolfram.com/RodriguesRotationFormula.html
## (right handed coordination system, positieve angel)
##
function retval = rotation(v, a)
  ## norm v
  vn = v / norm(v);
  cosa = cos(a);
  sina = sin(a);
  x = vn(1); y = vn(2); z = vn(3);
  retval = [x*x + cosa*(1-x*x), \ 
            x*y - cosa*x*y - sina*z, \
            x*z - cosa*x*z + sina*y
            
            y*x - cosa*y*x + sina*z, \
            y*y + cosa*(1-y*y), \
            y*z - cosa*y*z - sina*x
            
            z*x - cosa*z*x - sina*y, \
            z*y - cosa*z*y + sina*x, \
            z*z + cosa*(1-z*z) ];
endfunction;

## returns the same vector with z == 0
## also works for matrixes
function retval = xy0(v)
  retval = [1, 0, 0
            0, 1, 0
            0, 0, 0] * v;
endfunction;


## calculates the position of the lowest point of the front wheel in
## radians t
##
## I actually simply found out of the derivation of the z component of
## roft*[0;cost;sint] ---it should be zero
##
## NOTE: DOES NOT NEED FLEVO.TRANSF
function t = flevo_t_min
  global flevo;

  ## angel of the wheel where it is deepest
  if(abs(flevo.rotf(3,2)) < 0.001)
    t = - sign(flevo.rotf(3,3)) * pi / 2;
  else
    t = atan(flevo.rotf(3,3)/flevo.rotf(3,2));
  endif
#  disp("t=");disp(t);
endfunction;

## sets the ldf according to a specified trail on the ground
function flevo_set_trail(v)
  ## the whole vehicle is actually going to tilt over.
  global flevo;
  
  flevo.ldf = flevo.rf / tan(flevo.aa) - v;
endfunction;

## the rotated/translated point on the back part of the bike. The
## coordinates are relative to the contact point of the back wheel
function retval = flevo_bpoint(v)
  global flevo;
  retval = flevo.transb + flevo.rotb * v;
endfunction

## the rotated/translated point on the back part of the bike. The
## coordinates are relative to the center of the front wheel.
## the result is relative to the contact point of the back wheel
function retval = flevo_fpoint(v)
  global flevo;
  retval = flevo.transf + flevo.rotf * v;
endfunction

## calculate the contact point c
function c = flevo_c
  global flevo;

  t_min = flevo_t_min();
  c = flevo_fpoint([0; flevo.rf*cos(t_min); flevo.rf*sin(t_min)]);
endfunction;

## must be called each time the parameters are changed of flevo global
## var to correct the trans*, rot* fields
function flevo_calc
  global flevo;

### (1) calculate everything, while only tilting the bike to the side.
### This will cause the front wheel to lift itself from the ground (or
### bang it self into the ground). The second part of
### flevo_calc will then tilt the bike around the backwheel
### axis to get the front wheel touching the ground again.
  rot_tilt_aj = rotation([0;1;0], flevo.aj);
  inv_rot_tilt_aj = inv(rot_tilt_aj);
  flevo.rotb = rot_tilt_aj;
  flevo.transb = [0; 0; 0];
  ## position of a point on the pivot axis
  pos_vs = [0; flevo.lwb - flevo.ldf; flevo.rf];
  ## vector to another point around the pivot axis. This time one unit
  ## heigher.
  dir_vs = [0; -cos(flevo.aa); sin(flevo.aa)];
  
  ## rotation matrix around vs (i.e. around the pivot)
  flevo.rotf = rotation(flevo_bpoint(dir_vs), flevo.ab);
  flevo.transf = flevo_bpoint(pos_vs) + flevo.rotf*[0;flevo.ldf;0];

### (2) now we tilt the bike forward (using the plane determined by aj
###
### NOTE: the following code is still slightly incorrect. This is
### because when we tilt the bicycle forward, the calculated contact
### point (posc) will not more be the lowest. Instead the lowest point
### (in the case of forward tilting) will be a little further. To
### correct this I could repeat following calculation a few times. But I
### guess the error will be near to invisible.
  old_t_min = flevo_t_min;
  ## position of the lowest point without tilting forward yet. Next
  ## step will be to get the z component to zero by tilting the whole
  ## bike in the aj plane
  posc = inv_rot_tilt_aj * flevo_c;   # and tilt the bike again upright (-aj)
  posc(1) = 0; # project posc to the Y-Z plane
  posc = rot_tilt_aj * posc; ## tilt the bike back sideways (aj) (now posc is in the aj plane)
  posc = posc / norm(posc); # norm it
  if(norm([0; 1; 0] - posc) < 0.00001) # its already there where we want it
    rot_tilt_forward = eye(3);
  else
    ## find the angle between posc and the Y axis
    v = cross(posc, [0;1;0]);
    a = asin(norm(v));
    rot_tilt_forward = rotation(v, a);
  endif 
  ## correct rotb and turn the back wheel so the backwheel axle is again above the ground point
  flevo.rotb = rot_tilt_forward * flevo.rotb; # rotate it back
  flevo.transb = flevo.transb - [0; flevo_bpoint([0;0;flevo.rf])(2); 0];
  ## now we can correct rotf:
  flevo.rotf = rotation(flevo.rotb*dir_vs, flevo.ab);
  flevo.transf = flevo_bpoint(pos_vs) + flevo.rotf*[0;flevo.ldf;0];
                                #  printf("t_min fault (degrees) = %f\n" , (flevo_t_min - old_t_min)*180/pi);

endfunction;

## calculates the effective wheel base 
function retval = flevo_ewb
  global flevo;

  p = flevo_c();
  p(3) = 0;
  retval = norm(p);
endfunction;

## calculate the height of the axis of the front wheel
function retval = flevo_zf
  global flevo;

  retval = flevo_fpoint([0;0;0])(3);
endfunction;

## the direction of the wheel in point c
function retval = flevo_dirc
  global flevo;

  t_min = flevo_t_min();
  retval = flevo.rotf*[0; -sin(t_min); cos(t_min)];
endfunction;
  
# ### kinematic stuff #########################################################

# ### coordinate systems:
# ###
# ### C0(t) is the "absolute" coordinate system connected to the ground and which
# ### coincides with C1 at time t
# ### 
# ### C1 is the coordinate system connected to the back part of the
# ### vehicle (not the back wheel!!!) and coincides with the contact point
# ### of the back wheel). C1z has the same direction as C0z, i.e. C1 does
# ### not tilt with changing aj. C1y is along the section of the ground
# ### plane and plane formed by the back wheel.

# ## the speed of point f (point f is the midpoint of the front wheel
# ## (position against coordinate system C1)
# function retval = flevo_vf
#   global flevo;

# ###  t_min = flevo_t_min();
# ###  retval = -flevo.rotf*cross([flevo.wf; 0; 0],
# ###                             [0
# ###                             flevo.rf*cos(t_min)
# ###                              flevo.rf*sin(t_min)]);
#   retval = - flevo_dirc * flevo.wf * flevo.rf;
# endfunction;

# ### the movement of the back part can be described by (pback0,wback)
# ### where pback0 is the point around which the back part rotates and
# ### wback is the speed by which the back parts rotates.
# ###
# function retval = flevo_pback0
#   global flevo;

#   ## vf is the speed of the back part in point f. The backpart is a
#   ## fixed body with the constraint that in point d vd is along the Y
#   ## axis (rel to C1)
#   vf = flevo_vf;

#   ## C1 can only move along the Y axis.
#   pback0 =