mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-29 11:06:18 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/survive: Report linear and angular velocities from survive

Browse source
This commit is contained in:
Christoph Haag 2020-06-22 20:42:45 +02:00
parent b7ea0ce2b3
commit 2a9a14c605
1 changed files with 25 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

26
src/xrt/drivers/survive/survive_driver.c
View file

@ -219,9 +219,14 @@ _get_survive_pose(const SurviveSimpleObject *survive_object,
math_quat_rotate(&down_rot, &out_rot, &out_rot);
// just to be sure
math_quat_normalize(&out_rot);
SurviveVelocity vel;
timecode =
survive_simple_object_get_latest_velocity(survive_object, &vel);
out_relation->pose.orientation = out_rot;
/* because the quat is rotated, y and z axes are switched. */
@ -229,6 +234,13 @@ _get_survive_pose(const SurviveSimpleObject *survive_object,
out_relation->pose.position.y = pose.Pos[2];
out_relation->pose.position.z = -pose.Pos[1];
struct xrt_vec3 linear_vel = {
.x = vel.Pos[0], .y = vel.Pos[2], .z = -vel.Pos[1]};
struct xrt_vec3 angular_vel = {.x = vel.AxisAngleRot[0],
.y = vel.AxisAngleRot[2],
.z = -vel.AxisAngleRot[1]};
if (math_quat_validate(&out_rot)) {
out_relation->relation_flags |=
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
@ -240,6 +252,18 @@ _get_survive_pose(const SurviveSimpleObject *survive_object,
XRT_SPACE_RELATION_POSITION_VALID_BIT |
XRT_SPACE_RELATION_POSITION_TRACKED_BIT;
}
out_relation->linear_velocity = linear_vel;
if (math_vec3_validate(&out_relation->linear_velocity)) {
out_relation->relation_flags |=
XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT;
}
out_relation->angular_velocity = angular_vel;
if (math_vec3_validate(&out_relation->angular_velocity)) {
out_relation->relation_flags |=
XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT;
}
}
static bool