void
psmove_fusion_get_position(PSMoveFusion *fusion, PSMove *move,
float *x, float *y, float *z)
{
psmove_return_if_fail(fusion != NULL);
psmove_return_if_fail(move != NULL);
float camX, camY, camR;
psmove_tracker_get_position(fusion->tracker, move, &camX, &camY, &camR);
float winX = (float)camX;
float winY = fusion->height - (float)camY;
float winZ = .5; /* start value for binary search */
float targetWidth = 2.*camR;
glm::vec3 obj;
/* Binary search for the best distance based on the current projection */
float step = .25;
while (step > PSMOVE_FUSION_STEP_EPSILON) {
/* Calculate center position of sphere */
obj = glm::unProject(glm::vec3(winX, winY, winZ),
glm::mat4(), fusion->projection, fusion->viewport);
/* Project left edge center of sphere */
glm::vec3 left = glm::project(glm::vec3(obj.x - .5, obj.y, obj.z),
glm::mat4(), fusion->projection, fusion->viewport);
/* Project right edge center of sphere */
glm::vec3 right = glm::project(glm::vec3(obj.x + .5, obj.y, obj.z),
glm::mat4(), fusion->projection, fusion->viewport);
float width = (right.x - left.x);
if (width > targetWidth) {
/* Too near */
winZ += step;
} else if (width < targetWidth) {
/* Too far away */
winZ -= step;
} else {
/* Perfect fit */
break;
}
step *= .5;
}
if (x != NULL) {
*x = obj.x;
}
if (y != NULL) {
*y = obj.y;
}
if (z != NULL) {
*z = obj.z;
}
}
void
psmove_orientation_set_sensor_data_transform(PSMoveOrientation *orientation_state, const PSMove_3AxisTransform *transform)
{
psmove_return_if_fail(orientation_state != NULL);
psmove_return_if_fail(transform != NULL);
orientation_state->sensor_transform= *transform;
}
void
psmove_orientation_free(PSMoveOrientation *orientation_state)
{
psmove_return_if_fail(orientation_state != NULL);
free(orientation_state);
}
void
psmove_orientation_get_quaternion(PSMoveOrientation *orientation_state,
float *q0, float *q1, float *q2, float *q3)
{
psmove_return_if_fail(orientation_state != NULL);
const glm::quat &reset_quaternion = orientation_state->reset_quaternion;
const glm::quat ¤t_quaternion = orientation_state->quaternion;
glm::quat result= reset_quaternion * current_quaternion;
if (q0) {
*q0 = result.w;
}
if (q1) {
*q1 = result.x;
}
if (q2) {
*q2 = result.y;
}
if (q3) {
*q3 = result.z;
}
}
void
psmove_calibration_map_accelerometer(PSMoveCalibration *calibration,
int *raw_input, float *ax, float *ay, float *az)
{
psmove_return_if_fail(calibration != NULL);
psmove_return_if_fail(raw_input != NULL);
if (ax) {
*ax = (float)raw_input[0] * calibration->ax + calibration->bx;
}
if (ay) {
*ay = (float)raw_input[1] * calibration->ay + calibration->by;
}
if (az) {
*az = (float)raw_input[2] * calibration->az + calibration->bz;
}
}
void
camera_control_set_deinterlace(CameraControl *cc,
enum PSMove_Bool enabled)
{
psmove_return_if_fail(cc != NULL);
cc->deinterlace = enabled;
}
void
psmove_calibration_free(PSMoveCalibration *calibration)
{
psmove_return_if_fail(calibration != NULL);
free(calibration->filename);
free(calibration->system_filename);
free(calibration);
}
void
psmove_calibration_map_gyroscope(PSMoveCalibration *calibration,
int *raw_input, float *gx, float *gy, float *gz)
{
psmove_return_if_fail(calibration != NULL);
psmove_return_if_fail(raw_input != NULL);
if (gx) {
*gx = (float)raw_input[0] * calibration->gx;
}
if (gy) {
*gy = (float)raw_input[1] * calibration->gy;
}
if (gz) {
*gz = (float)raw_input[2] * calibration->gz;
}
}
void
psmove_orientation_reset_quaternion(PSMoveOrientation *orientation_state)
{
psmove_return_if_fail(orientation_state != NULL);
Eigen::Quaternionf q_inverse = orientation_state->quaternion.conjugate();
psmove_quaternion_normalize_with_default(q_inverse, Eigen::Quaternionf::Identity());
orientation_state->reset_quaternion = q_inverse;
}
void
psmove_orientation_reset_quaternion(PSMoveOrientation *orientation_state)
{
psmove_return_if_fail(orientation_state != NULL);
glm::quat q_inverse = glm::conjugate(orientation_state->quaternion);
psmove_quaternion_normalize_with_default(q_inverse, *k_psmove_quaternion_identity);
orientation_state->reset_quaternion= q_inverse;
}
void
psmove_calibration_dump(PSMoveCalibration *calibration)
{
psmove_return_if_fail(calibration != NULL);
printf("File: %s\n", calibration->filename);
printf("System file: %s\n", calibration->system_filename);
printf("Flags: %x\n", calibration->flags);
if (calibration->flags & CalibrationFlag_HaveUSB) {
printf("Have USB calibration:\n");
psmove_calibration_dump_usb(calibration);
}
}
void
psmove_orientation_get_quaternion(PSMoveOrientation *orientation,
float *q0, float *q1, float *q2, float *q3)
{
psmove_return_if_fail(orientation != NULL);
/* first factor (reset quaternion) */
float a_s = orientation->reset_quaternion[0];
float a_x = orientation->reset_quaternion[1];
float a_y = orientation->reset_quaternion[2];
float a_z = orientation->reset_quaternion[3];
/* second factor (quaternion) */
float b_s = orientation->quaternion[0];
float b_x = orientation->quaternion[1];
float b_y = orientation->quaternion[2];
float b_z = orientation->quaternion[3];
/**
* Quaternion multiplication:
* http://lxr.kde.org/source/qt/src/gui/math3d/qquaternion.h#198
**/
float ww = (a_z + a_x) * (b_x + b_y);
float yy = (a_s - a_y) * (b_s + b_z);
float zz = (a_s + a_y) * (b_s - b_z);
float xx = ww + yy + zz;
float qq = .5f * (xx + (a_z - a_x) * (b_x - b_y));
/* Result */
float r_s = qq - ww + (a_z - a_y) * (b_y - b_z);
float r_x = qq - xx + (a_x + a_s) * (b_x + b_s);
float r_y = qq - yy + (a_s - a_x) * (b_y + b_z);
float r_z = qq - zz + (a_z + a_y) * (b_s - b_x);
if (q0) {
*q0 = r_s;
}
if (q1) {
*q1 = r_x;
}
if (q2) {
*q2 = r_y;
}
if (q3) {
*q3 = r_z;
}
}
void
psmove_orientation_reset_quaternion(PSMoveOrientation *orientation)
{
psmove_return_if_fail(orientation != NULL);
float q0 = orientation->quaternion[0];
float q1 = orientation->quaternion[1];
float q2 = orientation->quaternion[2];
float q3 = orientation->quaternion[3];
/**
* Normalize and conjugate in one step:
* - Normalize via the length
* - Conjugate using (scalar, x, y, z) -> (scalar, -x, -y, -z)
**/
double length = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
orientation->reset_quaternion[0] = q0 / length;
orientation->reset_quaternion[1] = -q1 / length;
orientation->reset_quaternion[2] = -q2 / length;
orientation->reset_quaternion[3] = -q3 / length;
}
void
psmove_orientation_get_quaternion(PSMoveOrientation *orientation_state,
float *q0, float *q1, float *q2, float *q3)
{
psmove_return_if_fail(orientation_state != NULL);
Eigen::Quaternionf result= orientation_state->reset_quaternion * orientation_state->quaternion;
if (q0) {
*q0 = result.w();
}
if (q1) {
*q1 = result.x();
}
if (q2) {
*q2 = result.y();
}
if (q3) {
*q3 = result.z();
}
}
void
psmove_orientation_update(PSMoveOrientation *orientation)
{
psmove_return_if_fail(orientation != NULL);
int frame;
long now = psmove_util_get_ticks();
if (now - orientation->sample_freq_measure_start >= 1000) {
float measured = ((float)orientation->sample_freq_measure_count) /
((float)(now-orientation->sample_freq_measure_start))*1000.;
psmove_DEBUG("Measured sample_freq: %f\n", measured);
orientation->sample_freq = measured;
orientation->sample_freq_measure_start = now;
orientation->sample_freq_measure_count = 0;
}
/* We get 2 measurements per call to psmove_poll() */
orientation->sample_freq_measure_count += 2;
psmove_get_magnetometer_vector(orientation->move,
&orientation->output[6],
&orientation->output[7],
&orientation->output[8]);
float q0 = orientation->quaternion[0];
float q1 = orientation->quaternion[1];
float q2 = orientation->quaternion[2];
float q3 = orientation->quaternion[3];
for (frame=0; frame<2; frame++) {
psmove_get_accelerometer_frame(orientation->move, frame,
&orientation->output[0],
&orientation->output[1],
&orientation->output[2]);
psmove_get_gyroscope_frame(orientation->move, frame,
&orientation->output[3],
&orientation->output[4],
&orientation->output[5]);
#if defined(PSMOVE_WITH_MADGWICK_AHRS)
MadgwickAHRSupdate(orientation->quaternion,
orientation->sample_freq,
0.5f,
/* Gyroscope */
orientation->output[3],
orientation->output[5],
-orientation->output[4],
/* Accelerometer */
orientation->output[0],
orientation->output[2],
-orientation->output[1],
/* Magnetometer */
orientation->output[6],
orientation->output[8],
orientation->output[7]
);
#else
psmove_DEBUG("Built without Madgwick AHRS - no orientation tracking");
return;
#endif
if (isnan(orientation->quaternion[0]) ||
isnan(orientation->quaternion[1]) ||
isnan(orientation->quaternion[2]) ||
isnan(orientation->quaternion[3])) {
psmove_DEBUG("Orientation is NaN!");
orientation->quaternion[0] = q0;
orientation->quaternion[1] = q1;
orientation->quaternion[2] = q2;
orientation->quaternion[3] = q3;
}
}
}
void
psmove_position_kalman_filter_free(PSMovePositionKalmanFilter *filter_state)
{
psmove_return_if_fail(filter_state != NULL);
free(filter_state);
}
void
psmove_orientation_update(PSMoveOrientation *orientation_state)
{
psmove_return_if_fail(orientation_state != NULL);
int frame_half;
long now = psmove_util_get_ticks();
if (now - orientation_state->sample_freq_measure_start >= 1000)
{
float measured = ((float)orientation_state->sample_freq_measure_count) /
((float)(now-orientation_state->sample_freq_measure_start))*1000.f;
psmove_DEBUG("Measured sample_freq: %f\n", measured);
orientation_state->sample_freq = measured;
orientation_state->sample_freq_measure_start = now;
orientation_state->sample_freq_measure_count = 0;
}
/* We get 2 measurements per call to psmove_poll() */
orientation_state->sample_freq_measure_count += 2;
Eigen::Quaternionf quaternion_backup = orientation_state->quaternion;
float deltaT = 1.f / fmax(orientation_state->sample_freq, SAMPLE_FREQUENCY); // time delta = 1/frequency
for (frame_half=0; frame_half<2; frame_half++)
{
switch (orientation_state->fusion_type)
{
case OrientationFusion_None:
break;
case OrientationFusion_MadgwickIMU:
{
// Get the sensor data transformed by the sensor_transform
PSMove_3AxisVector a=
psmove_orientation_get_accelerometer_normalized_vector(orientation_state, (enum PSMove_Frame)(frame_half));
PSMove_3AxisVector omega=
psmove_orientation_get_gyroscope_vector(orientation_state, (enum PSMove_Frame)(frame_half));
// Apply the filter
_psmove_orientation_fusion_imu_update(
orientation_state,
deltaT,
/* Gyroscope */
Eigen::Vector3f(omega.x, omega.y, omega.z),
/* Accelerometer */
Eigen::Vector3f(a.x, a.y, a.z));
}
break;
case OrientationFusion_MadgwickMARG:
{
PSMove_3AxisVector m = psmove_orientation_get_magnetometer_normalized_vector(orientation_state);
PSMove_3AxisVector a = psmove_orientation_get_accelerometer_normalized_vector(orientation_state, (enum PSMove_Frame)(frame_half));
PSMove_3AxisVector omega = psmove_orientation_get_gyroscope_vector(orientation_state, (enum PSMove_Frame)(frame_half));
_psmove_orientation_fusion_madgwick_marg_update(orientation_state, deltaT,
/* Gyroscope */
Eigen::Vector3f(omega.x, omega.y, omega.z),
/* Accelerometer */
Eigen::Vector3f(a.x, a.y, a.z),
/* Magnetometer */
Eigen::Vector3f(m.x, m.y, m.z));
}
break;
case OrientationFusion_ComplementaryMARG:
{
PSMove_3AxisVector m=
psmove_orientation_get_magnetometer_normalized_vector(orientation_state);
PSMove_3AxisVector a=
psmove_orientation_get_accelerometer_normalized_vector(orientation_state, (enum PSMove_Frame)(frame_half));
PSMove_3AxisVector omega=
psmove_orientation_get_gyroscope_vector(orientation_state, (enum PSMove_Frame)(frame_half));
// Apply the filter
_psmove_orientation_fusion_complementary_marg_update(
orientation_state,
deltaT,
/* Gyroscope */
Eigen::Vector3f(omega.x, omega.y, omega.z),
/* Accelerometer */
Eigen::Vector3f(a.x, a.y, a.z),
/* Magnetometer */
Eigen::Vector3f(m.x, m.y, m.z));
}
break;
}
if (!psmove_quaternion_is_valid(orientation_state->quaternion))
{
psmove_DEBUG("Orientation is NaN!");
orientation_state->quaternion = quaternion_backup;
}
}
}
