light remote vicon control

Light_remote.m

% Program options
TransmitMulticast = false;
EnableHapticFeedbackTest = false;
HapticOnList = {'ViconAP_001';'ViconAP_002'};
bReadCentroids = false;

% A dialog to stop the loop
MessageBox = msgbox( 'DataStream for Light Control is Running. Click OK to Stop.', 'Vicon DataStream' );


% Load the SDK
fprintf( 'Loading SDK...' );
Client.LoadViconDataStreamSDK();
fprintf( 'done\n' );

% Program options
HostName = '169.254.215.174:801';

%UDP settings, corresponds to 
u = udp('169.254.187.95',8001);

%Open UDP Socket
fopen(u);
StudioLights_Init(u);

% Name of Rigid Body Objects to be Tracked
Object_Name_List = {'LightRemote_1'};

% Make a new client
MyClient = Client();

% Connect to a server
fprintf( 'Connecting to %s ...', HostName );
while ~MyClient.IsConnected().Connected
    % Direct connection
    MyClient.Connect( HostName );
    
    % Multicast connection
    % MyClient.ConnectToMulticast( HostName, '224.0.0.0' );
    
    fprintf( '.' );
end
fprintf( '\n' );

% Enable some different data types
MyClient.EnableSegmentData();
MyClient.EnableMarkerData();
MyClient.EnableUnlabeledMarkerData();
MyClient.EnableDeviceData();
if bReadCentroids
    MyClient.EnableCentroidData();
end

fprintf( 'Segment Data Enabled: %s\n',          AdaptBool( MyClient.IsSegmentDataEnabled().Enabled ) );
fprintf( 'Marker Data Enabled: %s\n',           AdaptBool( MyClient.IsMarkerDataEnabled().Enabled ) );
fprintf( 'Unlabeled Marker Data Enabled: %s\n', AdaptBool( MyClient.IsUnlabeledMarkerDataEnabled().Enabled ) );
fprintf( 'Device Data Enabled: %s\n',           AdaptBool( MyClient.IsDeviceDataEnabled().Enabled ) );
fprintf( 'Centroid Data Enabled: %s\n',         AdaptBool( MyClient.IsCentroidDataEnabled().Enabled ) );

% Set the streaming mode
MyClient.SetStreamMode( StreamMode.ClientPull );
% MyClient.SetStreamMode( StreamMode.ClientPullPreFetch );
% MyClient.SetStreamMode( StreamMode.ServerPush );

% Set the global up axis
MyClient.SetAxisMapping( Direction.Forward, ...
    Direction.Left,    ...
    Direction.Up );    % Z-up
% MyClient.SetAxisMapping( Direction.Forward, ...
%                          Direction.Up,      ...
%                          Direction.Right ); % Y-up

Output_GetAxisMapping = MyClient.GetAxisMapping();
fprintf( 'Axis Mapping: X-%s Y-%s Z-%s\n', Output_GetAxisMapping.XAxis.ToString(), ...
    Output_GetAxisMapping.YAxis.ToString(), ...
    Output_GetAxisMapping.ZAxis.ToString() );


% Discover the version number
Output_GetVersion = MyClient.GetVersion();
fprintf( 'Version: %d.%d.%d\n', Output_GetVersion.Major, ...
    Output_GetVersion.Minor, ...
    Output_GetVersion.Point );

if TransmitMulticast
    MyClient.StartTransmittingMulticast( 'localhost', '224.0.0.0' );
end

Critical_Cone_Angle = 15;
Cone_Threshold = tan((pi/180)*Critical_Cone_Angle);

Light_Position_Global = 1000*[...
    0.0759  -1.3121  2.0500;...
    1.2438  -1.3833  2.0500;...
    2.0972  -1.4367  2.0500;...
    3.0905  -1.3869  2.0500;...
    4.0794  -1.4333  2.0500;...
    6.5685  1.0032  2.0500;...
    6.4845  2.2737  2.0500;...
    6.4952  4.4907  2.0500;...
    4.9159  5.6261  2.0500;...
    2.9755  5.5155  2.0500;...
    1.1698  5.5206  2.0500;...
    -0.1983 5.6141  2.0500;...
    -1.8172 4.3653  2.0500;...
    -1.8530 2.3229  2.0500;...
    -1.2884 0.7456  2.0500;...
    -0.3729 2.1789  2.0500;...
    2.0681  1.9934  2.0500;...
    4.3911  2.1633  2.0500;...
    2.0617  3.2602  2.0500;...
    2.1651  0.6428  2.0500;...
    5.0983  -1.5531  2.0500];

