Add scene manager for room objects

common/src/main/cpp/classes/xr_scene_manager.cpp

@@ -0,0 +1,39 @@
+#include "classes/xr_scene_manager.h"
+
+#include <godot_cpp/core/error_macros.hpp>
+#include <godot_cpp/classes/worker_thread_pool.hpp>
+
+#include "classes/xr_scene_provider_openxr.h"
+
+using namespace godot;
+
+XrSceneManager::XrSceneManager() {}
+
+void XrSceneManager::query_room() {
+  XrSceneProviderOpenXR::get_singleton()->query_room([=](std::vector<XrSceneObjectInternal> results) {
+    std::string val = "query_room complete with result count: " + std::to_string(results.size());
+    WARN_PRINT(String(val.c_str()));
+    sceneObjects_ = results;
+    emit_signal("query_room_complete");
+  });
+}
+
+void XrSceneManager::load_xr_scene_object(int index, Ref<XrSceneObject> object) {
+  if (index >= 0 && index < sceneObjects_.size()) {
+    object->init(sceneObjects_[index]);
+  } else {
+    WARN_PRINT("Index out of bounds");
+  }
+};
+
+int XrSceneManager::get_xr_scene_object_count() {
+  return sceneObjects_.size();
+};
+
+void XrSceneManager::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("query_room"), &XrSceneManager::query_room);
+	ClassDB::bind_method(D_METHOD("load_xr_scene_object"), &XrSceneManager::load_xr_scene_object);
+	ClassDB::bind_method(D_METHOD("get_xr_scene_object_count"), &XrSceneManager::get_xr_scene_object_count);
+
+  ADD_SIGNAL(MethodInfo("query_room_complete"));
+}

common/src/main/cpp/classes/xr_scene_object.cpp

@@ -0,0 +1,99 @@
+#include "classes/xr_scene_object.h"
+
+#include <godot_cpp/core/class_db.hpp>
+
+#include "classes/xr_scene_provider_openxr.h"
+#include "utils/xr_godot_utils.h"
+
+using namespace godot;
+
+void XrSceneObject::_bind_methods() {
+	ClassDB::bind_method(D_METHOD("get_id"), &XrSceneObject::get_id);
+	ClassDB::bind_method(D_METHOD("get_label"), &XrSceneObject::get_label);
+	ClassDB::bind_method(D_METHOD("update_transform"), &XrSceneObject::update_transform);
+	ClassDB::bind_method(D_METHOD("transform_valid"), &XrSceneObject::transform_valid);
+	ClassDB::bind_method(D_METHOD("get_transform"), &XrSceneObject::get_transform);
+	ClassDB::bind_method(D_METHOD("get_bounding_box_2d"), &XrSceneObject::get_bounding_box_2d);
+	ClassDB::bind_method(D_METHOD("get_bounds_2d_count"), &XrSceneObject::get_bounds_2d_count);
+	ClassDB::bind_method(D_METHOD("get_bounds_2d_vertex"), &XrSceneObject::get_bounds_2d_vertex);
+}
+
+XrSceneObject::XrSceneObject() {}
+XrSceneObject::~XrSceneObject() {}
+
+void XrSceneObject::init(XrSceneObjectInternal state) {
+	state_ = state;
+}
+
+String XrSceneObject::get_id() {
+  return String(XrGodotUtils::uuidToString(state_.uuid).c_str());
+}
+
+String XrSceneObject::get_label() {
+	if (state_.label != std::nullopt) {
+	  return *state_.label;
+	}
+	return String();
+}
+
+void XrSceneObject::update_transform() {
+  // Reset the input structs
+  velocity_ = {
+		XR_TYPE_SPACE_VELOCITY, // type
+		nullptr, // next
+		0, // velocityFlags
+		{ 0.0, 0.0, 0.0 }, // linearVelocity
+		{ 0.0, 0.0, 0.0 } // angularVelocity
+	};
+
+	location_ = {
+		XR_TYPE_SPACE_LOCATION, // type
+		&velocity_, // next
+		0, // locationFlags
+		{
+			{ 0.0, 0.0, 0.0, 0.0 }, // orientation
+			{ 0.0, 0.0, 0.0 } // position
+		} // pose
+	};
+	XrSceneProviderOpenXR::get_singleton()->locate_space(state_.space, &location_);
+}
+
+bool XrSceneObject::transform_valid() {
+  return location_.locationFlags & XR_SPACE_LOCATION_POSITION_VALID_BIT;
+}
+
+Transform3D XrSceneObject::get_transform() {
+	return XrGodotUtils::poseToTransform(location_.pose);
+}
+
+Rect2 XrSceneObject::get_bounding_box_2d() {
+	if (state_.boundingBox2D == std::nullopt) {
+		return Rect2();
+	}
+
+	return Rect2(
+		{state_.boundingBox2D->offset.x, state_.boundingBox2D->offset.y},
+		{state_.boundingBox2D->extent.width, state_.boundingBox2D->extent.height}
+	);
+}
+
+int XrSceneObject::get_bounds_2d_count() {
+	if (state_.boundary2D != std::nullopt) {
+		return state_.boundary2D->size();
+	}
+	return 0;
+}
+
+Vector2 XrSceneObject::get_bounds_2d_vertex(int index) {
+	if (state_.boundary2D != std::nullopt) {
+		if (index >= 0 && index < state_.boundary2D->size()) {
+			return {
+				(*state_.boundary2D)[index].x,
+				(*state_.boundary2D)[index].y,
+			};
+		}
+	}
+	return {
+		0.0f, 0.0f
+	};
+}

