function [OutputVector] = SimulinkVehicle(InputVector);
%SimulinkVehicle - called by Simulink to calculate vehicle trajectories.
%
%  USAGE - This function is called be the Simulink playback example,
%     PlaybackExample.mdl to generate vehicle trajectories to be saved
%     for AVDS playback.%

%   Steve Rasmussen, April 2001
%   Copyright (c) 2001 by RasSimTech Ltd
%   $Revision: 1.0.0 $  $Date: 2001/04/15 00:00:00 $

%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Inputs
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
ElapsedTime = InputVector(1);
VehicleID = InputVector(2);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% PARAMETERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
MaxNumberVehicles = 10;

iHorizontalSpacing = 50;	%feet
iVerticalSpacing = 10;	%feet
iInitAltitude = 1000;	%feet

NorthVelocity = 250.0;	%feet/sec	
EastVelocity = 0.0;	%feet/sec	
VerticalVelocity = 0.0;	%feet/sec

XAngularRate = 10.0; %degrees/sec
YAngularRate = 0.0; %degrees/sec
ZAngularRate = 0.0; %degrees/sec



%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% STORAGE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
persistent ElapsedTimeLast;
persistent NorthPosition;
persistent EastPosition;
persistent DownPosition;
persistent XRotation;
persistent YRotation;
persistent ZRotation;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Initialization
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if(ElapsedTime <= 0.0),
	ElapsedTimeLast = zeros(MaxNumberVehicles,1);
	NorthPosition = zeros(MaxNumberVehicles,1);
	EastPosition = [iHorizontalSpacing:iHorizontalSpacing:MaxNumberVehicles*iHorizontalSpacing]';
	DownPosition = [-MaxNumberVehicles*iVerticalSpacing:iVerticalSpacing:-iVerticalSpacing]'-iInitAltitude;
	XRotation = zeros(MaxNumberVehicles,1);
	YRotation = zeros(MaxNumberVehicles,1);
   ZRotation = zeros(MaxNumberVehicles,1);
end;	%if(ElapsedTime <= 0.0),


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% build vehicle trajectories and store state data
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
TimeIncrement = ElapsedTime - ElapsedTimeLast(VehicleID);
ElapsedTimeLast(VehicleID) = ElapsedTime;

NorthVelocityInc = NorthVelocity*TimeIncrement;
EastVelocityInc = EastVelocity*TimeIncrement;
VerticalVelocityInc = VerticalVelocity*TimeIncrement;

XAngularRateInc = XAngularRate*TimeIncrement;
YAngularRateInc = YAngularRate*TimeIncrement;
ZAngularRateInc = ZAngularRate*TimeIncrement;

NorthPosition(VehicleID) = NorthPosition(VehicleID) + NorthVelocityInc;
EastPosition(VehicleID) = EastPosition(VehicleID) + EastVelocityInc;
DownPosition(VehicleID) = DownPosition(VehicleID) - VerticalVelocityInc;
XRotation(VehicleID) = XRotation(VehicleID) + XAngularRateInc;
YRotation(VehicleID) = YRotation(VehicleID) + YAngularRateInc;
ZRotation(VehicleID) = ZRotation(VehicleID) + ZAngularRateInc;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% build output vector
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
OutputVector = [ElapsedTime,  ...
      				NorthPosition(VehicleID),EastPosition(VehicleID),DownPosition(VehicleID), ...
            		XRotation(VehicleID), YRotation(VehicleID), ZRotation(VehicleID)];
            
return;		%SimulinkVehicle