synth.m

用ＭＡＴＬＡＢ实现ＬＭ算法库

function y = synth( cmds, data, num_steps )
%  SYNTH cmd list based 3D trajectory generator
%  This file generates translation and rotation vectors
%  for a camera moving in 3 space, from a command list and a
%  set of motion parameters.
%
%  Written 11/95,  J. Watlington
%
%  The format of the command vector is :
%  cmds = [ num_frames next_index num_frames next_index ... ]
%
%  Where the trajectory is initialized with the first three rows of
%  the data.  The cmds vector is then traversed, starting at elem 0, not
%  changing the motion parameters (dx/dt and d2x/dt2) for num_frames,
%  at which point the next_index provides a new d2x/dt2.
%
%  The format of the trajectory data is :
%  tx ty tz wx wy wz B   per row output
%

traj_vec_size = 7;
y = zeros( [ num_steps, traj_vec_size] );   %  preallocate output

%  Initialize the state variables

state     = zeros([1,traj_vec_size]);
state_vel = zeros([1,traj_vec_size]);
state_acc = zeros([1,traj_vec_size]);

state     = data( 1, : );
state_vel = data( 2, : );
state_acc = data( 3, : );

%  Init a pointer into the cmd tree, and some cmd state info

[m, num_cmds] = size( cmds );
num_cmds = num_cmds - 1;
cmd_index = 2;
step_count = cmds( 1 );

for time = 1:num_steps    %  main time loop

  %  Output the current state

  y( time, : ) = state;

  %  And calculate the next state.  First we check
  %  to see if it is time to change motion parameters

  if step_count <= 1
    if cmd_index < num_cmds
      state_acc = data( cmds( cmd_index ), : );
      cmd_index = cmd_index + 1;
      step_count = cmds( cmd_index );
      cmd_index = cmd_index + 1;      
    else   %  end of command script
      state_acc = data( 3, : );     %  Turn off driving forces
      step_count = 200000;
    end
  else
    step_count = step_count - 1;
  end

  %  Actually update the state.  All vel and acc are in unit step terms

  state = state + state_vel;
  state_vel = state_vel + state_acc;
end