common/src/main/cpp/classes/xr_scene_provider_openxr.cpp

@@ -0,0 +1,166 @@
+#include "classes/xr_scene_provider_openxr.h"
+
+#include <algorithm>
+#include <string>
+#include <memory>
+
+#include <godot_cpp/core/class_db.hpp>
+#include <godot_cpp/core/error_macros.hpp>
+
+#include "utils/xr_godot_utils.h"
+#include "extensions/openxr_fb_scene_extension_wrapper.h"
+#include "extensions/openxr_fb_spatial_entity_extension_wrapper.h"
+#include "extensions/openxr_fb_spatial_entity_container_extension_wrapper.h"
+#include "extensions/openxr_fb_spatial_entity_query_extension_wrapper.h"
+#include "extensions/openxr_fb_spatial_entity_storage_extension_wrapper.h"
+
+#define SESSION  (XrSession) get_openxr_api()->get_session()
+
+using namespace godot;
+
+static const uint32_t MAX_PERSISTENT_SPACES = 100;
+
+// Base OpenXR APIs we still need
+PFN_xrLocateSpace XrSceneProviderOpenXR::xrLocateSpace = nullptr;
+
+// Singleton
+XrSceneProviderOpenXR *XrSceneProviderOpenXR::singleton = nullptr;
+
+XrSceneProviderOpenXR* XrSceneProviderOpenXR::get_singleton() {
+	if (singleton == nullptr) {
+		singleton = memnew(XrSceneProviderOpenXR());
+	}
+  return singleton;
+}
+
+XrSceneProviderOpenXR::XrSceneProviderOpenXR() {
+  singleton = this;
+}
+
+XrSceneProviderOpenXR::~XrSceneProviderOpenXR() {
+  singleton = nullptr;
+}
+
+void XrSceneProviderOpenXR::_on_instance_created(uint64_t instance) {
+  // Base OpenXR
+  xrLocateSpace = (PFN_xrLocateSpace) get_openxr_api()->get_instance_proc_addr("xrLocateSpace");
+}
+
+void XrSceneProviderOpenXR::try_adding_output_for_uuid(XrUuidEXT uuid, const std::map<XrUuidEXT, XrSpaceQueryResultFB, XrUuidExtCmp>& from, std::vector<XrSceneObjectInternal>& objects) {
+    if (XrGodotUtils::isUuidValid(uuid) && from.count(uuid) > 0) {
+      auto result = from.at(uuid);
+
+      // Ensure the anchor we are adding is locatable
+      if (OpenXRFbSpatialEntityExtensionWrapper::get_singleton()->is_component_supported(result.space, XR_SPACE_COMPONENT_TYPE_LOCATABLE_FB)) {
+        if (!OpenXRFbSpatialEntityExtensionWrapper::get_singleton()->is_component_enabled(result.space, XR_SPACE_COMPONENT_TYPE_LOCATABLE_FB)) {
+          OpenXRFbSpatialEntityExtensionWrapper::get_singleton()->set_component_enabled(
+              result.space,
+              XR_SPACE_COMPONENT_TYPE_LOCATABLE_FB,
+              true,
+              [uuid](const XrEventDataSpaceSetStatusCompleteFB* eventData) {
+                if (!eventData || !XR_SUCCEEDED(eventData->result)) {
+                  std::string err = "Unable to enable XR_SPACE_COMPONENT_TYPE_LOCATABLE_FB for XrSpace " + XrGodotUtils::uuidToString(uuid);
+                  WARN_PRINT(String(err.c_str()));
+                }
+              });
+        } else {
+          std::string msg = "XR_SPACE_COMPONENT_TYPE_LOCATABLE_FB already enabled for XrSpace " + XrGodotUtils::uuidToString(uuid);
+          WARN_PRINT(String(msg.c_str()));
+        }
+      } else {
+        std::string err = "XR_SPACE_COMPONENT_TYPE_LOCATABLE_FB not supported for XrSpace " + XrGodotUtils::uuidToString(uuid);
+        WARN_PRINT(String(err.c_str()));
+      }
+
+     // Grab the semantic label if we can
+      auto labels = OpenXRFbSceneExtensionWrapper::get_singleton()->get_semantic_labels(result.space);
+
+      XrSceneObjectInternal obj = {
+        result.uuid,
+        result.space,
+        labels,
+        std::nullopt,
+        std::nullopt,
+        std::nullopt,
+      };
+
+      // Grab the 2D or 3D shapes
+      OpenXRFbSceneExtensionWrapper::get_singleton()->get_shapes(result.space, obj);
+
+      objects.push_back(obj);
+    } else {
+      std::string error = "Uuid invalid or unavailable: " + XrGodotUtils::uuidToString(uuid);
+      WARN_PRINT(String(error.c_str()));
+    }
+}
+
+void XrSceneProviderOpenXR::query_room(QueryAnchorCallback_t callback) {
+  XrSpaceQueryInfoFB queryInfo = {
+      XR_TYPE_SPACE_QUERY_INFO_FB,
+      nullptr,
+      XR_SPACE_QUERY_ACTION_LOAD_FB,
+      MAX_PERSISTENT_SPACES,
+      0,
+      nullptr,
+      nullptr};
+  OpenXRFbSpatialEntityQueryExtensionWrapper::get_singleton()->query_spatial_entities((XrSpaceQueryInfoBaseHeaderFB*) &queryInfo, [=](Vector<XrSpaceQueryResultFB> results) {
+    // There is exactly 1 room space, find that first
+    std::optional<XrSpaceQueryResultFB> room;
+    for (const auto& result : results) {
+      if (OpenXRFbSpatialEntityExtensionWrapper::get_singleton()->is_component_enabled(result.space, XR_SPACE_COMPONENT_TYPE_ROOM_LAYOUT_FB)) {
+        room = result;
+        break;
+      }
+    }
+
+    std::vector<XrSceneObjectInternal> objects;
+    if (room == std::nullopt) {
+      WARN_PRINT("No room available, bailing!");
+      callback(objects);
+      return;
+    }
+
+    // Store results into a map by UUID for easier lookup later
+    std::map<XrUuidEXT, XrSpaceQueryResultFB, XrUuidExtCmp> resultMap;
+    for (const auto& result : results) {
+      resultMap[result.uuid] = result;
+      WARN_PRINT("Found UID: " + String(XrGodotUtils::uuidToString(result.uuid).c_str()));
+      OpenXRFbSpatialEntityExtensionWrapper::get_singleton()->print_supported_components(result.space);
+    }
+
+    // Get the room info: Unused for now because the same info is returned by xrGetSpaceContainerFB
+    // with semantic labels (keeping as reference in case the exact layout matters layer)
+    // XrRoomLayoutFB roomLayout = {XR_TYPE_ROOM_LAYOUT_FB};
+    // xrGetSpaceRoomLayoutFB(SESSION, room->space, &roomLayout);
+    // std::vector<XrUuidEXT> wallUuids(roomLayout.wallUuidCountOutput);
+    // roomLayout.wallUuidCapacityInput = wallUuids.size();
+    // roomLayout.wallUuids = wallUuids.data();
+    // xrGetSpaceRoomLayoutFB(SESSION, room->space, &roomLayout);
+    //
+    // try_adding_output_for_uuid(roomLayout.floorUuid, event->requestId, objects);
+    // try_adding_output_for_uuid(roomLayout.ceilingUuid, event->requestId, objects);
+    // for (int i = 0; i < roomLayout.wallUuidCountOutput; i++) {
+    //   try_adding_output_for_uuid(roomLayout.wallUuids[i], event->requestId, objects);
+    // }
+
+    // Get other contained anchors
+    auto uuids = OpenXRFbSpatialEntityContainerExtensionWrapper::get_singleton()->get_contained_uuids(room->space);
+
+    // Add contained anchors to the output too
+    for (auto uuid : uuids) {
+      try_adding_output_for_uuid(uuid, resultMap, objects);
+    }
+
+    callback(objects);
+  });
+}
+
+void XrSceneProviderOpenXR::locate_space(const XrSpace space, XrSpaceLocation* location) {
+  XrTime display_time = (XrTime) get_openxr_api()->get_next_frame_time();
+  XrSpace play_space = (XrSpace) get_openxr_api()->get_play_space();
+  xrLocateSpace(space, play_space, display_time, location);
+}
+
+void XrSceneProviderOpenXR::_bind_methods() {
+  ClassDB::bind_static_method("XrSceneProviderOpenXR", D_METHOD("get_singleton"), &XrSceneProviderOpenXR::get_singleton);
+}

