diff --git a/demo/example.gd b/demo/example.gd index 49db5fd..9357cb3 100644 --- a/demo/example.gd +++ b/demo/example.gd @@ -1,6 +1,8 @@ extends Node func _ready() -> void: - print(%OrbbecDevices.get_devices_ips()) - %OrbbecPointCloud.set_device_from_ip("10.10.30.182") + var ips = %OrbbecDevices.get_devices_ips() + ips.sort() + print(ips) + %OrbbecPointCloud.set_device_from_ip(ips[0]) %OrbbecPointCloud.start_stream() diff --git a/demo/example.tscn b/demo/example.tscn index c6a5ff9..86b453e 100644 --- a/demo/example.tscn +++ b/demo/example.tscn @@ -14,6 +14,7 @@ script = ExtResource("1_jdh55") unique_name_in_owner = true [node name="OrbbecPointCloud" type="OrbbecPointCloud" parent="."] +thinning = 0.0 unique_name_in_owner = true [node name="WorldEnvironment" type="WorldEnvironment" parent="."] diff --git a/demo/multi_mesh_instance_3d.gd b/demo/multi_mesh_instance_3d.gd index 6914577..45dc548 100644 --- a/demo/multi_mesh_instance_3d.gd +++ b/demo/multi_mesh_instance_3d.gd @@ -1,12 +1,13 @@ extends MultiMeshInstance3D var points: PackedVector3Array +var raw_points: PackedFloat32Array var redraw:bool = false func _ready(): multimesh = MultiMesh.new() multimesh.transform_format = MultiMesh.TRANSFORM_3D - + var pmesh := PointMesh.new() var material := StandardMaterial3D.new() material.shading_mode = BaseMaterial3D.SHADING_MODE_UNSHADED @@ -19,14 +20,10 @@ func _ready(): func _process(_delta): if redraw: multimesh.instance_count = len(points) - # TODO: there is a way to set a whole PackedFloat32Array as the multimesh's buffer. - # This may be faster than iterating and creating a transform3D. - # see https://docs.godotengine.org/en/stable/classes/class_renderingserver.html#class-renderingserver-method-multimesh-set-buffer - # it references rendering server but it works in gdscript too. We could even generate the packed array in C++ directly. - for i in multimesh.instance_count: - multimesh.set_instance_transform(i, Transform3D(Basis(), points[i])) + multimesh.buffer = raw_points redraw = false -func _on_orbbec_point_cloud_frame(new_point_cloud_frame: PackedVector3Array) -> void: +func _on_orbbec_point_cloud_frame(new_point_cloud_frame: PackedVector3Array, raw_buffer: PackedFloat32Array) -> void: points = new_point_cloud_frame + raw_points = raw_buffer redraw = true diff --git a/src/orbbec.cpp b/src/orbbec.cpp index b1384a5..900a328 100644 --- a/src/orbbec.cpp +++ b/src/orbbec.cpp @@ -16,7 +16,6 @@ void OrbbecDevices::refresh_device_list() { void OrbbecDevices::_ready() { refresh_device_list(); - emit_signal("point_cloud_frame", this); } PackedStringArray OrbbecDevices::get_devices_ips() { @@ -64,7 +63,7 @@ void OrbbecPointCloud::_bind_methods() { godot::ClassDB::bind_method(D_METHOD("get_thinning"), &OrbbecPointCloud::get_thinning); godot::ClassDB::bind_method(D_METHOD("set_thinning", "p_thinning"), &OrbbecPointCloud::set_thinning); ADD_PROPERTY(PropertyInfo(Variant::FLOAT, "thinning"), "set_thinning", "get_thinning"); - ADD_SIGNAL(MethodInfo("point_cloud_frame", PropertyInfo(Variant::PACKED_VECTOR3_ARRAY, "new_point_cloud_frame"))); + ADD_SIGNAL(MethodInfo("point_cloud_frame", PropertyInfo(Variant::PACKED_VECTOR3_ARRAY, "points"), PropertyInfo(Variant::PACKED_FLOAT32_ARRAY, "raw_buffer"))); } void OrbbecPointCloud::set_device_from_predicate(predicate_type predicate) { @@ -121,29 +120,51 @@ void OrbbecPointCloud::start_stream() { config->setFrameAggregateOutputMode(OB_FRAME_AGGREGATE_OUTPUT_ALL_TYPE_FRAME_REQUIRE); // 4.Start the pipeline with config and callback. pipeline->start(config, [&, this](std::shared_ptr frameSet) { - // print_line("got frameset"); auto frame = point_cloud_filter->process(frameSet)->as(); uint32_t width = frame->getWidth(); uint32_t height = frame->getHeight(); PackedVector3Array point_cloud_data; + PackedFloat32Array point_cloud_raw_buffer; point_cloud_data.resize(width*height); + point_cloud_raw_buffer.resize(width*height*floats_per_raw_point); const uint8_t* data = frame->getData(); uint32_t real_size = 0; OBPoint *points = reinterpret_cast(const_cast(data)); + const auto& basis = identity_transform.basis.rows; for(uint32_t y = 0; y < height; ++y) { for(uint32_t x = 0; x < width; ++x) { int idx = y * width + x; const auto &pt = points[idx]; if (thinning_mask[idx%thinning_mask_size] > thinning && std::fabs(pt.z) >= min_point_value) { - point_cloud_data[real_size] = Vector3(pt.x/1000.0f, -pt.y/1000.0f, pt.z/1000.0f); + float x = pt.x/1000.0f; + float y = pt.y/1000.0f; + float z = pt.z/1000.0f; + + point_cloud_data[real_size] = Vector3(x, y, z); + + // doc for this order is here : https://docs.godotengine.org/en/stable/classes/class_renderingserver.html#class-renderingserver-method-multimesh-set-buffer + point_cloud_raw_buffer[real_size * floats_per_raw_point] = basis[0][0]; + point_cloud_raw_buffer[real_size * floats_per_raw_point + 1] = basis[1][0]; + point_cloud_raw_buffer[real_size * floats_per_raw_point + 2] = basis[2][0]; + point_cloud_raw_buffer[real_size * floats_per_raw_point + 3] = x; + point_cloud_raw_buffer[real_size * floats_per_raw_point + 4] = basis[0][1]; + point_cloud_raw_buffer[real_size * floats_per_raw_point + 5] = basis[1][1]; + point_cloud_raw_buffer[real_size * floats_per_raw_point + 6] = basis[2][1]; + point_cloud_raw_buffer[real_size * floats_per_raw_point + 7] = y; + point_cloud_raw_buffer[real_size * floats_per_raw_point + 8] = basis[0][2]; + point_cloud_raw_buffer[real_size * floats_per_raw_point + 9] = basis[1][2]; + point_cloud_raw_buffer[real_size * floats_per_raw_point + 10] = basis[2][2]; + point_cloud_raw_buffer[real_size * floats_per_raw_point + 11] = z; + real_size+=1; } } } point_cloud_data.resize(real_size); + point_cloud_raw_buffer.resize(real_size*floats_per_raw_point); // need to call_deferred because this code ends up being called outside of the engine thread. - call_deferred("emit_signal", "point_cloud_frame", point_cloud_data); + call_deferred("emit_signal", "point_cloud_frame", point_cloud_data, point_cloud_raw_buffer); }); } catch( const std::exception & ex ) { diff --git a/src/orbbec.hpp b/src/orbbec.hpp index acc76a6..7bfef50 100644 --- a/src/orbbec.hpp +++ b/src/orbbec.hpp @@ -4,6 +4,7 @@ #include "godot_cpp/classes/wrapped.hpp" #include "godot_cpp/variant/variant.hpp" #include "godot_cpp/variant/char_string.hpp" +#include "godot_cpp/variant/transform3d.hpp" #include #include @@ -67,7 +68,12 @@ class OrbbecPointCloud : public Node { float get_thinning(); private: float thinning = 0.5; + /** + * used to create raw multimesh buffers from point cloud data + */ + const Transform3D identity_transform{}; static constexpr size_t thinning_mask_size = 100000; + static constexpr size_t floats_per_raw_point = 12; std::array thinning_mask; std::unique_ptr pipeline; std::unique_ptr point_cloud_filter = std::make_unique();