Implement motion controls in JoyKit, implement accel/gyro in DualSense and DualShock 4, implement motion controls in Cocoa

Cocoa/GBView.m

@@ -462,6 +462,11 @@ static const uint8_t workboy_vk_to_key[] = {
 - (bool)shouldControllerUseJoystickForMotion:(JOYController *)controller
 {
     if (!GB_has_accelerometer(_gb)) return false;
+    for (JOYAxes3D *axes in controller.axes3D) {
+        if (axes.usage == JOYAxes3DUsageOrientation || axes.usage == JOYAxes3DUsageAcceleration) {
+            return false;
+        }
+    }
     return true;
 }
 
@@ -494,6 +499,24 @@ static const uint8_t workboy_vk_to_key[] = {
     }
 }
 
+- (void)controller:(JOYController *)controller movedAxes3D:(JOYAxes3D *)axes
+{
+    if (axes.usage == JOYAxes3DUsageOrientation) {
+        for (JOYAxes3D *axes in controller.axes3D) {
+            // Only use orientation if there's no acceleration axes
+            if (axes.usage == JOYAxes3DUsageAcceleration) {
+                return;
+            }
+        }
+        JOYPoint3D point = axes.normalizedValue;
+        GB_set_accelerometer_values(_gb, point.x, point.z);
+    }
+    else if (axes.usage == JOYAxes3DUsageAcceleration) {
+        JOYPoint3D point = axes.gUnitsValue;
+        GB_set_accelerometer_values(_gb, point.x, point.z);
+    }
+}
+
 - (void)controller:(JOYController *)controller buttonChangedState:(JOYButton *)button
 {
     if (![self.window isMainWindow]) return;

JoyKit/ControllerConfiguration.inc

@@ -110,6 +110,22 @@ hacksByManufacturer = @{
             @{@"reportID": @(0x31), @"size":@1, @"offset":@0x50, @"usagePage":@(kHIDPage_Button), @"usage":@13},
             @{@"reportID": @(0x31), @"size":@1, @"offset":@0x51, @"usagePage":@(kHIDPage_Button), @"usage":@14},
             @{@"reportID": @(0x31), @"size":@1, @"offset":@0x52, @"usagePage":@(kHIDPage_Button), @"usage":@15},
+            
+            @{@"reportID": @(0x31), @"size":@16, @"offset":@0x80, @"usagePage":@(kHIDPage_Sensor), @"min": @-0x7FFF, @"max": @0x7FFF, @"usage":@(kHIDUsage_Snsr_Data_Motion_AngularVelocityXAxis)},
+            @{@"reportID": @(0x31), @"size":@16, @"offset":@0x90, @"usagePage":@(kHIDPage_Sensor), @"min": @-0x7FFF, @"max": @0x7FFF, @"usage":@(kHIDUsage_Snsr_Data_Motion_AngularVelocityYAxis)},
+            @{@"reportID": @(0x31), @"size":@16, @"offset":@0xA0, @"usagePage":@(kHIDPage_Sensor), @"min": @-0x7FFF, @"max": @0x7FFF, @"usage":@(kHIDUsage_Snsr_Data_Motion_AngularVelocityZAxis)},
+            @{@"reportID": @(0x31), @"size":@16, @"offset":@0xB0, @"usagePage":@(kHIDPage_Sensor), @"min": @-0x7FFF, @"max": @0x7FFF, @"usage":@(kHIDUsage_Snsr_Data_Motion_AccelerationAxisX)},
+            @{@"reportID": @(0x31), @"size":@16, @"offset":@0xC0, @"usagePage":@(kHIDPage_Sensor), @"min": @-0x7FFF, @"max": @0x7FFF, @"usage":@(kHIDUsage_Snsr_Data_Motion_AccelerationAxisY)},
+            @{@"reportID": @(0x31), @"size":@16, @"offset":@0xD0, @"usagePage":@(kHIDPage_Sensor), @"min": @-0x7FFF, @"max": @0x7FFF, @"usage":@(kHIDUsage_Snsr_Data_Motion_AccelerationAxisZ)},
+            ],
+            
+            @(1): @[
+                @{@"reportID": @(1), @"size":@16, @"offset":@0x78, @"usagePage":@(kHIDPage_Sensor), @"min": @-0x7FFF, @"max": @0x7FFF, @"usage":@(kHIDUsage_Snsr_Data_Motion_AngularVelocityXAxis)},
+                @{@"reportID": @(1), @"size":@16, @"offset":@0x88, @"usagePage":@(kHIDPage_Sensor), @"min": @-0x7FFF, @"max": @0x7FFF, @"usage":@(kHIDUsage_Snsr_Data_Motion_AngularVelocityYAxis)},
+                @{@"reportID": @(1), @"size":@16, @"offset":@0x98, @"usagePage":@(kHIDPage_Sensor), @"min": @-0x7FFF, @"max": @0x7FFF, @"usage":@(kHIDUsage_Snsr_Data_Motion_AngularVelocityZAxis)},
+                @{@"reportID": @(1), @"size":@16, @"offset":@0xA8, @"usagePage":@(kHIDPage_Sensor), @"min": @-0x7FFF, @"max": @0x7FFF, @"usage":@(kHIDUsage_Snsr_Data_Motion_AccelerationAxisX)},
+                @{@"reportID": @(1), @"size":@16, @"offset":@0xB8, @"usagePage":@(kHIDPage_Sensor), @"min": @-0x7FFF, @"max": @0x7FFF, @"usage":@(kHIDUsage_Snsr_Data_Motion_AccelerationAxisY)},
+                @{@"reportID": @(1), @"size":@16, @"offset":@0xC8, @"usagePage":@(kHIDPage_Sensor), @"min": @-0x7FFF, @"max": @0x7FFF, @"usage":@(kHIDUsage_Snsr_Data_Motion_AccelerationAxisZ)},
             ],
         },
          

