C++ (Cpp) psmove_quaternion_normalized_lerpの例

コード例 #1

ファイル: psmove_orientation.cpp プロジェクト: MorS25/psmoveapi

static void
_psmove_orientation_fusion_complementary_marg_update(
    PSMoveOrientation *orientation_state,
    float delta_t,
    const glm::vec3 &current_omega,
    const glm::vec3 &current_g,
    const glm::vec3 &current_m)
{
    // Get the direction of the magnetic fields in the identity pose	
    PSMove_3AxisVector identity_m= psmove_orientation_get_magnetometer_calibration_direction(orientation_state);
    glm::vec3 k_identity_m_direction = glm::vec3(identity_m.x, identity_m.y, identity_m.z);

    // Get the direction of the gravitational field in the identity pose
    PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state);
    glm::vec3 k_identity_g_direction = glm::vec3(identity_g.x, identity_g.y, identity_g.z);

    // Angular Rotation (AR) Update
    //-----------------------------
    // Compute the rate of change of the orientation purely from the gyroscope
    // q_dot = 0.5*q*omega
    glm::quat q_current= orientation_state->quaternion;

    glm::quat q_omega = glm::quat(0.f, current_omega.x, current_omega.y, current_omega.z);
    glm::quat q_derivative = (q_current*0.5f) * q_omega;

    // Integrate the rate of change to get a new orientation
    // q_new= q + q_dot*dT
    glm::quat q_step = q_derivative * delta_t;
    glm::quat ar_orientation = q_current + q_step;

    // Make sure the resulting quaternion is normalized
    ar_orientation= glm::normalize(ar_orientation);

    // Magnetic/Gravity (MG) Update
    //-----------------------------
    const glm::vec3* mg_from[2] = { &k_identity_g_direction, &k_identity_m_direction };
    const glm::vec3* mg_to[2] = { &current_g, &current_m };
    glm::quat mg_orientation;

    // Always attempt to align with the identity_mg, even if we don't get within the alignment tolerance.
    // More often then not we'll be better off moving forward with what we have and trying to refine
    // the alignment next frame.
    psmove_alignment_quaternion_between_vector_frames(
        mg_from, mg_to, 0.1f, q_current, mg_orientation);

    // Blending Update
    //----------------
    // The final rotation is a blend between the integrated orientation and absolute rotation from the earth-frame
    float mg_wight = orientation_state->fusion_state.complementary_marg_state.mg_weight;
    orientation_state->quaternion= 
        psmove_quaternion_normalized_lerp(ar_orientation, mg_orientation, mg_wight);

    // Update the blend weight
    orientation_state->fusion_state.complementary_marg_state.mg_weight =
        lerp_clampf(mg_wight, k_base_earth_frame_align_weight, 0.9f);
}

コード例 #2

ファイル: psmove_orientation.cpp プロジェクト: bagobor/psmoveapi

static void
_psmove_orientation_fusion_complementary_marg_update(
	PSMoveOrientation *orientation_state,
	float delta_t,
	const Eigen::Vector3f &current_omega,
	const Eigen::Vector3f &current_g,
	const Eigen::Vector3f &current_m)
{
    // TODO: Following variable is unused
	PSMoveMadgwickMARGState *marg_state = &orientation_state->fusion_state.madgwick_marg_state;

	// Get the direction of the magnetic fields in the identity pose	
	PSMove_3AxisVector identity_m= psmove_orientation_get_magnetometer_calibration_direction(orientation_state);
	Eigen::Vector3f k_identity_m_direction = Eigen::Vector3f(identity_m.x, identity_m.y, identity_m.z);

	// Get the direction of the gravitational field in the identity pose
	PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state);
	Eigen::Vector3f k_identity_g_direction = Eigen::Vector3f(identity_g.x, identity_g.y, identity_g.z);

	// Angular Rotation (AR) Update
	//-----------------------------
	// Compute the rate of change of the orientation purely from the gyroscope
	// q_dot = 0.5*q*omega
	Eigen::Quaternionf q_omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z());
	Eigen::Quaternionf q_derivative = Eigen::Quaternionf(orientation_state->quaternion.coeffs()*0.5f) * q_omega;

	// Integrate the rate of change to get a new orientation
	// q_new= q + q_dot*dT
	Eigen::Quaternionf q_step = Eigen::Quaternionf(q_derivative.coeffs() * delta_t);
	Eigen::Quaternionf ar_orientation = Eigen::Quaternionf(orientation_state->quaternion.coeffs() + q_step.coeffs());

	// Make sure the resulting quaternion is normalized
	ar_orientation.normalize();

	// Magnetic/Gravity (MG) Update
	//-----------------------------
	const Eigen::Vector3f* mg_from[2] = { &k_identity_g_direction, &k_identity_m_direction };
	const Eigen::Vector3f* mg_to[2] = { &current_g, &current_m };
	Eigen::Quaternionf mg_orientation;
	bool mg_align_success =
		psmove_alignment_quaternion_between_vector_frames(
			mg_from, mg_to, 0.1f, orientation_state->quaternion, mg_orientation);

	// Blending Update
	//----------------
	if (mg_align_success)
	{
		// The final rotation is a blend between the integrated orientation and absolute rotation from the earth-frame
		float mg_wight = orientation_state->fusion_state.complementary_marg_state.mg_weight;
		orientation_state->quaternion =
			psmove_quaternion_normalized_lerp(ar_orientation, mg_orientation, mg_wight);

		// Update the blend weight
		orientation_state->fusion_state.complementary_marg_state.mg_weight =
			lerp_clampf(mg_wight, k_base_earth_frame_align_weight, 0.9f);
	}
	else
	{
		orientation_state->quaternion = ar_orientation;
	}
}