Simuv3 update

## -*- Mode: octave -*- 

1;

init_angle = 0;#randn;
q = quaternion(rand(3,1), init_angle);
w = rand(3,1)*20;
s=[3, 3, 3]';
x = [0, -5, 3]';
u = [0, rand*20, rand*5]';
dt=0.01;
m=4;
Im=[3,3,3]';
E_kinetic = m*u'*u/2;
E_dynamic = m*9.81*x(3);
E_rotational = w'*w;
E = E_dynamic + E_kinetic + E_rotational
for t=1:1000

## x - position vector
## q - rotation quaternion
## u - velocity vector
## w - moment of inertia vector
## m - mass
## Im - rotational inertia
## s - size of the polyhedron
#function polyhedral_collide (x, q, u, w, m, Im, s)
  ## object has eight corners
  c = zeros(8, 3);
  length = s(1);
  width = s(2);
  height = s(3);
  ## Front Aft Left Right Top Bottom
  c(1,:) = [+length/2, -width/2, +height/2]; # FLT
  c(2,:) = [+length/2, +width/2, +height/2]; # FRT
  c(3,:) = [-length/2, -width/2, +height/2]; # ALT
  c(4,:) = [-length/2, +width/2, +height/2]; # ART
  c(5,:) = [+length/2, -width/2, -height/2]; # FLT
  c(6,:) = [+length/2, +width/2, -height/2]; # FRT
  c(7,:) = [-length/2, -width/2, -height/2]; # ALT
  c(8,:) = [-length/2, +width/2, -height/2]; # ART
  c = c'; ## local points
  qc = c; ## global points
  ## transform all points to global frame
  for i=1:8
    qc(:,i) = x + qtransv(c(:,i), q);
  end


  dheight = 0;
  collision_i = [];
  for i=1:8
    dci = qc(3, i);
    if (dci < dheight)
      collision_i = [collision_i, i];
      dheight = dci;
    end
  end
  F = [0 0 -9.81*m]';
  M = [0 0 0]';
  E_prev = E;

  ## and get Q derivative
  dq = qderivmat(w./Im)*q';
  q_inv = qinv(q);
  ## trying to get the rotation-only velocity of each point
  
  q_delta = q + 0.01*dq';
  q_delta /= sum(q_delta.*q_delta);
  
  if (1)
    if (size(collision_i) > 0 && u(3) < 0)
      for ci = collision_i
        ## the force required to stop the cube's center of mass
        N_global = -u(3)*m/dt; 

        ## transform from global to local
        N = qtransv([0 0 N_global], q_inv);
        
        ## force on the axis... hm.. irrelevant?
        Fx = N(1);
        Fy = N(2);
        Fz = N(3);

        d = c(:, ci);

        ## rotation-only velocity of each point
        #ru = qtransv(d, dq);#q*0.01);
        ## the velocity of each point in the global fra
        ## rotation-only velocity of each point
        ##ru = qtransv(d, dq);
        ## the velocity of each point in the global frame
        cu_global = u + (qtransv(d, q_delta) - qtransv(d,q))/0.1;
        ## the velocity of each point in the rotating frame
        cu = qtransv(cu_global, q_inv);
        
        ## calculate friction globally
        Friction_global = zeros(3,1);
        mu = 0.1;
        Friction_global(1) -= mu*abs(N_global)*cu_global(1);
        Friction_global(2) -= mu*abs(N_global)*cu_global(2);
        ## z-axis speed is not important here.
        Friction_local = qtransv(Friction_global, q_inv);
        Fx += Friction_local(1);
        Fy += Friction_local(2);
        Fz += Friction_local(3);

        M(1) -= -Fz*d(2) + Fy*d(3);
        M(2) -= +Fz*d(1) - Fx*d(3);
        M(3) -= +Fx*d(2) - Fy*d(1);
        F_local = [Fx Fy Fz]';
        F += qtransv(F_local, q);
      endfor
      x -= [0 0 dheight+u(3)*dt]';
    endif
  else
    collision_i =1;
      Fu = 1000;
      F(3) += Fu;
      M += c(:,collision_i)*Fu;
      ##u(3) = 0;
  endif

  if (mod(t-1, 5)==0)
    hold off;
    plot3(qc(1,:), qc(2,:), qc(3,:), '@-');
    hold on;
    plot3([x(1), x(1)+F(1)/100], [x(2), x(2)+F(2)/100], [x(3), x(3)+F(3)/100],'1-')
    view(75, 10);
    axis([-20, 20, -20, 20, -1, 40], "manual", "square");
    if (0) #size(collision_i) > 0)
      for ci = 1:8 #collision_i
        d = c(:, ci);
        ## the velocity of each point in the global frame
        cu_global = (qtransv(d, q_delta) - qtransv(d,q))/0.1;
        plot3([qc(1,ci), qc(1,ci)+cu_global(1)], [qc(2,ci), qc(2,ci)+cu_global(2)], [qc(3,ci), qc(3,ci)+cu_global(3)],'4-')
      endfor
    endif
    view(75, 10);
    axis([-20, 20, -20, 20, -1, 40], "manual", "square");
  endif

  #if (size(collision_i) > 0)
  #  sleep(0.1);
  #endif

  #g = [0 0 -9.81]' + F/m;
  #g = [0 0 -9.81]' + F/m;

  #M, dheight
  w += M./Im *dt;
  u += F/m*dt;
  x += u*dt;
  E_kinetic = m*u'*u/2;
  E_dynamic = m*9.81*x(3);
  E_rotational = w'*diag(Im)*w;
  E = E_dynamic + E_kinetic + E_rotational;
  ##E_prev *=0.999;
  if (E > 1.01*E_prev) 
    E_diff = E - E_prev;
    E_movement = E_kinetic + E_rotational;
    scale = E_diff /  E_movement;
    E_movement = (1 - scale)*E_movement;
    E_kinetic *= (1 - scale);
    E_rotational *= (1 - scale);
    if (scale>=1)
      u *= 0;
      w *= 0;
    else
      u*=sqrt(1-scale);
      w*=sqrt(1-scale);
    endif
    E = E_dynamic + E_kinetic + E_rotational;
    printf ("E_diff: %f -- (%f) --> %f\n", E_diff, scale, E-E_prev);
  endif
  fflush(stdout);
  ## translate rotational inertia to rotation
  q += dq'*dt;
  q = q / sum(q.^2);
  sleep(0);

  
endfor
#endfunction