JoyKit/JOYAxes3D.h

@@ -0,0 +1,27 @@
+#import <Foundation/Foundation.h>
+
+typedef enum {
+    JOYAxes3DUsageNone,
+    JOYAxes3DUsageAcceleration,
+    JOYAxes3DUsageOrientation,
+    JOYAxes3DUsageGyroscope,
+    JOYAxes3DUsageNonGenericMax,
+    
+    JOYAxes3DUsageGeneric0 = 0x10000,
+} JOYAxes3DUsage;
+
+typedef struct {
+    double x, y, z;
+} JOYPoint3D;
+
+@interface JOYAxes3D : NSObject
+- (NSString *)usageString;
++ (NSString *)usageToString: (JOYAxes3DUsage) usage;
+- (uint64_t)uniqueID;
+- (JOYPoint3D)rawValue;
+- (JOYPoint3D)normalizedValue; // For orientation
+- (JOYPoint3D)gUnitsValue; // For acceleration
+@property JOYAxes3DUsage usage;
+@end
+
+

JoyKit/JOYAxes3D.m

@@ -0,0 +1,108 @@
+#import "JOYAxes3D.h"
+#import "JOYElement.h"
+
+@implementation JOYAxes3D
+{
+    JOYElement *_element1, *_element2, *_element3;
+    double _state1, _state2, _state3;
+    int32_t _minX, _minY, _minZ;
+    int32_t _maxX, _maxY, _maxZ;
+    double _gApproximation;
+}
+
++ (NSString *)usageToString: (JOYAxes3DUsage) usage
+{
+    if (usage < JOYAxes3DUsageNonGenericMax) {
+        return (NSString *[]) {
+            @"None",
+            @"Acceleretion",
+            @"Orientation",
+            @"Gyroscope",
+        }[usage];
+    }
+    if (usage >= JOYAxes3DUsageGeneric0) {
+        return [NSString stringWithFormat:@"Generic 3D Analog Control %d", usage - JOYAxes3DUsageGeneric0];
+    }
+    
+    return [NSString stringWithFormat:@"Unknown Usage 3D Axes %d", usage];
+}
+
+- (NSString *)usageString
+{
+    return [self.class usageToString:_usage];
+}
+
+- (uint64_t)uniqueID
+{
+    return _element1.uniqueID;
+}
+
+- (NSString *)description
+{
+    return [NSString stringWithFormat:@"<%@: %p, %@ (%llu); State: (%.2f, %.2f, %.2f)>", self.className, self, self.usageString, self.uniqueID, _state1, _state2, _state3];
+}
+
+- (instancetype)initWithFirstElement:(JOYElement *)element1 secondElement:(JOYElement *)element2 thirdElement:(JOYElement *)element3
+{
+    self = [super init];
+    if (!self) return self;
+    
+    _element1 = element1;
+    _element2 = element2;
+    _element3 = element3;
+
+    _maxX = element1? element1.max :  1;
+    _maxY = element2? element2.max :  1;
+    _maxZ = element3? element3.max :  1;
+    _minX = element1? element1.min : -1;
+    _minY = element2? element2.min : -1;
+    _minZ = element3? element3.min : -1;
+    
+    return self;
+}
+
+- (JOYPoint3D)rawValue
+{
+    return (JOYPoint3D){_state1, _state2, _state3};
+}
+
+- (JOYPoint3D)normalizedValue
+{
+    double distance = sqrt(_state1 * _state1 + _state2 * _state2 + _state3 * _state3);
+    if (distance == 0) {
+        distance = 1;
+    }
+    return (JOYPoint3D){_state1 / distance, _state2 / distance, _state3 / distance};
+}
+
+- (JOYPoint3D)gUnitsValue
+{
+    double distance = _gApproximation ?: 1;
+    return (JOYPoint3D){_state1 / distance, _state2 / distance, _state3 / distance};
+}
+
+- (bool)updateState
+{
+    int32_t x = [_element1 value];
+    int32_t y = [_element2 value];
+    int32_t z = [_element3 value];
+
+    if (x == 0 && y == 0 && z == 0) return false;
+    
+    double old1 = _state1, old2 = _state2, old3 = _state3;
+    _state1 = (x - _minX) / (double)(_maxX - _minX) * 2 - 1;
+    _state2 = (y - _minY) / (double)(_maxY - _minY) * 2 - 1;
+    _state3 = (z - _minZ) / (double)(_maxZ - _minZ) * 2 - 1;
+    
+    double distance = sqrt(_state1 * _state1 + _state2 * _state2 + _state3 * _state3);
+    if (_gApproximation == 0) {
+        _gApproximation = distance;
+    }
+    else {
+        _gApproximation = _gApproximation * 0.9999 + distance * 0.0001;
+    }
+
+    return old1 != _state1 || old2 != _state2 || old3 != _state3;
+}
+
+@end