Counter = 1;
% Loop until the message box is dismissed
while ishandle( MessageBox )
    drawnow;
    Counter = Counter + 1;
    
    % Get a frame
    fprintf( '\n\nWaiting for new frame...' );
    while MyClient.GetFrame().Result.Value ~= Result.Success
    end% while
    fprintf( '\n' );
    
    % Get the frame number
    Output_GetFrameNumber = MyClient.GetFrameNumber();
    fprintf( 'Frame Number: %d\n', Output_GetFrameNumber.FrameNumber );
    
    % Count the number of subjects
    SubjectCount = MyClient.GetSubjectCount().SubjectCount;
    fprintf( 'Subjects (%d):\n', SubjectCount );
    for SubjectIndex = 1:SubjectCount
        SubjectName = MyClient.GetSubjectName( SubjectIndex ).SubjectName;
        SegmentCount = MyClient.GetSegmentCount( SubjectName ).SegmentCount;
        switch SubjectName
            case Object_Name_List{1}
                for SegmentIndex = 1:SegmentCount
                    SegmentName = MyClient.GetSegmentName( SubjectName, SegmentIndex ).SegmentName;
                    fprintf( '    Name: %s\n', SubjectName );
                    if strcmp(SegmentName,Object_Name_List{1})
                        % Get the global segment translation
                        Output_GetSegmentGlobalTranslation = MyClient.GetSegmentGlobalTranslation( SubjectName, SegmentName );
                        x_1 = Output_GetSegmentGlobalTranslation.Translation(1);
                        y_1 = Output_GetSegmentGlobalTranslation.Translation(2);
                        z_1 = Output_GetSegmentGlobalTranslation.Translation(3);
                        Wand_Position = [x_1 y_1 z_1]'
                        %Wand_Position'
                        %
                        % Get the global segment rotation as a matrix
                        Output_GetSegmentGlobalRotationMatrix = MyClient.GetSegmentGlobalRotationMatrix( SubjectName, SegmentName );
                        zAxis_1 = Output_GetSegmentGlobalRotationMatrix.Rotation( 3 );
                        zAxis_2 = Output_GetSegmentGlobalRotationMatrix.Rotation( 6 );
                        zAxis_3 = Output_GetSegmentGlobalRotationMatrix.Rotation( 9 );
                        zAxis = [zAxis_1 zAxis_2 zAxis_3]';
                    end % end of if
                end% SegmentIndex
                %
                %             case Object_Name_List{2}
                %                 for SegmentIndex = 1:SegmentCount
                %                     SegmentName = MyClient.GetSegmentName( SubjectName, SegmentIndex ).SegmentName;
                %                     fprintf( '    Name: %s\n', SubjectName );
                %                     if strcmp(SegmentName,Object_Name_List{2})
                %                         % Get the global segment translation
                %                         Output_GetSegmentGlobalTranslation = MyClient.GetSegmentGlobalTranslation( SubjectName, SegmentName );
                %                         x_1 = Output_GetSegmentGlobalTranslation.Translation(1);
                %                         y_1 = Output_GetSegmentGlobalTranslation.Translation(2);
                %                         z_1 = Output_GetSegmentGlobalTranslation.Translation(3);
                %                     end % end of if
                %                 end% SegmentIndex
                %
                %
        end% end of SWITCH
    end% SubjectIndex
    %
    %
    if (norm(Wand_Position,2)>0)
        Light_Position_Relative = (Light_Position_Global' - repmat(Wand_Position,1,size(Light_Position_Global,1)))';
        Light_Index = (1:size(Light_Position_Relative,1))';
        
        Dot_Product = Light_Position_Relative*zAxis;
        
        %[Light_Index Dot_Product]
        
        Light_Position_Relative((Dot_Product<0),:) = [];
        Light_Index((Dot_Product<0),:) = [];
        
        Distance_1 = Dot_Product;
        Distance_1((Dot_Product<0),:) = [];
        
        Distance_2 = sqrt(sum(Light_Position_Relative.^2,2) - Distance_1.^2);
        
        Ratio = Distance_2./Distance_1;
        [Closest_Dist,min_idx] = min(Ratio);
        %Light_Index((Ratio>=Cone_Threshold),:) = []
        Next_Transit_Index = Light_Index(min_idx)
                
        if ~isempty(Light_Index) && Next_Transit_Index ~= Transit_Index
            Transit_Index = Next_Transit_Index
            StudioLights_Init(u);
            StudioLights_Control(u,{Transit_Index},'Red',75);
        end
    end
    
end% while true

if TransmitMulticast
    MyClient.StopTransmittingMulticast();
end

% Disconnect and dispose
MyClient.Disconnect();
fclose(u);

% Unload the SDK
fprintf( 'Unloading SDK...' );
Client.UnloadViconDataStreamSDK();
fprintf( 'done\n' );