implement lasers for grabbing

src/app.cpp

@@ -1,6 +1,8 @@
 #include "app.h"
+#include "util.h"
 #include <X11/extensions/Xrandr.h>
 #include <cassert>
+#include <glm/matrix.hpp>
 
 App::App()
 {
@@ -156,9 +158,9 @@ void App::UpdateInput()
 	main.ulRestrictedToDevice = 0;
 	vr_input->UpdateActionState(&main, sizeof(vr::VRActiveActionSet_t), 1);
 
-	vr_sys->GetDeviceToAbsoluteTrackingPose(_tracking_origin, 0, _tracker_poses, vr::k_unMaxTrackedDeviceCount);
+	vr_sys->GetDeviceToAbsoluteTrackingPose(_tracking_origin, 0, _tracker_poses, MAX_TRACKERS);
 
-	for (unsigned int i = 0; i < vr::k_unMaxTrackedDeviceCount; i++)
+	for (unsigned int i = 0; i < MAX_TRACKERS; i++)
 	{
 		if (IsInputJustPressed(i, _input_handles.toggle))
 		{
@@ -168,8 +170,29 @@ void App::UpdateInput()
 			{
 				panel.SetHidden(_hidden);
 			}
+			for (auto &laser : _lasers)
+			{
+				if (laser.has_value())
+				{
+					laser->SetHidden(_hidden);
+				}
+			}
 			break;
 		}
+		auto type = vr_sys->GetTrackedDeviceClass(i);
+		if (type == vr::TrackedDeviceClass_Controller)
+		{
+			if (!_lasers[i].has_value())
+			{
+				_lasers[i] = Laser(this, i);
+			}
+			_lasers[i]->SetHidden(_hidden);
+			_lasers[i]->Update();
+		}
+		else if (_lasers[i].has_value())
+		{
+			_lasers[i]->SetHidden(true);
+		}
 	}
 }
 
@@ -220,6 +243,32 @@ bool App::IsInputJustPressed(TrackerID controller, vr::VRActionHandle_t action)
 	return data.bState && data.bChanged;
 }
 
+Ray App::IntersectRay(glm::vec3 origin, glm::vec3 direction, float max_len)
+{
+	Ray ray;
+	ray.distance = max_len;
+	ray.overlay = nullptr;
+	{
+		float root_dist = _root_overlay.IntersectRay(origin, direction, max_len);
+		if (root_dist < ray.distance)
+		{
+			ray.distance = root_dist;
+			ray.overlay = &_root_overlay;
+		}
+	}
+
+	for (auto &panel : _panels)
+	{
+		float dist = panel.GetOverlay()->IntersectRay(origin, direction, max_len);
+		if (dist < ray.distance)
+		{
+			ray.distance = dist;
+			ray.overlay = panel.GetOverlay();
+		}
+	}
+	return ray;
+}
+
 CursorPos App::GetCursorPosition()
 {
 	Window curr_root;

src/app.h

@@ -1,12 +1,14 @@
 #pragma once
 #define GL_GLEXT_PROTOTYPES
 
+#include "laser.h"
 #include "overlay.h"
 #include "panel.h"
 #include "util.h"
 #include <GLFW/glfw3.h>
 #include <X11/Xutil.h>
 #include <filesystem>
+#include <optional>
 #include <vector>
 
 struct CursorPos
@@ -22,6 +24,12 @@ struct InputHandles
 	vr::VRActionHandle_t grab;
 };
 
