mirror of
https://gitlab.freedesktop.org/monado/monado.git
synced 2025-02-03 04:18:28 +00:00
116 lines
3.5 KiB
C
116 lines
3.5 KiB
C
// Copyright 2020-2021, Collabora, Ltd.
|
|
// SPDX-License-Identifier: BSL-1.0
|
|
/*!
|
|
* @file
|
|
* @brief Simple function to predict a new pose from a given pose.
|
|
* @author Jakob Bornecrantz <jakob@collabora.com>
|
|
* @ingroup aux_math
|
|
*/
|
|
|
|
#include "m_api.h"
|
|
#include "m_vec3.h"
|
|
#include "m_predict.h"
|
|
#include "util/u_trace_marker.h"
|
|
|
|
|
|
static void
|
|
do_orientation(const struct xrt_space_relation *rel,
|
|
enum xrt_space_relation_flags flags,
|
|
double delta_s,
|
|
struct xrt_space_relation *out_rel)
|
|
{
|
|
if (delta_s == 0) {
|
|
out_rel->pose.orientation = rel->pose.orientation;
|
|
out_rel->angular_velocity = rel->angular_velocity;
|
|
return;
|
|
}
|
|
|
|
struct xrt_vec3 accum = {0};
|
|
bool valid_orientation = (flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0;
|
|
bool valid_angular_velocity = (flags & XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT) != 0;
|
|
|
|
if (valid_angular_velocity) {
|
|
// angular velocity needs to be in body space for prediction
|
|
struct xrt_vec3 ang_vel_body_space;
|
|
|
|
struct xrt_quat orientation_inv;
|
|
math_quat_invert(&rel->pose.orientation, &orientation_inv);
|
|
|
|
math_quat_rotate_derivative(&orientation_inv, &rel->angular_velocity, &ang_vel_body_space);
|
|
|
|
accum.x += ang_vel_body_space.x;
|
|
accum.y += ang_vel_body_space.y;
|
|
accum.z += ang_vel_body_space.z;
|
|
}
|
|
|
|
// We don't want the angular acceleration, it's way too noisy.
|
|
#if 0
|
|
if (valid_angular_acceleration) {
|
|
accum.x += delta_s / 2 * rel->angular_acceleration.x;
|
|
accum.y += delta_s / 2 * rel->angular_acceleration.y;
|
|
accum.z += delta_s / 2 * rel->angular_acceleration.z;
|
|
}
|
|
#endif
|
|
|
|
if (valid_orientation) {
|
|
math_quat_integrate_velocity(&rel->pose.orientation, // Old orientation
|
|
&accum, // Angular velocity
|
|
(float)delta_s, // Delta in seconds
|
|
&out_rel->pose.orientation); // Result
|
|
}
|
|
|
|
// We use everything we integrated in as the new angular_velocity.
|
|
if (valid_angular_velocity) {
|
|
// angular velocity is returned in base space.
|
|
// use the predicted orientation for this calculation.
|
|
struct xrt_vec3 predicted_ang_vel_base_space;
|
|
math_quat_rotate_derivative(&out_rel->pose.orientation, &accum, &predicted_ang_vel_base_space);
|
|
|
|
out_rel->angular_velocity = predicted_ang_vel_base_space;
|
|
}
|
|
}
|
|
|
|
static void
|
|
do_position(const struct xrt_space_relation *rel,
|
|
enum xrt_space_relation_flags flags,
|
|
double delta_s,
|
|
struct xrt_space_relation *out_rel)
|
|
{
|
|
if (delta_s == 0) {
|
|
out_rel->pose.position = rel->pose.position;
|
|
out_rel->linear_velocity = rel->linear_velocity;
|
|
return;
|
|
}
|
|
|
|
struct xrt_vec3 accum = {0};
|
|
bool valid_position = (flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0;
|
|
bool valid_linear_velocity = (flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) != 0;
|
|
|
|
if (valid_linear_velocity) {
|
|
accum.x += rel->linear_velocity.x;
|
|
accum.y += rel->linear_velocity.y;
|
|
accum.z += rel->linear_velocity.z;
|
|
}
|
|
|
|
if (valid_position) {
|
|
out_rel->pose.position = m_vec3_add(rel->pose.position, m_vec3_mul_scalar(accum, (float)delta_s));
|
|
}
|
|
|
|
// We use the new linear velocity with the acceleration integrated.
|
|
if (valid_linear_velocity) {
|
|
out_rel->linear_velocity = accum;
|
|
}
|
|
}
|
|
|
|
void
|
|
m_predict_relation(const struct xrt_space_relation *rel, double delta_s, struct xrt_space_relation *out_rel)
|
|
{
|
|
XRT_TRACE_MARKER();
|
|
enum xrt_space_relation_flags flags = rel->relation_flags;
|
|
|
|
do_orientation(rel, flags, delta_s, out_rel);
|
|
do_position(rel, flags, delta_s, out_rel);
|
|
|
|
out_rel->relation_flags = flags;
|
|
}
|