src/sensor/windows/SDL_windowssensor.c
author Sam Lantinga <slouken@libsdl.org>
Wed, 08 Apr 2020 09:00:10 -0700
changeset 13714 0d9ea2541827
parent 13710 a1a6885cff94
child 13716 d2647d9e93c7
permissions -rw-r--r--
Fixed memory leak and removed debug code from 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 (bstr_name != NULL) {
   285         SysFreeString(bstr_name);
   286     }
   287 	if (!name) {
   288 		return SDL_OutOfMemory();
   289 	}
   290 
   291 	SDL_LockSensors();
   292 	new_sensors = (SDL_Windows_Sensor *)SDL_realloc(SDL_sensors, (SDL_num_sensors + 1) * sizeof(SDL_Windows_Sensor));
   293 	if (new_sensors == NULL) {
   294 		SDL_UnlockSensors();
   295 		return SDL_OutOfMemory();
   296 	}
   297 
   298 	ISensor_AddRef(sensor);
   299 	ISensor_SetEventSink(sensor, &sensor_events);
   300 
   301 	SDL_sensors = new_sensors;
   302 	new_sensor = &SDL_sensors[SDL_num_sensors];
   303 	++SDL_num_sensors;
   304 
   305 	new_sensor->id = SDL_GetNextSensorInstanceID();
   306 	new_sensor->sensor = sensor;
   307 	new_sensor->type = type;
   308 	new_sensor->name = name;
   309 
   310 	SDL_UnlockSensors();
   311 
   312 	return 0;
   313 }
   314 
   315 static int DisconnectSensor(ISensor *sensor)
   316 {
   317 	SDL_Windows_Sensor *old_sensor;
   318 	int i;
   319 
   320 	SDL_LockSensors();
   321 	for (i = 0; i < SDL_num_sensors; ++i) {
   322 		old_sensor = &SDL_sensors[i];
   323 		if (sensor == old_sensor->sensor) {
   324 			ISensor_SetEventSink(sensor, NULL);
   325 			ISensor_Release(sensor);
   326 			SDL_free(old_sensor->name);
   327 			--SDL_num_sensors;
   328 			if (i < SDL_num_sensors) {
   329 				SDL_memmove(&SDL_sensors[i], &SDL_sensors[i + 1], (SDL_num_sensors - i) * sizeof(SDL_sensors[i]));
   330 			}
   331 			break;
   332 		}
   333 	}
   334 	SDL_UnlockSensors();
   335 
   336 	return 0;
   337 }
   338 
   339 static int
   340 SDL_WINDOWS_SensorInit(void)
   341 {
   342 	HRESULT hr;
   343 	ISensorCollection *sensor_collection = NULL;
   344 
   345 	if (WIN_CoInitialize() == S_OK) {
   346 		SDL_windowscoinit = SDL_TRUE;
   347 	}
   348 
   349 	hr = CoCreateInstance(&CLSID_SensorManager, NULL, CLSCTX_INPROC_SERVER, &IID_SensorManager, &SDL_sensor_manager);
   350 	if (FAILED(hr)) {
   351 		return SDL_SetError("Couldn't create the sensor manager: 0x%.4x", hr);
   352 	}
   353 
   354 	hr = ISensorManager_SetEventSink(SDL_sensor_manager, &sensor_manager_events);
   355 	if (FAILED(hr)) {
   356 		ISensorManager_Release(SDL_sensor_manager);
   357 		return SDL_SetError("Couldn't set the sensor manager event sink: 0x%.4x", hr);
   358 	}
   359 
   360 	hr = ISensorManager_GetSensorsByCategory(SDL_sensor_manager, &SENSOR_CATEGORY_ALL, &sensor_collection);
   361 	if (SUCCEEDED(hr)) {
   362 		ULONG i, count;
   363 
   364 		hr = ISensorCollection_GetCount(sensor_collection, &count);
   365 		if (SUCCEEDED(hr)) {
   366 			for (i = 0; i < count; ++i) {
   367 				ISensor *sensor;
   368 
   369 				hr = ISensorCollection_GetAt(sensor_collection, i, &sensor);
   370 				if (SUCCEEDED(hr)) {
   371 					SensorState state;
   372 
   373 					hr = ISensor_GetState(sensor, &state);
   374 					if (SUCCEEDED(hr)) {
   375 						ISensorManagerEventsVtbl_OnSensorEnter(&sensor_manager_events, sensor, state);
   376 					}
   377 					ISensorManager_Release(sensor);
   378 				}
   379 			}
   380 		}
   381 		ISensorCollection_Release(sensor_collection);
   382 	}
   383     return 0;
   384 }
   385 
   386 static int
   387 SDL_WINDOWS_SensorGetCount(void)
   388 {
   389     return SDL_num_sensors;
   390 }
   391 
   392 static void
   393 SDL_WINDOWS_SensorDetect(void)
   394 {
   395 }
   396 
   397 static const char *
   398 SDL_WINDOWS_SensorGetDeviceName(int device_index)
   399 {
   400     return SDL_sensors[device_index].name;
   401 }
   402 
   403 static SDL_SensorType
   404 SDL_WINDOWS_SensorGetDeviceType(int device_index)
   405 {
   406 	return SDL_sensors[device_index].type;
   407 }
   408 
   409 static int
   410 SDL_WINDOWS_SensorGetDeviceNonPortableType(int device_index)
   411 {
   412     return -1;
   413 }
   414 
   415 static SDL_SensorID
   416 SDL_WINDOWS_SensorGetDeviceInstanceID(int device_index)
   417 {
   418     return SDL_sensors[device_index].id;
   419 }
   420 
   421 static int
   422 SDL_WINDOWS_SensorOpen(SDL_Sensor *sensor, int device_index)
   423 {
   424 	SDL_sensors[device_index].sensor_opened = sensor;
   425     return 0;
   426 }
   427 
   428 static void
   429 SDL_WINDOWS_SensorUpdate(SDL_Sensor *sensor)
   430 {
   431 }
   432 
   433 static void
   434 SDL_WINDOWS_SensorClose(SDL_Sensor *sensor)
   435 {
   436 	int i;
   437 
   438 	for (i = 0; i < SDL_num_sensors; ++i) {
   439 		if (sensor == SDL_sensors[i].sensor_opened) {
   440 			SDL_sensors[i].sensor_opened = NULL;
   441 			break;
   442 		}
   443 	}
   444 }
   445 
   446 static void
   447 SDL_WINDOWS_SensorQuit(void)
   448 {
   449 	while (SDL_num_sensors > 0) {
   450 		DisconnectSensor(SDL_sensors[0].sensor);
   451 	}
   452 
   453 	if (SDL_sensor_manager) {
   454 		ISensorManager_SetEventSink(SDL_sensor_manager, NULL);
   455 		ISensorManager_Release(SDL_sensor_manager);
   456 		SDL_sensor_manager = NULL;
   457 	}
   458 
   459 	if (SDL_windowscoinit) {
   460 		WIN_CoUninitialize();
   461 	}
   462 }
   463 
   464 SDL_SensorDriver SDL_WINDOWS_SensorDriver =
   465 {
   466     SDL_WINDOWS_SensorInit,
   467     SDL_WINDOWS_SensorGetCount,
   468     SDL_WINDOWS_SensorDetect,
   469     SDL_WINDOWS_SensorGetDeviceName,
   470     SDL_WINDOWS_SensorGetDeviceType,
   471     SDL_WINDOWS_SensorGetDeviceNonPortableType,
   472     SDL_WINDOWS_SensorGetDeviceInstanceID,
   473     SDL_WINDOWS_SensorOpen,
   474     SDL_WINDOWS_SensorUpdate,
   475     SDL_WINDOWS_SensorClose,
   476     SDL_WINDOWS_SensorQuit,
   477 };
   478 
   479 #endif /* SDL_SENSOR_WINDOWS */
   480 
   481 /* vi: set ts=4 sw=4 expandtab: */