src/sensor/windows/SDL_windowssensor.c
author Sam Lantinga <slouken@libsdl.org>
Wed, 08 Apr 2020 10:27:30 -0700
changeset 13716 d2647d9e93c7
parent 13714 0d9ea2541827
child 13768 5e7c6765ceda
permissions -rw-r--r--
Fixed Windows accelerometer data units
     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 * SDL_STANDARD_GRAVITY;
   171                         values[1] = (float)valueY.dblVal * SDL_STANDARD_GRAVITY;
   172                         values[2] = (float)valueZ.dblVal * SDL_STANDARD_GRAVITY;
   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                         const float DEGREES_TO_RADIANS = (float)(M_PI / 180.0f);
   183                         float values[3];
   184 
   185                         values[0] = (float)valueX.dblVal * DEGREES_TO_RADIANS;
   186                         values[1] = (float)valueY.dblVal * DEGREES_TO_RADIANS;
   187                         values[2] = (float)valueZ.dblVal * DEGREES_TO_RADIANS;
   188                         SDL_PrivateSensorUpdate(SDL_sensors[i].sensor_opened, values, 3);
   189                     }
   190                     break;
   191                 default:
   192                     /* FIXME: Need to know how to interpret the data for this sensor */
   193                     break;
   194                 }
   195             }
   196             break;
   197         }
   198     }
   199     SDL_UnlockSensors();
   200 
   201     return S_OK;
   202 }
   203 
   204 static HRESULT STDMETHODCALLTYPE ISensorEventsVtbl_OnEvent(ISensorEvents * This, ISensor *pSensor, REFGUID eventID, IPortableDeviceValues *pEventData)
   205 {
   206 #ifdef DEBUG_SENSORS
   207     int i;
   208 
   209     SDL_LockSensors();
   210     for (i = 0; i < SDL_num_sensors; ++i) {
   211         if (pSensor == SDL_sensors[i].sensor) {
   212             SDL_Log("Sensor %s event occurred\n", SDL_sensors[i].name);
   213         }
   214     }
   215     SDL_UnlockSensors();
   216 #endif
   217     return S_OK;
   218 }
   219 
   220 static HRESULT STDMETHODCALLTYPE ISensorEventsVtbl_OnLeave(ISensorEvents * This, REFSENSOR_ID ID)
   221 {
   222     int i;
   223 
   224     SDL_LockSensors();
   225     for (i = 0; i < SDL_num_sensors; ++i) {
   226         if (WIN_IsEqualIID(ID, &SDL_sensors[i].sensor_id)) {
   227 #ifdef DEBUG_SENSORS
   228             SDL_Log("Sensor %s disconnected\n", SDL_sensors[i].name);
   229 #endif
   230             DisconnectSensor(SDL_sensors[i].sensor);
   231         }
   232     }
   233     SDL_UnlockSensors();
   234 
   235     return S_OK;
   236 }
   237 
   238 static ISensorEventsVtbl sensor_events_vtbl = {
   239     ISensorEventsVtbl_QueryInterface,
   240     ISensorEventsVtbl_AddRef,
   241     ISensorEventsVtbl_Release,
   242     ISensorEventsVtbl_OnStateChanged,
   243     ISensorEventsVtbl_OnDataUpdated,
   244     ISensorEventsVtbl_OnEvent,
   245     ISensorEventsVtbl_OnLeave
   246 };
   247 static ISensorEvents sensor_events = {
   248     &sensor_events_vtbl
   249 };
   250 
   251 static int ConnectSensor(ISensor *sensor)
   252 {
   253     SDL_Windows_Sensor *new_sensor, *new_sensors;
   254     HRESULT hr;
   255     SENSOR_ID sensor_id;
   256     SENSOR_TYPE_ID type_id;
   257     SDL_SensorType type;
   258     BSTR bstr_name = NULL;
   259     char *name;
   260 
   261     hr = ISensor_GetID(sensor, &sensor_id);
   262     if (FAILED(hr)) {
   263         return SDL_SetError("Couldn't get sensor ID: 0x%.4x", hr);
   264     }
   265 
   266     hr = ISensor_GetType(sensor, &type_id);
   267     if (FAILED(hr)) {
   268         return SDL_SetError("Couldn't get sensor type: 0x%.4x", hr);
   269     }
   270 
   271     if (WIN_IsEqualIID(&type_id, &SENSOR_TYPE_ACCELEROMETER_3D)) {
   272         type = SDL_SENSOR_ACCEL;
   273     } else if (WIN_IsEqualIID(&type_id, &SENSOR_TYPE_GYROMETER_3D)) {
   274         type = SDL_SENSOR_GYRO;
   275     } else {
   276         return SDL_SetError("Unknown sensor type");
   277     }
   278 
   279     hr = ISensor_GetFriendlyName(sensor, &bstr_name);
   280     if (SUCCEEDED(hr) && bstr_name) {
   281         name = WIN_StringToUTF8(bstr_name);
   282     } else {
   283         name = SDL_strdup("Unknown Sensor");
   284     }
   285     if (bstr_name != NULL) {
   286         SysFreeString(bstr_name);
   287     }
   288     if (!name) {
   289         return SDL_OutOfMemory();
   290     }
   291 
   292     SDL_LockSensors();
   293     new_sensors = (SDL_Windows_Sensor *)SDL_realloc(SDL_sensors, (SDL_num_sensors + 1) * sizeof(SDL_Windows_Sensor));
   294     if (new_sensors == NULL) {
   295         SDL_UnlockSensors();
   296         return SDL_OutOfMemory();
   297     }
   298 
   299     ISensor_AddRef(sensor);
   300     ISensor_SetEventSink(sensor, &sensor_events);
   301 
   302     SDL_sensors = new_sensors;
   303     new_sensor = &SDL_sensors[SDL_num_sensors];
   304     ++SDL_num_sensors;
   305 
   306     SDL_zerop(new_sensor);
   307     new_sensor->id = SDL_GetNextSensorInstanceID();
   308     new_sensor->sensor = sensor;
   309     new_sensor->type = type;
   310     new_sensor->name = name;
   311 
   312     SDL_UnlockSensors();
   313 
   314     return 0;
   315 }
   316 
   317 static int DisconnectSensor(ISensor *sensor)
   318 {
   319     SDL_Windows_Sensor *old_sensor;
   320     int i;
   321 
   322     SDL_LockSensors();
   323     for (i = 0; i < SDL_num_sensors; ++i) {
   324         old_sensor = &SDL_sensors[i];
   325         if (sensor == old_sensor->sensor) {
   326             ISensor_SetEventSink(sensor, NULL);
   327             ISensor_Release(sensor);
   328             SDL_free(old_sensor->name);
   329             --SDL_num_sensors;
   330             if (i < SDL_num_sensors) {
   331                 SDL_memmove(&SDL_sensors[i], &SDL_sensors[i + 1], (SDL_num_sensors - i) * sizeof(SDL_sensors[i]));
   332             }
   333             break;
   334         }
   335     }
   336     SDL_UnlockSensors();
   337 
   338     return 0;
   339 }
   340 
   341 static int
   342 SDL_WINDOWS_SensorInit(void)
   343 {
   344     HRESULT hr;
   345     ISensorCollection *sensor_collection = NULL;
   346 
   347     if (WIN_CoInitialize() == S_OK) {
   348         SDL_windowscoinit = SDL_TRUE;
   349     }
   350 
   351     hr = CoCreateInstance(&CLSID_SensorManager, NULL, CLSCTX_INPROC_SERVER, &IID_SensorManager, &SDL_sensor_manager);
   352     if (FAILED(hr)) {
   353         return SDL_SetError("Couldn't create the sensor manager: 0x%.4x", hr);
   354     }
   355 
   356     hr = ISensorManager_SetEventSink(SDL_sensor_manager, &sensor_manager_events);
   357     if (FAILED(hr)) {
   358         ISensorManager_Release(SDL_sensor_manager);
   359         return SDL_SetError("Couldn't set the sensor manager event sink: 0x%.4x", hr);
   360     }
   361 
   362     hr = ISensorManager_GetSensorsByCategory(SDL_sensor_manager, &SENSOR_CATEGORY_ALL, &sensor_collection);
   363     if (SUCCEEDED(hr)) {
   364         ULONG i, count;
   365 
   366         hr = ISensorCollection_GetCount(sensor_collection, &count);
   367         if (SUCCEEDED(hr)) {
   368             for (i = 0; i < count; ++i) {
   369                 ISensor *sensor;
   370 
   371                 hr = ISensorCollection_GetAt(sensor_collection, i, &sensor);
   372                 if (SUCCEEDED(hr)) {
   373                     SensorState state;
   374 
   375                     hr = ISensor_GetState(sensor, &state);
   376                     if (SUCCEEDED(hr)) {
   377                         ISensorManagerEventsVtbl_OnSensorEnter(&sensor_manager_events, sensor, state);
   378                     }
   379                     ISensorManager_Release(sensor);
   380                 }
   381             }
   382         }
   383         ISensorCollection_Release(sensor_collection);
   384     }
   385     return 0;
   386 }
   387 
   388 static int
   389 SDL_WINDOWS_SensorGetCount(void)
   390 {
   391     return SDL_num_sensors;
   392 }
   393 
   394 static void
   395 SDL_WINDOWS_SensorDetect(void)
   396 {
   397 }
   398 
   399 static const char *
   400 SDL_WINDOWS_SensorGetDeviceName(int device_index)
   401 {
   402     return SDL_sensors[device_index].name;
   403 }
   404 
   405 static SDL_SensorType
   406 SDL_WINDOWS_SensorGetDeviceType(int device_index)
   407 {
   408     return SDL_sensors[device_index].type;
   409 }
   410 
   411 static int
   412 SDL_WINDOWS_SensorGetDeviceNonPortableType(int device_index)
   413 {
   414     return -1;
   415 }
   416 
   417 static SDL_SensorID
   418 SDL_WINDOWS_SensorGetDeviceInstanceID(int device_index)
   419 {
   420     return SDL_sensors[device_index].id;
   421 }
   422 
   423 static int
   424 SDL_WINDOWS_SensorOpen(SDL_Sensor *sensor, int device_index)
   425 {
   426     SDL_sensors[device_index].sensor_opened = sensor;
   427     return 0;
   428 }
   429 
   430 static void
   431 SDL_WINDOWS_SensorUpdate(SDL_Sensor *sensor)
   432 {
   433 }
   434 
   435 static void
   436 SDL_WINDOWS_SensorClose(SDL_Sensor *sensor)
   437 {
   438     int i;
   439 
   440     for (i = 0; i < SDL_num_sensors; ++i) {
   441         if (sensor == SDL_sensors[i].sensor_opened) {
   442             SDL_sensors[i].sensor_opened = NULL;
   443             break;
   444         }
   445     }
   446 }
   447 
   448 static void
   449 SDL_WINDOWS_SensorQuit(void)
   450 {
   451     while (SDL_num_sensors > 0) {
   452         DisconnectSensor(SDL_sensors[0].sensor);
   453     }
   454 
   455     if (SDL_sensor_manager) {
   456         ISensorManager_SetEventSink(SDL_sensor_manager, NULL);
   457         ISensorManager_Release(SDL_sensor_manager);
   458         SDL_sensor_manager = NULL;
   459     }
   460 
   461     if (SDL_windowscoinit) {
   462         WIN_CoUninitialize();
   463     }
   464 }
   465 
   466 SDL_SensorDriver SDL_WINDOWS_SensorDriver =
   467 {
   468     SDL_WINDOWS_SensorInit,
   469     SDL_WINDOWS_SensorGetCount,
   470     SDL_WINDOWS_SensorDetect,
   471     SDL_WINDOWS_SensorGetDeviceName,
   472     SDL_WINDOWS_SensorGetDeviceType,
   473     SDL_WINDOWS_SensorGetDeviceNonPortableType,
   474     SDL_WINDOWS_SensorGetDeviceInstanceID,
   475     SDL_WINDOWS_SensorOpen,
   476     SDL_WINDOWS_SensorUpdate,
   477     SDL_WINDOWS_SensorClose,
   478     SDL_WINDOWS_SensorQuit,
   479 };
   480 
   481 #endif /* SDL_SENSOR_WINDOWS */
   482 
   483 /* vi: set ts=4 sw=4 expandtab: */