From 94efc9e876489b82b13549df7ebbc010a19fb7a8 Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Mon, 25 Jan 2021 14:09:16 +0100 Subject: [PATCH 01/21] Update cardboard_xr_unity.cc --- sdk/unity/xr_unity_plugin/cardboard_xr_unity.cc | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/sdk/unity/xr_unity_plugin/cardboard_xr_unity.cc b/sdk/unity/xr_unity_plugin/cardboard_xr_unity.cc index 0cee973e..1a453c04 100644 --- a/sdk/unity/xr_unity_plugin/cardboard_xr_unity.cc +++ b/sdk/unity/xr_unity_plugin/cardboard_xr_unity.cc @@ -480,7 +480,12 @@ class CardboardApi::CardboardApiImpl { // @return The monotonic time count in nano seconds. static int64_t GetMonotonicTimeNano() { struct timespec res; - clock_gettime(CLOCK_MONOTONIC, &res); + // Sensor timestamps are recorded as the time since boot +#ifdef __APPLE__ + clock_gettime(CLOCK_UPTIME_RAW, &res); +#else + clock_gettime(CLOCK_BOOTTIME, &res); +#endif return (res.tv_sec * CardboardApi::CardboardApiImpl::kNanosInSeconds) + res.tv_nsec; } From 12aad53324b8c336396634c3ae21a28416963b0d Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Mon, 25 Jan 2021 14:12:52 +0100 Subject: [PATCH 02/21] Update util.cc Changed from CLOCK_MONOTONIC to CLOCK_BOOTTIME to be in line with the sensor timestamps and do pose prediction correctly. --- hellocardboard-android/src/main/jni/util.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hellocardboard-android/src/main/jni/util.cc b/hellocardboard-android/src/main/jni/util.cc index 0da55d8c..e45337b3 100644 --- a/hellocardboard-android/src/main/jni/util.cc +++ b/hellocardboard-android/src/main/jni/util.cc @@ -430,7 +430,7 @@ static constexpr uint64_t kNanosInSeconds = 1000000000; long GetMonotonicTimeNano() { struct timespec res; - clock_gettime(CLOCK_MONOTONIC, &res); + clock_gettime(CLOCK_BOOTTIME, &res); return (res.tv_sec * kNanosInSeconds) + res.tv_nsec; } From 448d46f8a1e7e6bce74fb2e03cad993d49800ec7 Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Fri, 12 Feb 2021 15:27:57 +0100 Subject: [PATCH 03/21] Muliple orientation support --- sdk/head_tracker.cc | 47 ++++++----- sdk/head_tracker.h | 6 ++ sdk/screen_params.h | 10 +++ .../sdk/screenparams/ScreenParamsUtils.java | 83 +++++++++++++++++++ sdk/screen_params/android/screen_params.cc | 26 ++++++ sdk/screen_params/ios/screen_params.mm | 74 +++++++++++++++++ 6 files changed, 227 insertions(+), 19 deletions(-) diff --git a/sdk/head_tracker.cc b/sdk/head_tracker.cc index 22318dfd..81bc6c6f 100644 --- a/sdk/head_tracker.cc +++ b/sdk/head_tracker.cc @@ -28,7 +28,8 @@ HeadTracker::HeadTracker() sensor_fusion_(new SensorFusionEkf()), latest_gyroscope_data_({0, 0, Vector3::Zero()}), accel_sensor_(new SensorEventProducer()), - gyro_sensor_(new SensorEventProducer()) { + gyro_sensor_(new SensorEventProducer(), + start_orientation_(screen_params::getScreenOrientation()) { // Aryzon multiple orientations sensor_fusion_->SetBiasEstimationEnabled(/*kGyroBiasEstimationEnabled*/ true); on_accel_callback_ = [&](const AccelerometerData& event) { OnAccelerometerData(event); @@ -36,6 +37,16 @@ HeadTracker::HeadTracker() on_gyro_callback_ = [&](const GyroscopeData& event) { OnGyroscopeData(event); }; + + // Aryzon multiple orientations + if (start_orientation_ == screen_params::LandscapeLeft) { + ekf_to_head_tracker = Rotation::FromYawPitchRoll(-M_PI / 2.0, 0, -M_PI / 2.0); + } else if (start_orientation_ == screen_params::LandscapeRight) { + ekf_to_head_tracker = Rotation::FromYawPitchRoll(M_PI / 2.0, 0, M_PI / 2.0); + } else { + // Portrait + ekf_to_head_tracker = Rotation::FromYawPitchRoll(M_PI / 2.0, M_PI / 2.0, M_PI / 2.0); + } } HeadTracker::~HeadTracker() { UnregisterCallbacks(); } @@ -67,29 +78,27 @@ void HeadTracker::GetPose(int64_t timestamp_ns, std::array& out_orientation) const { Rotation predicted_rotation; const PoseState pose_state = sensor_fusion_->GetLatestPoseState(); - if (!sensor_fusion_->IsFullyInitialized()) { - CARDBOARD_LOGI( - "Head Tracker not fully initialized yet. Using pose prediction only."); - predicted_rotation = pose_prediction::PredictPose(timestamp_ns, pose_state); - } else { - predicted_rotation = pose_state.sensor_from_start_rotation; - } + predicted_rotation = pose_prediction::PredictPose(timestamp_ns, pose_state); // In order to update our pose as the sensor changes, we begin with the // inverse default orientation (the orientation returned by a reset sensor), // apply the current sensor transformation, and then transform into display // space. - // TODO(b/135488467): Support different screen orientations. - const Rotation ekf_to_head_tracker = - Rotation::FromYawPitchRoll(-M_PI / 2.0, 0, -M_PI / 2.0); - const Rotation sensor_to_display = - Rotation::FromAxisAndAngle(Vector3(0, 0, 1), M_PI / 2.0); - - const Vector4 q = - (sensor_to_display * predicted_rotation * ekf_to_head_tracker) - .GetQuaternion(); - Rotation rotation; - rotation.SetQuaternion(q); + Rotation sensor_to_display; + + // Aryzon multiple orientations + // Very fast implementation on iOS, pretty fast for Android + screen_params::ScreenOrientation orientation = screen_params::getScreenOrientation(); + + if (orientation == screen_params::LandscapeLeft) { + sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), M_PI / 2.0); + } else if (orientation == screen_params::LandscapeRight) { + sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), -M_PI / 2.0); + } else { // Portrait + sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), 0.0); + } + + Rotation rotation = sensor_to_display * predicted_rotation * ekf_to_head_tracker; out_orientation[0] = static_cast(rotation.GetQuaternion()[0]); out_orientation[1] = static_cast(rotation.GetQuaternion()[1]); diff --git a/sdk/head_tracker.h b/sdk/head_tracker.h index 3ba66ca2..6db799ac 100644 --- a/sdk/head_tracker.h +++ b/sdk/head_tracker.h @@ -26,6 +26,9 @@ #include "sensors/sensor_fusion_ekf.h" #include "util/rotation.h" +// Aryzon multiple orientations +#include "screen_params.h" + namespace cardboard { // HeadTracker encapsulates pose tracking by connecting sensors @@ -83,6 +86,9 @@ class HeadTracker { // Callback functions registered to the input SingleTypeEventProducer. std::function on_accel_callback_; std::function on_gyro_callback_; + + // Aryzon multiple orientations + const screen_params::ScreenOrientation start_orientation_; }; } // namespace cardboard diff --git a/sdk/screen_params.h b/sdk/screen_params.h index 5ab02405..94899b1e 100644 --- a/sdk/screen_params.h +++ b/sdk/screen_params.h @@ -28,6 +28,16 @@ void initializeAndroid(JavaVM* vm, jobject context); #endif void getScreenSizeInMeters(int width_pixels, int height_pixels, float* out_width_meters, float* out_height_meters); + +// Aryzon multiple orientations +enum ScreenOrientation { + LandscapeLeft, + Portrait, + LandscapeRight, + PortraitUpsideDown +}; +ScreenOrientation getScreenOrientation(); + } // namespace screen_params } // namespace cardboard diff --git a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java index ba30dbd1..4d44c682 100644 --- a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java +++ b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java @@ -21,6 +21,13 @@ import android.util.DisplayMetrics; import android.view.WindowManager; +// Aryzon multiple orientations +import android.content.res.Configuration; +import android.hardware.SensorManager; +import android.view.Display; +import android.view.OrientationEventListener; +import android.view.Surface; + /** Utility methods to manage the screen parameters. */ public abstract class ScreenParamsUtils { /** Holds the screen pixel density. */ @@ -68,4 +75,80 @@ public static ScreenPixelDensity getScreenPixelDensity(Context context) { } return new ScreenPixelDensity(displayMetrics.xdpi, displayMetrics.ydpi); } + + // Aryzon multiple orientations + public static class ScreenOrientation { + /** + * The screen orientation + * 0 = lanscape left + * 1 = portrait + * 2 = landscape right + * 3 = portrait upside-down + */ + public static Context context; + public static int orientation; + public static int previousOrientation; + public static OrientationEventListener orientationEventListener; + + public ScreenOrientation(final Context context) { + this.context = context; + this.orientation = CurrentOrientation(context); + + this.orientationEventListener = new OrientationEventListener(context, SensorManager.SENSOR_DELAY_UI) { + public void onOrientationChanged(int orientation_) { + + int newOrientation = context.getResources().getConfiguration().orientation; + + if (orientation_ != ORIENTATION_UNKNOWN && previousOrientation != newOrientation) { + orientation = CurrentOrientation(ScreenOrientation.context); + previousOrientation = newOrientation; + } + } + }; + if (this.orientationEventListener.canDetectOrientation()) { + this.orientationEventListener.enable(); + } + } + } + + private static ScreenOrientation screenOrientation; + + public static int getScreenOrientation(Context context) { + if (screenOrientation == null) { + + screenOrientation = new ScreenOrientation(context); + } + return screenOrientation.orientation; + + } + + private static int CurrentOrientation (Context context) { + if (defaultDisplay == null) { + WindowManager windowManager = (WindowManager) context.getSystemService(Context.WINDOW_SERVICE); + defaultDisplay = windowManager.getDefaultDisplay(); + } + + int orientation = context.getResources().getConfiguration().orientation; + // This call takes a long time so we don't call this every frame, only when rotation changes + int rotation = defaultDisplay.getRotation(); + + if (orientation == Configuration.ORIENTATION_LANDSCAPE + && (rotation == Surface.ROTATION_0 + || rotation == Surface.ROTATION_90)) { + return 0; + } else if (orientation == Configuration.ORIENTATION_PORTRAIT + && (rotation == Surface.ROTATION_0 + || rotation == Surface.ROTATION_90)) { + return 1; + } else if (orientation == Configuration.ORIENTATION_LANDSCAPE + && (rotation == Surface.ROTATION_180 + || rotation == Surface.ROTATION_270)) { + return 2; + } else if (orientation == Configuration.ORIENTATION_PORTRAIT + && (rotation == Surface.ROTATION_180 + || rotation == Surface.ROTATION_270)) { + return 3; + } + return 3; + } } diff --git a/sdk/screen_params/android/screen_params.cc b/sdk/screen_params/android/screen_params.cc index fad79ef0..6272883c 100644 --- a/sdk/screen_params/android/screen_params.cc +++ b/sdk/screen_params/android/screen_params.cc @@ -29,6 +29,10 @@ jobject context_; jclass screen_pixel_density_class_; jclass screen_params_utils_class_; +// Aryzon multiple orientations +jclass screen_orientation_class_; +jmethodID get_screen_orientation_method; + struct DisplayMetrics { float xdpi; float ydpi; @@ -41,6 +45,16 @@ void LoadJNIResources(JNIEnv* env) { cardboard::jni::LoadJClass(env, "com/google/cardboard/sdk/screenparams/" "ScreenParamsUtils$ScreenPixelDensity"); + + // Aryzon multiple orientations + screen_orientation_class_ = + cardboard::jni::LoadJClass(env, + "com/google/cardboard/sdk/screenparams/" + "ScreenParamsUtils$ScreenOrientation"); + + get_screen_orientation_method = env->GetStaticMethodID(screen_params_utils_class_, + "getScreenOrientation", + "(Landroid/content/Context;)I"); } DisplayMetrics getDisplayMetrics() { @@ -82,5 +96,17 @@ void getScreenSizeInMeters(int width_pixels, int height_pixels, *out_height_meters = (height_pixels / display_metrics.ydpi) * kMetersPerInch; } +// Aryzon multiple orientations +ScreenOrientation getScreenOrientation() { + + JNIEnv* env; + cardboard::jni::LoadJNIEnv(vm_, &env); + + int screen_orientation = env->CallStaticIntMethod( + screen_params_utils_class_, get_screen_orientation_method, context_); + + return (ScreenOrientation)screen_orientation; +} + } // namespace screen_params } // namespace cardboard diff --git a/sdk/screen_params/ios/screen_params.mm b/sdk/screen_params/ios/screen_params.mm index 1ecfe80f..105f0b26 100644 --- a/sdk/screen_params/ios/screen_params.mm +++ b/sdk/screen_params/ios/screen_params.mm @@ -18,6 +18,74 @@ #import #import +// Aryzon multiple orientations +@interface OrientationHelper : NSObject { + +} +@property cardboard::screen_params::ScreenOrientation orientation; +-(void) orientationChanged:(NSNotification *)note; +@end + +@implementation OrientationHelper { + +} +@synthesize orientation; + +-(id)init +{ + self = [super init]; + if (self) { + UIDevice *device = [UIDevice currentDevice]; + [self setOrientationValue:device]; + [device beginGeneratingDeviceOrientationNotifications]; + + [[NSNotificationCenter defaultCenter] + addObserver:self selector:@selector(orientationChanged:) + name:UIDeviceOrientationDidChangeNotification + object:device]; + } + return self; +} + +-(void) orientationChanged:(NSNotification *)note { + [self setOrientationValue:note.object]; +} + +-(void)setOrientationValue:(UIDevice *)device { + UIDeviceOrientation orientation = [device orientation]; + + if (!UIDeviceOrientationIsValidInterfaceOrientation(orientation)) { + return; + } + UIInterfaceOrientationMask supportedInterfaces = -1; + for (UIWindow *window in [[UIApplication sharedApplication] windows]) { + if (!window.hidden && window.subviews.count > 0) { + for (int i=0;i Date: Mon, 15 Feb 2021 10:05:36 +0100 Subject: [PATCH 04/21] Muliple orientation support, fixed errors --- multiple_orientations.patch | 380 ++++++++++++++++++ sdk/cardboard.cc | 12 + sdk/head_tracker.cc | 2 +- sdk/head_tracker.h | 1 + .../sdk/screenparams/ScreenParamsUtils.java | 1 + sdk/sixdof/position_data.cc | 82 ++++ sdk/sixdof/position_data.h | 55 +++ sdk/sixdof/rotation_data.cc | 87 ++++ sdk/sixdof/rotation_data.h | 55 +++ 9 files changed, 674 insertions(+), 1 deletion(-) create mode 100644 multiple_orientations.patch create mode 100644 sdk/sixdof/position_data.cc create mode 100644 sdk/sixdof/position_data.h create mode 100644 sdk/sixdof/rotation_data.cc create mode 100644 sdk/sixdof/rotation_data.h diff --git a/multiple_orientations.patch b/multiple_orientations.patch new file mode 100644 index 00000000..e46246e0 --- /dev/null +++ b/multiple_orientations.patch @@ -0,0 +1,380 @@ +From 448d46f8a1e7e6bce74fb2e03cad993d49800ec7 Mon Sep 17 00:00:00 2001 +From: maartenslaa +Date: Fri, 12 Feb 2021 15:27:57 +0100 +Subject: [PATCH] Muliple orientation support + +--- + sdk/head_tracker.cc | 47 ++++++----- + sdk/head_tracker.h | 6 ++ + sdk/screen_params.h | 10 +++ + .../sdk/screenparams/ScreenParamsUtils.java | 83 +++++++++++++++++++ + sdk/screen_params/android/screen_params.cc | 26 ++++++ + sdk/screen_params/ios/screen_params.mm | 74 +++++++++++++++++ + 6 files changed, 227 insertions(+), 19 deletions(-) + +diff --git a/sdk/head_tracker.cc b/sdk/head_tracker.cc +index 22318df..81bc6c6 100644 +--- a/sdk/head_tracker.cc ++++ b/sdk/head_tracker.cc +@@ -28,7 +28,8 @@ HeadTracker::HeadTracker() + sensor_fusion_(new SensorFusionEkf()), + latest_gyroscope_data_({0, 0, Vector3::Zero()}), + accel_sensor_(new SensorEventProducer()), +- gyro_sensor_(new SensorEventProducer()) { ++ gyro_sensor_(new SensorEventProducer(), ++ start_orientation_(screen_params::getScreenOrientation()) { // Aryzon multiple orientations + sensor_fusion_->SetBiasEstimationEnabled(/*kGyroBiasEstimationEnabled*/ true); + on_accel_callback_ = [&](const AccelerometerData& event) { + OnAccelerometerData(event); +@@ -36,6 +37,16 @@ HeadTracker::HeadTracker() + on_gyro_callback_ = [&](const GyroscopeData& event) { + OnGyroscopeData(event); + }; ++ ++ // Aryzon multiple orientations ++ if (start_orientation_ == screen_params::LandscapeLeft) { ++ ekf_to_head_tracker = Rotation::FromYawPitchRoll(-M_PI / 2.0, 0, -M_PI / 2.0); ++ } else if (start_orientation_ == screen_params::LandscapeRight) { ++ ekf_to_head_tracker = Rotation::FromYawPitchRoll(M_PI / 2.0, 0, M_PI / 2.0); ++ } else { ++ // Portrait ++ ekf_to_head_tracker = Rotation::FromYawPitchRoll(M_PI / 2.0, M_PI / 2.0, M_PI / 2.0); ++ } + } + + HeadTracker::~HeadTracker() { UnregisterCallbacks(); } +@@ -67,29 +78,27 @@ void HeadTracker::GetPose(int64_t timestamp_ns, + std::array& out_orientation) const { + Rotation predicted_rotation; + const PoseState pose_state = sensor_fusion_->GetLatestPoseState(); +- if (!sensor_fusion_->IsFullyInitialized()) { +- CARDBOARD_LOGI( +- "Head Tracker not fully initialized yet. Using pose prediction only."); +- predicted_rotation = pose_prediction::PredictPose(timestamp_ns, pose_state); +- } else { +- predicted_rotation = pose_state.sensor_from_start_rotation; +- } ++ predicted_rotation = pose_prediction::PredictPose(timestamp_ns, pose_state); + + // In order to update our pose as the sensor changes, we begin with the + // inverse default orientation (the orientation returned by a reset sensor), + // apply the current sensor transformation, and then transform into display + // space. +- // TODO(b/135488467): Support different screen orientations. +- const Rotation ekf_to_head_tracker = +- Rotation::FromYawPitchRoll(-M_PI / 2.0, 0, -M_PI / 2.0); +- const Rotation sensor_to_display = +- Rotation::FromAxisAndAngle(Vector3(0, 0, 1), M_PI / 2.0); +- +- const Vector4 q = +- (sensor_to_display * predicted_rotation * ekf_to_head_tracker) +- .GetQuaternion(); +- Rotation rotation; +- rotation.SetQuaternion(q); ++ Rotation sensor_to_display; ++ ++ // Aryzon multiple orientations ++ // Very fast implementation on iOS, pretty fast for Android ++ screen_params::ScreenOrientation orientation = screen_params::getScreenOrientation(); ++ ++ if (orientation == screen_params::LandscapeLeft) { ++ sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), M_PI / 2.0); ++ } else if (orientation == screen_params::LandscapeRight) { ++ sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), -M_PI / 2.0); ++ } else { // Portrait ++ sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), 0.0); ++ } ++ ++ Rotation rotation = sensor_to_display * predicted_rotation * ekf_to_head_tracker; + + out_orientation[0] = static_cast(rotation.GetQuaternion()[0]); + out_orientation[1] = static_cast(rotation.GetQuaternion()[1]); +diff --git a/sdk/head_tracker.h b/sdk/head_tracker.h +index 3ba66ca..6db799a 100644 +--- a/sdk/head_tracker.h ++++ b/sdk/head_tracker.h +@@ -26,6 +26,9 @@ + #include "sensors/sensor_fusion_ekf.h" + #include "util/rotation.h" + ++// Aryzon multiple orientations ++#include "screen_params.h" ++ + namespace cardboard { + + // HeadTracker encapsulates pose tracking by connecting sensors +@@ -83,6 +86,9 @@ class HeadTracker { + // Callback functions registered to the input SingleTypeEventProducer. + std::function on_accel_callback_; + std::function on_gyro_callback_; ++ ++ // Aryzon multiple orientations ++ const screen_params::ScreenOrientation start_orientation_; + }; + + } // namespace cardboard +diff --git a/sdk/screen_params.h b/sdk/screen_params.h +index 5ab0240..94899b1 100644 +--- a/sdk/screen_params.h ++++ b/sdk/screen_params.h +@@ -28,6 +28,16 @@ void initializeAndroid(JavaVM* vm, jobject context); + #endif + void getScreenSizeInMeters(int width_pixels, int height_pixels, + float* out_width_meters, float* out_height_meters); ++ ++// Aryzon multiple orientations ++enum ScreenOrientation { ++ LandscapeLeft, ++ Portrait, ++ LandscapeRight, ++ PortraitUpsideDown ++}; ++ScreenOrientation getScreenOrientation(); ++ + } // namespace screen_params + } // namespace cardboard + +diff --git a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java +index ba30dbd..4d44c68 100644 +--- a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java ++++ b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java +@@ -21,6 +21,13 @@ import android.os.Build.VERSION_CODES; + import android.util.DisplayMetrics; + import android.view.WindowManager; + ++// Aryzon multiple orientations ++import android.content.res.Configuration; ++import android.hardware.SensorManager; ++import android.view.Display; ++import android.view.OrientationEventListener; ++import android.view.Surface; ++ + /** Utility methods to manage the screen parameters. */ + public abstract class ScreenParamsUtils { + /** Holds the screen pixel density. */ +@@ -68,4 +75,80 @@ public abstract class ScreenParamsUtils { + } + return new ScreenPixelDensity(displayMetrics.xdpi, displayMetrics.ydpi); + } ++ ++ // Aryzon multiple orientations ++ public static class ScreenOrientation { ++ /** ++ * The screen orientation ++ * 0 = lanscape left ++ * 1 = portrait ++ * 2 = landscape right ++ * 3 = portrait upside-down ++ */ ++ public static Context context; ++ public static int orientation; ++ public static int previousOrientation; ++ public static OrientationEventListener orientationEventListener; ++ ++ public ScreenOrientation(final Context context) { ++ this.context = context; ++ this.orientation = CurrentOrientation(context); ++ ++ this.orientationEventListener = new OrientationEventListener(context, SensorManager.SENSOR_DELAY_UI) { ++ public void onOrientationChanged(int orientation_) { ++ ++ int newOrientation = context.getResources().getConfiguration().orientation; ++ ++ if (orientation_ != ORIENTATION_UNKNOWN && previousOrientation != newOrientation) { ++ orientation = CurrentOrientation(ScreenOrientation.context); ++ previousOrientation = newOrientation; ++ } ++ } ++ }; ++ if (this.orientationEventListener.canDetectOrientation()) { ++ this.orientationEventListener.enable(); ++ } ++ } ++ } ++ ++ private static ScreenOrientation screenOrientation; ++ ++ public static int getScreenOrientation(Context context) { ++ if (screenOrientation == null) { ++ ++ screenOrientation = new ScreenOrientation(context); ++ } ++ return screenOrientation.orientation; ++ ++ } ++ ++ private static int CurrentOrientation (Context context) { ++ if (defaultDisplay == null) { ++ WindowManager windowManager = (WindowManager) context.getSystemService(Context.WINDOW_SERVICE); ++ defaultDisplay = windowManager.getDefaultDisplay(); ++ } ++ ++ int orientation = context.getResources().getConfiguration().orientation; ++ // This call takes a long time so we don't call this every frame, only when rotation changes ++ int rotation = defaultDisplay.getRotation(); ++ ++ if (orientation == Configuration.ORIENTATION_LANDSCAPE ++ && (rotation == Surface.ROTATION_0 ++ || rotation == Surface.ROTATION_90)) { ++ return 0; ++ } else if (orientation == Configuration.ORIENTATION_PORTRAIT ++ && (rotation == Surface.ROTATION_0 ++ || rotation == Surface.ROTATION_90)) { ++ return 1; ++ } else if (orientation == Configuration.ORIENTATION_LANDSCAPE ++ && (rotation == Surface.ROTATION_180 ++ || rotation == Surface.ROTATION_270)) { ++ return 2; ++ } else if (orientation == Configuration.ORIENTATION_PORTRAIT ++ && (rotation == Surface.ROTATION_180 ++ || rotation == Surface.ROTATION_270)) { ++ return 3; ++ } ++ return 3; ++ } + } +diff --git a/sdk/screen_params/android/screen_params.cc b/sdk/screen_params/android/screen_params.cc +index fad79ef..6272883 100644 +--- a/sdk/screen_params/android/screen_params.cc ++++ b/sdk/screen_params/android/screen_params.cc +@@ -29,6 +29,10 @@ jobject context_; + jclass screen_pixel_density_class_; + jclass screen_params_utils_class_; + ++// Aryzon multiple orientations ++jclass screen_orientation_class_; ++jmethodID get_screen_orientation_method; ++ + struct DisplayMetrics { + float xdpi; + float ydpi; +@@ -41,6 +45,16 @@ void LoadJNIResources(JNIEnv* env) { + cardboard::jni::LoadJClass(env, + "com/google/cardboard/sdk/screenparams/" + "ScreenParamsUtils$ScreenPixelDensity"); ++ ++ // Aryzon multiple orientations ++ screen_orientation_class_ = ++ cardboard::jni::LoadJClass(env, ++ "com/google/cardboard/sdk/screenparams/" ++ "ScreenParamsUtils$ScreenOrientation"); ++ ++ get_screen_orientation_method = env->GetStaticMethodID(screen_params_utils_class_, ++ "getScreenOrientation", ++ "(Landroid/content/Context;)I"); + } + + DisplayMetrics getDisplayMetrics() { +@@ -82,5 +96,17 @@ void getScreenSizeInMeters(int width_pixels, int height_pixels, + *out_height_meters = (height_pixels / display_metrics.ydpi) * kMetersPerInch; + } + ++// Aryzon multiple orientations ++ScreenOrientation getScreenOrientation() { ++ ++ JNIEnv* env; ++ cardboard::jni::LoadJNIEnv(vm_, &env); ++ ++ int screen_orientation = env->CallStaticIntMethod( ++ screen_params_utils_class_, get_screen_orientation_method, context_); ++ ++ return (ScreenOrientation)screen_orientation; ++} ++ + } // namespace screen_params + } // namespace cardboard +diff --git a/sdk/screen_params/ios/screen_params.mm b/sdk/screen_params/ios/screen_params.mm +index 1ecfe80..105f0b2 100644 +--- a/sdk/screen_params/ios/screen_params.mm ++++ b/sdk/screen_params/ios/screen_params.mm +@@ -18,6 +18,74 @@ + #import + #import + ++// Aryzon multiple orientations ++@interface OrientationHelper : NSObject { ++ ++} ++@property cardboard::screen_params::ScreenOrientation orientation; ++-(void) orientationChanged:(NSNotification *)note; ++@end ++ ++@implementation OrientationHelper { ++ ++} ++@synthesize orientation; ++ ++-(id)init ++{ ++ self = [super init]; ++ if (self) { ++ UIDevice *device = [UIDevice currentDevice]; ++ [self setOrientationValue:device]; ++ [device beginGeneratingDeviceOrientationNotifications]; ++ ++ [[NSNotificationCenter defaultCenter] ++ addObserver:self selector:@selector(orientationChanged:) ++ name:UIDeviceOrientationDidChangeNotification ++ object:device]; ++ } ++ return self; ++} ++ ++-(void) orientationChanged:(NSNotification *)note { ++ [self setOrientationValue:note.object]; ++} ++ ++-(void)setOrientationValue:(UIDevice *)device { ++ UIDeviceOrientation orientation = [device orientation]; ++ ++ if (!UIDeviceOrientationIsValidInterfaceOrientation(orientation)) { ++ return; ++ } ++ UIInterfaceOrientationMask supportedInterfaces = -1; ++ for (UIWindow *window in [[UIApplication sharedApplication] windows]) { ++ if (!window.hidden && window.subviews.count > 0) { ++ for (int i=0;i(head_tracker)->AddSixDoFData(timestamp_ns, position, orientation); +} + void CardboardQrCode_getSavedDeviceParams(uint8_t** encoded_device_params, int* size) { if (CARDBOARD_IS_NOT_INITIALIZED() || diff --git a/sdk/head_tracker.cc b/sdk/head_tracker.cc index 81bc6c6f..f19fe9e9 100644 --- a/sdk/head_tracker.cc +++ b/sdk/head_tracker.cc @@ -28,7 +28,7 @@ HeadTracker::HeadTracker() sensor_fusion_(new SensorFusionEkf()), latest_gyroscope_data_({0, 0, Vector3::Zero()}), accel_sensor_(new SensorEventProducer()), - gyro_sensor_(new SensorEventProducer(), + gyro_sensor_(new SensorEventProducer()), start_orientation_(screen_params::getScreenOrientation()) { // Aryzon multiple orientations sensor_fusion_->SetBiasEstimationEnabled(/*kGyroBiasEstimationEnabled*/ true); on_accel_callback_ = [&](const AccelerometerData& event) { diff --git a/sdk/head_tracker.h b/sdk/head_tracker.h index 6db799ac..c3ad4743 100644 --- a/sdk/head_tracker.h +++ b/sdk/head_tracker.h @@ -88,6 +88,7 @@ class HeadTracker { std::function on_gyro_callback_; // Aryzon multiple orientations + Rotation ekf_to_head_tracker; const screen_params::ScreenOrientation start_orientation_; }; diff --git a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java index 4d44c682..6fffcaef 100644 --- a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java +++ b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java @@ -112,6 +112,7 @@ public void onOrientationChanged(int orientation_) { } private static ScreenOrientation screenOrientation; + private static Display defaultDisplay; public static int getScreenOrientation(Context context) { if (screenOrientation == null) { diff --git a/sdk/sixdof/position_data.cc b/sdk/sixdof/position_data.cc new file mode 100644 index 00000000..6c2d255d --- /dev/null +++ b/sdk/sixdof/position_data.cc @@ -0,0 +1,82 @@ +/* + * Copyright 2019 Google LLC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include "sixdof/position_data.h" + +#include +#include +#include "util/logging.h" + +namespace cardboard { + +PositionData::PositionData(size_t buffer_size) : buffer_size_(buffer_size) {} + +void PositionData::AddSample(const Vector3& sample, const int64_t timestamp_ns) { + buffer_.push_back(sample); + if (buffer_.size() > buffer_size_) { + buffer_.pop_front(); + } + + timestamp_buffer_.push_back(timestamp_ns); + if (timestamp_buffer_.size() > buffer_size_) { + timestamp_buffer_.pop_front(); + } +} + +bool PositionData::IsValid() const { return buffer_.size() == buffer_size_; } + +long long PositionData::GetLatestTimestamp() const { + if (timestamp_buffer_.size() > 0) { + return timestamp_buffer_[timestamp_buffer_.size() -1]; + } + return 0; +} + +Vector3 PositionData::GetLatestData() const { + if (buffer_.size() > 0) { + return buffer_[buffer_.size() -1]; + } + return Vector3::Zero(); +} + + +Vector3 PositionData::GetExtrapolatedForTimeStamp(const int64_t timestamp_ns) { + + if (!IsValid() || buffer_size_ < 2) { + return {0.0,0.0,1.0}; + } + + Vector3 vSum = {0,0,0}; + + for (int i=1; i + +#include "util/vector.h" + +namespace cardboard { + +// Fixed window FIFO mean filter for vectors of the given dimension. +class PositionData { + public: + // Create a buffer to hold rotation data of size buffer_size. + // @param buffer_size size of samples to buffer. + explicit PositionData(size_t buffer_size); + + // Add sample to buffer_ if buffer_ is full it drop the oldest sample. + void AddSample(const Vector3& sample, const int64_t timestamp_ns); + + // Returns true if buffer has buffer_size sample, false otherwise. + bool IsValid() const; + + // Returns the latest value stored in the internal buffer. + Vector3 GetLatestData() const; + long long GetLatestTimestamp() const; + Vector3 GetExtrapolatedForTimeStamp(const int64_t timestamp_ns); + void Reset(); + private: + const size_t buffer_size_; + std::deque buffer_; + std::deque timestamp_buffer_; +}; + +} // namespace cardboard + + +#endif /* rotation_data_h */ diff --git a/sdk/sixdof/rotation_data.cc b/sdk/sixdof/rotation_data.cc new file mode 100644 index 00000000..83391166 --- /dev/null +++ b/sdk/sixdof/rotation_data.cc @@ -0,0 +1,87 @@ +/* + * Copyright 2019 Google LLC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include "sixdof/rotation_data.h" + +#include +#include + +namespace cardboard { + +RotationData::RotationData(size_t buffer_size) : buffer_size_(buffer_size) {} + +void RotationData::AddSample(const Vector4& sample, const int64_t timestamp_ns) { + buffer_.push_back(sample); + if (buffer_.size() > buffer_size_) { + buffer_.pop_front(); + } + + timestamp_buffer_.push_back(timestamp_ns); + if (timestamp_buffer_.size() > buffer_size_) { + timestamp_buffer_.pop_front(); + } +} + +bool RotationData::IsValid() const { return buffer_.size() == buffer_size_; } + + Vector4 RotationData::GetLatestData() const { + if (buffer_.size() > 0) { + return buffer_[buffer_.size() -1]; + } + return Vector4::Zero(); + } + + +Vector4 RotationData::GetInterpolatedForTimeStamp(const int64_t timestamp_ns) const{ + + if (!IsValid()) { + return {0.0,0.0,0.0,1.0}; + } + int64_t smaller = -1; + int64_t larger = -1; + + bool didPassLarger = false; + int i=0; + + while (!didPassLarger && i < buffer_size_) { + int64_t currentTs = timestamp_buffer_[i]; + if (currentTs <= timestamp_ns) { + smaller = currentTs; + } else { + larger = currentTs; + didPassLarger = true; + } + i++; + } + + float interpolationValue = 0.5f; + Vector4 q; + Vector4 q0; + Vector4 q1; + + if (smaller > 0 && larger > 0) { + int64_t delta = larger - smaller; + interpolationValue = (timestamp_ns - smaller) / (double)(delta); + q0 = buffer_[i-1]; + q1 = buffer_[i]; + q = interpolationValue*(q1-q0) + q0; + } else { + q = buffer_[buffer_size_]; + } + + return q; +} + +} // namespace cardboard diff --git a/sdk/sixdof/rotation_data.h b/sdk/sixdof/rotation_data.h new file mode 100644 index 00000000..7528be44 --- /dev/null +++ b/sdk/sixdof/rotation_data.h @@ -0,0 +1,55 @@ +/* + * Copyright 2019 Google LLC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// Aryzon 6DoF + +#ifndef rotation_data_h +#define rotation_data_h + +#include + +#include "util/vector.h" + +namespace cardboard { + +// Fixed window FIFO mean filter for vectors of the given dimension. +class RotationData { + public: + // Create a buffer to hold rotation data of size buffer_size. + // @param buffer_size size of samples to buffer. + explicit RotationData(size_t buffer_size); + + // Add sample to buffer_ if buffer_ is full it drop the oldest sample. + void AddSample(const Vector4& sample, const int64_t timestamp_ns); + + // Returns true if buffer has buffer_size sample, false otherwise. + bool IsValid() const; + + // Returns the latest value stored in the internal buffer. + Vector4 GetLatestData() const; + + Vector4 GetInterpolatedForTimeStamp(const int64_t timestamp_ns) const; + + private: + const size_t buffer_size_; + std::deque buffer_; + std::deque timestamp_buffer_; +}; + +} // namespace cardboard + + +#endif /* rotation_data_h */ From 43afbe0d87dd5311aa08cb32e0127420893fd6a1 Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Mon, 15 Feb 2021 10:25:01 +0100 Subject: [PATCH 05/21] removed temp files --- multiple_orientations.patch | 380 ------------------------------------ sdk/sixdof/position_data.cc | 82 -------- sdk/sixdof/position_data.h | 55 ------ sdk/sixdof/rotation_data.cc | 87 --------- sdk/sixdof/rotation_data.h | 55 ------ 5 files changed, 659 deletions(-) delete mode 100644 multiple_orientations.patch delete mode 100644 sdk/sixdof/position_data.cc delete mode 100644 sdk/sixdof/position_data.h delete mode 100644 sdk/sixdof/rotation_data.cc delete mode 100644 sdk/sixdof/rotation_data.h diff --git a/multiple_orientations.patch b/multiple_orientations.patch deleted file mode 100644 index e46246e0..00000000 --- a/multiple_orientations.patch +++ /dev/null @@ -1,380 +0,0 @@ -From 448d46f8a1e7e6bce74fb2e03cad993d49800ec7 Mon Sep 17 00:00:00 2001 -From: maartenslaa -Date: Fri, 12 Feb 2021 15:27:57 +0100 -Subject: [PATCH] Muliple orientation support - ---- - sdk/head_tracker.cc | 47 ++++++----- - sdk/head_tracker.h | 6 ++ - sdk/screen_params.h | 10 +++ - .../sdk/screenparams/ScreenParamsUtils.java | 83 +++++++++++++++++++ - sdk/screen_params/android/screen_params.cc | 26 ++++++ - sdk/screen_params/ios/screen_params.mm | 74 +++++++++++++++++ - 6 files changed, 227 insertions(+), 19 deletions(-) - -diff --git a/sdk/head_tracker.cc b/sdk/head_tracker.cc -index 22318df..81bc6c6 100644 ---- a/sdk/head_tracker.cc -+++ b/sdk/head_tracker.cc -@@ -28,7 +28,8 @@ HeadTracker::HeadTracker() - sensor_fusion_(new SensorFusionEkf()), - latest_gyroscope_data_({0, 0, Vector3::Zero()}), - accel_sensor_(new SensorEventProducer()), -- gyro_sensor_(new SensorEventProducer()) { -+ gyro_sensor_(new SensorEventProducer(), -+ start_orientation_(screen_params::getScreenOrientation()) { // Aryzon multiple orientations - sensor_fusion_->SetBiasEstimationEnabled(/*kGyroBiasEstimationEnabled*/ true); - on_accel_callback_ = [&](const AccelerometerData& event) { - OnAccelerometerData(event); -@@ -36,6 +37,16 @@ HeadTracker::HeadTracker() - on_gyro_callback_ = [&](const GyroscopeData& event) { - OnGyroscopeData(event); - }; -+ -+ // Aryzon multiple orientations -+ if (start_orientation_ == screen_params::LandscapeLeft) { -+ ekf_to_head_tracker = Rotation::FromYawPitchRoll(-M_PI / 2.0, 0, -M_PI / 2.0); -+ } else if (start_orientation_ == screen_params::LandscapeRight) { -+ ekf_to_head_tracker = Rotation::FromYawPitchRoll(M_PI / 2.0, 0, M_PI / 2.0); -+ } else { -+ // Portrait -+ ekf_to_head_tracker = Rotation::FromYawPitchRoll(M_PI / 2.0, M_PI / 2.0, M_PI / 2.0); -+ } - } - - HeadTracker::~HeadTracker() { UnregisterCallbacks(); } -@@ -67,29 +78,27 @@ void HeadTracker::GetPose(int64_t timestamp_ns, - std::array& out_orientation) const { - Rotation predicted_rotation; - const PoseState pose_state = sensor_fusion_->GetLatestPoseState(); -- if (!sensor_fusion_->IsFullyInitialized()) { -- CARDBOARD_LOGI( -- "Head Tracker not fully initialized yet. Using pose prediction only."); -- predicted_rotation = pose_prediction::PredictPose(timestamp_ns, pose_state); -- } else { -- predicted_rotation = pose_state.sensor_from_start_rotation; -- } -+ predicted_rotation = pose_prediction::PredictPose(timestamp_ns, pose_state); - - // In order to update our pose as the sensor changes, we begin with the - // inverse default orientation (the orientation returned by a reset sensor), - // apply the current sensor transformation, and then transform into display - // space. -- // TODO(b/135488467): Support different screen orientations. -- const Rotation ekf_to_head_tracker = -- Rotation::FromYawPitchRoll(-M_PI / 2.0, 0, -M_PI / 2.0); -- const Rotation sensor_to_display = -- Rotation::FromAxisAndAngle(Vector3(0, 0, 1), M_PI / 2.0); -- -- const Vector4 q = -- (sensor_to_display * predicted_rotation * ekf_to_head_tracker) -- .GetQuaternion(); -- Rotation rotation; -- rotation.SetQuaternion(q); -+ Rotation sensor_to_display; -+ -+ // Aryzon multiple orientations -+ // Very fast implementation on iOS, pretty fast for Android -+ screen_params::ScreenOrientation orientation = screen_params::getScreenOrientation(); -+ -+ if (orientation == screen_params::LandscapeLeft) { -+ sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), M_PI / 2.0); -+ } else if (orientation == screen_params::LandscapeRight) { -+ sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), -M_PI / 2.0); -+ } else { // Portrait -+ sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), 0.0); -+ } -+ -+ Rotation rotation = sensor_to_display * predicted_rotation * ekf_to_head_tracker; - - out_orientation[0] = static_cast(rotation.GetQuaternion()[0]); - out_orientation[1] = static_cast(rotation.GetQuaternion()[1]); -diff --git a/sdk/head_tracker.h b/sdk/head_tracker.h -index 3ba66ca..6db799a 100644 ---- a/sdk/head_tracker.h -+++ b/sdk/head_tracker.h -@@ -26,6 +26,9 @@ - #include "sensors/sensor_fusion_ekf.h" - #include "util/rotation.h" - -+// Aryzon multiple orientations -+#include "screen_params.h" -+ - namespace cardboard { - - // HeadTracker encapsulates pose tracking by connecting sensors -@@ -83,6 +86,9 @@ class HeadTracker { - // Callback functions registered to the input SingleTypeEventProducer. - std::function on_accel_callback_; - std::function on_gyro_callback_; -+ -+ // Aryzon multiple orientations -+ const screen_params::ScreenOrientation start_orientation_; - }; - - } // namespace cardboard -diff --git a/sdk/screen_params.h b/sdk/screen_params.h -index 5ab0240..94899b1 100644 ---- a/sdk/screen_params.h -+++ b/sdk/screen_params.h -@@ -28,6 +28,16 @@ void initializeAndroid(JavaVM* vm, jobject context); - #endif - void getScreenSizeInMeters(int width_pixels, int height_pixels, - float* out_width_meters, float* out_height_meters); -+ -+// Aryzon multiple orientations -+enum ScreenOrientation { -+ LandscapeLeft, -+ Portrait, -+ LandscapeRight, -+ PortraitUpsideDown -+}; -+ScreenOrientation getScreenOrientation(); -+ - } // namespace screen_params - } // namespace cardboard - -diff --git a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java -index ba30dbd..4d44c68 100644 ---- a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java -+++ b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java -@@ -21,6 +21,13 @@ import android.os.Build.VERSION_CODES; - import android.util.DisplayMetrics; - import android.view.WindowManager; - -+// Aryzon multiple orientations -+import android.content.res.Configuration; -+import android.hardware.SensorManager; -+import android.view.Display; -+import android.view.OrientationEventListener; -+import android.view.Surface; -+ - /** Utility methods to manage the screen parameters. */ - public abstract class ScreenParamsUtils { - /** Holds the screen pixel density. */ -@@ -68,4 +75,80 @@ public abstract class ScreenParamsUtils { - } - return new ScreenPixelDensity(displayMetrics.xdpi, displayMetrics.ydpi); - } -+ -+ // Aryzon multiple orientations -+ public static class ScreenOrientation { -+ /** -+ * The screen orientation -+ * 0 = lanscape left -+ * 1 = portrait -+ * 2 = landscape right -+ * 3 = portrait upside-down -+ */ -+ public static Context context; -+ public static int orientation; -+ public static int previousOrientation; -+ public static OrientationEventListener orientationEventListener; -+ -+ public ScreenOrientation(final Context context) { -+ this.context = context; -+ this.orientation = CurrentOrientation(context); -+ -+ this.orientationEventListener = new OrientationEventListener(context, SensorManager.SENSOR_DELAY_UI) { -+ public void onOrientationChanged(int orientation_) { -+ -+ int newOrientation = context.getResources().getConfiguration().orientation; -+ -+ if (orientation_ != ORIENTATION_UNKNOWN && previousOrientation != newOrientation) { -+ orientation = CurrentOrientation(ScreenOrientation.context); -+ previousOrientation = newOrientation; -+ } -+ } -+ }; -+ if (this.orientationEventListener.canDetectOrientation()) { -+ this.orientationEventListener.enable(); -+ } -+ } -+ } -+ -+ private static ScreenOrientation screenOrientation; -+ -+ public static int getScreenOrientation(Context context) { -+ if (screenOrientation == null) { -+ -+ screenOrientation = new ScreenOrientation(context); -+ } -+ return screenOrientation.orientation; -+ -+ } -+ -+ private static int CurrentOrientation (Context context) { -+ if (defaultDisplay == null) { -+ WindowManager windowManager = (WindowManager) context.getSystemService(Context.WINDOW_SERVICE); -+ defaultDisplay = windowManager.getDefaultDisplay(); -+ } -+ -+ int orientation = context.getResources().getConfiguration().orientation; -+ // This call takes a long time so we don't call this every frame, only when rotation changes -+ int rotation = defaultDisplay.getRotation(); -+ -+ if (orientation == Configuration.ORIENTATION_LANDSCAPE -+ && (rotation == Surface.ROTATION_0 -+ || rotation == Surface.ROTATION_90)) { -+ return 0; -+ } else if (orientation == Configuration.ORIENTATION_PORTRAIT -+ && (rotation == Surface.ROTATION_0 -+ || rotation == Surface.ROTATION_90)) { -+ return 1; -+ } else if (orientation == Configuration.ORIENTATION_LANDSCAPE -+ && (rotation == Surface.ROTATION_180 -+ || rotation == Surface.ROTATION_270)) { -+ return 2; -+ } else if (orientation == Configuration.ORIENTATION_PORTRAIT -+ && (rotation == Surface.ROTATION_180 -+ || rotation == Surface.ROTATION_270)) { -+ return 3; -+ } -+ return 3; -+ } - } -diff --git a/sdk/screen_params/android/screen_params.cc b/sdk/screen_params/android/screen_params.cc -index fad79ef..6272883 100644 ---- a/sdk/screen_params/android/screen_params.cc -+++ b/sdk/screen_params/android/screen_params.cc -@@ -29,6 +29,10 @@ jobject context_; - jclass screen_pixel_density_class_; - jclass screen_params_utils_class_; - -+// Aryzon multiple orientations -+jclass screen_orientation_class_; -+jmethodID get_screen_orientation_method; -+ - struct DisplayMetrics { - float xdpi; - float ydpi; -@@ -41,6 +45,16 @@ void LoadJNIResources(JNIEnv* env) { - cardboard::jni::LoadJClass(env, - "com/google/cardboard/sdk/screenparams/" - "ScreenParamsUtils$ScreenPixelDensity"); -+ -+ // Aryzon multiple orientations -+ screen_orientation_class_ = -+ cardboard::jni::LoadJClass(env, -+ "com/google/cardboard/sdk/screenparams/" -+ "ScreenParamsUtils$ScreenOrientation"); -+ -+ get_screen_orientation_method = env->GetStaticMethodID(screen_params_utils_class_, -+ "getScreenOrientation", -+ "(Landroid/content/Context;)I"); - } - - DisplayMetrics getDisplayMetrics() { -@@ -82,5 +96,17 @@ void getScreenSizeInMeters(int width_pixels, int height_pixels, - *out_height_meters = (height_pixels / display_metrics.ydpi) * kMetersPerInch; - } - -+// Aryzon multiple orientations -+ScreenOrientation getScreenOrientation() { -+ -+ JNIEnv* env; -+ cardboard::jni::LoadJNIEnv(vm_, &env); -+ -+ int screen_orientation = env->CallStaticIntMethod( -+ screen_params_utils_class_, get_screen_orientation_method, context_); -+ -+ return (ScreenOrientation)screen_orientation; -+} -+ - } // namespace screen_params - } // namespace cardboard -diff --git a/sdk/screen_params/ios/screen_params.mm b/sdk/screen_params/ios/screen_params.mm -index 1ecfe80..105f0b2 100644 ---- a/sdk/screen_params/ios/screen_params.mm -+++ b/sdk/screen_params/ios/screen_params.mm -@@ -18,6 +18,74 @@ - #import - #import - -+// Aryzon multiple orientations -+@interface OrientationHelper : NSObject { -+ -+} -+@property cardboard::screen_params::ScreenOrientation orientation; -+-(void) orientationChanged:(NSNotification *)note; -+@end -+ -+@implementation OrientationHelper { -+ -+} -+@synthesize orientation; -+ -+-(id)init -+{ -+ self = [super init]; -+ if (self) { -+ UIDevice *device = [UIDevice currentDevice]; -+ [self setOrientationValue:device]; -+ [device beginGeneratingDeviceOrientationNotifications]; -+ -+ [[NSNotificationCenter defaultCenter] -+ addObserver:self selector:@selector(orientationChanged:) -+ name:UIDeviceOrientationDidChangeNotification -+ object:device]; -+ } -+ return self; -+} -+ -+-(void) orientationChanged:(NSNotification *)note { -+ [self setOrientationValue:note.object]; -+} -+ -+-(void)setOrientationValue:(UIDevice *)device { -+ UIDeviceOrientation orientation = [device orientation]; -+ -+ if (!UIDeviceOrientationIsValidInterfaceOrientation(orientation)) { -+ return; -+ } -+ UIInterfaceOrientationMask supportedInterfaces = -1; -+ for (UIWindow *window in [[UIApplication sharedApplication] windows]) { -+ if (!window.hidden && window.subviews.count > 0) { -+ for (int i=0;i -#include -#include "util/logging.h" - -namespace cardboard { - -PositionData::PositionData(size_t buffer_size) : buffer_size_(buffer_size) {} - -void PositionData::AddSample(const Vector3& sample, const int64_t timestamp_ns) { - buffer_.push_back(sample); - if (buffer_.size() > buffer_size_) { - buffer_.pop_front(); - } - - timestamp_buffer_.push_back(timestamp_ns); - if (timestamp_buffer_.size() > buffer_size_) { - timestamp_buffer_.pop_front(); - } -} - -bool PositionData::IsValid() const { return buffer_.size() == buffer_size_; } - -long long PositionData::GetLatestTimestamp() const { - if (timestamp_buffer_.size() > 0) { - return timestamp_buffer_[timestamp_buffer_.size() -1]; - } - return 0; -} - -Vector3 PositionData::GetLatestData() const { - if (buffer_.size() > 0) { - return buffer_[buffer_.size() -1]; - } - return Vector3::Zero(); -} - - -Vector3 PositionData::GetExtrapolatedForTimeStamp(const int64_t timestamp_ns) { - - if (!IsValid() || buffer_size_ < 2) { - return {0.0,0.0,1.0}; - } - - Vector3 vSum = {0,0,0}; - - for (int i=1; i - -#include "util/vector.h" - -namespace cardboard { - -// Fixed window FIFO mean filter for vectors of the given dimension. -class PositionData { - public: - // Create a buffer to hold rotation data of size buffer_size. - // @param buffer_size size of samples to buffer. - explicit PositionData(size_t buffer_size); - - // Add sample to buffer_ if buffer_ is full it drop the oldest sample. - void AddSample(const Vector3& sample, const int64_t timestamp_ns); - - // Returns true if buffer has buffer_size sample, false otherwise. - bool IsValid() const; - - // Returns the latest value stored in the internal buffer. - Vector3 GetLatestData() const; - long long GetLatestTimestamp() const; - Vector3 GetExtrapolatedForTimeStamp(const int64_t timestamp_ns); - void Reset(); - private: - const size_t buffer_size_; - std::deque buffer_; - std::deque timestamp_buffer_; -}; - -} // namespace cardboard - - -#endif /* rotation_data_h */ diff --git a/sdk/sixdof/rotation_data.cc b/sdk/sixdof/rotation_data.cc deleted file mode 100644 index 83391166..00000000 --- a/sdk/sixdof/rotation_data.cc +++ /dev/null @@ -1,87 +0,0 @@ -/* - * Copyright 2019 Google LLC - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#include "sixdof/rotation_data.h" - -#include -#include - -namespace cardboard { - -RotationData::RotationData(size_t buffer_size) : buffer_size_(buffer_size) {} - -void RotationData::AddSample(const Vector4& sample, const int64_t timestamp_ns) { - buffer_.push_back(sample); - if (buffer_.size() > buffer_size_) { - buffer_.pop_front(); - } - - timestamp_buffer_.push_back(timestamp_ns); - if (timestamp_buffer_.size() > buffer_size_) { - timestamp_buffer_.pop_front(); - } -} - -bool RotationData::IsValid() const { return buffer_.size() == buffer_size_; } - - Vector4 RotationData::GetLatestData() const { - if (buffer_.size() > 0) { - return buffer_[buffer_.size() -1]; - } - return Vector4::Zero(); - } - - -Vector4 RotationData::GetInterpolatedForTimeStamp(const int64_t timestamp_ns) const{ - - if (!IsValid()) { - return {0.0,0.0,0.0,1.0}; - } - int64_t smaller = -1; - int64_t larger = -1; - - bool didPassLarger = false; - int i=0; - - while (!didPassLarger && i < buffer_size_) { - int64_t currentTs = timestamp_buffer_[i]; - if (currentTs <= timestamp_ns) { - smaller = currentTs; - } else { - larger = currentTs; - didPassLarger = true; - } - i++; - } - - float interpolationValue = 0.5f; - Vector4 q; - Vector4 q0; - Vector4 q1; - - if (smaller > 0 && larger > 0) { - int64_t delta = larger - smaller; - interpolationValue = (timestamp_ns - smaller) / (double)(delta); - q0 = buffer_[i-1]; - q1 = buffer_[i]; - q = interpolationValue*(q1-q0) + q0; - } else { - q = buffer_[buffer_size_]; - } - - return q; -} - -} // namespace cardboard diff --git a/sdk/sixdof/rotation_data.h b/sdk/sixdof/rotation_data.h deleted file mode 100644 index 7528be44..00000000 --- a/sdk/sixdof/rotation_data.h +++ /dev/null @@ -1,55 +0,0 @@ -/* - * Copyright 2019 Google LLC - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -// Aryzon 6DoF - -#ifndef rotation_data_h -#define rotation_data_h - -#include - -#include "util/vector.h" - -namespace cardboard { - -// Fixed window FIFO mean filter for vectors of the given dimension. -class RotationData { - public: - // Create a buffer to hold rotation data of size buffer_size. - // @param buffer_size size of samples to buffer. - explicit RotationData(size_t buffer_size); - - // Add sample to buffer_ if buffer_ is full it drop the oldest sample. - void AddSample(const Vector4& sample, const int64_t timestamp_ns); - - // Returns true if buffer has buffer_size sample, false otherwise. - bool IsValid() const; - - // Returns the latest value stored in the internal buffer. - Vector4 GetLatestData() const; - - Vector4 GetInterpolatedForTimeStamp(const int64_t timestamp_ns) const; - - private: - const size_t buffer_size_; - std::deque buffer_; - std::deque timestamp_buffer_; -}; - -} // namespace cardboard - - -#endif /* rotation_data_h */ From f5ab5914f4b83f52ad05be3f53468027b3164d02 Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Mon, 15 Feb 2021 10:40:45 +0100 Subject: [PATCH 06/21] removed 6dof lines --- sdk/cardboard.cc | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/sdk/cardboard.cc b/sdk/cardboard.cc index c05597eb..39be2e02 100644 --- a/sdk/cardboard.cc +++ b/sdk/cardboard.cc @@ -321,18 +321,6 @@ void CardboardHeadTracker_getPose(CardboardHeadTracker* head_tracker, std::memcpy(orientation, &out_orientation[0], 4 * sizeof(float)); } -// Aryzon 6DoF -void CardboardHeadTracker_addSixDoFData(CardboardHeadTracker* head_tracker, - int64_t timestamp_ns, - float* position, - float* orientation) { - if (CARDBOARD_IS_NOT_INITIALIZED() || CARDBOARD_IS_ARG_NULL(head_tracker)) { - return; - } - - static_cast(head_tracker)->AddSixDoFData(timestamp_ns, position, orientation); -} - void CardboardQrCode_getSavedDeviceParams(uint8_t** encoded_device_params, int* size) { if (CARDBOARD_IS_NOT_INITIALIZED() || From 6a4f0337a798714130d72367773e95a4beea4c7d Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Mon, 15 Feb 2021 15:10:58 +0100 Subject: [PATCH 07/21] 6DoF added --- sdk/cardboard.cc | 12 +++ sdk/head_tracker.cc | 75 ++++++++++++++-- sdk/head_tracker.h | 15 ++++ sdk/include/cardboard.h | 16 ++++ sdk/sdk.xcodeproj/project.pbxproj | 20 +++++ sdk/sixdof/position_data.cc | 82 +++++++++++++++++ sdk/sixdof/position_data.h | 55 ++++++++++++ sdk/sixdof/rotation_data.cc | 87 +++++++++++++++++++ sdk/sixdof/rotation_data.h | 55 ++++++++++++ sdk/unity/xr_provider/input.cc | 18 +++- sdk/unity/xr_provider/math_tools.cc | 27 ++++++ sdk/unity/xr_provider/math_tools.h | 7 ++ .../xr_unity_plugin/cardboard_xr_unity.cc | 19 ++++ .../xr_unity_plugin/cardboard_xr_unity.h | 7 ++ 14 files changed, 488 insertions(+), 7 deletions(-) create mode 100644 sdk/sixdof/position_data.cc create mode 100644 sdk/sixdof/position_data.h create mode 100644 sdk/sixdof/rotation_data.cc create mode 100644 sdk/sixdof/rotation_data.h diff --git a/sdk/cardboard.cc b/sdk/cardboard.cc index 39be2e02..c05597eb 100644 --- a/sdk/cardboard.cc +++ b/sdk/cardboard.cc @@ -321,6 +321,18 @@ void CardboardHeadTracker_getPose(CardboardHeadTracker* head_tracker, std::memcpy(orientation, &out_orientation[0], 4 * sizeof(float)); } +// Aryzon 6DoF +void CardboardHeadTracker_addSixDoFData(CardboardHeadTracker* head_tracker, + int64_t timestamp_ns, + float* position, + float* orientation) { + if (CARDBOARD_IS_NOT_INITIALIZED() || CARDBOARD_IS_ARG_NULL(head_tracker)) { + return; + } + + static_cast(head_tracker)->AddSixDoFData(timestamp_ns, position, orientation); +} + void CardboardQrCode_getSavedDeviceParams(uint8_t** encoded_device_params, int* size) { if (CARDBOARD_IS_NOT_INITIALIZED() || diff --git a/sdk/head_tracker.cc b/sdk/head_tracker.cc index f19fe9e9..a97972a6 100644 --- a/sdk/head_tracker.cc +++ b/sdk/head_tracker.cc @@ -23,12 +23,20 @@ namespace cardboard { +// Aryzon 6DoF +const int rotation_samples = 10; +const int position_samples = 3; +const int64_t max6DoFTimeDifference = 200000000; // Maximum time difference between last pose state timestamp and last 6DoF timestamp, if it takes longer than this the last known location of sixdof will be used + HeadTracker::HeadTracker() : is_tracking_(false), sensor_fusion_(new SensorFusionEkf()), latest_gyroscope_data_({0, 0, Vector3::Zero()}), accel_sensor_(new SensorEventProducer()), gyro_sensor_(new SensorEventProducer()), + // Aryzon 6DoF + rotation_data_(new RotationData(rotation_samples)), + position_data_(new PositionData(position_samples)), start_orientation_(screen_params::getScreenOrientation()) { // Aryzon multiple orientations sensor_fusion_->SetBiasEstimationEnabled(/*kGyroBiasEstimationEnabled*/ true); on_accel_callback_ = [&](const AccelerometerData& event) { @@ -100,12 +108,43 @@ void HeadTracker::GetPose(int64_t timestamp_ns, Rotation rotation = sensor_to_display * predicted_rotation * ekf_to_head_tracker; - out_orientation[0] = static_cast(rotation.GetQuaternion()[0]); - out_orientation[1] = static_cast(rotation.GetQuaternion()[1]); - out_orientation[2] = static_cast(rotation.GetQuaternion()[2]); - out_orientation[3] = static_cast(rotation.GetQuaternion()[3]); - - out_position = ApplyNeckModel(out_orientation, 1.0); + // Aryzon 6DoF + // Save rotation sample with timestamp to be used in AddSixDoFData() + rotation_data_->AddSample(rotation.GetQuaternion(), timestamp_ns); + + if (position_data_->IsValid() && pose_state.timestamp - position_data_->GetLatestTimestamp() < max6DoFTimeDifference) { + // 6DoF is recently updated + + rotation = rotation * -difference_to_6DoF_; + + Vector3 p = position_data_->GetExtrapolatedForTimeStamp(timestamp_ns); + std::array predicted_position_ = {(float)p[0], (float)p[1], (float)p[2]}; + + out_orientation[0] = static_cast(rotation.GetQuaternion()[0]); + out_orientation[1] = static_cast(rotation.GetQuaternion()[1]); + out_orientation[2] = static_cast(rotation.GetQuaternion()[2]); + out_orientation[3] = static_cast(rotation.GetQuaternion()[3]); + + out_position = predicted_position_; + } else { + // 6DoF is not recently updated + + out_orientation[0] = static_cast(rotation.GetQuaternion()[0]); + out_orientation[1] = static_cast(rotation.GetQuaternion()[1]); + out_orientation[2] = static_cast(rotation.GetQuaternion()[2]); + out_orientation[3] = static_cast(rotation.GetQuaternion()[3]); + + out_position = ApplyNeckModel(out_orientation, 1.0); + + if (position_data_->IsValid()) { + // Apply last known 6DoF position if 6DoF was data previously added, while still applying neckmodel. + + Vector3 last_known_position_ = position_data_->GetLatestData(); + out_position[0] += (float)last_known_position_[0]; + out_position[1] += (float)last_known_position_[1]; + out_position[2] += (float)last_known_position_[2]; + } + } } Rotation HeadTracker::GetDefaultOrientation() const { @@ -138,4 +177,28 @@ void HeadTracker::OnGyroscopeData(const GyroscopeData& event) { sensor_fusion_->ProcessGyroscopeSample(event); } +// Aryzon 6DoF +void HeadTracker::AddSixDoFData(int64_t timestamp_ns, float* pos, float* orientation) { + if (!is_tracking_) { + return; + } + position_data_->AddSample(Vector3(pos[0], pos[1], pos[2]), timestamp_ns); + + if (position_data_->IsValid() && rotation_data_->IsValid()) { + // 6DoF timestamp needs to be before the latest rotation_data timestamp + Rotation gyroAtTimeOfSixDoF = Rotation::FromQuaternion(rotation_data_->GetInterpolatedForTimeStamp(timestamp_ns)); + Rotation sixDoFRotation = Rotation::FromQuaternion(Vector4(0, orientation[1], 0, orientation[3])); + + Rotation difference = gyroAtTimeOfSixDoF * -sixDoFRotation; + + // Only synchronize rotation around the y axis + Vector4 diffQ = difference.GetQuaternion(); + diffQ[0] = 0; + diffQ[2] = 0; + + // Quaternion will be normalized in this call: + difference_to_6DoF_.SetQuaternion(diffQ); + } +} + } // namespace cardboard diff --git a/sdk/head_tracker.h b/sdk/head_tracker.h index c3ad4743..c704e51a 100644 --- a/sdk/head_tracker.h +++ b/sdk/head_tracker.h @@ -26,6 +26,10 @@ #include "sensors/sensor_fusion_ekf.h" #include "util/rotation.h" +// Aryzon 6DoF +#include "sixdof/rotation_data.h" +#include "sixdof/position_data.h" + // Aryzon multiple orientations #include "screen_params.h" @@ -49,6 +53,12 @@ class HeadTracker { // TODO(b/135488467): Support different display to sensor orientations. void GetPose(int64_t timestamp_ns, std::array& out_position, std::array& out_orientation) const; + + // Function to be called when receiving SixDoFData. + // + // Aryzon 6DoF + // @param event sensor event. + void AddSixDoFData(int64_t timestamp_ns, float* position, float* orientation); private: // Function called when receiving AccelerometerData. @@ -87,6 +97,11 @@ class HeadTracker { std::function on_accel_callback_; std::function on_gyro_callback_; + // Aryzon 6DoF + RotationData *rotation_data_; + PositionData *position_data_; + Rotation difference_to_6DoF_; + // Aryzon multiple orientations Rotation ekf_to_head_tracker; const screen_params::ScreenOrientation start_orientation_; diff --git a/sdk/include/cardboard.h b/sdk/include/cardboard.h index 74a0a677..2f5d4f62 100644 --- a/sdk/include/cardboard.h +++ b/sdk/include/cardboard.h @@ -398,6 +398,22 @@ void CardboardHeadTracker_getPose(CardboardHeadTracker* head_tracker, int64_t timestamp_ns, float* position, float* orientation); +/// Aryzon 6DoF +/// Sends through the event with pose and timestamp data from 6DoF tracker +/// +/// @pre @p head_tracker Must not be null. +/// When it is unmet, a call to this function results in a no-op. +/// +/// @param[in] head_tracker Head tracker object pointer. +/// @param[in] timestamp_ns The timestamp for the data in +/// nanoseconds in system monotonic clock. +/// @param[out] position 3 floats for (x, y, z). +/// @param[out] orientation 4 floats for quaternion +void CardboardHeadTracker_addSixDoFData(CardboardHeadTracker* head_tracker, + int64_t timestamp_ns, + float* position, + float* orientation); + /// @} ///////////////////////////////////////////////////////////////////////////// diff --git a/sdk/sdk.xcodeproj/project.pbxproj b/sdk/sdk.xcodeproj/project.pbxproj index 64cae6d7..b7eb069e 100644 --- a/sdk/sdk.xcodeproj/project.pbxproj +++ b/sdk/sdk.xcodeproj/project.pbxproj @@ -41,6 +41,8 @@ 0FD2025F2357613600B3C342 /* cardboard_device.pb.cc in Sources */ = {isa = PBXBuildFile; fileRef = 0FD2025E2357613600B3C342 /* cardboard_device.pb.cc */; }; 0FD2027C235766E800B3C342 /* cardboard.h in CopyFiles */ = {isa = PBXBuildFile; fileRef = 0FD2022B23575F3B00B3C342 /* cardboard.h */; }; 0FF99B2724587233001FF78F /* cardboard_xr_unity.cc in Sources */ = {isa = PBXBuildFile; fileRef = 0FF99B2624587233001FF78F /* cardboard_xr_unity.cc */; }; + 65D9D58F25DA7DED00A3F5E2 /* rotation_data.cc in Sources */ = {isa = PBXBuildFile; fileRef = 65D9D58B25DA7DEC00A3F5E2 /* rotation_data.cc */; }; + 65D9D59025DA7DED00A3F5E2 /* position_data.cc in Sources */ = {isa = PBXBuildFile; fileRef = 65D9D58C25DA7DEC00A3F5E2 /* position_data.cc */; }; 7B2ADACB24E4779500FEBAA8 /* opengl_es2_distortion_renderer.cc in Sources */ = {isa = PBXBuildFile; fileRef = 7B2ADAC924E4779500FEBAA8 /* opengl_es2_distortion_renderer.cc */; }; 7B76813A24A3FA6B00E92050 /* input.cc in Sources */ = {isa = PBXBuildFile; fileRef = 7B76813424A3FA6B00E92050 /* input.cc */; }; 7B76813B24A3FA6B00E92050 /* display.cc in Sources */ = {isa = PBXBuildFile; fileRef = 7B76813524A3FA6B00E92050 /* display.cc */; }; @@ -134,6 +136,10 @@ 0FD202B92357C0F200B3C342 /* sdk.bundle */ = {isa = PBXFileReference; lastKnownFileType = "wrapper.plug-in"; name = sdk.bundle; path = qrcode/ios/sdk.bundle; sourceTree = ""; }; 0FF99B2524587233001FF78F /* cardboard_xr_unity.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = cardboard_xr_unity.h; sourceTree = ""; }; 0FF99B2624587233001FF78F /* cardboard_xr_unity.cc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = cardboard_xr_unity.cc; sourceTree = ""; }; + 65D9D58B25DA7DEC00A3F5E2 /* rotation_data.cc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = rotation_data.cc; sourceTree = ""; }; + 65D9D58C25DA7DEC00A3F5E2 /* position_data.cc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = position_data.cc; sourceTree = ""; }; + 65D9D58D25DA7DED00A3F5E2 /* position_data.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = position_data.h; sourceTree = ""; }; + 65D9D58E25DA7DED00A3F5E2 /* rotation_data.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = rotation_data.h; sourceTree = ""; }; 7B2ADAC924E4779500FEBAA8 /* opengl_es2_distortion_renderer.cc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = opengl_es2_distortion_renderer.cc; sourceTree = ""; }; 7B76813424A3FA6B00E92050 /* input.cc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = input.cc; sourceTree = ""; }; 7B76813524A3FA6B00E92050 /* display.cc */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = display.cc; sourceTree = ""; }; @@ -199,6 +205,7 @@ 0FD2023123575F3B00B3C342 /* qrcode */, 0FD201F823575F3A00B3C342 /* screen_params */, 0FD2022923575F3B00B3C342 /* screen_params.h */, + 65D9D58A25DA7DD400A3F5E2 /* sixdof */, 0FD2020C23575F3B00B3C342 /* sensors */, 0FF99B2324587233001FF78F /* unity */, 0FD201FC23575F3A00B3C342 /* util */, @@ -360,6 +367,17 @@ path = xr_unity_plugin; sourceTree = ""; }; + 65D9D58A25DA7DD400A3F5E2 /* sixdof */ = { + isa = PBXGroup; + children = ( + 65D9D58C25DA7DEC00A3F5E2 /* position_data.cc */, + 65D9D58D25DA7DED00A3F5E2 /* position_data.h */, + 65D9D58B25DA7DEC00A3F5E2 /* rotation_data.cc */, + 65D9D58E25DA7DED00A3F5E2 /* rotation_data.h */, + ); + path = sixdof; + sourceTree = ""; + }; 7B2ADAC824E4779500FEBAA8 /* rendering */ = { isa = PBXGroup; children = ( @@ -472,6 +490,7 @@ 0FD2025323575F3B00B3C342 /* polynomial_radial_distortion.cc in Sources */, 0FD2025923575F3B00B3C342 /* distortion_mesh.cc in Sources */, 0FD2024623575F3B00B3C342 /* lowpass_filter.cc in Sources */, + 65D9D59025DA7DED00A3F5E2 /* position_data.cc in Sources */, 0FD2024823575F3B00B3C342 /* neck_model.cc in Sources */, 0FD2024D23575F3B00B3C342 /* sensor_event_producer.mm in Sources */, 0F29AA62255AC3A200154BD0 /* is_initialized.cc in Sources */, @@ -485,6 +504,7 @@ 0FD2024723575F3B00B3C342 /* median_filter.cc in Sources */, 0FD2024523575F3B00B3C342 /* head_tracker.cc in Sources */, 0FD2024123575F3B00B3C342 /* matrix_4x4.cc in Sources */, + 65D9D58F25DA7DED00A3F5E2 /* rotation_data.cc in Sources */, 0FD2024C23575F3B00B3C342 /* device_accelerometer_sensor.mm in Sources */, 0FD2025423575F3B00B3C342 /* qr_scan_view_controller.mm in Sources */, 0FD2025123575F3B00B3C342 /* cardboard.cc in Sources */, diff --git a/sdk/sixdof/position_data.cc b/sdk/sixdof/position_data.cc new file mode 100644 index 00000000..6c2d255d --- /dev/null +++ b/sdk/sixdof/position_data.cc @@ -0,0 +1,82 @@ +/* + * Copyright 2019 Google LLC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include "sixdof/position_data.h" + +#include +#include +#include "util/logging.h" + +namespace cardboard { + +PositionData::PositionData(size_t buffer_size) : buffer_size_(buffer_size) {} + +void PositionData::AddSample(const Vector3& sample, const int64_t timestamp_ns) { + buffer_.push_back(sample); + if (buffer_.size() > buffer_size_) { + buffer_.pop_front(); + } + + timestamp_buffer_.push_back(timestamp_ns); + if (timestamp_buffer_.size() > buffer_size_) { + timestamp_buffer_.pop_front(); + } +} + +bool PositionData::IsValid() const { return buffer_.size() == buffer_size_; } + +long long PositionData::GetLatestTimestamp() const { + if (timestamp_buffer_.size() > 0) { + return timestamp_buffer_[timestamp_buffer_.size() -1]; + } + return 0; +} + +Vector3 PositionData::GetLatestData() const { + if (buffer_.size() > 0) { + return buffer_[buffer_.size() -1]; + } + return Vector3::Zero(); +} + + +Vector3 PositionData::GetExtrapolatedForTimeStamp(const int64_t timestamp_ns) { + + if (!IsValid() || buffer_size_ < 2) { + return {0.0,0.0,1.0}; + } + + Vector3 vSum = {0,0,0}; + + for (int i=1; i + +#include "util/vector.h" + +namespace cardboard { + +// Fixed window FIFO mean filter for vectors of the given dimension. +class PositionData { + public: + // Create a buffer to hold rotation data of size buffer_size. + // @param buffer_size size of samples to buffer. + explicit PositionData(size_t buffer_size); + + // Add sample to buffer_ if buffer_ is full it drop the oldest sample. + void AddSample(const Vector3& sample, const int64_t timestamp_ns); + + // Returns true if buffer has buffer_size sample, false otherwise. + bool IsValid() const; + + // Returns the latest value stored in the internal buffer. + Vector3 GetLatestData() const; + long long GetLatestTimestamp() const; + Vector3 GetExtrapolatedForTimeStamp(const int64_t timestamp_ns); + void Reset(); + private: + const size_t buffer_size_; + std::deque buffer_; + std::deque timestamp_buffer_; +}; + +} // namespace cardboard + + +#endif /* rotation_data_h */ diff --git a/sdk/sixdof/rotation_data.cc b/sdk/sixdof/rotation_data.cc new file mode 100644 index 00000000..83391166 --- /dev/null +++ b/sdk/sixdof/rotation_data.cc @@ -0,0 +1,87 @@ +/* + * Copyright 2019 Google LLC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include "sixdof/rotation_data.h" + +#include +#include + +namespace cardboard { + +RotationData::RotationData(size_t buffer_size) : buffer_size_(buffer_size) {} + +void RotationData::AddSample(const Vector4& sample, const int64_t timestamp_ns) { + buffer_.push_back(sample); + if (buffer_.size() > buffer_size_) { + buffer_.pop_front(); + } + + timestamp_buffer_.push_back(timestamp_ns); + if (timestamp_buffer_.size() > buffer_size_) { + timestamp_buffer_.pop_front(); + } +} + +bool RotationData::IsValid() const { return buffer_.size() == buffer_size_; } + + Vector4 RotationData::GetLatestData() const { + if (buffer_.size() > 0) { + return buffer_[buffer_.size() -1]; + } + return Vector4::Zero(); + } + + +Vector4 RotationData::GetInterpolatedForTimeStamp(const int64_t timestamp_ns) const{ + + if (!IsValid()) { + return {0.0,0.0,0.0,1.0}; + } + int64_t smaller = -1; + int64_t larger = -1; + + bool didPassLarger = false; + int i=0; + + while (!didPassLarger && i < buffer_size_) { + int64_t currentTs = timestamp_buffer_[i]; + if (currentTs <= timestamp_ns) { + smaller = currentTs; + } else { + larger = currentTs; + didPassLarger = true; + } + i++; + } + + float interpolationValue = 0.5f; + Vector4 q; + Vector4 q0; + Vector4 q1; + + if (smaller > 0 && larger > 0) { + int64_t delta = larger - smaller; + interpolationValue = (timestamp_ns - smaller) / (double)(delta); + q0 = buffer_[i-1]; + q1 = buffer_[i]; + q = interpolationValue*(q1-q0) + q0; + } else { + q = buffer_[buffer_size_]; + } + + return q; +} + +} // namespace cardboard diff --git a/sdk/sixdof/rotation_data.h b/sdk/sixdof/rotation_data.h new file mode 100644 index 00000000..7528be44 --- /dev/null +++ b/sdk/sixdof/rotation_data.h @@ -0,0 +1,55 @@ +/* + * Copyright 2019 Google LLC + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// Aryzon 6DoF + +#ifndef rotation_data_h +#define rotation_data_h + +#include + +#include "util/vector.h" + +namespace cardboard { + +// Fixed window FIFO mean filter for vectors of the given dimension. +class RotationData { + public: + // Create a buffer to hold rotation data of size buffer_size. + // @param buffer_size size of samples to buffer. + explicit RotationData(size_t buffer_size); + + // Add sample to buffer_ if buffer_ is full it drop the oldest sample. + void AddSample(const Vector4& sample, const int64_t timestamp_ns); + + // Returns true if buffer has buffer_size sample, false otherwise. + bool IsValid() const; + + // Returns the latest value stored in the internal buffer. + Vector4 GetLatestData() const; + + Vector4 GetInterpolatedForTimeStamp(const int64_t timestamp_ns) const; + + private: + const size_t buffer_size_; + std::deque buffer_; + std::deque timestamp_buffer_; +}; + +} // namespace cardboard + + +#endif /* rotation_data_h */ diff --git a/sdk/unity/xr_provider/input.cc b/sdk/unity/xr_provider/input.cc index ee42a192..0dbc758a 100644 --- a/sdk/unity/xr_provider/input.cc +++ b/sdk/unity/xr_provider/input.cc @@ -131,10 +131,19 @@ class CardboardInputProvider { cardboard_api_->GetHeadTrackerPose(out_position.data(), out_orientation.data()); // TODO(b/151817737): Compute pose position within SDK with custom rotation. - head_pose_ = cardboard::unity::CardboardRotationToUnityPose(out_orientation); + //head_pose_ = cardboard::unity::CardboardRotationToUnityPose(out_orientation); + + // Aryzon 6DoF changed to include position + head_pose_ = cardboard::unity::CardboardPoseToUnityPose(out_orientation, out_position); + return kUnitySubsystemErrorCodeSuccess; } + // Aryzon 6DoF + void Add6DoF(int64_t timestamp_ns, float* position, float* orientation) { + cardboard_api_->AddSixDoFData(timestamp_ns, position, orientation); + } + UnitySubsystemErrorCode FillDeviceDefinition( UnityXRInternalInputDeviceId device_id, UnityXRInputDeviceDefinition* definition) { @@ -252,3 +261,10 @@ UnitySubsystemErrorCode LoadInput(IUnityInterfaces* xr_interfaces) { } void UnloadInput() { CardboardInputProvider::GetInstance().reset(); } + +// Aryzon 6DoF +extern "C" { + void CardboardUnity_AddSixDoFData(CardboardInputProvider* ptr, int64_t timestamp_ns, float* position, float* orientation) { + ptr->GetInstance()->Add6DoF(timestamp_ns, position, orientation); + } +} diff --git a/sdk/unity/xr_provider/math_tools.cc b/sdk/unity/xr_provider/math_tools.cc index dfa01467..214dcb95 100644 --- a/sdk/unity/xr_provider/math_tools.cc +++ b/sdk/unity/xr_provider/math_tools.cc @@ -132,6 +132,33 @@ UnityXRPose CardboardRotationToUnityPose(const std::array& rotation) { return result; } +// Aryzon 6DoF added CardboardPoseToUnityPose to include also position instead of only rotation +UnityXRPose CardboardPoseToUnityPose(const std::array& rotation, const std::array& position) { + UnityXRPose result; + + // Sets Unity Pose's rotation. Unity expects forward as positive z axis, + // whereas OpenGL expects forward as negative z. + result.rotation.x = rotation.at(0); + result.rotation.y = rotation.at(1); + result.rotation.z = -rotation.at(2); + result.rotation.w = rotation.at(3); + + result.position.x = position.at(0); + result.position.y = position.at(1); + result.position.z = -position.at(2); + + // Computes Unity Pose's position. + // Reasoning: It's easier to compute the position directly applying the neck + // model to Unity's rotation instead of using the one provided by the SDK. To + // use the provided position we should perform the following computation: + // 1. Compute inverse rotation quaternion (OpenGL's coordinates frame). + // 2. Apply the inverse rotation to the provided position. + // 3. Modify the position vector to suit Unity's coordinates frame. + // 4. Apply the new rotation (Unity's coordinates frame). + + return result; +} + // TODO(b/155113586): refactor this function to be part of the same // transformation as the above. UnityXRPose CardboardTransformToUnityPose( diff --git a/sdk/unity/xr_provider/math_tools.h b/sdk/unity/xr_provider/math_tools.h index ce482f41..39e41a00 100644 --- a/sdk/unity/xr_provider/math_tools.h +++ b/sdk/unity/xr_provider/math_tools.h @@ -30,6 +30,13 @@ namespace unity { /// @returns A UnityXRPose from Cardboard @p rotation. UnityXRPose CardboardRotationToUnityPose(const std::array& rotation); +/// Aryzon 6DoF +/// @brief Creates a UnityXRPose from a Cardboard rotation. +/// @param rotation A Cardboard rotation quaternion expressed as [x, y, z, w]. +/// @param position A Cardboard position vector expressed as [x, y, z]. +/// @returns A UnityXRPose from Cardboard @p rotation. +UnityXRPose CardboardPoseToUnityPose(const std::array& rotation, const std::array& position); + /// @brief Creates a UnityXRPose from a Cardboard transformation matrix. /// @param transform A 4x4 float transformation matrix. /// @returns A UnityXRPose from Cardboard @p transform. diff --git a/sdk/unity/xr_unity_plugin/cardboard_xr_unity.cc b/sdk/unity/xr_unity_plugin/cardboard_xr_unity.cc index 1a453c04..6db1c063 100644 --- a/sdk/unity/xr_unity_plugin/cardboard_xr_unity.cc +++ b/sdk/unity/xr_unity_plugin/cardboard_xr_unity.cc @@ -276,6 +276,20 @@ class CardboardApi::CardboardApiImpl { position, orientation); } + // Aryzon 6DoF + void AddSixDoFData(int64_t timestamp_nano, float* position, float* orientation) { + //LOGW("Head tracker was queried when setting 6DoF data."); + if (head_tracker_ == nullptr) { + LOGW("Uninitialized head tracker was queried when setting 6DoF data."); + return; + } + // Convert from Unity space to Cardboard space + position[2] = -position[2]; + orientation[2] = -orientation[2]; + + CardboardHeadTracker_addSixDoFData(head_tracker_.get(), timestamp_nano, position, orientation); + } + static void ScanDeviceParams() { CardboardQrCode_scanQrCodeAndSaveDeviceParams(); } @@ -716,6 +730,11 @@ void CardboardApi::GetHeadTrackerPose(float* position, float* orientation) { p_impl_->GetHeadTrackerPose(position, orientation); } +// Aryzon 6DoF +void CardboardApi::AddSixDoFData(int64_t timestamp_nano, float *position, float *orientation) { + p_impl_->AddSixDoFData(timestamp_nano, position, orientation); +} + void CardboardApi::ScanDeviceParams() { CardboardApiImpl::ScanDeviceParams(); } void CardboardApi::UpdateDeviceParams() { p_impl_->UpdateDeviceParams(); } diff --git a/sdk/unity/xr_unity_plugin/cardboard_xr_unity.h b/sdk/unity/xr_unity_plugin/cardboard_xr_unity.h index 6ff165f2..dd3f13ab 100644 --- a/sdk/unity/xr_unity_plugin/cardboard_xr_unity.h +++ b/sdk/unity/xr_unity_plugin/cardboard_xr_unity.h @@ -84,6 +84,13 @@ class CardboardApi { // TODO(b/154305848): Move argument types to std::array*. void GetHeadTrackerPose(float* position, float* orientation); + /// Aryzon 6DoF + /// @brief Add a 6DoF pose sample to the HeadTracker module. + /// @param[in] timestamp_nano A timestamp of the moment the 6DoF data was captured in nanoseconds + /// @param[in] position A pointer to an array with three floats that holds the 6DoF position + /// @param[in] orientation A pointer to an array with four floats that holds the 6DoF orientation + void AddSixDoFData(int64_t timestamp_nano, float* position, float* orientation); + /// @brief Triggers a device parameters scan. /// @pre When using Android, the pointer to `JavaVM` must be previously set. void ScanDeviceParams(); From 41b608d23b2b993c3c0835492a4934538a2bbc67 Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Mon, 15 Feb 2021 15:48:35 +0100 Subject: [PATCH 08/21] unity example files: how to 6DoF --- sixdof-unity/CardboardSubsystemLoader.cs | 129 ++++++++++ sixdof-unity/SixDoFPoseDriver.cs | 305 +++++++++++++++++++++++ 2 files changed, 434 insertions(+) create mode 100644 sixdof-unity/CardboardSubsystemLoader.cs create mode 100644 sixdof-unity/SixDoFPoseDriver.cs diff --git a/sixdof-unity/CardboardSubsystemLoader.cs b/sixdof-unity/CardboardSubsystemLoader.cs new file mode 100644 index 00000000..b030ed52 --- /dev/null +++ b/sixdof-unity/CardboardSubsystemLoader.cs @@ -0,0 +1,129 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.Runtime.InteropServices; + +using UnityEngine; +using UnityEngine.XR; + +using Google.XR.Cardboard; + +namespace Aryzon +{ + + public class CardboardSubsystemLoader : MonoBehaviour + { + private static IntPtr _inputPointer; + public static IntPtr inputPointer + { + get { if (isStarted) { return _inputPointer; } else { return IntPtr.Zero; } } + set { _inputPointer = value; } + } + + private static IntPtr _displayPointer; + public static IntPtr displayPointer + { + get { if (isStarted) { return _displayPointer; } else { return IntPtr.Zero; } } + set { _displayPointer = value; } + } + + private XRLoader loader; + + public static bool isInitialized = false; + public static bool isStarted = false; + + private string inputMatch = "Input"; + private string displayMatch = "Display"; + + private void Start() + { + StartCardboard(); + } + + public void StartCardboard() + { + if (!loader) + { + loader = new XRLoader(); + } +#if !UNITY_EDITOR + loader.Initialize(); +#endif + loader.Start(); + ConnectCardboardInputSystem(); + + isStarted = true; + + ReloadDeviceParams(); + + if (!Api.HasDeviceParams()) + { + Api.ScanDeviceParams(); + } + } + + public void StopCardboard() + { + if (loader) + { + loader.Stop(); + loader.Deinitialize(); + } + isStarted = false; + } + + public void ReloadDeviceParams() + { + if (!isStarted) + { + return; + } + Api.ReloadDeviceParams(); + } + + public void Update() + { + if (!isStarted) + { + return; + } + + if (Api.IsGearButtonPressed) + { + Api.ScanDeviceParams(); + } + + if (Api.IsCloseButtonPressed) + { + Application.Quit(); + } + + if (Api.HasNewDeviceParams()) + { + Api.ReloadDeviceParams(); + } + + Api.UpdateScreenParams(); + } + + private void ConnectCardboardInputSystem() + { + List inputs = new List(); + SubsystemManager.GetSubsystemDescriptors(inputs); + + foreach (var d in inputs) + { + if (d.id.Equals(inputMatch)) + { + XRInputSubsystem inputInst = d.Create(); + + if (inputInst != null) + { + GCHandle handle = GCHandle.Alloc(inputInst); + inputPointer = GCHandle.ToIntPtr(handle); + } + } + } + } + } +} \ No newline at end of file diff --git a/sixdof-unity/SixDoFPoseDriver.cs b/sixdof-unity/SixDoFPoseDriver.cs new file mode 100644 index 00000000..74c6c1a6 --- /dev/null +++ b/sixdof-unity/SixDoFPoseDriver.cs @@ -0,0 +1,305 @@ + +using System; +using System.Collections; +using System.Collections.Generic; +using System.Runtime.CompilerServices; +using System.Runtime.InteropServices; +using UnityEngine; +using UnityEngine.XR; +#if ARYZON_ARFOUNDATION +using UnityEngine.XR.ARFoundation; +#endif +using Google.XR.Cardboard; + +namespace Aryzon +{ + public class SixDoFPoseDriver : MonoBehaviour + { + [DllImport(Constants.CardboardApi)] + private static extern void CardboardUnity_AddSixDoFData(IntPtr ptr, Int64 timestamp, [In] float[] position, [In] float[] orientation); + +#if UNITY_EDITOR + public Transform editorPoseProviderTransform; +#endif +#if ARYZON_ARFOUNDATION + private ARCameraManager _arCameraManager; + public ARCameraManager arCameraManager + { + get { + if (!_arCameraManager) + { + foreach (Camera camera in Camera.allCameras) + { + ARCameraManager _cameraManager = camera.gameObject.GetComponent(); + if (_cameraManager && _cameraManager.enabled) + { + _arCameraManager = _cameraManager; + break; + } + } + } + if (!_arCameraManager) + { + Debug.LogError("[Aryzon] No ARCameraManager found, make sure there is one attached to an active camera."); + } + return _arCameraManager; + } + set { _arCameraManager = value; } + } +#endif + + internal struct NullablePose + { + internal Vector3? position; + internal Quaternion? rotation; + } + + public void OnEnable() + { + Application.onBeforeRender += OnBeforeRender; +#if ARYZON_ARFOUNDATION + arCameraManager.frameReceived += ArCameraManager_frameReceived; +#endif +#if UNITY_2020_1_OR_NEWER + List devices = new List(); + InputDevices.GetDevicesWithCharacteristics(InputDeviceCharacteristics.TrackedDevice, devices); + foreach (var device in devices) + { + if (device.characteristics.HasFlag(InputDeviceCharacteristics.TrackedDevice)) + { + CheckConnectedDevice(device, false); + } + } + + InputDevices.deviceConnected += OnInputDeviceConnected; +#endif // UNITY_UNITY_2020_1_OR_NEWER + } + + public void OnDisable() + { + Application.onBeforeRender -= OnBeforeRender; +#if ARYZON_ARFOUNDATION + arCameraManager.frameReceived -= ArCameraManager_frameReceived; +#endif +#if UNITY_2020_1_OR_NEWER + InputDevices.deviceConnected -= OnInputDeviceConnected; +#endif // UNITY_UNITY_2020_1_OR_NEWER + } + + //void Update() => PerformUpdate(); + + void OnBeforeRender() => PerformUpdate(); + + void PerformUpdate() + { +#if UNITY_EDITOR + pose.position = editorPoseProviderTransform.position; + pose.rotation = editorPoseProviderTransform.rotation; +#else + var updatedPose = GetPoseData(); + + if (updatedPose.position.HasValue) + { + transform.localPosition = updatedPose.position.Value; + } + if (updatedPose.rotation.HasValue) + { + transform.localRotation = updatedPose.rotation.Value; + } +#endif + } + +#if UNITY_2020_1_OR_NEWER + static internal InputDevice? s_InputTrackingDevice = null; + static internal InputDevice? s_CardboardHMDInputTrackingDevice = null; + + void OnInputDeviceConnected(InputDevice device) => CheckConnectedDevice(device); + + void CheckConnectedDevice(InputDevice device, bool displayWarning = true) + { + var positionSuccess = false; + var rotationSuccess = false; + if (!(positionSuccess = device.TryGetFeatureValue(CommonUsages.centerEyePosition, out Vector3 position))) + positionSuccess = device.TryGetFeatureValue(CommonUsages.colorCameraPosition, out position); + if (!(rotationSuccess = device.TryGetFeatureValue(CommonUsages.centerEyeRotation, out Quaternion rotation))) + rotationSuccess = device.TryGetFeatureValue(CommonUsages.colorCameraRotation, out rotation); + + if (positionSuccess && rotationSuccess) + { + if (s_InputTrackingDevice == null) + { + s_InputTrackingDevice = device; + } + else + { + Debug.LogWarning($"An input device {device.name} with the TrackedDevice characteristic was registered but the ARPoseDriver is already consuming data from {s_InputTrackingDevice.Value.name}."); + if (s_CardboardHMDInputTrackingDevice == null && device.name == "Cardboard HMD") + { + s_CardboardHMDInputTrackingDevice = device; + //arCameraManager.frameReceived += ArCameraManager_frameReceived; + } + } + } + } + +#else + static internal List nodeStates = new List(); +#endif // UNITY_2020_1_OR_NEWER + + public void AddSixDoFData(Vector3 position, Quaternion rotation, long timestampNs) + { + float[] positionArray = { position.x, position.y, position.z }; + float[] rotationArray = { rotation.x, rotation.y, rotation.z, rotation.w }; + CardboardUnity_AddSixDoFData(AryzonCardboardSubsystemLoader.inputPointer, timestampNs, positionArray, rotationArray); + } + +#if ARYZON_ARFOUNDATION + + private void ArCameraManager_frameReceived(ARCameraFrameEventArgs obj) + { + if (!AryzonCardboardSubsystemLoader.isStarted) + { + return; + } +#if UNITY_2020_1_OR_NEWER + + if (s_InputTrackingDevice != null) + { + var pose = Pose.identity; + var positionSuccess = false; + var rotationSuccess = false; + + if (!(positionSuccess = s_InputTrackingDevice.Value.TryGetFeatureValue(CommonUsages.centerEyePosition, out pose.position))) + positionSuccess = s_InputTrackingDevice.Value.TryGetFeatureValue(CommonUsages.colorCameraPosition, out pose.position); + if (!(rotationSuccess = s_InputTrackingDevice.Value.TryGetFeatureValue(CommonUsages.centerEyeRotation, out pose.rotation))) + rotationSuccess = s_InputTrackingDevice.Value.TryGetFeatureValue(CommonUsages.colorCameraRotation, out pose.rotation); + + if (positionSuccess && rotationSuccess) + { + AddSixDoFData(pose.position, pose.rotation, (long)obj.timestampNs); + } + } +#else + UnityEngine.XR.InputTracking.GetNodeStates(nodeStates); + foreach (var nodeState in nodeStates) + { + if (nodeState.nodeType == UnityEngine.XR.XRNode.CenterEye) + { + var pose = Pose.identity; + var positionSuccess = nodeState.TryGetPosition(out pose.position); + var rotationSuccess = nodeState.TryGetRotation(out pose.rotation); + + if (positionSuccess && rotationSuccess) + { + AddSixDoFData(pose.position, pose.rotation, (long)obj.timestampNs); + } + break; + } + } +#endif + } +#endif + static internal NullablePose GetPoseData() + { + NullablePose resultPose = new NullablePose(); + +#if UNITY_2020_1_OR_NEWER + if (!AryzonCardboardSubsystemLoader.isStarted && s_CardboardHMDInputTrackingDevice != null) + { + s_CardboardHMDInputTrackingDevice = null; + } + + if (s_CardboardHMDInputTrackingDevice != null) + { + var pose = Pose.identity; + var positionSuccess = false; + var rotationSuccess = false; + + if (!(positionSuccess = s_CardboardHMDInputTrackingDevice.Value.TryGetFeatureValue(CommonUsages.centerEyePosition, out pose.position))) + positionSuccess = s_CardboardHMDInputTrackingDevice.Value.TryGetFeatureValue(CommonUsages.colorCameraPosition, out pose.position); + if (!(rotationSuccess = s_CardboardHMDInputTrackingDevice.Value.TryGetFeatureValue(CommonUsages.centerEyeRotation, out pose.rotation))) + rotationSuccess = s_CardboardHMDInputTrackingDevice.Value.TryGetFeatureValue(CommonUsages.colorCameraRotation, out pose.rotation); + + if (positionSuccess) + resultPose.position = pose.position; + if (rotationSuccess) + resultPose.rotation = pose.rotation; + + Debug.Log("x: " + pose.position.x + " y: " + pose.position.y + " z: " + pose.position.z); + + if (positionSuccess || rotationSuccess) + return resultPose; + } + else if (s_InputTrackingDevice != null) + { + var pose = Pose.identity; + var positionSuccess = false; + var rotationSuccess = false; + + if (!(positionSuccess = s_InputTrackingDevice.Value.TryGetFeatureValue(CommonUsages.centerEyePosition, out pose.position))) + positionSuccess = s_InputTrackingDevice.Value.TryGetFeatureValue(CommonUsages.colorCameraPosition, out pose.position); + if (!(rotationSuccess = s_InputTrackingDevice.Value.TryGetFeatureValue(CommonUsages.centerEyeRotation, out pose.rotation))) + rotationSuccess = s_InputTrackingDevice.Value.TryGetFeatureValue(CommonUsages.colorCameraRotation, out pose.rotation); + + if (positionSuccess) + resultPose.position = pose.position; + if (rotationSuccess) + resultPose.rotation = pose.rotation; + + if (positionSuccess || rotationSuccess) + return resultPose; + } +#else + UnityEngine.XR.InputTracking.GetNodeStates(nodeStates); + + List states = new List(); + + if (!AryzonCardboardSubsystemLoader.isStarted) + { + foreach (UnityEngine.XR.XRNodeState nodeState in nodeStates) + { + if (nodeState.nodeType == UnityEngine.XR.XRNode.CenterEye) + { + var pose = Pose.identity; + var positionSuccess = nodeState.TryGetPosition(out pose.position); + var rotationSuccess = nodeState.TryGetRotation(out pose.rotation); + + if (positionSuccess) + resultPose.position = pose.position; + if (rotationSuccess) + resultPose.rotation = pose.rotation; + + break; + } + } + } + else + { + foreach (UnityEngine.XR.XRNodeState nodeState in nodeStates) + { + if (nodeState.nodeType == UnityEngine.XR.XRNode.CenterEye) + { + states.Add(nodeState); + } + } + + if (nodeStates.Count > 0) + { + UnityEngine.XR.XRNodeState nodeState = nodeStates[nodeStates.Count - 1]; + var pose = Pose.identity; + var positionSuccess = nodeState.TryGetPosition(out pose.position); + var rotationSuccess = nodeState.TryGetRotation(out pose.rotation); + + if (positionSuccess) + resultPose.position = pose.position; + if (rotationSuccess) + resultPose.rotation = pose.rotation; + + return resultPose; + } + } +#endif // UNITY_2020_1_OR_NEWER + return resultPose; + } + } +} \ No newline at end of file From 7c919a642ce780a3e5fb1a0bf861dc0b9d41743e Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Thu, 4 Mar 2021 14:03:00 +0100 Subject: [PATCH 09/21] Changes for pull request multiple orientation support --- .../src/main/AndroidManifest.xml | 1 - .../HelloCardboardViewController.mm | 16 ++-- sdk/cardboard.cc | 16 ++++ sdk/head_tracker.cc | 46 +++++----- sdk/head_tracker.h | 6 +- sdk/include/cardboard.h | 26 ++++++ sdk/screen_params.h | 18 ++-- .../sdk/screenparams/ScreenParamsUtils.java | 89 ++++++++++--------- sdk/screen_params/android/screen_params.cc | 13 ++- sdk/screen_params/ios/screen_params.mm | 19 ++-- 10 files changed, 150 insertions(+), 100 deletions(-) diff --git a/hellocardboard-android/src/main/AndroidManifest.xml b/hellocardboard-android/src/main/AndroidManifest.xml index 850b8fee..9eb74ab7 100644 --- a/hellocardboard-android/src/main/AndroidManifest.xml +++ b/hellocardboard-android/src/main/AndroidManifest.xml @@ -21,7 +21,6 @@ android:name="com.google.cardboard.VrActivity" android:label="@string/title_activity_vr" android:theme="@style/Theme.AppCompat.NoActionBar" - android:screenOrientation="landscape" android:exported="true"> diff --git a/hellocardboard-ios/HelloCardboardViewController.mm b/hellocardboard-ios/HelloCardboardViewController.mm index 9d94406a..e587736e 100644 --- a/hellocardboard-ios/HelloCardboardViewController.mm +++ b/hellocardboard-ios/HelloCardboardViewController.mm @@ -85,9 +85,14 @@ - (BOOL)prefersHomeIndicatorAutoHidden { return true; } +- (void)viewLayoutMarginsDidChange { + [super viewLayoutMarginsDidChange]; + _updateParams = YES; +} + - (UIInterfaceOrientationMask)supportedInterfaceOrientations { - // Cardboard only supports landscape right orientation for inserting the phone in the viewer. - return UIInterfaceOrientationMaskLandscapeRight; + // Cardboard supports all screen orientations except Portrait Upside Down + return UIInterfaceOrientationMaskAllButUpsideDown; } - (void)glkView:(GLKView *)view drawInRect:(CGRect)rect { @@ -119,13 +124,6 @@ - (BOOL)updateCardboardParams { int height = screenRect.size.height * screenScale; int width = screenRect.size.width * screenScale; - // Rendering coordinates asumes landscape orientation. - if (height > width) { - int temp = height; - height = width; - width = temp; - } - // Create CardboardLensDistortion. CardboardLensDistortion_destroy(_cardboardLensDistortion); _cardboardLensDistortion = diff --git a/sdk/cardboard.cc b/sdk/cardboard.cc index 39be2e02..fbf346ba 100644 --- a/sdk/cardboard.cc +++ b/sdk/cardboard.cc @@ -119,6 +119,22 @@ void Cardboard_initializeAndroid(JavaVM* vm, jobject context) { } #endif +CardboardScreenOrientation CardboardScreenParameters_getScreenOrientation() { + switch (cardboard::screen_params::getScreenOrientation()) { + case cardboard::screen_params::LandscapeLeft: + return kLandscapeLeft; + case cardboard::screen_params::LandscapeRight: + return kLandscapeRight; + case cardboard::screen_params::Portrait: + return kPortrait; + case cardboard::screen_params::PortraitUpsideDown: + return kPortraitUpsideDown; + default: + break; + } + return kUnknown; +} + CardboardLensDistortion* CardboardLensDistortion_create( const uint8_t* encoded_device_params, int size, int display_width, int display_height) { diff --git a/sdk/head_tracker.cc b/sdk/head_tracker.cc index f19fe9e9..9b1561e5 100644 --- a/sdk/head_tracker.cc +++ b/sdk/head_tracker.cc @@ -21,6 +21,9 @@ #include "util/vector.h" #include "util/vectorutils.h" +// Aryzon multiple orientations +#include "screen_params.h" + namespace cardboard { HeadTracker::HeadTracker() @@ -28,8 +31,7 @@ HeadTracker::HeadTracker() sensor_fusion_(new SensorFusionEkf()), latest_gyroscope_data_({0, 0, Vector3::Zero()}), accel_sensor_(new SensorEventProducer()), - gyro_sensor_(new SensorEventProducer()), - start_orientation_(screen_params::getScreenOrientation()) { // Aryzon multiple orientations + gyro_sensor_(new SensorEventProducer()) { sensor_fusion_->SetBiasEstimationEnabled(/*kGyroBiasEstimationEnabled*/ true); on_accel_callback_ = [&](const AccelerometerData& event) { OnAccelerometerData(event); @@ -37,15 +39,17 @@ HeadTracker::HeadTracker() on_gyro_callback_ = [&](const GyroscopeData& event) { OnGyroscopeData(event); }; - - // Aryzon multiple orientations - if (start_orientation_ == screen_params::LandscapeLeft) { - ekf_to_head_tracker = Rotation::FromYawPitchRoll(-M_PI / 2.0, 0, -M_PI / 2.0); - } else if (start_orientation_ == screen_params::LandscapeRight) { - ekf_to_head_tracker = Rotation::FromYawPitchRoll(M_PI / 2.0, 0, M_PI / 2.0); - } else { - // Portrait - ekf_to_head_tracker = Rotation::FromYawPitchRoll(M_PI / 2.0, M_PI / 2.0, M_PI / 2.0); + + switch(screen_params::getScreenOrientation()) { + case screen_params::LandscapeLeft: + ekf_to_head_tracker_ = Rotation::FromYawPitchRoll(-M_PI / 2.0, 0, -M_PI / 2.0); + break; + case screen_params::LandscapeRight: + ekf_to_head_tracker_ = Rotation::FromYawPitchRoll(M_PI / 2.0, 0, M_PI / 2.0); + break; + default: // Portrait + ekf_to_head_tracker_ = Rotation::FromYawPitchRoll(M_PI / 2.0, M_PI / 2.0, M_PI / 2.0); + break; } } @@ -86,19 +90,19 @@ void HeadTracker::GetPose(int64_t timestamp_ns, // space. Rotation sensor_to_display; - // Aryzon multiple orientations - // Very fast implementation on iOS, pretty fast for Android - screen_params::ScreenOrientation orientation = screen_params::getScreenOrientation(); - - if (orientation == screen_params::LandscapeLeft) { + switch(screen_params::getScreenOrientation()) { + case screen_params::LandscapeLeft: sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), M_PI / 2.0); - } else if (orientation == screen_params::LandscapeRight) { + break; + case screen_params::LandscapeRight: sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), -M_PI / 2.0); - } else { // Portrait - sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), 0.0); + break; + default: // Portrait + sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), 0.); + break; } - - Rotation rotation = sensor_to_display * predicted_rotation * ekf_to_head_tracker; + + const Rotation rotation = sensor_to_display * predicted_rotation * ekf_to_head_tracker_; out_orientation[0] = static_cast(rotation.GetQuaternion()[0]); out_orientation[1] = static_cast(rotation.GetQuaternion()[1]); diff --git a/sdk/head_tracker.h b/sdk/head_tracker.h index c3ad4743..c9bd61f8 100644 --- a/sdk/head_tracker.h +++ b/sdk/head_tracker.h @@ -26,9 +26,6 @@ #include "sensors/sensor_fusion_ekf.h" #include "util/rotation.h" -// Aryzon multiple orientations -#include "screen_params.h" - namespace cardboard { // HeadTracker encapsulates pose tracking by connecting sensors @@ -88,8 +85,7 @@ class HeadTracker { std::function on_gyro_callback_; // Aryzon multiple orientations - Rotation ekf_to_head_tracker; - const screen_params::ScreenOrientation start_orientation_; + Rotation ekf_to_head_tracker_; }; } // namespace cardboard diff --git a/sdk/include/cardboard.h b/sdk/include/cardboard.h index 74a0a677..12ae4827 100644 --- a/sdk/include/cardboard.h +++ b/sdk/include/cardboard.h @@ -42,6 +42,20 @@ typedef enum CardboardEye { kRight = 1, } CardboardEye; +/// Enum to distinguish device screen orientations +typedef enum CardboardScreenOrientation { + /// Landscape Left orientation. + kLandscapeLeft = 0, + /// Portrait orientation. + kPortrait = 1, + /// Landscape Right orientation. + kLandscapeRight = 2, + /// Portrait Upside Down orientation. + kPortraitUpsideDown = 3, + /// Orientation unknown + kUnknown = -1, +} CardboardScreenOrientation; + /// Struct representing a 3D mesh with 3D vertices and corresponding UV /// coordinates. typedef struct CardboardMesh { @@ -126,6 +140,18 @@ void Cardboard_initializeAndroid(JavaVM* vm, jobject context); /// @} +///////////////////////////////////////////////////////////////////////////// +// Screen Parameters +///////////////////////////////////////////////////////////////////////////// +/// @defgroup screen-parameters Screen Parameters +/// @brief This module calculates the screen size and current screen orientation +/// @{ +/// Returns the current device screen orientation +/// +/// @return CardboardScreenOrientation enum +CardboardScreenOrientation CardboardScreenParameters_getScreenOrientation(); +/// @} + ///////////////////////////////////////////////////////////////////////////// // Lens Distortion ///////////////////////////////////////////////////////////////////////////// diff --git a/sdk/screen_params.h b/sdk/screen_params.h index 94899b1e..25d195e6 100644 --- a/sdk/screen_params.h +++ b/sdk/screen_params.h @@ -23,19 +23,23 @@ namespace cardboard { namespace screen_params { static constexpr float kMetersPerInch = 0.0254f; -#ifdef __ANDROID__ -void initializeAndroid(JavaVM* vm, jobject context); -#endif -void getScreenSizeInMeters(int width_pixels, int height_pixels, - float* out_width_meters, float* out_height_meters); -// Aryzon multiple orientations +/// @brief Enumerates the possible screen orientations. enum ScreenOrientation { LandscapeLeft, Portrait, LandscapeRight, - PortraitUpsideDown + PortraitUpsideDown, + Unknown }; + +#ifdef __ANDROID__ +void initializeAndroid(JavaVM* vm, jobject context); +#endif +void getScreenSizeInMeters(int width_pixels, int height_pixels, + float* out_width_meters, float* out_height_meters); + +/// @brief Returns the current screen orientation. ScreenOrientation getScreenOrientation(); } // namespace screen_params diff --git a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java index 6fffcaef..c605b135 100644 --- a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java +++ b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java @@ -36,7 +36,7 @@ public static class ScreenPixelDensity { public final float xdpi; /** The exact number of pixels per inch in the y direction. */ public final float ydpi; - + /** * Constructor. * @@ -78,29 +78,44 @@ public static ScreenPixelDensity getScreenPixelDensity(Context context) { // Aryzon multiple orientations public static class ScreenOrientation { - /** - * The screen orientation - * 0 = lanscape left - * 1 = portrait - * 2 = landscape right - * 3 = portrait upside-down - */ - public static Context context; - public static int orientation; - public static int previousOrientation; - public static OrientationEventListener orientationEventListener; + + public static final int LandscapeLeft = 0; + public static final int Portrait = 1; + public static final int LandscapeRight = 2; + public static final int PortraitUpsideDown = 3; + public static final int Unknown = -1; + + private static Context context; + private static int orientation; + private static int previousOrientation; + private static OrientationEventListener orientationEventListener; public ScreenOrientation(final Context context) { this.context = context; - this.orientation = CurrentOrientation(context); + this.orientation = getCurrentOrientation(context); this.orientationEventListener = new OrientationEventListener(context, SensorManager.SENSOR_DELAY_UI) { - public void onOrientationChanged(int orientation_) { + public void onOrientationChanged(int orientationIn) { - int newOrientation = context.getResources().getConfiguration().orientation; + // A call to getCurrentOrientation() can take a long time so we do not want to call it every frame. + // context.getResources().getConfiguration().orientation detects landscape to portrait changes, this + // is used to detect a change between any landscape (left or right) and any portrait (up or down). + // It does not tell us the difference between landscape left to landscape right. We therefore do + // another check near the range where a rotation change may occur from landscape left to right and vice + // versa. This is between 60 and 120 for landscape right and between 240 and 300 for landscape left. - if (orientation_ != ORIENTATION_UNKNOWN && previousOrientation != newOrientation) { - orientation = CurrentOrientation(ScreenOrientation.context); + int newOrientation = context.getResources().getConfiguration().orientation; + if (orientationIn != ORIENTATION_UNKNOWN) { + if (previousOrientation != newOrientation) { + // Device rotate to/from landscape to/from portrait + orientation = getCurrentOrientation(ScreenOrientation.context); + } else if (orientationIn >= 60 && orientationIn <= 120 && orientation != LandscapeRight) { + // Device possibly rotated from landscape left to landscape right rotation + orientation = getCurrentOrientation(ScreenOrientation.context); + } else if (orientationIn <= 300 && orientationIn >= 240 && orientation != LandscapeLeft) { + // Device possibly rotated from landscape right to landscape left rotation + orientation = getCurrentOrientation(ScreenOrientation.context); + } previousOrientation = newOrientation; } } @@ -112,7 +127,6 @@ public void onOrientationChanged(int orientation_) { } private static ScreenOrientation screenOrientation; - private static Display defaultDisplay; public static int getScreenOrientation(Context context) { if (screenOrientation == null) { @@ -123,33 +137,26 @@ public static int getScreenOrientation(Context context) { } - private static int CurrentOrientation (Context context) { - if (defaultDisplay == null) { - WindowManager windowManager = (WindowManager) context.getSystemService(Context.WINDOW_SERVICE); - defaultDisplay = windowManager.getDefaultDisplay(); - } + // This call takes a long time so we don't call this every frame, only when rotation changes + private static int getCurrentOrientation (Context context) { + WindowManager windowManager = (WindowManager) context.getSystemService(Context.WINDOW_SERVICE); + Display defaultDisplay = windowManager.getDefaultDisplay(); int orientation = context.getResources().getConfiguration().orientation; - // This call takes a long time so we don't call this every frame, only when rotation changes int rotation = defaultDisplay.getRotation(); - if (orientation == Configuration.ORIENTATION_LANDSCAPE - && (rotation == Surface.ROTATION_0 - || rotation == Surface.ROTATION_90)) { - return 0; - } else if (orientation == Configuration.ORIENTATION_PORTRAIT - && (rotation == Surface.ROTATION_0 - || rotation == Surface.ROTATION_90)) { - return 1; - } else if (orientation == Configuration.ORIENTATION_LANDSCAPE - && (rotation == Surface.ROTATION_180 - || rotation == Surface.ROTATION_270)) { - return 2; - } else if (orientation == Configuration.ORIENTATION_PORTRAIT - && (rotation == Surface.ROTATION_180 - || rotation == Surface.ROTATION_270)) { - return 3; + if (orientation == Configuration.ORIENTATION_LANDSCAPE) { + if (rotation == Surface.ROTATION_0 || rotation == Surface.ROTATION_90) { + return ScreenOrientation.LandscapeLeft; + } + return ScreenOrientation.LandscapeRight; + } else if (orientation == Configuration.ORIENTATION_PORTRAIT) { + if (rotation == Surface.ROTATION_0 || rotation == Surface.ROTATION_90) { + return ScreenOrientation.Portrait; + } + return ScreenOrientation.PortraitUpsideDown; } - return 3; + // Unexpected orientation value. + return ScreenOrientation.Unknown; } } diff --git a/sdk/screen_params/android/screen_params.cc b/sdk/screen_params/android/screen_params.cc index 6272883c..f923d332 100644 --- a/sdk/screen_params/android/screen_params.cc +++ b/sdk/screen_params/android/screen_params.cc @@ -31,7 +31,6 @@ jclass screen_params_utils_class_; // Aryzon multiple orientations jclass screen_orientation_class_; -jmethodID get_screen_orientation_method; struct DisplayMetrics { float xdpi; @@ -51,10 +50,6 @@ void LoadJNIResources(JNIEnv* env) { cardboard::jni::LoadJClass(env, "com/google/cardboard/sdk/screenparams/" "ScreenParamsUtils$ScreenOrientation"); - - get_screen_orientation_method = env->GetStaticMethodID(screen_params_utils_class_, - "getScreenOrientation", - "(Landroid/content/Context;)I"); } DisplayMetrics getDisplayMetrics() { @@ -102,10 +97,14 @@ ScreenOrientation getScreenOrientation() { JNIEnv* env; cardboard::jni::LoadJNIEnv(vm_, &env); - int screen_orientation = env->CallStaticIntMethod( + jmethodID get_screen_orientation_method = env->GetStaticMethodID(screen_params_utils_class_, + "getScreenOrientation", + "(Landroid/content/Context;)I"); + + const int screen_orientation = env->CallStaticIntMethod( screen_params_utils_class_, get_screen_orientation_method, context_); - return (ScreenOrientation)screen_orientation; + return static_cast(screen_orientation); } } // namespace screen_params diff --git a/sdk/screen_params/ios/screen_params.mm b/sdk/screen_params/ios/screen_params.mm index 105f0b26..3638bd07 100644 --- a/sdk/screen_params/ios/screen_params.mm +++ b/sdk/screen_params/ios/screen_params.mm @@ -19,14 +19,14 @@ #import // Aryzon multiple orientations -@interface OrientationHelper : NSObject { +@interface ScreenOrientationHelper : NSObject { } @property cardboard::screen_params::ScreenOrientation orientation; -(void) orientationChanged:(NSNotification *)note; @end -@implementation OrientationHelper { +@implementation ScreenOrientationHelper { } @synthesize orientation; @@ -73,15 +73,16 @@ -(void)setOrientationValue:(UIDevice *)device { } } - if (orientation == UIDeviceOrientationPortrait && (supportedInterfaces & UIInterfaceOrientationMaskPortrait) != 0) { - NSLog(@"Setting portrait"); - self.orientation = cardboard::screen_params::Portrait; - } else if (orientation == UIDeviceOrientationLandscapeLeft && (supportedInterfaces & UIInterfaceOrientationMaskLandscapeRight) != 0) { - NSLog(@"Setting landscapeleft"); + if (orientation == UIDeviceOrientationLandscapeLeft && (supportedInterfaces & UIInterfaceOrientationMaskLandscapeRight) != 0) { self.orientation = cardboard::screen_params::LandscapeLeft; } else if (orientation == UIDeviceOrientationLandscapeRight && (supportedInterfaces & UIInterfaceOrientationMaskLandscapeLeft) != 0) { - NSLog(@"Setting landscaperight"); self.orientation = cardboard::screen_params::LandscapeRight; + } else if (orientation == UIDeviceOrientationPortrait && (supportedInterfaces & UIInterfaceOrientationMaskPortrait) != 0) { + self.orientation = cardboard::screen_params::Portrait; + } else if (orientation == UIDeviceOrientationPortraitUpsideDown && (supportedInterfaces & UIInterfaceOrientationMaskPortraitUpsideDown) != 0) { + self.orientation = cardboard::screen_params::PortraitUpsideDown; + } else { + self.orientation = cardboard::screen_params::Unknown; } } @end @@ -132,7 +133,7 @@ -(void)setOrientationValue:(UIDevice *)device { static CGFloat const kIPhone12MiniDpi = 476.0f; static CGFloat const kIPhone12Dpi = 460.0f; -static OrientationHelper *helper = [[OrientationHelper alloc] init]; +static ScreenOrientationHelper *helper = [[ScreenOrientationHelper alloc] init]; ScreenOrientation getScreenOrientation() { return [helper orientation]; From 414fec1a6f7f9dc84a6663d1f784744b68d25641 Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Thu, 4 Mar 2021 14:15:39 +0100 Subject: [PATCH 10/21] Change for pull request multiple orientation support --- .../cardboard/sdk/screenparams/ScreenParamsUtils.java | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java index c605b135..b137909c 100644 --- a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java +++ b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java @@ -139,8 +139,14 @@ public static int getScreenOrientation(Context context) { // This call takes a long time so we don't call this every frame, only when rotation changes private static int getCurrentOrientation (Context context) { - WindowManager windowManager = (WindowManager) context.getSystemService(Context.WINDOW_SERVICE); - Display defaultDisplay = windowManager.getDefaultDisplay(); + Display defaultDisplay; + + if (VERSION.SDK_INT <= VERSION_CODES.Q) { + defaultDisplay = ((WindowManager) context.getSystemService(Context.WINDOW_SERVICE)) + .getDefaultDisplay(); + } else { + defaultDisplay = context.getDisplay(); + } int orientation = context.getResources().getConfiguration().orientation; int rotation = defaultDisplay.getRotation(); From 52a62479cf1d44e8949061c058970ea1a8670144 Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Fri, 5 Mar 2021 11:04:38 +0100 Subject: [PATCH 11/21] bug fix screen orientation --- hellocardboard-android/src/main/AndroidManifest.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/hellocardboard-android/src/main/AndroidManifest.xml b/hellocardboard-android/src/main/AndroidManifest.xml index 9eb74ab7..3438f7f5 100644 --- a/hellocardboard-android/src/main/AndroidManifest.xml +++ b/hellocardboard-android/src/main/AndroidManifest.xml @@ -18,6 +18,7 @@ android:supportsRtl="true" android:theme="@style/AppTheme"> Date: Wed, 10 Mar 2021 13:55:53 +0100 Subject: [PATCH 12/21] Changes for PR #208 Multiple orientation support --- sdk/CMakeLists.txt | 5 +- sdk/cardboard.cc | 15 +- sdk/head_tracker.cc | 14 +- sdk/head_tracker.h | 1 - sdk/screen_params.h | 13 +- .../sdk/screenparams/ScreenParamsUtils.java | 150 +++++++++--------- sdk/screen_params/android/screen_params.cc | 7 +- sdk/screen_params/ios/screen_params.mm | 15 +- 8 files changed, 97 insertions(+), 123 deletions(-) diff --git a/sdk/CMakeLists.txt b/sdk/CMakeLists.txt index 6a6342d2..e1ff1db3 100644 --- a/sdk/CMakeLists.txt +++ b/sdk/CMakeLists.txt @@ -79,8 +79,9 @@ add_library(cardboard_api SHARED ${cardboard_xr_provider_srcs}) # Includes -target_include_directories(cardboard_api - PRIVATE ../third_party/unity_plugin_api) +target_include_directories(cardboard_api PRIVATE + ../third_party/unity_plugin_api + include) # Build target_link_libraries(cardboard_api diff --git a/sdk/cardboard.cc b/sdk/cardboard.cc index fbf346ba..d13964ef 100644 --- a/sdk/cardboard.cc +++ b/sdk/cardboard.cc @@ -120,19 +120,10 @@ void Cardboard_initializeAndroid(JavaVM* vm, jobject context) { #endif CardboardScreenOrientation CardboardScreenParameters_getScreenOrientation() { - switch (cardboard::screen_params::getScreenOrientation()) { - case cardboard::screen_params::LandscapeLeft: - return kLandscapeLeft; - case cardboard::screen_params::LandscapeRight: - return kLandscapeRight; - case cardboard::screen_params::Portrait: - return kPortrait; - case cardboard::screen_params::PortraitUpsideDown: - return kPortraitUpsideDown; - default: - break; - } + if (CARDBOARD_IS_NOT_INITIALIZED()) { return kUnknown; + } + return cardboard::screen_params::getScreenOrientation(); } CardboardLensDistortion* CardboardLensDistortion_create( diff --git a/sdk/head_tracker.cc b/sdk/head_tracker.cc index 9b1561e5..44c4b415 100644 --- a/sdk/head_tracker.cc +++ b/sdk/head_tracker.cc @@ -20,8 +20,6 @@ #include "util/logging.h" #include "util/vector.h" #include "util/vectorutils.h" - -// Aryzon multiple orientations #include "screen_params.h" namespace cardboard { @@ -41,13 +39,13 @@ HeadTracker::HeadTracker() }; switch(screen_params::getScreenOrientation()) { - case screen_params::LandscapeLeft: + case kLandscapeLeft: ekf_to_head_tracker_ = Rotation::FromYawPitchRoll(-M_PI / 2.0, 0, -M_PI / 2.0); break; - case screen_params::LandscapeRight: + case kLandscapeRight: ekf_to_head_tracker_ = Rotation::FromYawPitchRoll(M_PI / 2.0, 0, M_PI / 2.0); break; - default: // Portrait + default: // Portrait and PortraitUpsideDown ekf_to_head_tracker_ = Rotation::FromYawPitchRoll(M_PI / 2.0, M_PI / 2.0, M_PI / 2.0); break; } @@ -91,13 +89,13 @@ void HeadTracker::GetPose(int64_t timestamp_ns, Rotation sensor_to_display; switch(screen_params::getScreenOrientation()) { - case screen_params::LandscapeLeft: + case kLandscapeLeft: sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), M_PI / 2.0); break; - case screen_params::LandscapeRight: + case kLandscapeRight: sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), -M_PI / 2.0); break; - default: // Portrait + default: // Portrait and PortraitUpsideDown sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), 0.); break; } diff --git a/sdk/head_tracker.h b/sdk/head_tracker.h index c9bd61f8..f6210bba 100644 --- a/sdk/head_tracker.h +++ b/sdk/head_tracker.h @@ -84,7 +84,6 @@ class HeadTracker { std::function on_accel_callback_; std::function on_gyro_callback_; - // Aryzon multiple orientations Rotation ekf_to_head_tracker_; }; diff --git a/sdk/screen_params.h b/sdk/screen_params.h index 25d195e6..630abcf0 100644 --- a/sdk/screen_params.h +++ b/sdk/screen_params.h @@ -20,19 +20,12 @@ #include #endif +#include "cardboard.h" + namespace cardboard { namespace screen_params { static constexpr float kMetersPerInch = 0.0254f; -/// @brief Enumerates the possible screen orientations. -enum ScreenOrientation { - LandscapeLeft, - Portrait, - LandscapeRight, - PortraitUpsideDown, - Unknown -}; - #ifdef __ANDROID__ void initializeAndroid(JavaVM* vm, jobject context); #endif @@ -40,7 +33,7 @@ void getScreenSizeInMeters(int width_pixels, int height_pixels, float* out_width_meters, float* out_height_meters); /// @brief Returns the current screen orientation. -ScreenOrientation getScreenOrientation(); +CardboardScreenOrientation getScreenOrientation(); } // namespace screen_params } // namespace cardboard diff --git a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java index b137909c..3d0b6b84 100644 --- a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java +++ b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java @@ -19,17 +19,18 @@ import android.os.Build.VERSION; import android.os.Build.VERSION_CODES; import android.util.DisplayMetrics; -import android.view.WindowManager; - -// Aryzon multiple orientations -import android.content.res.Configuration; -import android.hardware.SensorManager; import android.view.Display; import android.view.OrientationEventListener; import android.view.Surface; +import android.view.WindowManager; +import android.content.res.Configuration; +import android.hardware.SensorManager; /** Utility methods to manage the screen parameters. */ public abstract class ScreenParamsUtils { + /** Holds the screen orientation. */ + private static ScreenOrientation screenOrientation; + /** Holds the screen pixel density. */ public static class ScreenPixelDensity { /** The exact number of pixels per inch in the x direction. */ @@ -76,93 +77,88 @@ public static ScreenPixelDensity getScreenPixelDensity(Context context) { return new ScreenPixelDensity(displayMetrics.xdpi, displayMetrics.ydpi); } - // Aryzon multiple orientations - public static class ScreenOrientation { + private static class ScreenOrientation { - public static final int LandscapeLeft = 0; - public static final int Portrait = 1; - public static final int LandscapeRight = 2; - public static final int PortraitUpsideDown = 3; - public static final int Unknown = -1; + public static final int LANDSCAPE_LEFT = 0; + public static final int PORTRAIT = 1; + public static final int LANDSCAPE_RIGHT = 2; + public static final int PORTRAIT_UPSIDE_DOWN = 3; + public static final int UNKNOWN = -1; - private static Context context; - private static int orientation; - private static int previousOrientation; - private static OrientationEventListener orientationEventListener; - - public ScreenOrientation(final Context context) { - this.context = context; - this.orientation = getCurrentOrientation(context); - - this.orientationEventListener = new OrientationEventListener(context, SensorManager.SENSOR_DELAY_UI) { - public void onOrientationChanged(int orientationIn) { - - // A call to getCurrentOrientation() can take a long time so we do not want to call it every frame. - // context.getResources().getConfiguration().orientation detects landscape to portrait changes, this - // is used to detect a change between any landscape (left or right) and any portrait (up or down). - // It does not tell us the difference between landscape left to landscape right. We therefore do - // another check near the range where a rotation change may occur from landscape left to right and vice - // versa. This is between 60 and 120 for landscape right and between 240 and 300 for landscape left. - - int newOrientation = context.getResources().getConfiguration().orientation; - if (orientationIn != ORIENTATION_UNKNOWN) { - if (previousOrientation != newOrientation) { - // Device rotate to/from landscape to/from portrait - orientation = getCurrentOrientation(ScreenOrientation.context); - } else if (orientationIn >= 60 && orientationIn <= 120 && orientation != LandscapeRight) { - // Device possibly rotated from landscape left to landscape right rotation - orientation = getCurrentOrientation(ScreenOrientation.context); - } else if (orientationIn <= 300 && orientationIn >= 240 && orientation != LandscapeLeft) { - // Device possibly rotated from landscape right to landscape left rotation - orientation = getCurrentOrientation(ScreenOrientation.context); - } - previousOrientation = newOrientation; + private static Context context; + private static int orientation; + private static int previousOrientation; + private static OrientationEventListener orientationEventListener; + + public ScreenOrientation(final Context context) { + this.context = context; + this.orientation = getCurrentOrientation(context); + + this.orientationEventListener = new OrientationEventListener(context, SensorManager.SENSOR_DELAY_UI) { + public void onOrientationChanged(int orientationIn) { + + // A call to getCurrentOrientation() can take a long time so we do not want to call it every frame. + // context.getResources().getConfiguration().orientation detects landscape to portrait changes, this + // is used to detect a change between any landscape (left or right) and any portrait (up or down). + // It does not tell us the difference between landscape left to landscape right. We therefore do + // another check near the range where a rotation change may occur from landscape left to right and vice + // versa. This is between 60 and 120 for landscape right and between 240 and 300 for landscape left. + + int newOrientation = context.getResources().getConfiguration().orientation; + if (orientationIn != ORIENTATION_UNKNOWN) { + if (previousOrientation != newOrientation) { + // Device rotate to/from landscape to/from portrait + orientation = getCurrentOrientation(ScreenOrientation.context); + } else if (orientationIn >= 60 && orientationIn <= 120 && orientation != LANDSCAPE_RIGHT) { + // Device possibly rotated from landscape left to landscape right rotation + orientation = getCurrentOrientation(ScreenOrientation.context); + } else if (orientationIn <= 300 && orientationIn >= 240 && orientation != LANDSCAPE_LEFT) { + // Device possibly rotated from landscape right to landscape left rotation + orientation = getCurrentOrientation(ScreenOrientation.context); } + previousOrientation = newOrientation; } - }; - if (this.orientationEventListener.canDetectOrientation()) { - this.orientationEventListener.enable(); } + }; + if (this.orientationEventListener.canDetectOrientation()) { + this.orientationEventListener.enable(); } } - - private static ScreenOrientation screenOrientation; + } - public static int getScreenOrientation(Context context) { - if (screenOrientation == null) { - - screenOrientation = new ScreenOrientation(context); - } - return screenOrientation.orientation; - + public static int getScreenOrientation(Context context) { + if (screenOrientation == null) { + screenOrientation = new ScreenOrientation(context); } + return screenOrientation.orientation; + } - // This call takes a long time so we don't call this every frame, only when rotation changes - private static int getCurrentOrientation (Context context) { - Display defaultDisplay; + // This call takes a long time so we don't call this every frame, only when rotation changes + private static int getCurrentOrientation (Context context) { + Display defaultDisplay; - if (VERSION.SDK_INT <= VERSION_CODES.Q) { - defaultDisplay = ((WindowManager) context.getSystemService(Context.WINDOW_SERVICE)) + if (VERSION.SDK_INT <= VERSION_CODES.Q) { + defaultDisplay = ((WindowManager) context.getSystemService(Context.WINDOW_SERVICE)) .getDefaultDisplay(); - } else { - defaultDisplay = context.getDisplay(); - } + } else { + defaultDisplay = context.getDisplay(); + } - int orientation = context.getResources().getConfiguration().orientation; - int rotation = defaultDisplay.getRotation(); + int orientation = context.getResources().getConfiguration().orientation; + int rotation = defaultDisplay.getRotation(); - if (orientation == Configuration.ORIENTATION_LANDSCAPE) { - if (rotation == Surface.ROTATION_0 || rotation == Surface.ROTATION_90) { - return ScreenOrientation.LandscapeLeft; - } - return ScreenOrientation.LandscapeRight; - } else if (orientation == Configuration.ORIENTATION_PORTRAIT) { - if (rotation == Surface.ROTATION_0 || rotation == Surface.ROTATION_90) { - return ScreenOrientation.Portrait; - } - return ScreenOrientation.PortraitUpsideDown; + if (orientation == Configuration.ORIENTATION_LANDSCAPE) { + if (rotation == Surface.ROTATION_0 || rotation == Surface.ROTATION_90) { + return ScreenOrientation.LANDSCAPE_LEFT; + } + return ScreenOrientation.LANDSCAPE_RIGHT; + } else if (orientation == Configuration.ORIENTATION_PORTRAIT) { + if (rotation == Surface.ROTATION_0 || rotation == Surface.ROTATION_90) { + return ScreenOrientation.PORTRAIT; } + return ScreenOrientation.PORTRAIT_UPSIDE_DOWN; + } // Unexpected orientation value. - return ScreenOrientation.Unknown; + return ScreenOrientation.UNKNOWN; } } diff --git a/sdk/screen_params/android/screen_params.cc b/sdk/screen_params/android/screen_params.cc index f923d332..bcb1dbf6 100644 --- a/sdk/screen_params/android/screen_params.cc +++ b/sdk/screen_params/android/screen_params.cc @@ -29,7 +29,6 @@ jobject context_; jclass screen_pixel_density_class_; jclass screen_params_utils_class_; -// Aryzon multiple orientations jclass screen_orientation_class_; struct DisplayMetrics { @@ -45,7 +44,6 @@ void LoadJNIResources(JNIEnv* env) { "com/google/cardboard/sdk/screenparams/" "ScreenParamsUtils$ScreenPixelDensity"); - // Aryzon multiple orientations screen_orientation_class_ = cardboard::jni::LoadJClass(env, "com/google/cardboard/sdk/screenparams/" @@ -91,8 +89,7 @@ void getScreenSizeInMeters(int width_pixels, int height_pixels, *out_height_meters = (height_pixels / display_metrics.ydpi) * kMetersPerInch; } -// Aryzon multiple orientations -ScreenOrientation getScreenOrientation() { +CardboardScreenOrientation getScreenOrientation() { JNIEnv* env; cardboard::jni::LoadJNIEnv(vm_, &env); @@ -104,7 +101,7 @@ ScreenOrientation getScreenOrientation() { const int screen_orientation = env->CallStaticIntMethod( screen_params_utils_class_, get_screen_orientation_method, context_); - return static_cast(screen_orientation); + return static_cast(screen_orientation); } } // namespace screen_params diff --git a/sdk/screen_params/ios/screen_params.mm b/sdk/screen_params/ios/screen_params.mm index 3638bd07..ce3ddf8b 100644 --- a/sdk/screen_params/ios/screen_params.mm +++ b/sdk/screen_params/ios/screen_params.mm @@ -18,11 +18,10 @@ #import #import -// Aryzon multiple orientations @interface ScreenOrientationHelper : NSObject { } -@property cardboard::screen_params::ScreenOrientation orientation; +@property CardboardScreenOrientation orientation; -(void) orientationChanged:(NSNotification *)note; @end @@ -74,15 +73,15 @@ -(void)setOrientationValue:(UIDevice *)device { } if (orientation == UIDeviceOrientationLandscapeLeft && (supportedInterfaces & UIInterfaceOrientationMaskLandscapeRight) != 0) { - self.orientation = cardboard::screen_params::LandscapeLeft; + self.orientation = kLandscapeLeft; } else if (orientation == UIDeviceOrientationLandscapeRight && (supportedInterfaces & UIInterfaceOrientationMaskLandscapeLeft) != 0) { - self.orientation = cardboard::screen_params::LandscapeRight; + self.orientation = kLandscapeRight; } else if (orientation == UIDeviceOrientationPortrait && (supportedInterfaces & UIInterfaceOrientationMaskPortrait) != 0) { - self.orientation = cardboard::screen_params::Portrait; + self.orientation = kPortrait; } else if (orientation == UIDeviceOrientationPortraitUpsideDown && (supportedInterfaces & UIInterfaceOrientationMaskPortraitUpsideDown) != 0) { - self.orientation = cardboard::screen_params::PortraitUpsideDown; + self.orientation = kPortraitUpsideDown; } else { - self.orientation = cardboard::screen_params::Unknown; + self.orientation = kUnknown; } } @end @@ -135,7 +134,7 @@ -(void)setOrientationValue:(UIDevice *)device { static ScreenOrientationHelper *helper = [[ScreenOrientationHelper alloc] init]; -ScreenOrientation getScreenOrientation() { +CardboardScreenOrientation getScreenOrientation() { return [helper orientation]; } From 7eecf4ae5f2e646a9657e35d46b2828579f75be6 Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Wed, 10 Mar 2021 14:01:36 +0100 Subject: [PATCH 13/21] Changes for PR #208 Multiple orientation support --- sdk/include/cardboard.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sdk/include/cardboard.h b/sdk/include/cardboard.h index 12ae4827..245a5f0f 100644 --- a/sdk/include/cardboard.h +++ b/sdk/include/cardboard.h @@ -148,6 +148,8 @@ void Cardboard_initializeAndroid(JavaVM* vm, jobject context); /// @{ /// Returns the current device screen orientation /// +/// @pre If the SDK is not initialized, this function will always return kUnknown +/// /// @return CardboardScreenOrientation enum CardboardScreenOrientation CardboardScreenParameters_getScreenOrientation(); /// @} From 64c0ae94623519044435a68cf34471dddf72d1f7 Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Wed, 10 Mar 2021 14:05:10 +0100 Subject: [PATCH 14/21] Changes for PR #208 Multiple orientation support --- .../google/cardboard/sdk/screenparams/ScreenParamsUtils.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java index 3d0b6b84..9af43d15 100644 --- a/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java +++ b/sdk/screen_params/android/java/com/google/cardboard/sdk/screenparams/ScreenParamsUtils.java @@ -160,5 +160,5 @@ private static int getCurrentOrientation (Context context) { } // Unexpected orientation value. return ScreenOrientation.UNKNOWN; - } + } } From 7834f08921aebb592e2d8045ae9a30a1a52187cd Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Wed, 10 Mar 2021 15:19:05 +0100 Subject: [PATCH 15/21] Fix for Unity orientation issue in PR #208 --- sdk/unity/xr_unity_plugin/cardboard_xr_unity.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/sdk/unity/xr_unity_plugin/cardboard_xr_unity.cc b/sdk/unity/xr_unity_plugin/cardboard_xr_unity.cc index 1a453c04..9e508434 100644 --- a/sdk/unity/xr_unity_plugin/cardboard_xr_unity.cc +++ b/sdk/unity/xr_unity_plugin/cardboard_xr_unity.cc @@ -390,6 +390,7 @@ class CardboardApi::CardboardApiImpl { static void SetUnityScreenParams(int x, int y, int width, int height) { unity_screen_params_ = ScreenParams{x, y, width, height}; + SetDeviceParametersChanged(); } static void GetUnityScreenParams(int* width, int* height) { From 5d62425f595e33a627bf135010e973b5c54eb410 Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Thu, 11 Mar 2021 12:38:50 +0100 Subject: [PATCH 16/21] Added new files to CmakeLists --- sdk/CMakeLists.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/sdk/CMakeLists.txt b/sdk/CMakeLists.txt index 6a6342d2..51bfbb18 100644 --- a/sdk/CMakeLists.txt +++ b/sdk/CMakeLists.txt @@ -48,6 +48,8 @@ file(GLOB screen_params_srcs "screen_params/android/*.cc") file(GLOB device_params_srcs "device_params/android/*.cc") # Rendering Sources file(GLOB rendering_srcs "rendering/*.cc") +# 6DoF sources +file(GLOB sixdof_srcs "sixdof/*.cc") # === Cardboard Unity JNI === file(GLOB cardboard_unity_jni_srcs "unity/android/*.cc") @@ -71,6 +73,7 @@ add_library(cardboard_api SHARED ${screen_params_srcs} ${device_params_srcs} ${rendering_srcs} + ${sixdof_srcs} # Cardboard Unity JNI sources ${cardboard_unity_jni_srcs} # Cardboard Unity Wrapper sources From a45569534e96bc4f3ac412e3fe98378e24ebc6d9 Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Wed, 17 Mar 2021 11:58:47 +0100 Subject: [PATCH 17/21] Changes for PR #210 --- sdk/sixdof/position_data.cc | 20 ++----- sdk/sixdof/position_data.h | 15 ++++- sdk/sixdof/rotation_data.cc | 45 ++++++--------- sdk/sixdof/rotation_data.h | 9 ++- ...temLoader.cs => SixDoFCardboardStartup.cs} | 19 ++++--- sixdof-unity/SixDoFPoseDriver.cs | 55 +++++++------------ 6 files changed, 73 insertions(+), 90 deletions(-) rename sixdof-unity/{CardboardSubsystemLoader.cs => SixDoFCardboardStartup.cs} (84%) diff --git a/sdk/sixdof/position_data.cc b/sdk/sixdof/position_data.cc index 6c2d255d..59a57500 100644 --- a/sdk/sixdof/position_data.cc +++ b/sdk/sixdof/position_data.cc @@ -1,5 +1,5 @@ /* - * Copyright 2019 Google LLC + * Copyright 2021 Google LLC * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -55,23 +55,15 @@ Vector3 PositionData::GetLatestData() const { Vector3 PositionData::GetExtrapolatedForTimeStamp(const int64_t timestamp_ns) { if (!IsValid() || buffer_size_ < 2) { - return {0.0,0.0,1.0}; + return {0.0,0.0,0.0}; } - Vector3 vSum = {0,0,0}; - - for (int i=1; i timestamp_buffer_[buffer_size_-1]) { + const Vector3 v = (buffer_[buffer_size_-1] - buffer_[buffer_size_-2]) / (timestamp_buffer_[buffer_size_-1] - timestamp_buffer_[buffer_size_-2]); + return buffer_[buffer_size_-1] + v * (timestamp_ns - timestamp_buffer_[buffer_size_ - 1]); } - Vector3 v = vSum / (double)buffer_size_; - - double dTTs = timestamp_ns - timestamp_buffer_[buffer_size_ - 1]; - Vector3 xTs = buffer_[buffer_size_-1] + v * dTTs; - - return xTs; + return buffer_[buffer_size_-1]; } void PositionData::Reset() { diff --git a/sdk/sixdof/position_data.h b/sdk/sixdof/position_data.h index 90ed7ebb..086baa4c 100644 --- a/sdk/sixdof/position_data.h +++ b/sdk/sixdof/position_data.h @@ -1,5 +1,5 @@ /* - * Copyright 2019 Google LLC + * Copyright 2021 Google LLC * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -25,10 +25,10 @@ namespace cardboard { -// Fixed window FIFO mean filter for vectors of the given dimension. +// This class holds a buffer of position data samples with corresponding timestamp samples class PositionData { public: - // Create a buffer to hold rotation data of size buffer_size. + // Create a buffer to hold position data of size buffer_size. // @param buffer_size size of samples to buffer. explicit PositionData(size_t buffer_size); @@ -40,8 +40,17 @@ class PositionData { // Returns the latest value stored in the internal buffer. Vector3 GetLatestData() const; + + // Returns the latest timestamp value stored in the internal timestampbuffer. long long GetLatestTimestamp() const; + + // Returns the position extrapolated from data stored in the internal buffers. + // A buffer size of 2 is required to work. + // It returns a zero Vector3 when not fully initialised. + // @param timestamp_ns the time in nanoseconds to get a position value for. Vector3 GetExtrapolatedForTimeStamp(const int64_t timestamp_ns); + + // Clear the internal buffers. void Reset(); private: const size_t buffer_size_; diff --git a/sdk/sixdof/rotation_data.cc b/sdk/sixdof/rotation_data.cc index 83391166..402a0a66 100644 --- a/sdk/sixdof/rotation_data.cc +++ b/sdk/sixdof/rotation_data.cc @@ -1,5 +1,5 @@ /* - * Copyright 2019 Google LLC + * Copyright 2021 Google LLC * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -36,15 +36,14 @@ void RotationData::AddSample(const Vector4& sample, const int64_t timestamp_ns) bool RotationData::IsValid() const { return buffer_.size() == buffer_size_; } - Vector4 RotationData::GetLatestData() const { - if (buffer_.size() > 0) { - return buffer_[buffer_.size() -1]; - } - return Vector4::Zero(); +Vector4 RotationData::GetLatestData() const { + if (buffer_.size() > 0) { + return buffer_[buffer_.size() -1]; } + return {0.0,0.0,0.0,1.0}; +} - -Vector4 RotationData::GetInterpolatedForTimeStamp(const int64_t timestamp_ns) const{ +Vector4 RotationData::GetInterpolatedForTimeStamp(const int64_t timestamp_ns) const { if (!IsValid()) { return {0.0,0.0,0.0,1.0}; @@ -52,36 +51,26 @@ Vector4 RotationData::GetInterpolatedForTimeStamp(const int64_t timestamp_ns) co int64_t smaller = -1; int64_t larger = -1; - bool didPassLarger = false; + bool did_pass_larger = false; int i=0; - while (!didPassLarger && i < buffer_size_) { - int64_t currentTs = timestamp_buffer_[i]; - if (currentTs <= timestamp_ns) { - smaller = currentTs; + while (!did_pass_larger && i < buffer_size_) { + int64_t current_ts = timestamp_buffer_[i]; + if (current_ts <= timestamp_ns) { + smaller = current_ts; } else { - larger = currentTs; - didPassLarger = true; + larger = current_ts; + did_pass_larger = true; } i++; } - - float interpolationValue = 0.5f; - Vector4 q; - Vector4 q0; - Vector4 q1; if (smaller > 0 && larger > 0) { - int64_t delta = larger - smaller; - interpolationValue = (timestamp_ns - smaller) / (double)(delta); - q0 = buffer_[i-1]; - q1 = buffer_[i]; - q = interpolationValue*(q1-q0) + q0; - } else { - q = buffer_[buffer_size_]; + const float interpolation_value = (timestamp_ns - smaller) / (double)(larger - smaller); + return buffer_[i-1] + interpolation_value * (buffer_[i] - buffer_[i-1]); } - return q; + return buffer_[buffer_size_-1]; } } // namespace cardboard diff --git a/sdk/sixdof/rotation_data.h b/sdk/sixdof/rotation_data.h index 7528be44..54607ba6 100644 --- a/sdk/sixdof/rotation_data.h +++ b/sdk/sixdof/rotation_data.h @@ -1,5 +1,5 @@ /* - * Copyright 2019 Google LLC + * Copyright 2021 Google LLC * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -14,8 +14,6 @@ * limitations under the License. */ -// Aryzon 6DoF - #ifndef rotation_data_h #define rotation_data_h @@ -41,6 +39,11 @@ class RotationData { // Returns the latest value stored in the internal buffer. Vector4 GetLatestData() const; + // Returns a rotation linearly interpolated from the data stored in the internal buffer. + // It returns an identity rotation when not fully initialised. + // It returns the last value added to the buffer when the requested timestamp is + // outside the buffered timestamps. + // @param timestamp_ns the time in nanoseconds to get a rotation value for. Vector4 GetInterpolatedForTimeStamp(const int64_t timestamp_ns) const; private: diff --git a/sixdof-unity/CardboardSubsystemLoader.cs b/sixdof-unity/SixDoFCardboardStartup.cs similarity index 84% rename from sixdof-unity/CardboardSubsystemLoader.cs rename to sixdof-unity/SixDoFCardboardStartup.cs index b030ed52..244cc16c 100644 --- a/sixdof-unity/CardboardSubsystemLoader.cs +++ b/sixdof-unity/SixDoFCardboardStartup.cs @@ -6,12 +6,12 @@ using UnityEngine; using UnityEngine.XR; -using Google.XR.Cardboard; - -namespace Aryzon +namespace Google.XR.Cardboard { - - public class CardboardSubsystemLoader : MonoBehaviour + /// + /// Initializes Cardboard XR Plugin for 6DoF use. + /// + public class SixDoFCardboardStartup : MonoBehaviour { private static IntPtr _inputPointer; public static IntPtr inputPointer @@ -33,7 +33,6 @@ public static IntPtr displayPointer public static bool isStarted = false; private string inputMatch = "Input"; - private string displayMatch = "Display"; private void Start() { @@ -42,6 +41,12 @@ private void Start() public void StartCardboard() { + // Configures the app to not shut down the screen and sets the brightness to maximum. + // Brightness control is expected to work only in iOS, see: + // https://docs.unity3d.com/ScriptReference/Screen-brightness.html. + Screen.sleepTimeout = SleepTimeout.NeverSleep; + Screen.brightness = 1.0f; + if (!loader) { loader = new XRLoader(); @@ -126,4 +131,4 @@ private void ConnectCardboardInputSystem() } } } -} \ No newline at end of file +} diff --git a/sixdof-unity/SixDoFPoseDriver.cs b/sixdof-unity/SixDoFPoseDriver.cs index 74c6c1a6..63365076 100644 --- a/sixdof-unity/SixDoFPoseDriver.cs +++ b/sixdof-unity/SixDoFPoseDriver.cs @@ -6,22 +6,23 @@ using System.Runtime.InteropServices; using UnityEngine; using UnityEngine.XR; -#if ARYZON_ARFOUNDATION using UnityEngine.XR.ARFoundation; -#endif -using Google.XR.Cardboard; -namespace Aryzon +namespace Google.XR.Cardboard { public class SixDoFPoseDriver : MonoBehaviour { - [DllImport(Constants.CardboardApi)] +#if UNITY_ANDROID + public const string CardboardApi = "cardboard_api"; +#elif UNITY_IOS + public const string CardboardApi = "__Internal"; +#else + public const string CardboardApi = "NOT_AVAILABLE"; +#endif + + [DllImport(CardboardApi)] private static extern void CardboardUnity_AddSixDoFData(IntPtr ptr, Int64 timestamp, [In] float[] position, [In] float[] orientation); -#if UNITY_EDITOR - public Transform editorPoseProviderTransform; -#endif -#if ARYZON_ARFOUNDATION private ARCameraManager _arCameraManager; public ARCameraManager arCameraManager { @@ -46,7 +47,6 @@ public ARCameraManager arCameraManager } set { _arCameraManager = value; } } -#endif internal struct NullablePose { @@ -57,9 +57,7 @@ internal struct NullablePose public void OnEnable() { Application.onBeforeRender += OnBeforeRender; -#if ARYZON_ARFOUNDATION arCameraManager.frameReceived += ArCameraManager_frameReceived; -#endif #if UNITY_2020_1_OR_NEWER List devices = new List(); InputDevices.GetDevicesWithCharacteristics(InputDeviceCharacteristics.TrackedDevice, devices); @@ -78,24 +76,18 @@ public void OnEnable() public void OnDisable() { Application.onBeforeRender -= OnBeforeRender; -#if ARYZON_ARFOUNDATION arCameraManager.frameReceived -= ArCameraManager_frameReceived; -#endif + #if UNITY_2020_1_OR_NEWER InputDevices.deviceConnected -= OnInputDeviceConnected; #endif // UNITY_UNITY_2020_1_OR_NEWER } - //void Update() => PerformUpdate(); - void OnBeforeRender() => PerformUpdate(); void PerformUpdate() { -#if UNITY_EDITOR - pose.position = editorPoseProviderTransform.position; - pose.rotation = editorPoseProviderTransform.rotation; -#else +#if !UNITY_EDITOR var updatedPose = GetPoseData(); if (updatedPose.position.HasValue) @@ -130,14 +122,9 @@ void CheckConnectedDevice(InputDevice device, bool displayWarning = true) { s_InputTrackingDevice = device; } - else + else if (s_CardboardHMDInputTrackingDevice == null && device.name == "Cardboard HMD") { - Debug.LogWarning($"An input device {device.name} with the TrackedDevice characteristic was registered but the ARPoseDriver is already consuming data from {s_InputTrackingDevice.Value.name}."); - if (s_CardboardHMDInputTrackingDevice == null && device.name == "Cardboard HMD") - { - s_CardboardHMDInputTrackingDevice = device; - //arCameraManager.frameReceived += ArCameraManager_frameReceived; - } + s_CardboardHMDInputTrackingDevice = device; } } } @@ -150,14 +137,12 @@ public void AddSixDoFData(Vector3 position, Quaternion rotation, long timestampN { float[] positionArray = { position.x, position.y, position.z }; float[] rotationArray = { rotation.x, rotation.y, rotation.z, rotation.w }; - CardboardUnity_AddSixDoFData(AryzonCardboardSubsystemLoader.inputPointer, timestampNs, positionArray, rotationArray); + CardboardUnity_AddSixDoFData(SixDoFCardboardStartup.inputPointer, timestampNs, positionArray, rotationArray); } -#if ARYZON_ARFOUNDATION - private void ArCameraManager_frameReceived(ARCameraFrameEventArgs obj) { - if (!AryzonCardboardSubsystemLoader.isStarted) + if (!SixDoFCardboardStartup.isStarted) { return; } @@ -198,13 +183,13 @@ private void ArCameraManager_frameReceived(ARCameraFrameEventArgs obj) } #endif } -#endif + static internal NullablePose GetPoseData() { NullablePose resultPose = new NullablePose(); #if UNITY_2020_1_OR_NEWER - if (!AryzonCardboardSubsystemLoader.isStarted && s_CardboardHMDInputTrackingDevice != null) + if (!SixDoFCardboardStartup.isStarted && s_CardboardHMDInputTrackingDevice != null) { s_CardboardHMDInputTrackingDevice = null; } @@ -254,7 +239,7 @@ static internal NullablePose GetPoseData() List states = new List(); - if (!AryzonCardboardSubsystemLoader.isStarted) + if (!SixDoFCardboardStartup.isStarted) { foreach (UnityEngine.XR.XRNodeState nodeState in nodeStates) { @@ -302,4 +287,4 @@ static internal NullablePose GetPoseData() return resultPose; } } -} \ No newline at end of file +} From 436092cd0e79a6123c736455fb3db21af4e5fe6b Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Wed, 5 May 2021 12:14:27 +0200 Subject: [PATCH 18/21] fixed issue in screen_params --- sdk/screen_params/android/screen_params.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sdk/screen_params/android/screen_params.cc b/sdk/screen_params/android/screen_params.cc index f4bd0714..ad69d0dd 100644 --- a/sdk/screen_params/android/screen_params.cc +++ b/sdk/screen_params/android/screen_params.cc @@ -44,12 +44,12 @@ void LoadJNIResources(JNIEnv* env) { screen_pixel_density_class_ = reinterpret_cast(env->NewGlobalRef( cardboard::jni::LoadJClass(env, "com/google/cardboard/sdk/screenparams/" - "ScreenParamsUtils$ScreenPixelDensity"); + "ScreenParamsUtils$ScreenPixelDensity"))); - screen_orientation_class_ = + screen_orientation_class_ = reinterpret_cast(env->NewGlobalRef( cardboard::jni::LoadJClass(env, "com/google/cardboard/sdk/screenparams/" - "ScreenParamsUtils$ScreenOrientation"); + "ScreenParamsUtils$ScreenOrientation"))); } From 4c32aaf703ed19d48e75b465e0d8644a6d5e7493 Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Fri, 7 May 2021 08:34:36 +0200 Subject: [PATCH 19/21] Fix for no stereoscopic rendering Unity Metal --- sdk/unity/xr_unity_plugin/metal_renderer.mm | 98 +++++++++------------ 1 file changed, 41 insertions(+), 57 deletions(-) diff --git a/sdk/unity/xr_unity_plugin/metal_renderer.mm b/sdk/unity/xr_unity_plugin/metal_renderer.mm index 790455fb..147f0739 100644 --- a/sdk/unity/xr_unity_plugin/metal_renderer.mm +++ b/sdk/unity/xr_unity_plugin/metal_renderer.mm @@ -182,51 +182,39 @@ void CreateRenderTexture(RenderTexture* render_texture, int screen_width, int screen_height) override { id mtl_device = metal_interface_->MetalDevice(); - // Create texture color buffer. + // Create texture color buffer. NSDictionary* color_surface_attribs = @{ - (NSString*)kIOSurfaceIsGlobal : @YES, + // (NSString*)kIOSurfaceIsGlobal : @YES, // Seems to not be necessary and less safe, maybe doesn't work with earlier versions of Unity? (NSString*)kIOSurfaceWidth : @(screen_width / 2), (NSString*)kIOSurfaceHeight : @(screen_height), (NSString*)kIOSurfaceBytesPerElement : @4u }; - color_surface_ = IOSurfaceCreate((CFDictionaryRef)color_surface_attribs); - MTLTextureDescriptor* texture_color_buffer_descriptor = [MTLTextureDescriptorClass new]; - texture_color_buffer_descriptor.textureType = MTLTextureType2D; - texture_color_buffer_descriptor.width = screen_width / 2; - texture_color_buffer_descriptor.height = screen_height; - texture_color_buffer_descriptor.pixelFormat = MTLPixelFormatRGBA8Unorm; - texture_color_buffer_descriptor.usage = MTLTextureUsageRenderTarget | MTLTextureUsageShaderRead; - color_texture_ = [mtl_device newTextureWithDescriptor:texture_color_buffer_descriptor - iosurface:color_surface_ - plane:0]; - render_texture->color_buffer = reinterpret_cast(color_surface_); - - // When using Metal, texture depth buffer is unused. - render_texture->depth_buffer = 0; - - // Create a black texture. It is used to hide a rendering previously performed by Unity. - // TODO(b/185478026): Prevent Unity from drawing a monocular scene when using Metal. - MTLTextureDescriptor* black_texture_descriptor = [MTLTextureDescriptorClass new]; - black_texture_descriptor.textureType = MTLTextureType2D; - black_texture_descriptor.width = screen_width; - black_texture_descriptor.height = screen_height; - black_texture_descriptor.pixelFormat = MTLPixelFormatRGBA8Unorm; - black_texture_descriptor.usage = MTLTextureUsageRenderTarget | MTLTextureUsageShaderRead; - black_texture_ = [mtl_device newTextureWithDescriptor:black_texture_descriptor]; - - std::vector black_texture_data(screen_width * screen_height, 0xFF000000); - MTLRegion region = MTLRegionMake2D(0, 0, screen_width, screen_height); - [black_texture_ replaceRegion:region - mipmapLevel:0 - withBytes:reinterpret_cast(black_texture_data.data()) - bytesPerRow:4 * screen_width]; - - black_texture_vertices_buffer_ = [mtl_device newBufferWithBytes:vertices - length:sizeof(vertices) - options:MTLResourceStorageModeShared]; - black_texture_uvs_buffer_ = [mtl_device newBufferWithBytes:uvs - length:sizeof(uvs) - options:MTLResourceStorageModeShared]; + + MTLTextureDescriptor* texture_color_buffer_descriptor = [MTLTextureDescriptorClass new]; + texture_color_buffer_descriptor.textureType = MTLTextureType2D; + texture_color_buffer_descriptor.width = screen_width / 2; + texture_color_buffer_descriptor.height = screen_height; + texture_color_buffer_descriptor.pixelFormat = MTLPixelFormatRGBA8Unorm; + texture_color_buffer_descriptor.usage = MTLTextureUsageRenderTarget | MTLTextureUsageShaderWrite; // MTLTextureUsageShaderRead is less optimized. + + IOSurfaceRef color_surface = IOSurfaceCreate((CFDictionaryRef)color_surface_attribs); + + render_texture->color_buffer = reinterpret_cast(color_surface); + + if (eyeIndex == 0) { + color_texture_left_ = [mtl_device newTextureWithDescriptor:texture_color_buffer_descriptor + iosurface:color_surface + plane:0]; + eyeIndex = 1; + } else { + color_texture_right_ = [mtl_device newTextureWithDescriptor:texture_color_buffer_descriptor + iosurface:color_surface + plane:0]; + eyeIndex = 0; + } + + // When using Metal, texture depth buffer is unused. + render_texture->depth_buffer = 0; } void DestroyRenderTexture(RenderTexture* render_texture) override { @@ -237,30 +225,24 @@ void DestroyRenderTexture(RenderTexture* render_texture) override { void RenderEyesToDisplay(CardboardDistortionRenderer* renderer, const ScreenParams& screen_params, const CardboardEyeTextureDescription* left_eye, const CardboardEyeTextureDescription* right_eye) override { - // Render black texture. It is used to hide a rendering previously performed by Unity. - // TODO(b/185478026): Prevent Unity from drawing a monocular scene when using Metal. - RenderBlackTexture(screen_params.width, screen_params.height); const CardboardDistortionRendererTargetConfig target_config{ - reinterpret_cast(CFBridgingRetain(metal_interface_->CurrentCommandEncoder())), - screen_params.width, screen_params.height}; + reinterpret_cast(CFBridgingRetain(metal_interface_->CurrentCommandEncoder())), + screen_params.width, screen_params.height}; - // An IOSurfaceRef was passed to Unity for drawing, but a reference to an id using - // it must be passed to the SDK. - CFTypeRef color_texture = CFBridgingRetain(color_texture_); CardboardEyeTextureDescription left_eye_description = *left_eye; - left_eye_description.texture = reinterpret_cast(color_texture); CardboardEyeTextureDescription right_eye_description = *right_eye; - right_eye_description.texture = reinterpret_cast(color_texture); + left_eye_description.texture = reinterpret_cast(color_texture_left_); + right_eye_description.texture = reinterpret_cast(color_texture_right_); + CardboardDistortionRenderer_renderEyeToDisplay( - renderer, reinterpret_cast(&target_config), screen_params.viewport_x, - screen_params.viewport_y, screen_params.viewport_width, screen_params.viewport_height, - &left_eye_description, &right_eye_description); + renderer, reinterpret_cast(&target_config), screen_params.viewport_x, + screen_params.viewport_y, screen_params.viewport_width, screen_params.viewport_height, + &left_eye_description, &right_eye_description); - CFBridgingRelease(color_texture); CFBridgingRelease(reinterpret_cast(target_config.render_command_encoder)); - } +} private: static constexpr float Lerp(float start, float end, float val) { @@ -326,14 +308,16 @@ void RenderBlackTexture(int screen_width, int screen_height) { IUnityGraphicsMetalV1* metal_interface_{nullptr}; id mtl_render_pipeline_state_; - IOSurfaceRef color_surface_; - id color_texture_; + id color_texture_left_; + id color_texture_right_; id black_texture_; id black_texture_vertices_buffer_; id black_texture_uvs_buffer_; bool are_widgets_setup_{false}; + + int eyeIndex = 0; }; } // namespace From 02c7172379349d5979eaa04795f86aafd51fc7b9 Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Tue, 18 May 2021 15:16:49 +0200 Subject: [PATCH 20/21] 6dof fixes --- sdk/head_tracker.cc | 156 +++++++++++--------- sdk/head_tracker.h | 2 +- sdk/sixdof/position_data.cc | 13 +- sdk/sixdof/rotation_data.cc | 10 +- sdk/sixdof/rotation_data.h | 3 + sdk/unity/xr_provider/input.cc | 28 ++-- sdk/unity/xr_unity_plugin/metal_renderer.mm | 27 ---- 7 files changed, 123 insertions(+), 116 deletions(-) diff --git a/sdk/head_tracker.cc b/sdk/head_tracker.cc index 6ec64b26..774bd2b1 100644 --- a/sdk/head_tracker.cc +++ b/sdk/head_tracker.cc @@ -25,8 +25,9 @@ namespace cardboard { // Aryzon 6DoF const int rotation_samples = 10; -const int position_samples = 3; +const int position_samples = 6; const int64_t max6DoFTimeDifference = 200000000; // Maximum time difference between last pose state timestamp and last 6DoF timestamp, if it takes longer than this the last known location of sixdof will be used +const float reduceBiasRate = 0.05; HeadTracker::HeadTracker() : is_tracking_(false), @@ -56,6 +57,8 @@ HeadTracker::HeadTracker() ekf_to_head_tracker_ = Rotation::FromYawPitchRoll(M_PI / 2.0, M_PI / 2.0, M_PI / 2.0); break; } + difference_to_6DoF_ = Rotation::Identity(); + y_bias_ = 0; } HeadTracker::~HeadTracker() { UnregisterCallbacks(); } @@ -65,7 +68,7 @@ void HeadTracker::Pause() { return; } - UnregisterCallbacks(); + /*UnregisterCallbacks(); // Create a gyro event with zero velocity. This effectively stops the // prediction. @@ -75,82 +78,79 @@ void HeadTracker::Pause() { OnGyroscopeData(event); is_tracking_ = false; + y_bias_ = 0; + initialised_6dof_ = false;*/ } void HeadTracker::Resume() { + if (!is_tracking_) { + RegisterCallbacks(); + } is_tracking_ = true; - RegisterCallbacks(); } void HeadTracker::GetPose(int64_t timestamp_ns, std::array& out_position, std::array& out_orientation) const { - const RotationState rotation_state = sensor_fusion_->GetLatestRotationState(); + + Rotation sensor_to_display; + + switch(screen_params::getScreenOrientation()) { + case kLandscapeLeft: + sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), M_PI / 2.0); + break; + case kLandscapeRight: + sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), -M_PI / 2.0); + break; + default: // Portrait and PortraitUpsideDown + sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), 0.); + break; + } + Rotation predicted_rotation = sensor_fusion_->PredictRotation(timestamp_ns); - // In order to update our pose as the sensor changes, we begin with the - // inverse default orientation (the orientation returned by a reset sensor), - // apply the current sensor transformation, and then transform into display - // space. - - Rotation sensor_to_display; - - switch(screen_params::getScreenOrientation()) { - case kLandscapeLeft: - sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), M_PI / 2.0); - break; - case kLandscapeRight: - sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), -M_PI / 2.0); - break; - default: // Portrait and PortraitUpsideDown - sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), 0.); - break; - } - - // Aryzon 6DoF - // Save rotation sample with timestamp to be used in AddSixDoFData() - rotation_data_->AddSample(predicted_rotation.GetQuaternion(), timestamp_ns); - + const RotationState rotation_state = sensor_fusion_->GetLatestRotationState(); + // Save rotation sample with timestamp to be used in AddSixDoFData() + rotation_data_->AddSample((sensor_to_display * predicted_rotation * + ekf_to_head_tracker_).GetQuaternion(), timestamp_ns); + if (position_data_->IsValid() && rotation_state.timestamp - position_data_->GetLatestTimestamp() < max6DoFTimeDifference) { - // 6DoF is recently updated - predicted_rotation = predicted_rotation * -difference_to_6DoF_; - - Vector3 p = position_data_->GetExtrapolatedForTimeStamp(timestamp_ns); - std::array predicted_position_ = {(float)p[0], (float)p[1], (float)p[2]}; - - const Vector4 orientation = (sensor_to_display * predicted_rotation * - ekf_to_head_tracker_) - .GetQuaternion(); + // 6DoF is recently updated + Vector3 p = position_data_->GetExtrapolatedForTimeStamp(timestamp_ns); + //printf("GetPose\t%llu\t%f\t%f\t%f\n",timestamp_ns, p[0], p[1], p[2]); + std::array predicted_position_ = {(float)p[0], (float)p[1], (float)p[2]}; + const Rotation orientationRotation = (sensor_to_display * predicted_rotation * + ekf_to_head_tracker_); + + const Vector4 orientation = (orientationRotation * -difference_to_6DoF_).GetQuaternion(); - out_orientation[0] = static_cast(orientation[0]); - out_orientation[1] = static_cast(orientation[1]); - out_orientation[2] = static_cast(orientation[2]); - out_orientation[3] = static_cast(orientation[3]); + out_orientation[0] = static_cast(orientation[0]); + out_orientation[1] = static_cast(orientation[1]); + out_orientation[2] = static_cast(orientation[2]); + out_orientation[3] = static_cast(orientation[3]); - out_position = predicted_position_; + out_position = predicted_position_; } else { - // 6DoF is not recently updated - - const Vector4 orientation = (sensor_to_display * predicted_rotation * - ekf_to_head_tracker_) - .GetQuaternion(); + // 6DoF is not recently updated + const Vector4 orientation = (sensor_to_display * predicted_rotation * + ekf_to_head_tracker_) + .GetQuaternion(); - out_orientation[0] = static_cast(orientation[0]); - out_orientation[1] = static_cast(orientation[1]); - out_orientation[2] = static_cast(orientation[2]); - out_orientation[3] = static_cast(orientation[3]); + out_orientation[0] = static_cast(orientation[0]); + out_orientation[1] = static_cast(orientation[1]); + out_orientation[2] = static_cast(orientation[2]); + out_orientation[3] = static_cast(orientation[3]); - out_position = ApplyNeckModel(out_orientation, 1.0); - - if (position_data_->IsValid()) { - // Apply last known 6DoF position if 6DoF was data previously added, while still applying neckmodel. + out_position = ApplyNeckModel(out_orientation, 1.0); + if (position_data_->IsValid()) { + // Apply last known 6DoF position if 6DoF data was previously added, while still applying neckmodel. - Vector3 last_known_position_ = position_data_->GetLatestData(); - out_position[0] += (float)last_known_position_[0]; - out_position[1] += (float)last_known_position_[1]; - out_position[2] += (float)last_known_position_[2]; - } + Vector3 last_known_position_ = position_data_->GetLatestData(); + out_position[0] += (float)last_known_position_[0]; + out_position[1] += (float)last_known_position_[1]; + out_position[2] += (float)last_known_position_[2]; + } } } @@ -184,23 +184,35 @@ void HeadTracker::AddSixDoFData(int64_t timestamp_ns, float* pos, float* orienta if (!is_tracking_) { return; } - position_data_->AddSample(Vector3(pos[0], pos[1], pos[2]), timestamp_ns); + if (position_data_->GetLatestTimestamp() != timestamp_ns) { + position_data_->AddSample(Vector3(pos[0], pos[1], pos[2]), timestamp_ns); + } + // Do not add data if no new data is available! if (position_data_->IsValid() && rotation_data_->IsValid()) { - // 6DoF timestamp needs to be before the latest rotation_data timestamp - Rotation gyroAtTimeOfSixDoF = Rotation::FromQuaternion(rotation_data_->GetInterpolatedForTimeStamp(timestamp_ns)); - Rotation sixDoFRotation = Rotation::FromQuaternion(Vector4(0, orientation[1], 0, orientation[3])); - - Rotation difference = gyroAtTimeOfSixDoF * -sixDoFRotation; + // 6DoF timestamp should be before the latest rotation_data timestamp + const Rotation gyroAtTimeOfSixDoF = Rotation::FromQuaternion(rotation_data_->GetInterpolatedForTimeStamp(timestamp_ns)); + const Rotation sixDoFRotation = Rotation::FromQuaternion(Vector4(orientation[0], orientation[1], orientation[2], orientation[3])); - // Only synchronize rotation around the y axis - Vector4 diffQ = difference.GetQuaternion(); - diffQ[0] = 0; - diffQ[2] = 0; + Vector4 difference = (gyroAtTimeOfSixDoF * -sixDoFRotation).GetQuaternion(); + double yDifference = 0; - // Quaternion will be normalized in this call: - difference_to_6DoF_.SetQuaternion(diffQ); + // Extract y euler angle from quaternion + const double sinp = 2 * (difference[3] * difference[1] - difference[2] * difference[0]); + if (std::abs(sinp) >= 1) { + yDifference = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range + } else { + yDifference = std::asin(sinp); + } + + if (yDifference > M_PI_2) { + yDifference -= M_PI; + } else if (yDifference < -M_PI_2) { + yDifference += M_PI; + } + + y_bias_ += (yDifference - y_bias_) * reduceBiasRate; + difference_to_6DoF_ = Rotation::FromRollPitchYaw(0, 0, y_bias_); } } - } // namespace cardboard diff --git a/sdk/head_tracker.h b/sdk/head_tracker.h index aacc5e87..904dd9ce 100644 --- a/sdk/head_tracker.h +++ b/sdk/head_tracker.h @@ -101,7 +101,7 @@ class HeadTracker { Rotation difference_to_6DoF_; Rotation ekf_to_head_tracker_; - + double y_bias_; }; } // namespace cardboard diff --git a/sdk/sixdof/position_data.cc b/sdk/sixdof/position_data.cc index 59a57500..9f827466 100644 --- a/sdk/sixdof/position_data.cc +++ b/sdk/sixdof/position_data.cc @@ -24,7 +24,9 @@ namespace cardboard { PositionData::PositionData(size_t buffer_size) : buffer_size_(buffer_size) {} void PositionData::AddSample(const Vector3& sample, const int64_t timestamp_ns) { + buffer_.push_back(sample); + if (buffer_.size() > buffer_size_) { buffer_.pop_front(); } @@ -59,10 +61,17 @@ Vector3 PositionData::GetExtrapolatedForTimeStamp(const int64_t timestamp_ns) { } if (timestamp_ns > timestamp_buffer_[buffer_size_-1]) { - const Vector3 v = (buffer_[buffer_size_-1] - buffer_[buffer_size_-2]) / (timestamp_buffer_[buffer_size_-1] - timestamp_buffer_[buffer_size_-2]); + const Vector3 v0 = (buffer_[buffer_size_-1] - buffer_[buffer_size_-2]) / (timestamp_buffer_[buffer_size_-1] - timestamp_buffer_[buffer_size_-2]); + const Vector3 v1 = (buffer_[buffer_size_-2] - buffer_[buffer_size_-3]) / (timestamp_buffer_[buffer_size_-2] - timestamp_buffer_[buffer_size_-3]); + const Vector3 v2 = (buffer_[buffer_size_-3] - buffer_[buffer_size_-4]) / (timestamp_buffer_[buffer_size_-3] - timestamp_buffer_[buffer_size_-4]); + const Vector3 v3 = (buffer_[buffer_size_-4] - buffer_[buffer_size_-5]) / (timestamp_buffer_[buffer_size_-4] - timestamp_buffer_[buffer_size_-5]); + const Vector3 v4 = (buffer_[buffer_size_-5] - buffer_[buffer_size_-6]) / (timestamp_buffer_[buffer_size_-5] - timestamp_buffer_[buffer_size_-6]); + + const Vector3 v = (v0 + v1 + v2 + v3 + v4) / 5; + + //printf("v\t%f\t%llu\t%llu\t%llu\t%f\t%f\t%f\n", v[0] * 1000000,timestamp_buffer_[buffer_size_ - 1],timestamp_buffer_[buffer_size_ - 2],timestamp_ns, newPoss[0], newPoss[1], newPoss[2]); return buffer_[buffer_size_-1] + v * (timestamp_ns - timestamp_buffer_[buffer_size_ - 1]); } - return buffer_[buffer_size_-1]; } diff --git a/sdk/sixdof/rotation_data.cc b/sdk/sixdof/rotation_data.cc index 402a0a66..1c3cce20 100644 --- a/sdk/sixdof/rotation_data.cc +++ b/sdk/sixdof/rotation_data.cc @@ -43,6 +43,14 @@ Vector4 RotationData::GetLatestData() const { return {0.0,0.0,0.0,1.0}; } +int64_t RotationData::GetLatestTimeStamp() const { + if (timestamp_buffer_.size() > 0) { + return timestamp_buffer_[timestamp_buffer_.size() -1]; + } + return 0; +} + + Vector4 RotationData::GetInterpolatedForTimeStamp(const int64_t timestamp_ns) const { if (!IsValid()) { @@ -54,7 +62,7 @@ Vector4 RotationData::GetInterpolatedForTimeStamp(const int64_t timestamp_ns) co bool did_pass_larger = false; int i=0; - while (!did_pass_larger && i < buffer_size_) { + while (!did_pass_larger && (const size_t)i < buffer_size_) { int64_t current_ts = timestamp_buffer_[i]; if (current_ts <= timestamp_ns) { smaller = current_ts; diff --git a/sdk/sixdof/rotation_data.h b/sdk/sixdof/rotation_data.h index 54607ba6..28cede82 100644 --- a/sdk/sixdof/rotation_data.h +++ b/sdk/sixdof/rotation_data.h @@ -38,6 +38,9 @@ class RotationData { // Returns the latest value stored in the internal buffer. Vector4 GetLatestData() const; + + // Returns the latest value stored in the internal timestamp buffer. + int64_t GetLatestTimeStamp() const; // Returns a rotation linearly interpolated from the data stored in the internal buffer. // It returns an identity rotation when not fully initialised. diff --git a/sdk/unity/xr_provider/input.cc b/sdk/unity/xr_provider/input.cc index 548101b7..4bcf6262 100644 --- a/sdk/unity/xr_provider/input.cc +++ b/sdk/unity/xr_provider/input.cc @@ -57,8 +57,8 @@ class CardboardInputProvider { UnityXRInputProvider input_provider; input_provider.userData = nullptr; input_provider.Tick = [](UnitySubsystemHandle, void*, - UnityXRInputUpdateType) { - return GetInstance()->Tick(); + UnityXRInputUpdateType updateType) { + return GetInstance()->Tick(updateType); }; input_provider.FillDeviceDefinition = [](UnitySubsystemHandle, void*, UnityXRInternalInputDeviceId device_id, @@ -126,17 +126,19 @@ class CardboardInputProvider { cardboard_api_->PauseHeadTracker(); } - UnitySubsystemErrorCode Tick() { - std::array out_orientation; - std::array out_position; - cardboard_api_->GetHeadTrackerPose(out_position.data(), - out_orientation.data()); - // TODO(b/151817737): Compute pose position within SDK with custom rotation. - //head_pose_ = cardboard::unity::CardboardRotationToUnityPose(out_orientation); - - // Aryzon 6DoF changed to include position - head_pose_ = cardboard::unity::CardboardPoseToUnityPose(out_orientation, out_position); - + // Aryzon 6DoF changed to include updateType and only update onBeforeRender to save computation power + UnitySubsystemErrorCode Tick(UnityXRInputUpdateType updateType) { + if (updateType == kUnityXRInputUpdateTypeBeforeRender) { + std::array out_orientation; + std::array out_position; + cardboard_api_->GetHeadTrackerPose(out_position.data(), + out_orientation.data()); + // TODO(b/151817737): Compute pose position within SDK with custom rotation. + //head_pose_ = cardboard::unity::CardboardRotationToUnityPose(out_orientation); + + // Aryzon 6DoF changed to include position + head_pose_ = cardboard::unity::CardboardPoseToUnityPose(out_orientation, out_position); + } return kUnitySubsystemErrorCodeSuccess; } diff --git a/sdk/unity/xr_unity_plugin/metal_renderer.mm b/sdk/unity/xr_unity_plugin/metal_renderer.mm index 147f0739..7b7fc63f 100644 --- a/sdk/unity/xr_unity_plugin/metal_renderer.mm +++ b/sdk/unity/xr_unity_plugin/metal_renderer.mm @@ -275,33 +275,6 @@ void RenderWidget(id mtl_render_command_encoder, int sc vertexCount:4]; } - void RenderBlackTexture(int screen_width, int screen_height) { - // Get Metal current render command encoder. - id mtl_render_command_encoder_ = - static_cast>(metal_interface_->CurrentCommandEncoder()); - - [mtl_render_command_encoder_ setRenderPipelineState:mtl_render_pipeline_state_]; - - [mtl_render_command_encoder_ - setViewport:(MTLViewport){0.0, 0.0, static_cast(screen_width), - static_cast(screen_height), 0.0, 1.0}]; - - [mtl_render_command_encoder_ setVertexBuffer:black_texture_vertices_buffer_ - offset:0 - atIndex:VertexInputIndexPosition]; - - [mtl_render_command_encoder_ setVertexBuffer:black_texture_uvs_buffer_ - offset:0 - atIndex:VertexInputIndexTexCoords]; - - [mtl_render_command_encoder_ setFragmentTexture:black_texture_ - atIndex:FragmentInputIndexTexture]; - - [mtl_render_command_encoder_ drawPrimitives:MTLPrimitiveTypeTriangleStrip - vertexStart:0 - vertexCount:4]; - } - constexpr static float vertices[] = {-1, -1, 1, -1, -1, 1, 1, 1}; constexpr static float uvs[] = {0, 0, 1, 0, 0, 1, 1, 1}; From b7fd17edfe74e243970b3ab3bfc6187f99949c86 Mon Sep 17 00:00:00 2001 From: maartenslaa Date: Wed, 19 May 2021 14:09:24 +0200 Subject: [PATCH 21/21] 6DoF fixes --- sdk/head_tracker.cc | 122 ++++++++++++++++++++++++++------------------ sdk/head_tracker.h | 9 ++-- 2 files changed, 78 insertions(+), 53 deletions(-) diff --git a/sdk/head_tracker.cc b/sdk/head_tracker.cc index 774bd2b1..b7625c9a 100644 --- a/sdk/head_tracker.cc +++ b/sdk/head_tracker.cc @@ -24,10 +24,10 @@ namespace cardboard { // Aryzon 6DoF -const int rotation_samples = 10; -const int position_samples = 6; -const int64_t max6DoFTimeDifference = 200000000; // Maximum time difference between last pose state timestamp and last 6DoF timestamp, if it takes longer than this the last known location of sixdof will be used -const float reduceBiasRate = 0.05; +constexpr int kRotationSamples = 10; +constexpr int kPositionSamples = 6; +constexpr int64_t kMaxSixDoFTimeDifference = 200000000; // Maximum time difference between last pose state timestamp and last 6DoF timestamp, if it takes longer than this the last known location of sixdof will be used +constexpr float kReduceBiasRate = 0.05; HeadTracker::HeadTracker() : is_tracking_(false), @@ -36,8 +36,8 @@ HeadTracker::HeadTracker() accel_sensor_(new SensorEventProducer()), gyro_sensor_(new SensorEventProducer()), // Aryzon 6DoF - rotation_data_(new RotationData(rotation_samples)), - position_data_(new PositionData(position_samples)) { + rotation_data_(new RotationData(kRotationSamples)), + position_data_(new PositionData(kPositionSamples)) { on_accel_callback_ = [&](const AccelerometerData& event) { OnAccelerometerData(event); @@ -57,8 +57,10 @@ HeadTracker::HeadTracker() ekf_to_head_tracker_ = Rotation::FromYawPitchRoll(M_PI / 2.0, M_PI / 2.0, M_PI / 2.0); break; } - difference_to_6DoF_ = Rotation::Identity(); - y_bias_ = 0; + ekf_to_sixDoF_ = Rotation::Identity(); + smooth_ekf_to_sixDoF_ = Rotation::Identity(); + steady_start_ = Rotation::Identity(); + steady_frames_ = -1; } HeadTracker::~HeadTracker() { UnregisterCallbacks(); } @@ -68,7 +70,7 @@ void HeadTracker::Pause() { return; } - /*UnregisterCallbacks(); + UnregisterCallbacks(); // Create a gyro event with zero velocity. This effectively stops the // prediction. @@ -76,16 +78,15 @@ void HeadTracker::Pause() { event.data = Vector3::Zero(); OnGyroscopeData(event); - is_tracking_ = false; - y_bias_ = 0; - initialised_6dof_ = false;*/ } void HeadTracker::Resume() { if (!is_tracking_) { RegisterCallbacks(); } + steady_frames_ = -1; + steady_start_ = Rotation::Identity(); is_tracking_ = true; } @@ -106,36 +107,35 @@ void HeadTracker::GetPose(int64_t timestamp_ns, sensor_to_display = Rotation::FromAxisAndAngle(Vector3(0, 0, 1), 0.); break; } - - Rotation predicted_rotation = sensor_fusion_->PredictRotation(timestamp_ns); - + const RotationState rotation_state = sensor_fusion_->GetLatestRotationState(); - // Save rotation sample with timestamp to be used in AddSixDoFData() - rotation_data_->AddSample((sensor_to_display * predicted_rotation * - ekf_to_head_tracker_).GetQuaternion(), timestamp_ns); + const Rotation unpredicted_rotation = rotation_state.sensor_from_start_rotation; + const Rotation predicted_rotation = sensor_fusion_->PredictRotation(timestamp_ns); + + const Rotation adjusted_unpredicted_rotation = (sensor_to_display * unpredicted_rotation * + ekf_to_head_tracker_); + + const Rotation adjusted_rotation = (sensor_to_display * predicted_rotation * + ekf_to_head_tracker_); - if (position_data_->IsValid() && rotation_state.timestamp - position_data_->GetLatestTimestamp() < max6DoFTimeDifference) { + // Save rotation sample with timestamp to be used in AddSixDoFData() + rotation_data_->AddSample(adjusted_unpredicted_rotation.GetQuaternion(), rotation_state.timestamp); + + if (position_data_->IsValid() && rotation_state.timestamp - position_data_->GetLatestTimestamp() < kMaxSixDoFTimeDifference) { // 6DoF is recently updated - Vector3 p = position_data_->GetExtrapolatedForTimeStamp(timestamp_ns); - //printf("GetPose\t%llu\t%f\t%f\t%f\n",timestamp_ns, p[0], p[1], p[2]); - std::array predicted_position_ = {(float)p[0], (float)p[1], (float)p[2]}; - const Rotation orientationRotation = (sensor_to_display * predicted_rotation * - ekf_to_head_tracker_); - - const Vector4 orientation = (orientationRotation * -difference_to_6DoF_).GetQuaternion(); + const Vector4 orientation = (adjusted_rotation * smooth_ekf_to_sixDoF_).GetQuaternion(); out_orientation[0] = static_cast(orientation[0]); out_orientation[1] = static_cast(orientation[1]); out_orientation[2] = static_cast(orientation[2]); out_orientation[3] = static_cast(orientation[3]); - - out_position = predicted_position_; + + Vector3 p = position_data_->GetExtrapolatedForTimeStamp(timestamp_ns); + out_position = {(float)p[0], (float)p[1], (float)p[2]}; } else { // 6DoF is not recently updated - const Vector4 orientation = (sensor_to_display * predicted_rotation * - ekf_to_head_tracker_) - .GetQuaternion(); + const Vector4 orientation = adjusted_rotation.GetQuaternion(); out_orientation[0] = static_cast(orientation[0]); out_orientation[1] = static_cast(orientation[1]); @@ -145,7 +145,6 @@ void HeadTracker::GetPose(int64_t timestamp_ns, out_position = ApplyNeckModel(out_orientation, 1.0); if (position_data_->IsValid()) { // Apply last known 6DoF position if 6DoF data was previously added, while still applying neckmodel. - Vector3 last_known_position_ = position_data_->GetLatestData(); out_position[0] += (float)last_known_position_[0]; out_position[1] += (float)last_known_position_[1]; @@ -179,6 +178,18 @@ void HeadTracker::OnGyroscopeData(const GyroscopeData& event) { sensor_fusion_->ProcessGyroscopeSample(event); } +Rotation ShortestRotation(Rotation a, Rotation b) { + + Vector4 aQ = a.GetQuaternion(); + Vector4 bQ = b.GetQuaternion(); + + if (Dot(aQ, bQ) < 0) { + return -a * Rotation::FromQuaternion(-bQ); + } else { + return -a * b; + } +} + // Aryzon 6DoF void HeadTracker::AddSixDoFData(int64_t timestamp_ns, float* pos, float* orientation) { if (!is_tracking_) { @@ -188,31 +199,42 @@ void HeadTracker::AddSixDoFData(int64_t timestamp_ns, float* pos, float* orienta position_data_->AddSample(Vector3(pos[0], pos[1], pos[2]), timestamp_ns); } - // Do not add data if no new data is available! + // There will be a difference in rotation between ekf and sixDoF. + // SixDoF sensor is the 'truth' but is slower then ekf + // When the device is steady the difference between rotatations is saved + // smooth_ekf_to_sixDoF is slowly adjusted to smoothly close the gap + // between ekf and sixDoF. This value is used in GetPose(). + if (position_data_->IsValid() && rotation_data_->IsValid()) { - // 6DoF timestamp should be before the latest rotation_data timestamp - const Rotation gyroAtTimeOfSixDoF = Rotation::FromQuaternion(rotation_data_->GetInterpolatedForTimeStamp(timestamp_ns)); - const Rotation sixDoFRotation = Rotation::FromQuaternion(Vector4(orientation[0], orientation[1], orientation[2], orientation[3])); + if ((steady_frames_ == 30 || steady_frames_ < 0) && rotation_data_->GetLatestTimeStamp() > timestamp_ns) { + // Match rotation timestamps of ekf to sixDoF by interpolating the saved ekf rotations + // 6DoF timestamp should be before the latest rotation_data timestamp otherwise extrapolation + // needs to happen which will be less accurate. + const Rotation ekf_at_time_of_sixDoF = Rotation::FromQuaternion(rotation_data_->GetInterpolatedForTimeStamp(timestamp_ns)); + const Rotation six_DoF_rotation = Rotation::FromQuaternion(Vector4(orientation[0], orientation[1], orientation[2], orientation[3])); + + ekf_to_sixDoF_ = ShortestRotation(ekf_at_time_of_sixDoF, six_DoF_rotation); + + } else if (steady_frames_ == 0) { + steady_start_ = Rotation::FromQuaternion(rotation_data_->GetLatestData()); + } - Vector4 difference = (gyroAtTimeOfSixDoF * -sixDoFRotation).GetQuaternion(); - double yDifference = 0; + const Rotation steady_difference = steady_start_ * -Rotation::FromQuaternion(rotation_data_->GetLatestData()); - // Extract y euler angle from quaternion - const double sinp = 2 * (difference[3] * difference[1] - difference[2] * difference[0]); - if (std::abs(sinp) >= 1) { - yDifference = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range + if (steady_difference.GetQuaternion()[3] > 0.9995) { + steady_frames_ += 1; } else { - yDifference = std::asin(sinp); + steady_frames_ = 0; } - if (yDifference > M_PI_2) { - yDifference -= M_PI; - } else if (yDifference < -M_PI_2) { - yDifference += M_PI; - } + const Rotation bias_to_fill = ShortestRotation(smooth_ekf_to_sixDoF_, ekf_to_sixDoF_); + Vector3 axis; + double angle; + bias_to_fill.GetAxisAndAngle(&axis, &angle); + + const Rotation add_to_bias = Rotation::FromAxisAndAngle(axis, angle * kReduceBiasRate); - y_bias_ += (yDifference - y_bias_) * reduceBiasRate; - difference_to_6DoF_ = Rotation::FromRollPitchYaw(0, 0, y_bias_); + smooth_ekf_to_sixDoF_ *= add_to_bias; } } } // namespace cardboard diff --git a/sdk/head_tracker.h b/sdk/head_tracker.h index 904dd9ce..e8fdac45 100644 --- a/sdk/head_tracker.h +++ b/sdk/head_tracker.h @@ -98,10 +98,13 @@ class HeadTracker { // Aryzon 6DoF RotationData *rotation_data_; PositionData *position_data_; - Rotation difference_to_6DoF_; - + + Rotation ekf_to_sixDoF_; + Rotation smooth_ekf_to_sixDoF_; Rotation ekf_to_head_tracker_; - double y_bias_; + + float steady_frames_; + Rotation steady_start_; }; } // namespace cardboard