From 2a9a14c6052d9bd993241985a2e0ad68428e240a Mon Sep 17 00:00:00 2001 From: Christoph Haag Date: Mon, 22 Jun 2020 20:42:45 +0200 Subject: [PATCH] d/survive: Report linear and angular velocities from survive --- src/xrt/drivers/survive/survive_driver.c | 26 +++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/src/xrt/drivers/survive/survive_driver.c b/src/xrt/drivers/survive/survive_driver.c index 13552fec3..13527659f 100644 --- a/src/xrt/drivers/survive/survive_driver.c +++ b/src/xrt/drivers/survive/survive_driver.c @@ -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