+struct Ray
+{
+	Overlay *overlay;
+	float distance;
+};
+
 class App
 {
   public:
@@ -36,6 +44,8 @@ class App
 	bool IsInputJustPressed(TrackerID controller, vr::VRActionHandle_t action);
 	CursorPos GetCursorPosition();
 
+	Ray IntersectRay(glm::vec3 origin, glm::vec3 direction, float max_len);
+
 	Display *_xdisplay;
 	Window _root_window;
 	GLFWwindow *_gl_window;
@@ -52,7 +62,8 @@ class App
 	vr::IVRInput *vr_input;
 
 	InputHandles _input_handles;
-	vr::TrackedDevicePose_t _tracker_poses[vr::k_unMaxTrackedDeviceCount];
+	vr::TrackedDevicePose_t _tracker_poses[MAX_TRACKERS];
+	std::optional<Laser> _lasers[MAX_TRACKERS];
 
 	Overlay _root_overlay;
 	std::vector<Panel> _panels;

src/laser.cpp

@@ -0,0 +1,45 @@
+#include "laser.h"
+#include "app.h"
+#include "util.h"
+#include <string>
+
+Laser::Laser(App *app, TrackerID index)
+	: _overlay(app, "laser_" + std::to_string(index)),
+	  _app(app),
+	  _controller(index)
+{
+	_overlay.SetHidden(true);
+	_overlay.SetTransformTracker(index, &VRMatIdentity);
+	_overlay.SetTextureToColor(255, 200, 255);
+	_overlay.SetAlpha(0.2f);
+}
+
+void Laser::Update()
+{
+	if (_overlay.IsHidden())
+	{
+		return;
+	}
+	const float width = 0.004f;
+	auto controller_pose = _app->GetTrackerPose(_controller);
+	auto origin = GetPos(controller_pose);
+	auto forward = -glm::vec3(controller_pose[2]);
+	auto ray = _app->IntersectRay(origin, forward, 5.0f);
+	float len = ray.distance;
+
+	VRMat transform = {{{width, 0, 0, 0}, {0, 0, width, 0}, {0, len, 0, len * -0.5f}}};
+	_overlay.SetTransformTracker(_controller, &transform);
+
+	if (ray.overlay != nullptr)
+	{
+		if (_app->IsInputJustPressed(_controller, _app->_input_handles.grab))
+		{
+			ray.overlay->ControllerGrab(_controller);
+		}
+	}
+}
+
+void Laser::SetHidden(bool state)
+{
+	_overlay.SetHidden(state);
+}

src/laser.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "overlay.h"
+#include "util.h"
+
+class App;
+
+class Laser
+{
+  public:
+	Laser(App *app, TrackerID index);
+
+	void Update();
+	void SetHidden(bool state);
+
+  private:
+	App *_app;
+	Overlay _overlay;
+	TrackerID _controller;
+};

src/overlay.cpp

@@ -1,6 +1,7 @@
 #include "overlay.h"
 #include "app.h"
 #include "util.h"
+#include <cstdint>
 #include <glm/fwd.hpp>
 
 Overlay::Overlay()
@@ -16,6 +17,7 @@ Overlay::Overlay(App *app, std::string name)
 	_is_held = false;
 	_active_hand = 0;
 	_width_m = 1;
+	_ratio = 1;
 
 	_target.type = TargetType::World;
 
@@ -33,7 +35,8 @@ Overlay::Overlay(App *app, std::string name)
 	// (flipping uv on y axis because opengl and xorg are opposite)
 	vr::VRTextureBounds_t bounds{0, 1, 1, 0};
 	_app->vr_overlay->SetOverlayTextureBounds(_id, &bounds);
-	SetHidden(false);
+	_hidden = false;
+	_app->vr_overlay->ShowOverlay(_id);
 }
 
 OverlayID Overlay::Id()
@@ -66,6 +69,11 @@ float Overlay::Width()
 	return _width_m;
 }
 
+float Overlay::Ratio()
+{
+	return _ratio;
+}
+
 void Overlay::SetWidth(float width_meters)
 {
 	_width_m = width_meters;
@@ -73,6 +81,8 @@ void Overlay::SetWidth(float width_meters)
 }
 
 void Overlay::SetHidden(bool state)
+{
+	if (state != _hidden)
 	{
 		_hidden = state;
 		if (_hidden)
@@ -80,6 +90,7 @@ void Overlay::SetHidden(bool state)
 		else
 			_app->vr_overlay->ShowOverlay(_id);
 	}
+}
 
 void Overlay::SetAlpha(float alpha)
 {
@@ -87,13 +98,25 @@ void Overlay::SetAlpha(float alpha)
 	_app->vr_overlay->SetOverlayAlpha(_id, alpha);
 }
 
+void Overlay::SetRatio(float ratio)
+{
+	_ratio = ratio;
+}
+
 void Overlay::SetTexture(vr::Texture_t *texture)
 {
 	auto set_texture_err = _app->vr_overlay->SetOverlayTexture(_id, texture);
 	assert(set_texture_err == 0);
 }
 
-void Overlay::SetTransformTracker(TrackerID tracker, VRMat *transform)
+void Overlay::SetTextureToColor(uint8_t r, uint8_t g, uint8_t b)
+{
+	uint8_t col[4] = {r, g, b, 255};
+	auto set_texture_err = _app->vr_overlay->SetOverlayRaw(_id, &col, 1, 1, 4);
+	assert(set_texture_err == 0);
+}
+
+void Overlay::SetTransformTracker(TrackerID tracker, const VRMat *transform)
 {
 	_app->vr_overlay->SetOverlayTransformTrackedDeviceRelative(_id, tracker, transform);
 	_target.type = TargetType::Tracker;
@@ -101,7 +124,7 @@ void Overlay::SetTransformTracker(TrackerID tracker, VRMat *transform)
 	_target.transform = *transform;
 }
 
-void Overlay::SetTransformWorld(VRMat *transform)
+void Overlay::SetTransformWorld(const VRMat *transform)
 {
 	_app->vr_overlay->SetOverlayTransformAbsolute(_id, vr::TrackingUniverseStanding, transform);
 	_target.type = TargetType::World;
@@ -115,6 +138,34 @@ void Overlay::SetTargetWorld()
 	_target.type = TargetType::World;
 }
 
