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; }