spicetools/misc/vrutil.h

140 lines
4.6 KiB
C++

#include <cmath>
#include "external/openvr/headers/openvr.h"
#include "external/linalg.h"
namespace vrutil {
enum class VRStatus {
Disabled, Error, Running,
};
extern VRStatus STATUS;
extern vr::IVRSystem *SYSTEM;
extern vr::TrackedDeviceIndex_t INDEX_LEFT;
extern vr::TrackedDeviceIndex_t INDEX_RIGHT;
void init();
void shutdown();
void scan(bool log = false);
bool get_hmd_pose(vr::TrackedDevicePose_t *pose);
bool get_con_pose(vr::TrackedDeviceIndex_t index,
vr::TrackedDevicePose_t *pose,
vr::VRControllerState_t *state);
/*
* Util
*/
inline linalg::aliases::float3 get_translation(vr::HmdMatrix34_t &m) {
return linalg::aliases::float3 {
m.m[0][3],
m.m[1][3],
m.m[2][3],
};
}
inline linalg::aliases::float3 get_direction_back(vr::HmdMatrix34_t &m) {
return linalg::normalize(linalg::aliases::float3 {
m.m[0][2],
m.m[1][2],
m.m[2][2],
});
}
inline linalg::aliases::float3 get_direction_front(vr::HmdMatrix34_t &m) {
return -get_direction_back(m);
}
inline linalg::aliases::float3 get_direction_up(vr::HmdMatrix34_t &m) {
return linalg::normalize(linalg::aliases::float3 {
m.m[0][1],
m.m[1][1],
m.m[2][1],
});
}
inline linalg::aliases::float3 get_direction_down(vr::HmdMatrix34_t &m) {
return -get_direction_up(m);
}
inline linalg::aliases::float3 get_direction_right(vr::HmdMatrix34_t &m) {
return linalg::normalize(linalg::aliases::float3 {
m.m[0][0],
m.m[1][0],
m.m[2][0],
});
}
inline linalg::aliases::float3 get_direction_left(vr::HmdMatrix34_t &m) {
return -get_direction_right(m);
}
template<class T>
inline linalg::aliases::float4 get_rotation(T &m) {
linalg::aliases::float4 q;
q.w = sqrtf(fmaxf(0, 1 + m[0][0] + m[1][1] + m[2][2])) * 0.5f;
q.x = sqrtf(fmaxf(0, 1 + m[0][0] - m[1][1] - m[2][2])) * 0.5f;
q.y = sqrtf(fmaxf(0, 1 - m[0][0] + m[1][1] - m[2][2])) * 0.5f;
q.z = sqrtf(fmaxf(0, 1 - m[0][0] - m[1][1] + m[2][2])) * 0.5f;
q.x = copysignf(q.x, m[2][1] - m[1][2]);
q.y = copysignf(q.y, m[0][2] - m[2][0]);
q.z = copysignf(q.z, m[1][0] - m[0][1]);
return q;
}
inline linalg::aliases::float4 get_rotation(
float pitch, float yaw, float roll) {
return linalg::aliases::float4 {
sinf(roll/2) * cosf(pitch/2) * cosf(yaw/2) - cosf(roll/2) * sinf(pitch/2) * sinf(yaw/2),
cosf(roll/2) * sinf(pitch/2) * cosf(yaw/2) + sinf(roll/2) * cosf(pitch/2) * sinf(yaw/2),
cosf(roll/2) * cosf(pitch/2) * sinf(yaw/2) - sinf(roll/2) * sinf(pitch/2) * cosf(yaw/2),
cosf(roll/2) * cosf(pitch/2) * cosf(yaw/2) + sinf(roll/2) * sinf(pitch/2) * sinf(yaw/2),
};
}
inline linalg::aliases::float3 orthogonal(
linalg::aliases::float3 v) {
auto x = std::fabs(v.x);
auto y = std::fabs(v.y);
auto z = std::fabs(v.z);
linalg::aliases::float3 o;
if (x < y) {
if (x < z) o = linalg::aliases::float3(1, 0, 0);
else o = linalg::aliases::float3(0, 0, 1);
} else {
if (y < z) o = linalg::aliases::float3(0, 1, 0);
else linalg::aliases::float3(0, 0, 1);
}
return linalg::cross(v, o);
}
inline linalg::aliases::float4 get_rotation(
linalg::aliases::float3 v1,
linalg::aliases::float3 v2) {
auto dot = linalg::dot(v1, v2);
auto scl = sqrtf(linalg::length2(v1) * linalg::length2(v2));
if (dot / scl == -1) {
auto v = linalg::normalize(orthogonal(v1));
return linalg::aliases::float4 {
v.x, v.y, v.z, 0,
};
}
auto cross = linalg::cross(v1, v2);
return linalg::normalize(linalg::aliases::float4 {
cross.x, cross.y, cross.z, dot + scl,
});
}
inline linalg::aliases::float3 intersect_point(
linalg::aliases::float3 ray_dir,
linalg::aliases::float3 ray_point,
linalg::aliases::float3 plane_normal,
linalg::aliases::float3 plane_point) {
auto delta = ray_point - plane_point;
auto delta_dot = linalg::dot(delta, plane_normal);
auto ray_dot = linalg::dot(ray_dir, plane_normal);
return ray_point - ray_dir * (delta_dot / ray_dot);
}
}