Commit e4a1285e authored by Ryan Pavlik's avatar 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!
Please register or to comment