function vehiclePolygon = exampleHelperTransformPointtoPolygon(thetad, vehiclePosition)
% exampleHelperInitializeVisualization is a helper function for plotting the
% scenario of the urban driving example


% Inputs:
%   -thetad:
%       Vehicle orientation in degrees
%   -vehiclePosition:
%      Vehicle center point (x,y)

% Outputs:
%   -vehiclePolygon:
%      Constructed vehicle polygon at the position of the vehicle.

% Example of Calling this function:
%       egoPolygon = exampleHelperTransformPointtoPolygon(Yaw, egoCenter);
% This is a helper script for example purposes and may be removed or
% modified in the future.

% Copyright 2019 The MathWorks, Inc.

%% Helper function that transforms vehicle point position to vehicle polygon

% Loading the predefined vehicle dimensions to be applied to
% ego vehicle and other vehicles in the scenario.
carPolygon = evalin('base', 'carPolygon');

pointPose = repmat(vehiclePosition, 4, 1)';
vehiclePolygon =  pointPose + carPolygon;
R = [cosd(thetad) -sind(thetad); sind(thetad) cosd(thetad)];
vehiclePolygon = R * (vehiclePolygon - pointPose) + pointPose;
end