WIP Working collision based touch

components/streams/src/renderers/screen_render.cpp

@@ -5,6 +5,8 @@
 #include <ftl/operators/antialiasing.hpp>
 #include <ftl/operators/gt_analysis.hpp>
 #include <ftl/algorithms/dbscan.hpp>
+#include <ftl/utility/matrix_conversion.hpp>
+#include <ftl/codecs/touch.hpp>
 
 #include <loguru.hpp>
 
@@ -157,8 +159,43 @@ bool ScreenRender::retrieve(ftl::data::Frame &frame_out) {
 			return neighbors;
 		}, 5, 16.0f, labels, clusters);
 
-		if (clusters.size() > 0) {
-			LOG(INFO) << "Found " << clusters.size() << " collisions";
+		// TODO: Support multi-touch
+		if (clusters.size() == 1) {
+			//LOG(INFO) << "Found " << clusters.size() << " collisions";
+			//LOG(INFO) << "  -- " << clusters[0].x << "," << clusters[0].y << " " << clusters[0].z;
+
+			// Find all frames that support touch
+			for (auto &s : sets) {
+				if (s->frameset() == my_id_) continue;
+
+				for (const auto &f : s->frames) {
+					if (f.has(Channel::Capabilities)) {
+						const auto &cap = f.get<std::unordered_set<Capability>>(Channel::Capabilities);
+
+						// If it supports touch, calculate the touch points
+						if (cap.count(Capability::TOUCH)){
+							const auto &rgbdf = f.cast<ftl::rgbd::Frame>();
+							auto pose = MatrixConversion::toCUDA((rgbdf.getPose().inverse() * rgbdframe.getPose()).cast<float>());
+							float3 campos = pose * rgbdframe.getLeft().screenToCam(clusters[0].x, clusters[0].y, clusters[0].z);
+							int2 pt = rgbdf.getLeft().camToScreen<int2>(campos);
+							//LOG(INFO) << "TOUCH AT " << pt.x << "," << pt.y << " - " << campos.z;
+
+							{
+								// Send the touch data
+								auto response = f.response();
+								auto &touches = response.create<std::vector<ftl::codecs::Touch>>(Channel::Touch);
+								auto &touch = touches.emplace_back();
+								touch.id = 0;
+								touch.x = pt.x;
+								touch.y = pt.y;
+								touch.type = ftl::codecs::TouchType::COLLISION;
+								touch.strength = 255;
+								touch.d = campos.z;
+							}
+						}
+					}
+				}
+			}
 		}
 
 		return true;

components/structures/include/ftl/data/new_frame.hpp

@@ -595,7 +595,7 @@ class Frame {
 	 * source will then see these changes in the next frame it attempt to
 	 * generate.
 	 */
-	Frame response();
+	Frame response() const;
 
 	/**
 	 * Convert this frame to another type. That type must not have any

components/structures/src/new_frame.cpp

@@ -391,7 +391,7 @@ void Frame::hardReset() {
 	available_ = 0;
 }
 
-Frame Frame::response() {
+Frame Frame::response() const {
 	if (!pool_) throw FTL_Error("Frame has no pool, cannot generate response");
 	Frame f = pool_->allocate(id_, ftl::timer::get_time());
 	f.mode_ = FrameMode::RESPONSE;