From fe94dec5eab631d564154235f7cac2c4d80d4cea Mon Sep 17 00:00:00 2001 From: Jakob Bornecrantz Date: Thu, 23 Jul 2020 12:12:03 +0100 Subject: [PATCH] m/space: Space graph functions --- src/xrt/auxiliary/CMakeLists.txt | 2 + src/xrt/auxiliary/math/m_space.cpp | 296 +++++++++++++++++++++++++++++ src/xrt/auxiliary/math/m_space.h | 173 +++++++++++++++++ src/xrt/auxiliary/meson.build | 2 + 4 files changed, 473 insertions(+) create mode 100644 src/xrt/auxiliary/math/m_space.cpp create mode 100644 src/xrt/auxiliary/math/m_space.h diff --git a/src/xrt/auxiliary/CMakeLists.txt b/src/xrt/auxiliary/CMakeLists.txt index ead54a3b5..e99449f87 100644 --- a/src/xrt/auxiliary/CMakeLists.txt +++ b/src/xrt/auxiliary/CMakeLists.txt @@ -23,6 +23,8 @@ set(MATH_SOURCE_FILES math/m_permutation.c math/m_permutation.h math/m_quatexpmap.cpp + math/m_space.cpp + math/m_space.h math/m_vec2.h math/m_vec3.h ) diff --git a/src/xrt/auxiliary/math/m_space.cpp b/src/xrt/auxiliary/math/m_space.cpp new file mode 100644 index 000000000..64675c78e --- /dev/null +++ b/src/xrt/auxiliary/math/m_space.cpp @@ -0,0 +1,296 @@ +// Copyright 2020, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Functions for manipulating a @ref xrt_space_graph struct. + * @author Jakob Bornecrantz + * @ingroup aux_math + */ + +#include "util/u_misc.h" + +#include "math/m_api.h" +#include "math/m_vec2.h" +#include "math/m_vec3.h" +#include "math/m_space.h" + +#include + + +/* + * + * Dump functions. + * + */ + +static void +dump_relation(const struct xrt_space_relation *r) +{ + fprintf(stderr, "%04x", r->relation_flags); + + if (r->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) { + fprintf(stderr, " P{%f %f %f}", r->pose.position.x, + r->pose.position.y, r->pose.position.z); + } + + if (r->relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) { + fprintf(stderr, " O{%f %f %f %f}", r->pose.orientation.x, + r->pose.orientation.y, r->pose.orientation.z, + r->pose.orientation.w); + } + + if (r->relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) { + fprintf(stderr, " LV{%f %f %f}", r->linear_velocity.x, + r->linear_velocity.y, r->linear_velocity.z); + } + + if (r->relation_flags & + XRT_SPACE_RELATION_LINEAR_ACCELERATION_VALID_BIT) { + fprintf(stderr, " LA{%f %f %f}", r->linear_acceleration.x, + r->linear_acceleration.y, r->linear_acceleration.z); + } + + if (r->relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) { + fprintf(stderr, " AV{%f %f %f}", r->angular_velocity.x, + r->angular_velocity.y, r->angular_velocity.z); + } + + if (r->relation_flags & + XRT_SPACE_RELATION_ANGULAR_ACCELERATION_VALID_BIT) { + fprintf(stderr, " AA{%f %f %f}", r->angular_acceleration.x, + r->angular_acceleration.y, r->angular_acceleration.z); + } + + fprintf(stderr, "\n"); +} + +static void +dump_graph(const struct xrt_space_graph *xsg) +{ + fprintf(stderr, "%s %u\n", __func__, xsg->num_steps); + for (uint32_t i = 0; i < xsg->num_steps; i++) { + const struct xrt_space_relation *r = &xsg->steps[i]; + fprintf(stderr, "\t%2u: ", i); + dump_relation(r); + } +} + + +/* + * + * Helper functions. + * + */ + +static bool +has_step_with_no_pose(const struct xrt_space_graph *xsg) +{ + const enum xrt_space_relation_flags pose_flags = + (enum xrt_space_relation_flags)( + XRT_SPACE_RELATION_POSITION_VALID_BIT | + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT); + + for (uint32_t i = 0; i < xsg->num_steps; i++) { + const struct xrt_space_relation *r = &xsg->steps[i]; + if ((r->relation_flags & pose_flags) == 0) { + return true; + } + } + + return false; +} + +struct flags +{ + unsigned int has_orientation : 1; + unsigned int has_position : 1; + unsigned int has_linear_velocity : 1; + unsigned int has_angular_velocity : 1; + unsigned int has_tracked_orientation : 1; + unsigned int has_tracked_position : 1; +}; + +flags +get_flags(const struct xrt_space_relation *r) +{ + // clang-format off + flags flags = {}; + flags.has_orientation = (r->relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0; + flags.has_position = (r->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0; + flags.has_linear_velocity = (r->relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) != 0; + flags.has_angular_velocity = (r->relation_flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) != 0; + flags.has_tracked_orientation = (r->relation_flags & XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT) != 0; + flags.has_tracked_position = (r->relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) != 0; + // clang-format on + + return flags; +} + +static void +make_valid_pose(flags flags, + const struct xrt_pose *in_pose, + struct xrt_pose *out_pose) +{ + if (flags.has_orientation) { + out_pose->orientation = in_pose->orientation; + } else { + out_pose->orientation.x = 0.0f; + out_pose->orientation.y = 0.0f; + out_pose->orientation.z = 0.0f; + out_pose->orientation.w = 1.0f; + } + + if (flags.has_position) { + out_pose->position = in_pose->position; + } else { + out_pose->position.x = 0.0f; + out_pose->position.y = 0.0f; + out_pose->position.z = 0.0f; + } +} + +static void +apply_relation(const struct xrt_space_relation *a, + const struct xrt_space_relation *b, + struct xrt_space_relation *out_relation) +{ + flags af = get_flags(a); + flags bf = get_flags(b); + + flags nf = {}; + struct xrt_pose pose = {}; + struct xrt_vec3 linear_velocity = {}; + struct xrt_vec3 angular_velocity = {}; + + + /* + * Linear velocity. + */ + + if (af.has_linear_velocity) { + nf.has_linear_velocity = true; + struct xrt_vec3 tmp = {}; + math_quat_rotate_vec3(&b->pose.orientation, // Base rotation + &a->linear_velocity, // In base space + &tmp); // Output + linear_velocity += tmp; + } + + if (bf.has_linear_velocity) { + nf.has_linear_velocity = true; + linear_velocity += b->linear_velocity; + } + + + /* + * Angular velocity. + */ + + if (af.has_angular_velocity) { + nf.has_angular_velocity = true; + struct xrt_vec3 tmp = {}; + + math_quat_rotate_derivative( + &b->pose.orientation, // Base rotation + &a->angular_velocity, // In base space + &tmp); // Output + + angular_velocity += tmp; + } + + if (bf.has_angular_velocity) { + nf.has_angular_velocity = true; + nf.has_linear_velocity = true; + angular_velocity += b->angular_velocity; + + struct xrt_vec3 rotated_position = {}; + struct xrt_vec3 position = {}; + struct xrt_quat orientation = {}; + struct xrt_vec3 tangental_velocity = {}; + + position = a->pose.position; // In the base space + orientation = b->pose.orientation; // Base space + + math_quat_rotate_vec3(&orientation, // Rotation + &position, // Vector + &rotated_position); // Result + + math_vec3_cross(&b->angular_velocity, // A + &rotated_position, // B + &tangental_velocity); // Result + + linear_velocity += tangental_velocity; + } + + + /* + * Apply the pose part. + */ + + struct xrt_pose body_pose = {}; + struct xrt_pose base_pose = {}; + make_valid_pose(af, &a->pose, &body_pose); + make_valid_pose(bf, &b->pose, &base_pose); + math_pose_transform(&base_pose, &body_pose, &pose); + + + /* + * Write everything out. + */ + + int new_flags = 0; + new_flags |= XRT_SPACE_RELATION_POSITION_VALID_BIT; + new_flags |= XRT_SPACE_RELATION_ORIENTATION_VALID_BIT; + if (af.has_tracked_position || bf.has_tracked_position) { + new_flags |= XRT_SPACE_RELATION_POSITION_TRACKED_BIT; + } + if (af.has_tracked_position || bf.has_tracked_position) { + new_flags |= XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT; + } + if (nf.has_linear_velocity) { + new_flags |= XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT; + } + if (nf.has_angular_velocity) { + new_flags |= XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT; + } + + struct xrt_space_relation tmp = {}; + tmp.relation_flags = (enum xrt_space_relation_flags)new_flags; + tmp.pose = pose; + tmp.linear_velocity = linear_velocity; + tmp.angular_velocity = angular_velocity; + + *out_relation = tmp; +} + + +/* + * + * Exported functions. + * + */ + +void +m_space_graph_resolve(const struct xrt_space_graph *xsg, + struct xrt_space_relation *out_relation) +{ + if (xsg->num_steps == 0 || has_step_with_no_pose(xsg)) { + U_ZERO(out_relation); + return; + } + + struct xrt_space_relation r = xsg->steps[0]; + for (uint32_t i = 1; i < xsg->num_steps; i++) { + apply_relation(&r, &xsg->steps[i], &r); + } + +#if 0 + dump_graph(xsg); + fprintf(stderr, "\tRR: "); + dump_relation(&r); +#else + (void)dump_graph; +#endif + + *out_relation = r; +} diff --git a/src/xrt/auxiliary/math/m_space.h b/src/xrt/auxiliary/math/m_space.h new file mode 100644 index 000000000..e32d3072a --- /dev/null +++ b/src/xrt/auxiliary/math/m_space.h @@ -0,0 +1,173 @@ +// Copyright 2020, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Functions for manipulating @ref xrt_pose, @ref xrt_space_relation and + * @ref xrt_space_graph structs. + * @author Jakob Bornecrantz + * @ingroup aux_math + */ + +#pragma once + +#include "xrt/xrt_defines.h" + +#include "math/m_api.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + +/*! + * @ingroup aux_math + * @{ + */ + +/* + * + * Pose functions. + * + */ + +static inline bool +m_pose_is_identity(struct xrt_pose *pose) +{ + struct xrt_pose p = *pose; + + if ((p.position.x == 0.0f || p.position.x == -0.0f) && // x + (p.position.y == 0.0f || p.position.y == -0.0f) && // y + (p.position.z == 0.0f || p.position.z == -0.0f) && // z + (p.orientation.x == 0.0f || p.orientation.x == -0.0f) && // x + (p.orientation.y == 0.0f || p.orientation.y == -0.0f) && // y + (p.orientation.z == 0.0f || p.orientation.z == -0.0f) && // z + (p.orientation.w == 1.0f || p.orientation.w == -1.0f) // w + ) { + return true; + } + + return false; +} + + +/* + * + * Space relation functions. + * + */ + +static inline void +m_space_relation_from_pose(const struct xrt_pose *pose, + struct xrt_space_relation *out_relation) +{ + enum xrt_space_relation_flags flags = (enum xrt_space_relation_flags)( + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | + XRT_SPACE_RELATION_POSITION_VALID_BIT); + struct xrt_space_relation relation = { + flags, *pose, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, + }; + + *out_relation = relation; +} + +static inline void +m_space_relation_ident(struct xrt_space_relation *out_relation) +{ + struct xrt_pose identity = { + {0, 0, 0, 1}, + {0, 0, 0}, + }; + + m_space_relation_from_pose(&identity, out_relation); +} + + +/* + * + * Space graph functions. + * + */ + +/*! + * Reserve a step in the graph and return a pointer to the relation. + */ +static inline struct xrt_space_relation * +m_space_graph_reserve(struct xrt_space_graph *xsg) +{ + if (xsg->num_steps < XRT_SPACE_GRAPHS_MAX) { + return &xsg->steps[xsg->num_steps++]; + } else { + return NULL; + } +} + +/*! + * Flattens a space graph into a single relation. + */ +static inline void +m_space_graph_add_relation(struct xrt_space_graph *xsg, + const struct xrt_space_relation *relation) +{ + if (xsg->num_steps >= XRT_SPACE_GRAPHS_MAX) { + return; + } + + if (relation->relation_flags == 0) { + return; + } + + xsg->steps[xsg->num_steps++] = *relation; +} + +static inline void +m_space_graph_add_pose(struct xrt_space_graph *xsg, const struct xrt_pose *pose) +{ + struct xrt_space_relation relation; + m_space_relation_from_pose(pose, &relation); + m_space_graph_add_relation(xsg, &relation); +} + +static inline void +m_space_graph_add_pose_if_not_identity(struct xrt_space_graph *xsg, + const struct xrt_pose *pose) +{ + struct xrt_pose p = *pose; + + if (m_pose_is_identity(&p)) { + return; + } + + m_space_graph_add_pose(xsg, &p); +} + +static inline void +m_space_graph_add_inverted_pose_if_not_identity(struct xrt_space_graph *xsg, + const struct xrt_pose *pose) +{ + struct xrt_pose p = *pose; + + if (m_pose_is_identity(&p)) { + return; + } + + struct xrt_pose invert; + math_pose_invert(&p, &invert); + m_space_graph_add_pose(xsg, &invert); +} + +/*! + * Flattens a space graph into a single relation. + */ +void +m_space_graph_resolve(const struct xrt_space_graph *xsg, + struct xrt_space_relation *out_relation); + +/*! + * @} + */ + + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/auxiliary/meson.build b/src/xrt/auxiliary/meson.build index 3bda47cc0..146e39653 100644 --- a/src/xrt/auxiliary/meson.build +++ b/src/xrt/auxiliary/meson.build @@ -125,6 +125,8 @@ lib_aux_math = static_library( 'math/m_permutation.c', 'math/m_permutation.h', 'math/m_quatexpmap.cpp', + 'math/m_space.cpp', + 'math/m_space.h', 'math/m_vec2.h', 'math/m_vec3.h', ),