common/src/main/cpp/include/classes/xr_scene_manager.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#include <optional>
+
+#include <godot_cpp/classes/ref_counted.hpp>
+#include <godot_cpp/variant/callable.hpp>
+
+#include "classes/xr_scene_object.h"
+#include "extensions/openxr_fb_scene_extension_wrapper.h"
+
+namespace godot {
+
+/**
+ * A real scene model provider, backed by OpenXR. Exposed to GDScript
+ */
+class XrSceneManager : public RefCounted {
+  GDCLASS(XrSceneManager, RefCounted)
+
+public:
+  XrSceneManager();
+  void query_room();
+  void load_xr_scene_object(int index, Ref<XrSceneObject>);
+  int get_xr_scene_object_count();
+
+protected:
+  static void _bind_methods();
+
+private:
+  std::vector<XrSceneObjectInternal> sceneObjects_;
+};
+
+}

common/src/main/cpp/include/classes/xr_scene_object.h

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <optional>
+
+#include <godot_cpp/classes/ref_counted.hpp>
+
+#include "extensions/openxr_fb_scene_extension_wrapper.h"
+
+namespace godot {
+
+class XrSceneObject : public RefCounted {
+  GDCLASS(XrSceneObject, RefCounted)
+
+public:
+  XrSceneObject();
+  ~XrSceneObject();
+
+  void init(XrSceneObjectInternal state);
+  void update_transform();
+
+  String get_id();
+  String get_label();
+
+  bool transform_valid();
+  Transform3D get_transform();
+
+  Rect2 get_bounding_box_2d();
+
+  int get_bounds_2d_count();
+  Vector2 get_bounds_2d_vertex(int index);
+
+protected:
+  static void _bind_methods();
+
+private:
+  XrSceneObjectInternal state_;
+
+  XrSpaceVelocity velocity_;
+  XrSpaceLocation location_;
+};
+
+} // namespace godot