src/sensor/windows/SDL_windowssensor.c
author Sam Lantinga <slouken@libsdl.org>
Wed, 08 Apr 2020 08:34:27 -0700
changeset 13710 a1a6885cff94
child 13714 0d9ea2541827
permissions -rw-r--r--
First pass at Windows sensor implementation
     1 /*
     2   Simple DirectMedia Layer
     3   Copyright (C) 1997-2020 Sam Lantinga <slouken@libsdl.org>
     4 
     5   This software is provided 'as-is', without any express or implied
     6   warranty.  In no event will the authors be held liable for any damages
     7   arising from the use of this software.
     8 
     9   Permission is granted to anyone to use this software for any purpose,
    10   including commercial applications, and to alter it and redistribute it
    11   freely, subject to the following restrictions:
    12 
    13   1. The origin of this software must not be misrepresented; you must not
    14      claim that you wrote the original software. If you use this software
    15      in a product, an acknowledgment in the product documentation would be
    16      appreciated but is not required.
    17   2. Altered source versions must be plainly marked as such, and must not be
    18      misrepresented as being the original software.
    19   3. This notice may not be removed or altered from any source distribution.
    20 */
    21 #include "../../SDL_internal.h"
    22 
    23 #include "SDL_config.h"
    24 
    25 #if defined(SDL_SENSOR_WINDOWS)
    26 
    27 #include "SDL_error.h"
    28 #include "SDL_mutex.h"
    29 #include "SDL_sensor.h"
    30 #include "SDL_windowssensor.h"
    31 #include "../SDL_syssensor.h"
    32 #include "../../core/windows/SDL_windows.h"
    33 
    34 #define COBJMACROS
    35 #include <InitGuid.h>
    36 #include <SensorsApi.h>
    37 #include <Sensors.h>
    38 
    39 DEFINE_GUID(CLSID_SensorManager, 0x77A1C827, 0xFCD2, 0x4689, 0x89, 0x15, 0x9D, 0x61, 0x3C, 0xC5, 0xFA, 0x3E);
    40 DEFINE_GUID(IID_SensorManager, 0xBD77DB67, 0x45A8, 0x42DC, 0x8D, 0x00, 0x6D, 0xCF, 0x15, 0xF8, 0x37, 0x7A);
    41 DEFINE_GUID(IID_SensorManagerEvents, 0x9B3B0B86, 0x266A, 0x4AAD, 0xB2, 0x1F, 0xFD, 0xE5, 0x50, 0x10, 0x01, 0xB7);
    42 DEFINE_GUID(IID_SensorEvents, 0x5D8DCC91, 0x4641, 0x47E7, 0xB7, 0xC3, 0xB7, 0x4F, 0x48, 0xA6, 0xC3, 0x91);
    43 
    44 DEFINE_PROPERTYKEY(SENSOR_DATA_TYPE_ANGULAR_VELOCITY_X_DEGREES_PER_SECOND, 0X3F8A69A2, 0X7C5, 0X4E48, 0XA9, 0X65, 0XCD, 0X79, 0X7A, 0XAB, 0X56, 0XD5, 10); //[VT_R8]
    45 DEFINE_PROPERTYKEY(SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Y_DEGREES_PER_SECOND, 0X3F8A69A2, 0X7C5, 0X4E48, 0XA9, 0X65, 0XCD, 0X79, 0X7A, 0XAB, 0X56, 0XD5, 11); //[VT_R8]
    46 DEFINE_PROPERTYKEY(SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Z_DEGREES_PER_SECOND, 0X3F8A69A2, 0X7C5, 0X4E48, 0XA9, 0X65, 0XCD, 0X79, 0X7A, 0XAB, 0X56, 0XD5, 12); //[VT_R8]
    47 
    48 typedef struct
    49 {
    50 	SDL_SensorID id;
    51 	ISensor *sensor;
    52 	SENSOR_ID sensor_id;
    53 	char *name;
    54 	SDL_SensorType type;
    55 	SDL_Sensor *sensor_opened;
    56 
    57 } SDL_Windows_Sensor;
    58 
    59 static SDL_bool SDL_windowscoinit;
    60 static ISensorManager *SDL_sensor_manager;
    61 static int SDL_num_sensors;
    62 static SDL_Windows_Sensor *SDL_sensors;
    63 
    64 static int ConnectSensor(ISensor *sensor);
    65 static int DisconnectSensor(ISensor *sensor);
    66 
    67 static HRESULT STDMETHODCALLTYPE ISensorManagerEventsVtbl_QueryInterface(ISensorManagerEvents * This, REFIID riid, void **ppvObject)
    68 {
    69 	if (!ppvObject) {
    70 		return E_INVALIDARG;
    71 	}
    72 
    73 	*ppvObject = NULL;
    74 	if (WIN_IsEqualIID(riid, &IID_IUnknown) || WIN_IsEqualIID(riid, &IID_SensorManagerEvents)) {
    75 		*ppvObject = This;
    76 		return S_OK;
    77 	}
    78 	return E_NOINTERFACE;
    79 }
    80 
    81 static ULONG STDMETHODCALLTYPE ISensorManagerEventsVtbl_AddRef(ISensorManagerEvents * This)
    82 {
    83 	return 1;
    84 }
    85 
    86 static ULONG STDMETHODCALLTYPE ISensorManagerEventsVtbl_Release(ISensorManagerEvents * This)
    87 {
    88 	return 1;
    89 }
    90 
    91 static HRESULT STDMETHODCALLTYPE ISensorManagerEventsVtbl_OnSensorEnter(ISensorManagerEvents * This, ISensor *pSensor, SensorState state)
    92 {
    93 	ConnectSensor(pSensor);
    94 	return S_OK;
    95 }
    96 
    97 static ISensorManagerEventsVtbl sensor_manager_events_vtbl = {
    98 	ISensorManagerEventsVtbl_QueryInterface,
    99 	ISensorManagerEventsVtbl_AddRef,
   100 	ISensorManagerEventsVtbl_Release,
   101 	ISensorManagerEventsVtbl_OnSensorEnter
   102 };
   103 static ISensorManagerEvents sensor_manager_events = {
   104 	&sensor_manager_events_vtbl
   105 };
   106 
   107 static HRESULT STDMETHODCALLTYPE ISensorEventsVtbl_QueryInterface(ISensorEvents * This, REFIID riid, void **ppvObject)
   108 {
   109 	if (!ppvObject) {
   110 		return E_INVALIDARG;
   111 	}
   112 
   113 	*ppvObject = NULL;
   114 	if (WIN_IsEqualIID(riid, &IID_IUnknown) || WIN_IsEqualIID(riid, &IID_SensorEvents)) {
   115 		*ppvObject = This;
   116 		return S_OK;
   117 	}
   118 	return E_NOINTERFACE;
   119 }
   120 
   121 static ULONG STDMETHODCALLTYPE ISensorEventsVtbl_AddRef(ISensorEvents * This)
   122 {
   123 	return 1;
   124 }
   125 
   126 static ULONG STDMETHODCALLTYPE ISensorEventsVtbl_Release(ISensorEvents * This)
   127 {
   128 	return 1;
   129 }
   130 
   131 static HRESULT STDMETHODCALLTYPE ISensorEventsVtbl_OnStateChanged(ISensorEvents * This, ISensor *pSensor, SensorState state)
   132 {
   133 #ifdef DEBUG_SENSORS
   134 	int i;
   135 
   136 	SDL_LockSensors();
   137 	for (i = 0; i < SDL_num_sensors; ++i) {
   138 		if (pSensor == SDL_sensors[i].sensor) {
   139 			SDL_Log("Sensor %s state changed to %d\n", SDL_sensors[i].name, state);
   140 		}
   141 	}
   142 	SDL_UnlockSensors();
   143 #endif
   144 	return S_OK;
   145 }
   146 
   147 static HRESULT STDMETHODCALLTYPE ISensorEventsVtbl_OnDataUpdated(ISensorEvents * This, ISensor *pSensor, ISensorDataReport *pNewData)
   148 {
   149 	int i;
   150 
   151 	SDL_LockSensors();
   152 	for (i = 0; i < SDL_num_sensors; ++i) {
   153 		if (pSensor == SDL_sensors[i].sensor) {
   154 			if (SDL_sensors[i].sensor_opened) {
   155 				HRESULT hrX, hrY, hrZ;
   156 				PROPVARIANT valueX, valueY, valueZ;
   157 
   158 #ifdef DEBUG_SENSORS
   159 				SDL_Log("Sensor %s data updated\n", SDL_sensors[i].name);
   160 #endif
   161 				switch (SDL_sensors[i].type) {
   162 				case SDL_SENSOR_ACCEL:
   163 					hrX = ISensorDataReport_GetSensorValue(pNewData, &SENSOR_DATA_TYPE_ACCELERATION_X_G, &valueX);
   164 					hrY = ISensorDataReport_GetSensorValue(pNewData, &SENSOR_DATA_TYPE_ACCELERATION_Y_G, &valueY);
   165 					hrZ = ISensorDataReport_GetSensorValue(pNewData, &SENSOR_DATA_TYPE_ACCELERATION_Z_G, &valueZ);
   166 					if (SUCCEEDED(hrX) && SUCCEEDED(hrY) && SUCCEEDED(hrZ) &&
   167 						valueX.vt == VT_R8 && valueY.vt == VT_R8 && valueZ.vt == VT_R8) {
   168 						float values[3];
   169 
   170 						values[0] = (float)valueX.dblVal;
   171 						values[1] = (float)valueY.dblVal;
   172 						values[2] = (float)valueZ.dblVal;
   173 						SDL_PrivateSensorUpdate(SDL_sensors[i].sensor_opened, values, 3);
   174 					}
   175 					break;
   176 				case SDL_SENSOR_GYRO:
   177 					hrX = ISensorDataReport_GetSensorValue(pNewData, &SENSOR_DATA_TYPE_ANGULAR_VELOCITY_X_DEGREES_PER_SECOND, &valueX);
   178 					hrY = ISensorDataReport_GetSensorValue(pNewData, &SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Y_DEGREES_PER_SECOND, &valueY);
   179 					hrZ = ISensorDataReport_GetSensorValue(pNewData, &SENSOR_DATA_TYPE_ANGULAR_VELOCITY_Z_DEGREES_PER_SECOND, &valueZ);
   180 					if (SUCCEEDED(hrX) && SUCCEEDED(hrY) && SUCCEEDED(hrZ) &&
   181 						valueX.vt == VT_R8 && valueY.vt == VT_R8 && valueZ.vt == VT_R8) {
   182 						float values[3];
   183 
   184 						values[0] = (float)(valueX.dblVal * (M_PI / 180.0));
   185 						values[1] = (float)(valueY.dblVal * (M_PI / 180.0));
   186 						values[2] = (float)(valueZ.dblVal * (M_PI / 180.0));
   187 						SDL_PrivateSensorUpdate(SDL_sensors[i].sensor_opened, values, 3);
   188 					}
   189 					break;
   190 				default:
   191 					/* FIXME: Need to know how to interpret the data for this sensor */
   192 					break;
   193 				}
   194 			}
   195 			break;
   196 		}
   197 	}
   198 	SDL_UnlockSensors();
   199 
   200 	return S_OK;
   201 }
   202 
   203 static HRESULT STDMETHODCALLTYPE ISensorEventsVtbl_OnEvent(ISensorEvents * This, ISensor *pSensor, REFGUID eventID, IPortableDeviceValues *pEventData)
   204 {
   205 #ifdef DEBUG_SENSORS
   206 	int i;
   207 
   208 	SDL_LockSensors();
   209 	for (i = 0; i < SDL_num_sensors; ++i) {
   210 		if (pSensor == SDL_sensors[i].sensor) {
   211 			SDL_Log("Sensor %s event occurred\n", SDL_sensors[i].name);
   212 		}
   213 	}
   214 	SDL_UnlockSensors();
   215 #endif
   216 	return S_OK;
   217 }
   218 
   219 static HRESULT STDMETHODCALLTYPE ISensorEventsVtbl_OnLeave(ISensorEvents * This, REFSENSOR_ID ID)
   220 {
   221 	int i;
   222 
   223 	SDL_LockSensors();
   224 	for (i = 0; i < SDL_num_sensors; ++i) {
   225 		if (WIN_IsEqualIID(ID, &SDL_sensors[i].sensor_id)) {
   226 #ifdef DEBUG_SENSORS
   227 			SDL_Log("Sensor %s disconnected\n", SDL_sensors[i].name);
   228 #endif
   229 			DisconnectSensor(SDL_sensors[i].sensor);
   230 		}
   231 	}
   232 	SDL_UnlockSensors();
   233 
   234 	return S_OK;
   235 }
   236 
   237 static ISensorEventsVtbl sensor_events_vtbl = {
   238 	ISensorEventsVtbl_QueryInterface,
   239 	ISensorEventsVtbl_AddRef,
   240 	ISensorEventsVtbl_Release,
   241 	ISensorEventsVtbl_OnStateChanged,
   242 	ISensorEventsVtbl_OnDataUpdated,
   243 	ISensorEventsVtbl_OnEvent,
   244 	ISensorEventsVtbl_OnLeave
   245 };
   246 static ISensorEvents sensor_events = {
   247 	&sensor_events_vtbl
   248 };
   249 
   250 static int ConnectSensor(ISensor *sensor)
   251 {
   252 	SDL_Windows_Sensor *new_sensor, *new_sensors;
   253 	HRESULT hr;
   254 	SENSOR_ID sensor_id;
   255 	SENSOR_TYPE_ID type_id;
   256 	SDL_SensorType type;
   257 	BSTR bstr_name = NULL;
   258 	char *name;
   259 
   260 	hr = ISensor_GetID(sensor, &sensor_id);
   261 	if (FAILED(hr)) {
   262 		return SDL_SetError("Couldn't get sensor ID: 0x%.4x", hr);
   263 	}
   264 
   265 	hr = ISensor_GetType(sensor, &type_id);
   266 	if (FAILED(hr)) {
   267 		return SDL_SetError("Couldn't get sensor type: 0x%.4x", hr);
   268 	}
   269 
   270 	if (WIN_IsEqualIID(&type_id, &SENSOR_TYPE_ACCELEROMETER_3D)) {
   271 		type = SDL_SENSOR_ACCEL;
   272 	} else if (WIN_IsEqualIID(&type_id, &SENSOR_TYPE_GYROMETER_3D)) {
   273 		type = SDL_SENSOR_GYRO;
   274 	} else {
   275 		return SDL_SetError("Unknown sensor type");
   276 	}
   277 
   278 	hr = ISensor_GetFriendlyName(sensor, &bstr_name);
   279 	if (SUCCEEDED(hr) && bstr_name) {
   280 		name = WIN_StringToUTF8(bstr_name);
   281 	} else {
   282 		name = SDL_strdup("Unknown Sensor");
   283 	}
   284 	if (!name) {
   285 		return SDL_OutOfMemory();
   286 	}
   287 
   288 	SDL_LockSensors();
   289 	new_sensors = (SDL_Windows_Sensor *)SDL_realloc(SDL_sensors, (SDL_num_sensors + 1) * sizeof(SDL_Windows_Sensor));
   290 	if (new_sensors == NULL) {
   291 		SDL_UnlockSensors();
   292 		return SDL_OutOfMemory();
   293 	}
   294 
   295 	ISensor_AddRef(sensor);
   296 	ISensor_SetEventSink(sensor, &sensor_events);
   297 
   298 	SDL_sensors = new_sensors;
   299 	new_sensor = &SDL_sensors[SDL_num_sensors];
   300 	++SDL_num_sensors;
   301 
   302 	new_sensor->id = SDL_GetNextSensorInstanceID();
   303 	new_sensor->sensor = sensor;
   304 	new_sensor->type = type;
   305 	new_sensor->name = name;
   306 
   307 	SDL_UnlockSensors();
   308 
   309 	return 0;
   310 }
   311 
   312 static int DisconnectSensor(ISensor *sensor)
   313 {
   314 	SDL_Windows_Sensor *old_sensor;
   315 	int i;
   316 
   317 	SDL_LockSensors();
   318 	for (i = 0; i < SDL_num_sensors; ++i) {
   319 		old_sensor = &SDL_sensors[i];
   320 		if (sensor == old_sensor->sensor) {
   321 			ISensor_SetEventSink(sensor, NULL);
   322 			ISensor_Release(sensor);
   323 			SDL_free(old_sensor->name);
   324 			--SDL_num_sensors;
   325 			if (i < SDL_num_sensors) {
   326 				SDL_memmove(&SDL_sensors[i], &SDL_sensors[i + 1], (SDL_num_sensors - i) * sizeof(SDL_sensors[i]));
   327 			}
   328 			break;
   329 		}
   330 	}
   331 	SDL_UnlockSensors();
   332 
   333 	return 0;
   334 }
   335 
   336 static int
   337 SDL_WINDOWS_SensorInit(void)
   338 {
   339 	HRESULT hr;
   340 	ISensorCollection *sensor_collection = NULL;
   341 
   342 	while (!IsDebuggerPresent()) Sleep(100);
   343 
   344 	if (WIN_CoInitialize() == S_OK) {
   345 		SDL_windowscoinit = SDL_TRUE;
   346 	}
   347 
   348 	hr = CoCreateInstance(&CLSID_SensorManager, NULL, CLSCTX_INPROC_SERVER, &IID_SensorManager, &SDL_sensor_manager);
   349 	if (FAILED(hr)) {
   350 		return SDL_SetError("Couldn't create the sensor manager: 0x%.4x", hr);
   351 	}
   352 
   353 	hr = ISensorManager_SetEventSink(SDL_sensor_manager, &sensor_manager_events);
   354 	if (FAILED(hr)) {
   355 		ISensorManager_Release(SDL_sensor_manager);
   356 		return SDL_SetError("Couldn't set the sensor manager event sink: 0x%.4x", hr);
   357 	}
   358 
   359 	hr = ISensorManager_GetSensorsByCategory(SDL_sensor_manager, &SENSOR_CATEGORY_ALL, &sensor_collection);
   360 	if (SUCCEEDED(hr)) {
   361 		ULONG i, count;
   362 
   363 		hr = ISensorCollection_GetCount(sensor_collection, &count);
   364 		if (SUCCEEDED(hr)) {
   365 			for (i = 0; i < count; ++i) {
   366 				ISensor *sensor;
   367 
   368 				hr = ISensorCollection_GetAt(sensor_collection, i, &sensor);
   369 				if (SUCCEEDED(hr)) {
   370 					SensorState state;
   371 
   372 					hr = ISensor_GetState(sensor, &state);
   373 					if (SUCCEEDED(hr)) {
   374 						ISensorManagerEventsVtbl_OnSensorEnter(&sensor_manager_events, sensor, state);
   375 					}
   376 					ISensorManager_Release(sensor);
   377 				}
   378 			}
   379 		}
   380 		ISensorCollection_Release(sensor_collection);
   381 	}
   382     return 0;
   383 }
   384 
   385 static int
   386 SDL_WINDOWS_SensorGetCount(void)
   387 {
   388     return SDL_num_sensors;
   389 }
   390 
   391 static void
   392 SDL_WINDOWS_SensorDetect(void)
   393 {
   394 }
   395 
   396 static const char *
   397 SDL_WINDOWS_SensorGetDeviceName(int device_index)
   398 {
   399     return SDL_sensors[device_index].name;
   400 }
   401 
   402 static SDL_SensorType
   403 SDL_WINDOWS_SensorGetDeviceType(int device_index)
   404 {
   405 	return SDL_sensors[device_index].type;
   406 }
   407 
   408 static int
   409 SDL_WINDOWS_SensorGetDeviceNonPortableType(int device_index)
   410 {
   411     return -1;
   412 }
   413 
   414 static SDL_SensorID
   415 SDL_WINDOWS_SensorGetDeviceInstanceID(int device_index)
   416 {
   417     return SDL_sensors[device_index].id;
   418 }
   419 
   420 static int
   421 SDL_WINDOWS_SensorOpen(SDL_Sensor *sensor, int device_index)
   422 {
   423 	SDL_sensors[device_index].sensor_opened = sensor;
   424     return 0;
   425 }
   426 
   427 static void
   428 SDL_WINDOWS_SensorUpdate(SDL_Sensor *sensor)
   429 {
   430 }
   431 
   432 static void
   433 SDL_WINDOWS_SensorClose(SDL_Sensor *sensor)
   434 {
   435 	int i;
   436 
   437 	for (i = 0; i < SDL_num_sensors; ++i) {
   438 		if (sensor == SDL_sensors[i].sensor_opened) {
   439 			SDL_sensors[i].sensor_opened = NULL;
   440 			break;
   441 		}
   442 	}
   443 }
   444 
   445 static void
   446 SDL_WINDOWS_SensorQuit(void)
   447 {
   448 	while (SDL_num_sensors > 0) {
   449 		DisconnectSensor(SDL_sensors[0].sensor);
   450 	}
   451 
   452 	if (SDL_sensor_manager) {
   453 		ISensorManager_SetEventSink(SDL_sensor_manager, NULL);
   454 		ISensorManager_Release(SDL_sensor_manager);
   455 		SDL_sensor_manager = NULL;
   456 	}
   457 
   458 	if (SDL_windowscoinit) {
   459 		WIN_CoUninitialize();
   460 	}
   461 }
   462 
   463 SDL_SensorDriver SDL_WINDOWS_SensorDriver =
   464 {
   465     SDL_WINDOWS_SensorInit,
   466     SDL_WINDOWS_SensorGetCount,
   467     SDL_WINDOWS_SensorDetect,
   468     SDL_WINDOWS_SensorGetDeviceName,
   469     SDL_WINDOWS_SensorGetDeviceType,
   470     SDL_WINDOWS_SensorGetDeviceNonPortableType,
   471     SDL_WINDOWS_SensorGetDeviceInstanceID,
   472     SDL_WINDOWS_SensorOpen,
   473     SDL_WINDOWS_SensorUpdate,
   474     SDL_WINDOWS_SensorClose,
   475     SDL_WINDOWS_SensorQuit,
   476 };
   477 
   478 #endif /* SDL_SENSOR_WINDOWS */
   479 
   480 /* vi: set ts=4 sw=4 expandtab: */