JoyKit/JOYController.h

@@ -2,6 +2,7 @@
 #import "JOYButton.h"
 #import "JOYAxis.h"
 #import "JOYAxes2D.h"
+#import "JOYAxes3D.h"
 #import "JOYHat.h"
 
 static NSString const *JOYAxes2DEmulateButtonsKey = @"JOYAxes2DEmulateButtons";
@@ -17,6 +18,7 @@ static NSString const *JOYHatsEmulateButtonsKey = @"JOYHatsEmulateButtons";
 -(void) controller:(JOYController *)controller buttonChangedState:(JOYButton *)button;
 -(void) controller:(JOYController *)controller movedAxis:(JOYAxis *)axis;
 -(void) controller:(JOYController *)controller movedAxes2D:(JOYAxes2D *)axes;
+-(void) controller:(JOYController *)controller movedAxes3D:(JOYAxes3D *)axes;
 -(void) controller:(JOYController *)controller movedHat:(JOYHat *)hat;
 
 @end
@@ -31,6 +33,7 @@ static NSString const *JOYHatsEmulateButtonsKey = @"JOYHatsEmulateButtons";
 - (NSArray<JOYButton *> *) buttons;
 - (NSArray<JOYAxis *> *) axes;
 - (NSArray<JOYAxes2D *> *) axes2D;
+- (NSArray<JOYAxes3D *> *) axes3D;
 - (NSArray<JOYHat *> *) hats;
 - (void)setRumbleAmplitude:(double)amp;
 - (void)setPlayerLEDs:(uint8_t)mask;

JoyKit/JOYController.m

@@ -71,6 +71,14 @@ static bool hatsEmulateButtons = false;
 - (bool)updateState;
 @end
 
