Commit e4a1285e by Ryan Pavlik

### wip

parent f6172677
Pipeline #70457 failed with stages
in 1 minute and 8 seconds
 ... ... @@ -96,6 +96,40 @@ void math_vec3_accum(const struct xrt_vec3 *additional, struct xrt_vec3 *inAndOut); /*! * Get the length (L2 norm) of the vector. * * Involves a square-root, so if you can survive using the squared norm, please * do. * * @see math_vec3_squared_norm * * @relates xrt_vec3 * @ingroup aux_math */ float math_vec3_norm(const struct xrt_vec3 *vec); /*! * Get the square of the length (L2 norm) of the vector. * * @see math_vec3_norm * * @relates xrt_vec3 * @ingroup aux_math */ float math_vec3_squared_norm(const struct xrt_vec3 *vec); /*! * Normalize the vector so the length is 1. * * @relates xrt_vec3 * @ingroup aux_math */ void math_vec3_normalize(struct xrt_vec3 *vec); /* * * Quat functions. ... ... @@ -180,6 +214,31 @@ math_quat_finite_difference(const struct xrt_quat *quat0, const float dt, struct xrt_vec3 *out_ang_vel); /*! * Create the rotation to rotate a to b. * * @relates xrt_quat * @relatesalso xrt_vec3 * @ingroup aux_math */ void math_quat_from_two_vecs(const struct xrt_vec3 *a, const struct xrt_vec3 *b, struct xrt_quat *out); /*! * Perform slerp (spherical linear interpolation) between two quats. * * @relates xrt_quat * @ingroup aux_math */ void math_quat_slerp(const struct xrt_quat *a, const struct xrt_quat *b, const float alpha, struct xrt_quat *out); /* * * Pose functions. ... ...
 ... ... @@ -73,6 +73,27 @@ math_vec3_accum(const struct xrt_vec3* additional, struct xrt_vec3* inAndOut) map_vec3(*inAndOut) += map_vec3(*additional); } extern "C" float math_vec3_norm(const struct xrt_vec3* vec) { assert(vec != NULL); return map_vec3(*vec).norm(); } extern "C" float math_vec3_squared_norm(const struct xrt_vec3* vec) { assert(vec != NULL); return map_vec3(*vec).squaredNorm(); } extern "C" void math_vec3_normalize(struct xrt_vec3* vec) { assert(vec != NULL); map_vec3(*vec).normalize(); } /* * ... ... @@ -144,6 +165,33 @@ math_quat_rotate_vec3(const struct xrt_quat* left, } extern "C" void math_quat_from_two_vecs(const struct xrt_vec3* a, const struct xrt_vec3* b, struct xrt_quat* out) { assert(a != NULL); assert(b != NULL); assert(out != NULL); Eigen::Quaternionf result = Eigen::Quaternionf::FromTwoVectors(map_vec3(*a), map_vec3(*b)); map_quat(*out) = result; } extern "C" void math_quat_slerp(const struct xrt_quat* a, const struct xrt_quat* b, const float t, struct xrt_quat* out) { assert(a != NULL); assert(b != NULL); assert(out != NULL); Eigen::Quaternionf result = map_quat(*a).slerp(t, map_quat(*b)); map_quat(*out) = result; } /* * * Exported pose functions. ... ...
 ... ... @@ -246,6 +246,9 @@ lgr100_parse_input(struct lgr100_device *lgd, return LGR100_IRQ_NULL; } static const float G = 9.8f; static const float ACCEL_RESTORE_RATE = 0.9f; static void update_fusion(struct lgr100_device *lgd, struct lgr100_parsed_input *sample, ... ... @@ -256,8 +259,40 @@ update_fusion(struct lgr100_device *lgd, lgd->read.accel = sample->accel; lgd->read.gyro = sample->gyro; math_quat_integrate_velocity(&lgd->fusion.rot, &lgd->read.gyro, dt, struct xrt_quat rot = lgd->fusion.rot; struct xrt_quat inverse = {-rot.x, -rot.y, -rot.z, rot.w}; struct xrt_vec3 xformed_vel; math_quat_rotate_vec3(&inverse, &(sample->gyro), &xformed_vel); math_quat_integrate_velocity(&rot, &lgd->read.gyro, dt, &lgd->fusion.rot); float diff_of_squares = fabsf(G * G - math_vec3_squared_norm(&(sample->accel))); // If gravity is exact, this is 1. it gets smaller the further away from // gravity magnitude. float goodness = 1.f - diff_of_squares; if (goodness > 0.f) { float scale = ACCEL_RESTORE_RATE * goodness; struct xrt_vec3 normalized = sample->accel; math_vec3_normalize(&normalized); struct xrt_vec3 transformed_gravity; math_quat_rotate_vec3(&inverse, &normalized, &transformed_gravity); struct xrt_vec3 zero = {0.f, 0.f, 0.f}; struct xrt_quat grav_adjust; math_quat_from_two_vecs(&transformed_gravity, &zero, &grav_adjust); struct xrt_quat identity = {0.f, 0.f, 0.f, 1.f}; struct xrt_quat scaled_rotate; math_quat_slerp(&identity, &grav_adjust, goodness * dt, &scaled_rotate); math_quat_rotate(&scaled_rotate, &lgd->fusion.rot, &lgd->fusion.rot); } } static void * ... ... @@ -328,6 +363,7 @@ teardown(struct lgr100_device *lgd) lgd->dev = NULL; } } /* * * xrt_device functions. ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!