int
psmove_calibration_save(PSMoveCalibration *calibration)
{
psmove_return_val_if_fail(calibration != NULL, 0);
FILE *fp = psmove_file_open(calibration->filename, "wb");
if (fp == NULL) {
psmove_CRITICAL("Unable to write USB calibration");
return 0;
}
if (fwrite(calibration->usb_calibration,
sizeof(calibration->usb_calibration),
1, fp) != 1) {
psmove_CRITICAL("Unable to write USB calibration");
psmove_file_close(fp);
return 0;
}
if (fwrite(&(calibration->flags),
sizeof(calibration->flags),
1, fp) != 1) {
psmove_CRITICAL("Unable to write USB calibration");
psmove_file_close(fp);
return 0;
}
psmove_file_close(fp);
return 1;
}
int
psmove_calibration_supported(PSMoveCalibration *calibration)
{
psmove_return_val_if_fail(calibration != NULL, 0);
return (calibration->flags & CalibrationFlag_HaveUSB) != 0;
}
int
psmove_calibration_load(PSMoveCalibration *calibration)
{
psmove_return_val_if_fail(calibration != NULL, 0);
FILE *fp = psmove_file_open(calibration->filename, "rb");
if (fp == NULL) {
// use system file in case local is not available
fp = psmove_file_open(calibration->system_filename, "rb");
if (fp == NULL) {
return 0;
}
}
if (fread(calibration->usb_calibration,
sizeof(calibration->usb_calibration), 1, fp) != 1) {
psmove_CRITICAL("Unable to read USB calibration");
psmove_file_close(fp);
return 0;
}
if (fread(&(calibration->flags),
sizeof(calibration->flags), 1, fp) != 1) {
psmove_CRITICAL("Unable to read USB calibration");
psmove_file_close(fp);
return 0;
}
psmove_file_close(fp);
return 1;
}
float *
psmove_fusion_get_projection_matrix(PSMoveFusion *fusion)
{
psmove_return_val_if_fail(fusion != NULL, NULL);
return glm::value_ptr(fusion->projection);
}
PSMoveOrientation *
psmove_orientation_new(PSMove *move)
{
psmove_return_val_if_fail(move != NULL, NULL);
if (!psmove_has_calibration(move)) {
psmove_DEBUG("Can't create orientation - no calibration!\n");
return NULL;
}
PSMoveOrientation *orientation = calloc(1, sizeof(PSMoveOrientation));
orientation->move = move;
/* Initial sampling frequency */
orientation->sample_freq = 120.0f;
/* Measurement */
orientation->sample_freq_measure_start = psmove_util_get_ticks();
orientation->sample_freq_measure_count = 0;
/* Initial quaternion */
orientation->quaternion[0] = 1.f;
orientation->quaternion[1] = 0.f;
orientation->quaternion[2] = 0.f;
orientation->quaternion[3] = 0.f;
return orientation;
}
PSMove_3AxisVector
psmove_orientation_get_accelerometer_normalized_vector(PSMoveOrientation *orientation_state, enum PSMove_Frame frame)
{
psmove_return_val_if_fail(orientation_state != NULL, *k_psmove_vector_zero);
PSMove_3AxisVector a= psmove_orientation_get_accelerometer_vector(orientation_state, frame);
// Normalize the accelerometer vector
psmove_3axisvector_normalize_with_default(&a, k_psmove_vector_zero);
return a;
}
PSMove_3AxisVector
psmove_orientation_get_magnetometer_normalized_vector(PSMoveOrientation *orientation_state)
{
psmove_return_val_if_fail(orientation_state != NULL, *k_psmove_vector_zero);
PSMove_3AxisVector m;
psmove_get_magnetometer_3axisvector(orientation_state->move, &m);
m= psmove_3axisvector_apply_transform(&m, &orientation_state->sensor_transform);
// Make sure we always return a normalized direction
psmove_3axisvector_normalize_with_default(&m, k_psmove_vector_zero);
return m;
}
PSMove_3AxisVector
psmove_orientation_get_gyroscope_vector(PSMoveOrientation *orientation_state, enum PSMove_Frame frame)
{
psmove_return_val_if_fail(orientation_state != NULL, *k_psmove_vector_zero);
float omega_x, omega_y, omega_z;
psmove_get_gyroscope_frame(orientation_state->move, frame, &omega_x, &omega_y, &omega_z);
// Apply the "identity pose" transform
PSMove_3AxisVector omega = psmove_3axisvector_xyz(omega_x, omega_y, omega_z);
omega= psmove_3axisvector_apply_transform(&omega, &orientation_state->sensor_transform);
return omega;
}
PSMove_3AxisVector
psmove_orientation_get_accelerometer_vector(PSMoveOrientation *orientation_state, enum PSMove_Frame frame)
{
psmove_return_val_if_fail(orientation_state != NULL, *k_psmove_vector_zero);
float ax, ay, az;
psmove_get_accelerometer_frame(orientation_state->move, frame, &ax, &ay, &az);
// Apply the "identity pose" transform
PSMove_3AxisVector a = psmove_3axisvector_xyz(ax, ay, az);
a= psmove_3axisvector_apply_transform(&a, &orientation_state->sensor_transform);
return a;
}
float *
psmove_fusion_get_modelview_matrix(PSMoveFusion *fusion, PSMove *move)
{
psmove_return_val_if_fail(fusion != NULL, NULL);
psmove_return_val_if_fail(move != NULL, NULL);
float q0, q1, q2, q3;
psmove_get_orientation(move, &q0, &q1, &q2, &q3);
if (psmove_tracker_get_mirror(fusion->tracker)) {
/* Need to invert these two axes if mirroring is enabled */
q3 *= -1.;
q2 *= -1.;
}
glm::quat quaternion(q3, q2, q1, q0);
float x, y, z;
psmove_fusion_get_position(fusion, move, &x, &y, &z);
fusion->modelview = glm::translate(glm::mat4(),
glm::vec3(x, y, z)) * glm::mat4_cast(quaternion);
return glm::value_ptr(fusion->modelview);
}
PSMove_3AxisVector
psmove_orientation_get_magnetometer_calibration_direction(PSMoveOrientation *orientation_state)
{
psmove_return_val_if_fail(orientation_state != NULL, *k_psmove_vector_zero);
PSMove_3AxisVector identity_m;
psmove_get_identity_magnetometer_calibration_direction(orientation_state->move, &identity_m);
// First apply the calibration data transform.
// This allows us to pretend the "identity pose" was some other orientation the vertical during calibration
identity_m= psmove_3axisvector_apply_transform(&identity_m, &orientation_state->calibration_transform);
// Next apply the sensor data transform.
// This allows us to pretend the sensors are in some other coordinate system (like OpenGL where +Y is up)
identity_m= psmove_3axisvector_apply_transform(&identity_m, &orientation_state->sensor_transform);
return identity_m;
}
//-- public methods -----
PSMoveOrientation *
psmove_orientation_new(PSMove *move)
{
psmove_return_val_if_fail(move != NULL, NULL);
if (!psmove_has_calibration(move)) {
psmove_DEBUG("Can't create orientation - no calibration!\n");
return NULL;
}
PSMoveOrientation *orientation_state = (PSMoveOrientation *)calloc(1, sizeof(PSMoveOrientation));
orientation_state->move = move;
/* Initial sampling frequency */
orientation_state->sample_freq = SAMPLE_FREQUENCY;
/* Measurement */
orientation_state->sample_freq_measure_start = psmove_util_get_ticks();
orientation_state->sample_freq_measure_count = 0;
/* Initial quaternion */
orientation_state->quaternion.setIdentity();
orientation_state->reset_quaternion.setIdentity();
/* Initialize data specific to the selected filter */
psmove_orientation_set_fusion_type(orientation_state, OrientationFusion_ComplementaryMARG);
/* Set the transform used re-orient the calibration data used by the orientation fusion algorithm */
psmove_orientation_set_calibration_transform(orientation_state, k_psmove_identity_pose_laying_flat);
/* Set the transform used re-orient the sensor data used by the orientation fusion algorithm */
psmove_orientation_set_sensor_data_transform(orientation_state, k_psmove_sensor_transform_opengl);
return orientation_state;
}