+@interface JOYAxes3D ()
+{
+    @public JOYElement *_element1, *_element2, *_element3;
+}
+- (instancetype)initWithFirstElement:(JOYElement *)element1 secondElement:(JOYElement *)element2 thirdElement:(JOYElement *)element2;
+- (bool)updateState;
+@end
+
 static NSDictionary *CreateHIDDeviceMatchDictionary(const UInt32 page, const UInt32 usage)
 {
     return @{
@@ -168,9 +176,11 @@ typedef union {
     NSMutableDictionary<JOYElement *, JOYButton *> *_buttons;
     NSMutableDictionary<JOYElement *, JOYAxis *> *_axes;
     NSMutableDictionary<JOYElement *, JOYAxes2D *> *_axes2D;
+    NSMutableDictionary<JOYElement *, JOYAxes3D *> *_axes3D;
     NSMutableDictionary<JOYElement *, JOYHat *> *_hats;
     NSMutableDictionary<NSNumber *, JOYFullReportElement *> *_fullReportElements;
     NSMutableDictionary<JOYFullReportElement *, NSArray<JOYElement *> *> *_multiElements;
+    JOYAxes3D *_lastAxes3D;
     
     // Button emulation
     NSMutableDictionary<NSNumber *, JOYEmulatedButton *> *_axisEmulatedButtons;
@@ -246,6 +256,69 @@ typedef union {
         @(kHIDUsage_GD_Rz): @(1),
     };
     
+    if (element.usagePage == kHIDPage_Sensor) {
+        JOYAxes3DUsage usage;
+        JOYElement *element1 = nil, *element2 = nil, *element3 = nil;
+        
+        switch (element.usage) {
+            case kHIDUsage_Snsr_Data_Motion_AccelerationAxisX:
+            case kHIDUsage_Snsr_Data_Motion_AccelerationAxisY:
+            case kHIDUsage_Snsr_Data_Motion_AccelerationAxisZ:
+                usage = JOYAxes3DUsageAcceleration;
+                break;
+            case kHIDUsage_Snsr_Data_Motion_AngularPositionXAxis:
+            case kHIDUsage_Snsr_Data_Motion_AngularPositionYAxis:
+            case kHIDUsage_Snsr_Data_Motion_AngularPositionZAxis:
+                usage = JOYAxes3DUsageOrientation;
+                break;
+            case kHIDUsage_Snsr_Data_Motion_AngularVelocityXAxis:
+            case kHIDUsage_Snsr_Data_Motion_AngularVelocityYAxis:
+            case kHIDUsage_Snsr_Data_Motion_AngularVelocityZAxis:
+                usage = JOYAxes3DUsageGyroscope;
+                break;
+            default:
+                return;
+        }
+        
+        switch (element.usage) {
+            case kHIDUsage_Snsr_Data_Motion_AccelerationAxisX:
+            case kHIDUsage_Snsr_Data_Motion_AngularPositionXAxis:
+            case kHIDUsage_Snsr_Data_Motion_AngularVelocityXAxis:
+                element1 = element;
+                if (_lastAxes3D && !_lastAxes3D->_element1 && _lastAxes3D.usage == usage) {
+                    element2 = _lastAxes3D->_element2;
+                    element3 = _lastAxes3D->_element3;
+                }
+                break;
+            case kHIDUsage_Snsr_Data_Motion_AccelerationAxisY:
+            case kHIDUsage_Snsr_Data_Motion_AngularPositionYAxis:
+            case kHIDUsage_Snsr_Data_Motion_AngularVelocityYAxis:
+                element2 = element;
+                if (_lastAxes3D && !_lastAxes3D->_element2 && _lastAxes3D.usage == usage) {
+                    element1 = _lastAxes3D->_element1;
+                    element3 = _lastAxes3D->_element3;
+                }
+                break;
+            case kHIDUsage_Snsr_Data_Motion_AccelerationAxisZ:
+            case kHIDUsage_Snsr_Data_Motion_AngularPositionZAxis:
+            case kHIDUsage_Snsr_Data_Motion_AngularVelocityZAxis:
+                element3 = element;
+                if (_lastAxes3D && !_lastAxes3D->_element3 && _lastAxes3D.usage == usage) {
+                    element1 = _lastAxes3D->_element1;
+                    element2 = _lastAxes3D->_element2;
+                }
+                break;
+        }
+        
+        _lastAxes3D = [[JOYAxes3D alloc] initWithFirstElement:element1 secondElement:element2 thirdElement:element3];
+        _lastAxes3D.usage = usage;
+        if (element1) _axes3D[element1] = _lastAxes3D;
+        if (element2) _axes3D[element2] = _lastAxes3D;
+        if (element3) _axes3D[element3] = _lastAxes3D;
+        
+        return;
+    }
+    
     axisGroups = _hacks[JOYAxisGroups] ?: axisGroups;
     
     if (element.usagePage == kHIDPage_Button ||
@@ -313,23 +386,6 @@ typedef union {
                         [[JOYEmulatedButton alloc] initWithUsage:JOYButtonUsageDPadDown type:JOYButtonTypeAxes2DEmulated uniqueID:axes.uniqueID | 0x400000000L],
                     ];
                 }
-                
-                /*
-                 for (NSArray *group in axes2d) {
-                 break;
-                 IOHIDElementRef first  = (__bridge IOHIDElementRef)group[0];
-                 IOHIDElementRef second = (__bridge IOHIDElementRef)group[1];
-                 if (IOHIDElementGetUsage(first)  > element.usage) continue;
-                 if (IOHIDElementGetUsage(second) > element.usage) continue;
-                 if (IOHIDElementGetReportID(first) != IOHIDElementGetReportID(element)) continue;
-                 if ((IOHIDElementGetUsage(first) - kHIDUsage_GD_X) / 3 != (element.usage - kHIDUsage_GD_X) / 3) continue;
-                 if (IOHIDElementGetParent(first) != IOHIDElementGetParent(element)) continue;
-                 
-                 [axes2d removeObject:group];
-                 [axes3d addObject:@[(__bridge id)first, (__bridge id)second, _element]];
-                 found = true;
-                 break;
-                 }*/
                 break;
             }
             case kHIDUsage_GD_Slider:
@@ -396,6 +452,7 @@ typedef union {
     _buttons = [NSMutableDictionary dictionary];
     _axes = [NSMutableDictionary dictionary];
     _axes2D = [NSMutableDictionary dictionary];
+    _axes3D = [NSMutableDictionary dictionary];
     _hats = [NSMutableDictionary dictionary];
     _axisEmulatedButtons = [NSMutableDictionary dictionary];
     _axes2DEmulatedButtons = [NSMutableDictionary dictionary];
@@ -619,6 +676,11 @@ typedef union {
     return [[NSSet setWithArray:[_axes2D allValues]] allObjects];
 }
 
+- (NSArray<JOYAxes3D *> *)axes3D
+{
+    return [[NSSet setWithArray:[_axes3D allValues]] allObjects];
+}
+
 - (NSArray<JOYHat *> *)hats
 {
     return [_hats allValues];
@@ -736,6 +798,20 @@ typedef union {
         }
     }
     
+    {
+        JOYAxes3D *axes = _axes3D[element];
+        if (axes) {
+            if ([axes updateState]) {
+                for (id<JOYListener> listener in listeners) {
+                    if ([listener respondsToSelector:@selector(controller:movedAxes3D:)]) {
+                        [listener controller:self movedAxes3D:axes];
+                    }
+                }
+            }
+            return;
+        }
+    }
+    
     {
         JOYHat *hat = _hats[element];
         if (hat) {
@@ -865,7 +941,7 @@ typedef union {
     if (_isDualShock3) {
         return 2 << player;
     }
-    if (_isUSBDualSense) {
+    if (_isDualSense) {
         switch (player) {
             case 0: return 0x04;
             case 1: return 0x0A;

JoyKit/JOYFullReportElement.m

@@ -61,9 +61,10 @@
     return self.uniqueID;
 }
 
-- (BOOL)isEqual:(id)object
+- (BOOL)isEqual:(JOYFullReportElement *)object
 {
-    return self.uniqueID == self.uniqueID;
+    if ([object isKindOfClass:self.class]) return false;
+    return self.uniqueID == object.uniqueID;
 }
 
 - (id)copyWithZone:(nullable NSZone *)zone;

JoyKit/JOYSubElement.m

@@ -57,6 +57,12 @@
     memcpy(temp, bytes + _offset / 8, (_offset + _size - 1) / 8 - _offset / 8 + 1);
     uint32_t ret = (*(uint32_t *)temp) >> (_offset % 8);
     ret &= (1 << _size) - 1;
+    //
+    if (_min < 0 || _max < 0) { // Uses unsigned values
+        if (ret & (1 << (_size - 1)) ) { // Is negative
+            ret |= ~((1 << _size) - 1); // Fill with 1s
+        }
+    }
     
     if (_max < _min) {
         return _max + _min - ret;