+float Overlay::IntersectRay(glm::vec3 origin, glm::vec3 direction, float max_len)
+{
+	float closest_dist = max_len;
+	auto end = origin + direction * max_len;
+
+	auto panel_transform = GetTransformAbsolute();
+	auto panel_pos = GetPos(panel_transform);
+	auto a = glm::inverse(panel_transform) * glm::vec4(origin - panel_pos, 0);
+	auto b = glm::inverse(panel_transform) * glm::vec4(end - panel_pos, 0);
+	float r = a.z / (a.z - b.z);
+	auto p = a + (b - a) * r;
+	// printf("panel pos: (%.2f,%.2f,%.2f)\n", panel_pos.x, panel_pos.y, panel_pos.z);
+	// printf("a: (%.2f,%.2f,%.2f)\n", a.x, a.y, a.z);
+	// printf("b: (%.2f,%.2f,%.2f)\n", b.x, b.y, b.z);
+	// printf("r: %.2f\n", r);
+	// printf("p: (%.2f,%.2f,%.2f)\n", p.x, p.y, p.z);
+
+	if (b.z < a.z && b.z < 0 && glm::abs(p.x) < (_width_m * 0.5f) && glm::abs(p.y) < (_width_m * 0.5f * _ratio))
+	{
+		float dist = r * max_len;
+		if (dist < closest_dist)
+		{
+			closest_dist = dist;
+		}
+	}
+	return closest_dist;
+}
+
 glm::mat4x4 Overlay::GetTransformAbsolute()
 {
 	if (_is_held)
@@ -155,30 +206,7 @@ void Overlay::Update()
 		assert(_initialized);
 	}
 
-	if (!_is_held)
-	{
-		for (auto controller : _app->GetControllers())
-		{
-			if (_app->IsInputJustPressed(controller, _app->_input_handles.grab))
-			{
-				auto overlay_pose = GetTransformAbsolute();
-				auto controller_pos = GetPos(_app->GetTrackerPose(controller));
-
-				auto local_pos = glm::inverse(overlay_pose) * glm::vec4(controller_pos - GetPos(overlay_pose), 0);
-
-				float grab_area_thickness = 0.3f;
-				bool close_enough = glm::abs(local_pos.z) < grab_area_thickness;
-				close_enough &= glm::abs(local_pos.x) < _width_m / 2.0f;
-				close_enough &= glm::abs(local_pos.y) < _width_m / 2.0f;
-
-				if (close_enough)
-				{
-					ControllerGrab(controller);
-				}
-			}
-		}
-	}
-	else
+	if (_is_held)
 	{
 		if (!_app->GetControllerInputDigital(_active_hand, _app->_input_handles.grab).bState)
 		{

src/overlay.h

@@ -30,20 +30,27 @@ class Overlay
 	bool IsHeld();
 	bool IsHidden();
 	TrackerID ActiveHand();
-	float Width();
 	float Alpha();
+	float Width();
+	float Ratio();
 
 	void SetWidth(float meters);
 	void SetHidden(bool state);
 	void SetAlpha(float alpha);
+	void SetRatio(float ratio);
 	void SetTexture(vr::Texture_t *texture);
+	void SetTextureToColor(uint8_t r, uint8_t g, uint8_t b);
 
-	void SetTransformTracker(TrackerID tracker, VRMat *transform);
-	void SetTransformWorld(VRMat *transform);
+	glm::mat4x4 GetTransformAbsolute();
+
+	void SetTransformTracker(TrackerID tracker, const VRMat *transform);
+	void SetTransformWorld(const VRMat *transform);
 
 	// void SetTargetTracker(TrackerID tracker);
 	void SetTargetWorld();
 
+	float IntersectRay(glm::vec3 origin, glm::vec3 direction, float max_len);
+
 	std::function<void(TrackerID)> _GrabBeginCallback;
 	std::function<void(TrackerID)> _GrabEndCallback;
 
@@ -51,8 +58,6 @@ class Overlay
 	void ControllerGrab(TrackerID controller);
 
   private:
-	glm::mat4x4 GetTransformAbsolute();
-
 	bool _initialized;
 
 	App *_app;
@@ -63,6 +68,7 @@ class Overlay
 	bool _hidden;
 	float _width_m;
 	float _alpha;
+	float _ratio;
 	TrackerID _active_hand;
 
 	Target _target;

src/panel.cpp

@@ -22,8 +22,7 @@ Panel::Panel(App *app, int index, int x, int y, int width, int height)
 	_texture.eColorSpace = vr::ColorSpace_Auto;
 	_texture.eType = vr::TextureType_OpenGL;
 	_texture.handle = (void *)(uintptr_t)_gl_texture;
-
-	// _overlay;
+	_overlay.SetRatio(height / (float)width);
 }
 
 void Panel::Update()

src/util.h

@@ -7,6 +7,9 @@ typedef vr::TrackedDeviceIndex_t TrackerID;
 typedef vr::VROverlayHandle_t OverlayID;
 typedef vr::HmdMatrix34_t VRMat;
 
+const VRMat VRMatIdentity{{{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}}};
+const int MAX_TRACKERS = vr::k_unMaxTrackedDeviceCount;
+
 inline void PrintMat(VRMat m)
 {
 	printf("[%.2f, %.2f, %.2f, %.2f]\n", m.m[0][0], m.m[0][1], m.m[0][2], m.m[0][3]);