Skip to content

Instantly share code, notes, and snippets.

@jonathanzong

jonathanzong/Light_remote.m Secret

Last active November 8, 2016 01:02
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save jonathanzong/ccff530a3daacd87d943739e533ea492 to your computer and use it in GitHub Desktop.
Save jonathanzong/ccff530a3daacd87d943739e533ea492 to your computer and use it in GitHub Desktop.
Download ZIP
light remote vicon control
Raw
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' );
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment