From 0d682fef37b6f84e2e09b61d953c37c105fa02d2 Mon Sep 17 00:00:00 2001 From: Christopher Snowhill Date: Tue, 3 Oct 2023 04:59:54 -0700 Subject: [PATCH] Initial implementation of positional audio for macOS Sonoma Signed-off-by: Christopher Snowhill --- Audio/Output/HeadphoneFilter.h | 9 +- Audio/Output/HeadphoneFilter.mm | 221 +++++++++++++++++++++++++------- Audio/Output/OutputCoreAudio.h | 9 ++ Audio/Output/OutputCoreAudio.m | 131 +++++++++++++++++-- Info.plist.template | 2 + en.lproj/InfoPlist.strings | 2 + 6 files changed, 318 insertions(+), 56 deletions(-) diff --git a/Audio/Output/HeadphoneFilter.h b/Audio/Output/HeadphoneFilter.h index 26f41cd6a..92053c65b 100644 --- a/Audio/Output/HeadphoneFilter.h +++ b/Audio/Output/HeadphoneFilter.h @@ -11,10 +11,15 @@ #import #import +#import + @interface HeadphoneFilter : NSObject { + NSURL *URL; + int bufferSize; int paddedBufferSize; int channelCount; + uint32_t config; float **mirroredImpulseResponses; @@ -25,7 +30,9 @@ + (BOOL)validateImpulseFile:(NSURL *)url; -- (id)initWithImpulseFile:(NSURL *)url forSampleRate:(double)sampleRate withInputChannels:(int)channels withConfig:(uint32_t)config; +- (id)initWithImpulseFile:(NSURL *)url forSampleRate:(double)sampleRate withInputChannels:(int)channels withConfig:(uint32_t)config withMatrix:(simd_float4x4)matrix; + +- (void)reloadWithMatrix:(simd_float4x4)matrix; - (void)process:(const float *)inBuffer sampleCount:(int)count toBuffer:(float *)outBuffer; diff --git a/Audio/Output/HeadphoneFilter.mm b/Audio/Output/HeadphoneFilter.mm index 02dfa8ff7..736c71aee 100644 --- a/Audio/Output/HeadphoneFilter.mm +++ b/Audio/Output/HeadphoneFilter.mm @@ -49,29 +49,135 @@ static const speakerPosition speakerPositions[18] = { { .elevation = DEGREES(+45.0), .azimuth = DEGREES(+135.0), .distance = 1.0 } }; -void getImpulse(NSURL *url, float **outImpulse, int *outSampleCount, int channelCount, uint32_t channelConfig) { - BOOL impulseFound = NO; - const float *impulseData = NULL; +static simd_float4x4 matX(float theta) { + simd_float4x4 mat = { + simd_make_float4(1.0f, 0.0f, 0.0f, 0.0f), + simd_make_float4(0.0f, cosf(theta), -sinf(theta), 0.0f), + simd_make_float4(0.0f, sinf(theta), cosf(theta), 0.0f), + simd_make_float4(0.0f, 0.0f, 0.0f, 1.0f) + }; + return mat; +}; + +static simd_float4x4 matY(float theta) { + simd_float4x4 mat = { + simd_make_float4(cosf(theta), 0.0f, sinf(theta), 0.0f), + simd_make_float4(0.0f, 1.0f, 0.0f, 0.0f), + simd_make_float4(-sinf(theta), 0.0f, cosf(theta), 0.0f), + simd_make_float4(0.0f, 0.0f, 0.0f, 1.0f) + }; + return mat; +} + +#if 0 +static simd_float4x4 matZ(float theta) { + simd_float4x4 mat = { + simd_make_float4(cosf(theta), -sinf(theta), 0.0f, 0.0f), + simd_make_float4(sinf(theta), cosf(theta), 0.0f, 0.0f), + simd_make_float4(0.0f, 0.0f, 1.0f, 0.0f), + simd_make_float4(0.0f, 0.0f, 0.0f, 1.0f) + }; + return mat; +}; +#endif + +static void transformPosition(float &elevation, float &azimuth, const simd_float4x4 &matrix) { + simd_float4x4 mat_x = matX(azimuth); + simd_float4x4 mat_y = matY(elevation); + //simd_float4x4 mat_z = matrix_identity_float4x4; + simd_float4x4 offsetMatrix = simd_mul(mat_x, mat_y); + //offsetMatrix = simd_mul(offsetMatrix, mat_z); + offsetMatrix = simd_mul(offsetMatrix, matrix); + + double sy = sqrt(offsetMatrix.columns[0].x * offsetMatrix.columns[0].x + offsetMatrix.columns[1].x * offsetMatrix.columns[1].x); + + bool singular = sy < 1e-6; // If + + float x, y/*, z*/; + if(!singular) { + x = atan2(offsetMatrix.columns[2].y, offsetMatrix.columns[2].z); + y = atan2(-offsetMatrix.columns[2].x, sy); + //z = atan2(offsetMatrix.columns[1].x, offsetMatrix.columns[0].x); + } else { + x = atan2(-offsetMatrix.columns[1].z, offsetMatrix.columns[1].y); + y = atan2(-offsetMatrix.columns[2].x, sy); + //z = 0; + } + + elevation = y; + azimuth = x; + + if(elevation < (M_PI * (-0.5))) { + elevation = (M_PI * (-0.5)); + } else if(elevation > M_PI * 0.5) { + elevation = M_PI * 0.5; + } + while(azimuth < (M_PI * (-2.0))) { + azimuth += M_PI * 2.0; + } + while(azimuth > M_PI * 2.0) { + azimuth -= M_PI * 2.0; + } +} + +@interface impulseSetCache : NSObject { + NSURL *URL; + HrtfData *data; +} ++ (impulseSetCache *)sharedController; +- (void)getImpulse:(NSURL *)url outImpulse:(float **)outImpulse outSampleCount:(int *)outSampleCount channelCount:(int)channelCount channelConfig:(uint32_t)channelConfig withMatrix:(simd_float4x4)matrix; +@end + +@implementation impulseSetCache +static impulseSetCache *_sharedController = nil; + ++ (impulseSetCache *)sharedController { + @synchronized(self) { + if(!_sharedController) { + _sharedController = [[impulseSetCache alloc] init]; + } + } + return _sharedController; +} + +- (id)init { + self = [super init]; + if(self) { + data = NULL; + } + return self; +} + +- (void)dealloc { + delete data; +} + +- (void)getImpulse:(NSURL *)url outImpulse:(float **)outImpulse outSampleCount:(int *)outSampleCount channelCount:(int)channelCount channelConfig:(uint32_t)channelConfig withMatrix:(simd_float4x4)matrix { double sampleRateOfSource = 0; int sampleCount = 0; - NSString *filePath = [url path]; + if(!data || ![url isEqualTo:URL]) { + delete data; + data = NULL; + URL = url; + NSString *filePath = [url path]; + try { + std::ifstream file([filePath UTF8String], std::fstream::binary); + if(!file.is_open()) { + throw std::logic_error("Cannot open file."); + } + data = new HrtfData(file); + file.close(); + } catch(std::exception &e) { + ALog(@"Exception caught: %s", e.what()); + } + } try { - std::ifstream file([filePath UTF8String], std::fstream::binary); + sampleRateOfSource = data->get_sample_rate(); - if(!file.is_open()) { - throw std::logic_error("Cannot open file."); - } - - HrtfData data(file); - - file.close(); - - sampleRateOfSource = data.get_sample_rate(); - - uint32_t sampleCountExact = data.get_response_length(); - sampleCount = sampleCountExact + ((data.get_longest_delay() + 2) >> 2); + uint32_t sampleCountExact = data->get_response_length(); + sampleCount = sampleCountExact + ((data->get_longest_delay() + 2) >> 2); sampleCount = (sampleCount + 15) & ~15; *outImpulse = (float *)calloc(sizeof(float), sampleCount * channelCount * 2); @@ -89,18 +195,24 @@ void getImpulse(NSURL *url, float **outImpulse, int *outSampleCount, int channel DirectionData hrtfLeft; DirectionData hrtfRight; - data.get_direction_data(speaker.elevation, speaker.azimuth, speaker.distance, hrtfLeft, hrtfRight); + float azimuth = speaker.azimuth; + float elevation = speaker.elevation; + + transformPosition(elevation, azimuth, matrix); + + data->get_direction_data(elevation, azimuth, speaker.distance, hrtfLeft, hrtfRight); cblas_scopy(sampleCountExact, &hrtfLeft.impulse_response[0], 1, &hrtfData[((hrtfLeft.delay + 2) >> 2) + sampleCount * i * 2], 1); cblas_scopy(sampleCountExact, &hrtfRight.impulse_response[0], 1, &hrtfData[((hrtfLeft.delay + 2) >> 2) + sampleCount * (i * 2 + 1)], 1); } } - + *outSampleCount = sampleCount; } catch(std::exception &e) { ALog(@"Exception caught: %s", e.what()); } } +@end @implementation HeadphoneFilter @@ -125,19 +237,21 @@ void getImpulse(NSURL *url, float **outImpulse, int *outSampleCount, int channel } } -- (id)initWithImpulseFile:(NSURL *)url forSampleRate:(double)sampleRate withInputChannels:(int)channels withConfig:(uint32_t)config { +- (id)initWithImpulseFile:(NSURL *)url forSampleRate:(double)sampleRate withInputChannels:(int)channels withConfig:(uint32_t)config withMatrix:(simd_float4x4)matrix { self = [super init]; if(self) { + URL = url; + channelCount = channels; + self->config = config; + float *impulseBuffer = NULL; int sampleCount = 0; - getImpulse(url, &impulseBuffer, &sampleCount, channels, config); + [[impulseSetCache sharedController] getImpulse:url outImpulse:&impulseBuffer outSampleCount:&sampleCount channelCount:channels channelConfig:config withMatrix:matrix]; if(!impulseBuffer) { return nil; } - channelCount = channels; - mirroredImpulseResponses = (float **)calloc(sizeof(float *), channelCount * 2); if(!mirroredImpulseResponses) { free(impulseBuffer); @@ -191,27 +305,48 @@ void getImpulse(NSURL *url, float **outImpulse, int *outSampleCount, int channel } } -- (void)process:(const float *)inBuffer sampleCount:(int)count toBuffer:(float *)outBuffer { - int sampleCount = paddedBufferSize; - while(count > 0) { - float left = 0, right = 0; - for(int i = 0; i < channelCount; ++i) { - float thisleft, thisright; - vDSP_vmul(prevInputs[i], 1, mirroredImpulseResponses[i * 2], 1, paddedSignal[0], 1, sampleCount); - vDSP_vmul(prevInputs[i], 1, mirroredImpulseResponses[i * 2 + 1], 1, paddedSignal[1], 1, sampleCount); - vDSP_sve(paddedSignal[0], 1, &thisleft, sampleCount); - vDSP_sve(paddedSignal[1], 1, &thisright, sampleCount); - left += thisleft; - right += thisright; - - memmove(prevInputs[i], prevInputs[i] + 1, sizeof(float) * (sampleCount - 1)); - prevInputs[i][sampleCount - 1] = *inBuffer++; +- (void)reloadWithMatrix:(simd_float4x4)matrix { + @synchronized (self) { + if(!mirroredImpulseResponses[0]) { + return; + } + + free(mirroredImpulseResponses[0]); + + float *impulseBuffer = NULL; + int sampleCount = 0; + [[impulseSetCache sharedController] getImpulse:URL outImpulse:&impulseBuffer outSampleCount:&sampleCount channelCount:channelCount channelConfig:config withMatrix:matrix]; + + for(int i = 0; i < channelCount * 2; ++i) { + mirroredImpulseResponses[i] = &impulseBuffer[sampleCount * i]; + vDSP_vrvrs(mirroredImpulseResponses[i], 1, sampleCount); + } + } +} + +- (void)process:(const float *)inBuffer sampleCount:(int)count toBuffer:(float *)outBuffer { + @synchronized (self) { + int sampleCount = paddedBufferSize; + while(count > 0) { + float left = 0, right = 0; + for(int i = 0; i < channelCount; ++i) { + float thisleft, thisright; + vDSP_vmul(prevInputs[i], 1, mirroredImpulseResponses[i * 2], 1, paddedSignal[0], 1, sampleCount); + vDSP_vmul(prevInputs[i], 1, mirroredImpulseResponses[i * 2 + 1], 1, paddedSignal[1], 1, sampleCount); + vDSP_sve(paddedSignal[0], 1, &thisleft, sampleCount); + vDSP_sve(paddedSignal[1], 1, &thisright, sampleCount); + left += thisleft; + right += thisright; + + memmove(prevInputs[i], prevInputs[i] + 1, sizeof(float) * (sampleCount - 1)); + prevInputs[i][sampleCount - 1] = *inBuffer++; + } + + outBuffer[0] = left; + outBuffer[1] = right; + outBuffer += 2; + --count; } - - outBuffer[0] = left; - outBuffer[1] = right; - outBuffer += 2; - --count; } } diff --git a/Audio/Output/OutputCoreAudio.h b/Audio/Output/OutputCoreAudio.h index 23b9316ea..3c4aa5cfe 100644 --- a/Audio/Output/OutputCoreAudio.h +++ b/Audio/Output/OutputCoreAudio.h @@ -26,6 +26,8 @@ using std::atomic_long; #import +#import + #import "HeadphoneFilter.h" //#define OUTPUT_LOG @@ -140,6 +142,11 @@ using std::atomic_long; float visResamplerInput[8192]; float visTemp[8192]; + BOOL referenceMatrixSet; + BOOL rotationMatrixUpdated; + simd_float4x4 rotationMatrix; + simd_float4x4 referenceMatrix; + #ifdef OUTPUT_LOG FILE *_logFile; #endif @@ -165,4 +172,6 @@ using std::atomic_long; - (void)sustainHDCD; +- (void)reportMotion:(simd_float4x4)matrix; + @end diff --git a/Audio/Output/OutputCoreAudio.m b/Audio/Output/OutputCoreAudio.m index 1160146dd..deaa4c0d1 100644 --- a/Audio/Output/OutputCoreAudio.m +++ b/Audio/Output/OutputCoreAudio.m @@ -17,15 +17,81 @@ #import +#import + #import "rsstate.h" #import "FSurroundFilter.h" extern void scale_by_volume(float *buffer, size_t count, float volume); -static NSString *CogPlaybackDidBeginNotficiation = @"CogPlaybackDidBeginNotficiation"; +static NSString *CogPlaybackDidBeginNotificiation = @"CogPlaybackDidBeginNotificiation"; + +simd_float4x4 convertMatrix(CMRotationMatrix r) { + simd_float4x4 matrix = { + simd_make_float4(r.m33, -r.m31, r.m32, 0.0f), + simd_make_float4(r.m13, -r.m11, r.m12, 0.0f), + simd_make_float4(r.m23, -r.m21, r.m22, 0.0f), + simd_make_float4(0.0f, 0.0f, 0.0f, 1.0f) + }; + return matrix; +} + +NSLock *motionManagerLock = nil; +API_AVAILABLE(macos(14.0)) CMHeadphoneMotionManager *motionManager = nil; +OutputCoreAudio *registeredMotionListener = nil; @implementation OutputCoreAudio ++ (void)initialize { + motionManagerLock = [[NSLock alloc] init]; + + if(@available(macOS 14, *)) { + CMAuthorizationStatus status = [CMHeadphoneMotionManager authorizationStatus]; + if(status == CMAuthorizationStatusDenied) { + ALog(@"Headphone motion not authorized"); + return; + } else if(status == CMAuthorizationStatusAuthorized) { + ALog(@"Headphone motion authorized"); + } else if(status == CMAuthorizationStatusRestricted) { + ALog(@"Headphone motion restricted"); + } else if(status == CMAuthorizationStatusNotDetermined) { + ALog(@"Headphone motion status not determined; will prompt for access"); + } + + motionManager = [[CMHeadphoneMotionManager alloc] init]; + } +} + +void registerMotionListener(OutputCoreAudio *listener) { + if(@available(macOS 14, *)) { + [motionManagerLock lock]; + if([motionManager isDeviceMotionActive]) { + [motionManager stopDeviceMotionUpdates]; + } + if([motionManager isDeviceMotionAvailable]) { + registeredMotionListener = listener; + [motionManager startDeviceMotionUpdatesToQueue:[NSOperationQueue mainQueue] withHandler:^(CMDeviceMotion * _Nullable motion, NSError * _Nullable error) { + if(motion) { + [motionManagerLock lock]; + [registeredMotionListener reportMotion:convertMatrix(motion.attitude.rotationMatrix)]; + [motionManagerLock unlock]; + } + }]; + } + [motionManagerLock unlock]; + } +} + +void unregisterMotionListener(void) { + if(@available(macOS 14, *)) { + [motionManagerLock lock]; + if([motionManager isDeviceMotionActive]) { + [motionManager stopDeviceMotionUpdates]; + } + registeredMotionListener = nil; + [motionManagerLock unlock]; + } +} static void *kOutputCoreAudioContext = &kOutputCoreAudioContext; @@ -97,7 +163,7 @@ static OSStatus eqRenderCallback(void *inRefCon, AudioUnitRenderActionFlags *ioA dmFormat.mBytesPerPacket = dmFormat.mBytesPerFrame * dmFormat.mFramesPerPacket; } UInt32 dstChannels = deviceFormat.mChannelsPerFrame; - if(srcChannels != dstChannels) { + if(dmChannels != dstChannels) { format.mChannelsPerFrame = dstChannels; format.mBytesPerFrame = ((format.mBitsPerChannel + 7) / 8) * dstChannels; format.mBytesPerPacket = format.mBytesPerFrame * format.mFramesPerPacket; @@ -728,13 +794,27 @@ current_device_listener(AudioObjectID inObjectID, UInt32 inNumberAddresses, cons if(enableHrtf) { NSURL *presetUrl = [[NSBundle mainBundle] URLForResource:@"SADIE_D02-96000" withExtension:@"mhr"]; + rotationMatrixUpdated = NO; + + simd_float4x4 matrix; + if(!referenceMatrixSet) { + matrix = matrix_identity_float4x4; + self->referenceMatrix = matrix; + registerMotionListener(self); + } else { + matrix = simd_mul(rotationMatrix, referenceMatrix); + } + [outputLock lock]; - hrtf = [[HeadphoneFilter alloc] initWithImpulseFile:presetUrl forSampleRate:realStreamFormat.mSampleRate withInputChannels:channels withConfig:channelConfig]; + hrtf = [[HeadphoneFilter alloc] initWithImpulseFile:presetUrl forSampleRate:realStreamFormat.mSampleRate withInputChannels:channels withConfig:channelConfig withMatrix:matrix]; [outputLock unlock]; channels = 2; channelConfig = AudioChannelSideLeft | AudioChannelSideRight; } else { + unregisterMotionListener(); + referenceMatrixSet = NO; + [outputLock lock]; hrtf = nil; [outputLock unlock]; @@ -827,9 +907,12 @@ current_device_listener(AudioObjectID inObjectID, UInt32 inNumberAddresses, cons OSStatus status; int inputRendered = inputBufferLastTime; int bytesRendered = inputRendered * realStreamFormat.mBytesPerPacket; - + if(resetStreamFormat) { [self updateStreamFormat]; + if([self processEndOfStream]) { + return 0; + } } while(inputRendered < 4096) { @@ -844,8 +927,6 @@ current_device_listener(AudioObjectID inObjectID, UInt32 inNumberAddresses, cons streamFormatChanged = NO; if(inputRendered) { resetStreamFormat = YES; - // This may not get called otherwise - [self processEndOfStream]; break; } else { [self updateStreamFormat]; @@ -856,8 +937,6 @@ current_device_listener(AudioObjectID inObjectID, UInt32 inNumberAddresses, cons inputBufferLastTime = inputRendered; - int samplesRenderedTotal = 0; - int samplesRendered = inputRendered; samplePtr = &inputBuffer[0]; @@ -893,6 +972,20 @@ current_device_listener(AudioObjectID inObjectID, UInt32 inNumberAddresses, cons [outputLock lock]; if(hrtf) { + if(rotationMatrixUpdated) { + rotationMatrixUpdated = NO; + simd_float4x4 mirrorTransform = { + simd_make_float4(-1.0, 0.0, 0.0, 0.0), + simd_make_float4(0.0, 1.0, 0.0, 0.0), + simd_make_float4(0.0, 0.0, 1.0, 0.0), + simd_make_float4(0.0, 0.0, 0.0, 1.0) + }; + + simd_float4x4 matrix = simd_mul(mirrorTransform, rotationMatrix); + matrix = simd_mul(matrix, referenceMatrix); + + [hrtf reloadWithMatrix:matrix]; + } [hrtf process:samplePtr sampleCount:samplesRendered toBuffer:&hrtfBuffer[0]]; samplePtr = &hrtfBuffer[0]; } @@ -939,9 +1032,8 @@ current_device_listener(AudioObjectID inObjectID, UInt32 inNumberAddresses, cons } inputBufferLastTime = 0; - samplesRenderedTotal += samplesRendered; - return samplesRenderedTotal; + return samplesRendered; } - (void)audioOutputBlock { @@ -969,8 +1061,8 @@ current_device_listener(AudioObjectID inObjectID, UInt32 inNumberAddresses, cons return 0; } if(inputRemain) { - int inputTodo = MIN(inputRemain, frameCount); - cblas_scopy(inputTodo * channels, _self->samplePtr, 1, inputData->mBuffers[0].mData, 1); + int inputTodo = MIN(inputRemain, frameCount - renderedSamples); + cblas_scopy(inputTodo * channels, _self->samplePtr, 1, ((float *)inputData->mBuffers[0].mData) + renderedSamples * channels, 1); _self->samplePtr += inputTodo * channels; inputRemain -= inputTodo; renderedSamples += inputTodo; @@ -1034,6 +1126,9 @@ current_device_listener(AudioObjectID inObjectID, UInt32 inNumberAddresses, cons secondsLatency = 0; visPushed = 0; + referenceMatrixSet = NO; + rotationMatrix = matrix_identity_float4x4; + AudioComponentDescription desc; NSError *err; @@ -1178,6 +1273,9 @@ current_device_listener(AudioObjectID inObjectID, UInt32 inNumberAddresses, cons } @synchronized(self) { stopInvoked = YES; + if(hrtf) { + unregisterMotionListener(); + } if(observersapplied) { [[NSUserDefaultsController sharedUserDefaultsController] removeObserver:self forKeyPath:@"values.outputDevice" context:kOutputCoreAudioContext]; [[NSUserDefaultsController sharedUserDefaultsController] removeObserver:self forKeyPath:@"values.GraphicEQenable" context:kOutputCoreAudioContext]; @@ -1283,4 +1381,13 @@ current_device_listener(AudioObjectID inObjectID, UInt32 inNumberAddresses, cons shouldPlayOutBuffer = s; } +- (void)reportMotion:(simd_float4x4)matrix { + rotationMatrix = matrix; + if(!referenceMatrixSet) { + referenceMatrix = simd_inverse(matrix); + referenceMatrixSet = YES; + } + rotationMatrixUpdated = YES; +} + @end diff --git a/Info.plist.template b/Info.plist.template index eaaab9fc5..514df5e3c 100644 --- a/Info.plist.template +++ b/Info.plist.template @@ -2069,6 +2069,8 @@ We may request related audio files from this folder for playback purposes. We will only play back files you specifically add, unless you enable the option to add an entire folder. Granting permission either for individual files or for parent folders ensures their contents will remain playable in future sessions. NSDesktopFolderUsageDescription We may request related audio files from this folder for playback purposes. We will only play back files you specifically add, unless you enable the option to add an entire folder. Granting permission either for individual files or for parent folders ensures their contents will remain playable in future sessions. + NSMotionUsageDescription + Cog optionally supports motion tracking headphones for head tracked positional audio, using its own low latency positioning model. OSAScriptingDefinition Cog.sdef SUFeedURL diff --git a/en.lproj/InfoPlist.strings b/en.lproj/InfoPlist.strings index 4f369dd2c..f188bb60f 100644 --- a/en.lproj/InfoPlist.strings +++ b/en.lproj/InfoPlist.strings @@ -33,3 +33,5 @@ /* Privacy - Desktop Folder Usage Description */ "NSDesktopFolderUsageDescription" = "We may request related audio files from this folder for playback purposes. We will only play back files you specifically add, unless you enable the option to add an entire folder. Granting permission either for individual files or for parent folders ensures their contents will remain playable in future sessions."; + +"NSMotionUsageDescription" = "Cog optionally supports motion tracking headphones for head tracked positional audio, using its own low latency positioning model.";