Commit 2c4fa013 authored by Jakob Bornecrantz's avatar Jakob Bornecrantz

aux/math: Add prediction module

parent 8da6617f
......@@ -13,6 +13,8 @@ set(MATH_SOURCE_FILES
math/m_imu_pre.c
math/m_imu_pre.h
math/m_optics.c
math/m_predict.c
math/m_predict.h
math/m_quatexpmap.cpp
math/m_vec2.h
math/m_vec3.h
......
// Copyright 2020, 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"
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)
{
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;
bool valid_angular_acceleration =
(flags & XRT_SPACE_RELATION_ANGULAR_ACCELERATION_VALID_BIT) != 0;
if (valid_angular_velocity) {
accum.x += rel->angular_velocity.x;
accum.y += rel->angular_velocity.y;
accum.z += rel->angular_velocity.z;
}
// We don't want the angular acceleration, it's way to 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
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) {
out_rel->angular_velocity = accum;
}
if (valid_angular_acceleration) {
out_rel->angular_acceleration = rel->angular_acceleration;
}
}
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)
{
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;
bool valid_linear_acceleration =
(flags & XRT_SPACE_RELATION_LINEAR_ACCELERATION_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_linear_acceleration) {
accum.x += delta_s / 2 * rel->linear_acceleration.x;
accum.y += delta_s / 2 * rel->linear_acceleration.y;
accum.z += delta_s / 2 * rel->linear_acceleration.z;
}
if (valid_position) {
out_rel->pose.position = m_vec3_add(
rel->pose.position, m_vec3_mul_scalar(accum, delta_s));
}
// We use the new linear velocity with the acceleration integrated.
if (valid_linear_velocity) {
out_rel->linear_velocity = accum;
}
if (valid_linear_acceleration) {
out_rel->linear_acceleration = rel->angular_acceleration;
}
}
void
m_predict_relation(const struct xrt_space_relation *rel,
double delta_s,
struct xrt_space_relation *out_rel)
{
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;
}
// Copyright 2020, 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
*/
#pragma once
#include "xrt/xrt_defines.h"
/*!
* Using the given @p xrt_space_relation predicts a new @p xrt_space_relation
* @p delta_s into the future.
*
* @aux_math
*/
void
m_predict_relation(const struct xrt_space_relation *rel,
double delta_s,
struct xrt_space_relation *out_rel);
......@@ -120,6 +120,8 @@ lib_aux_math = static_library(
'math/m_imu_pre.c',
'math/m_imu_pre.h',
'math/m_optics.c',
'math/m_predict.c',
'math/m_predict.h',
'math/m_quatexpmap.cpp',
'math/m_vec2.h',
'math/m_vec3.h',
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment