C++ Vector3f::isZero方法代码示例

void GLKinectFrame::setFrame(KinectFrame* frame)
{
	try
	{
		const int color_map_width = 1920;
		const int color_map_height = 1080;
		const int depth_map_width = 512;
		const int depth_map_height = 424;
		const float fovy = 70.0f;
		const float aspect_ratio = static_cast<float>(depth_map_width) / static_cast<float>(depth_map_height);
		const float near_plane = 0.1f;
		const float far_plane = 10240.0f;

		std::vector<Eigen::Vector3f> vertices;
		std::vector<Eigen::Vector3f> normals;
		std::vector<Eigen::Vector3f> colors;

		const float depth_to_color_width = color_map_width / depth_map_width;
		const float depth_to_color_height = color_map_height / depth_map_height;

		for (int x = 1; x < depth_map_width - 1; ++x)
		{
			for (int y = 1; y < depth_map_height - 1; ++y)
			{
				const float depth = static_cast<float>(frame->depth[y * depth_map_width + x]) / 100.f;
				const Eigen::Vector3f vert_uv = window_coord_to_3d(Eigen::Vector2f(x, y), depth, fovy, aspect_ratio, near_plane, far_plane, depth_map_width, depth_map_height);
				const Eigen::Vector3f vert_u1v = window_coord_to_3d(Eigen::Vector2f(x + 1, y), depth, fovy, aspect_ratio, near_plane, far_plane, depth_map_width, depth_map_height);
				const Eigen::Vector3f vert_uv1 = window_coord_to_3d(Eigen::Vector2f(x, y + 1), depth, fovy, aspect_ratio, near_plane, far_plane, depth_map_width, depth_map_height);

				float x_color = x * depth_to_color_width;
				float y_color = y * depth_to_color_height;

				if (!vert_uv.isZero() && !vert_u1v.isZero() && !vert_uv1.isZero())
				{
					const Eigen::Vector3f n1 = vert_u1v - vert_uv;
					const Eigen::Vector3f n2 = vert_uv1 - vert_uv;
					const Eigen::Vector3f n = n1.cross(n2).normalized();

					vertices.push_back(vert_uv);
					normals.push_back(n);

					const uchar r = static_cast<uchar>(frame->color[4 * y_color * color_map_width + x_color + 0]);
					const uchar g = static_cast<uchar>(frame->color[4 * y_color * color_map_width + x_color + 1]);
					const uchar b = static_cast<uchar>(frame->color[4 * y_color * color_map_width + x_color + 2]);

					//colors.push_back((n * 0.5f + Eigen::Vector3f(0.5, 0.5, 0.5)) * 255.0f);
					//colors.push_back(Eigen::Vector3f(0, 1, 0));

					colors.push_back(Eigen::Vector3f(static_cast<float>(r) / 255.f, static_cast<float>(g) / 255.f, static_cast<float>(b) / 255.f));
				}
			}
		}

		vertexBuf.bind();
		vertexBuf.allocate(&vertices[0][0], vertices.size() * sizeof(Eigen::Vector3f));

		colorBuf.bind();
		colorBuf.allocate(&colors[0][0], colors.size() * sizeof(Eigen::Vector3f));

		normalBuf.bind();
		normalBuf.allocate(&normals[0][0], normals.size() * sizeof(Eigen::Vector3f));
	}
	catch (const std::exception& ex)
	{
		std::cerr << "Error: " << ex.what() << std::endl;
	}
}

// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper:
// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays"
// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf
static void 
_psmove_orientation_fusion_madgwick_marg_update(
	PSMoveOrientation *orientation_state,
	float delta_t,
	const Eigen::Vector3f &current_omega,
	const Eigen::Vector3f &current_g,
	const Eigen::Vector3f &current_m)
{
	// If there isn't a valid magnetometer or accelerometer vector, fall back to the IMU style update
	if (current_g.isZero(k_normal_epsilon) || current_m.isZero(k_normal_epsilon))
	{
		_psmove_orientation_fusion_imu_update(
			orientation_state,
			delta_t,
			current_omega,
			current_g);
		return;
	}

	PSMoveMadgwickMARGState *marg_state = &orientation_state->fusion_state.madgwick_marg_state;

	// Current orientation from earth frame to sensor frame
	Eigen::Quaternionf SEq = orientation_state->quaternion;

	// Get the direction of the magnetic fields in the identity pose.	
	// NOTE: In the original paper we converge on this vector over time automatically (See Eqn 45 & 46)
	// but since we've already done the work in calibration to get this vector, let's just use it.
	// This also removes the last assumption in this function about what 
	// the orientation of the identity-pose is (handled by the sensor transform).
	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 fields 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);

	// Eqn 15) Applied to the gravity and magnetometer vectors
	// Fill in the 6x1 objective function matrix f(SEq, Sa, Eb, Sm) =|f_g|
	//                                                               |f_b|
	Eigen::Matrix<float, 3, 1> f_g;
	psmove_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL);

	Eigen::Matrix<float, 3, 1> f_m;
	psmove_alignment_compute_objective_vector(SEq, k_identity_m_direction, current_m, f_m, NULL);

	Eigen::Matrix<float, 6, 1> f_gb;
	f_gb.block<3, 1>(0, 0) = f_g;
	f_gb.block<3, 1>(3, 0) = f_m;

	// Eqn 21) Applied to the gravity and magnetometer vectors
	// Fill in the 4x6 objective function Jacobian matrix: J_gb(SEq, Eb)= [J_g|J_b]
	Eigen::Matrix<float, 4, 3> J_g;
	psmove_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g);

	Eigen::Matrix<float, 4, 3> J_m;
	psmove_alignment_compute_objective_jacobian(SEq, k_identity_m_direction, J_m);

	Eigen::Matrix<float, 4, 6> J_gb;
	J_gb.block<4, 3>(0, 0) = J_g; J_gb.block<4, 3>(0, 3) = J_m;

	// Eqn 34) gradient_F= J_gb(SEq, Eb)*f(SEq, Sa, Eb, Sm)
	// Compute the gradient of the objective function
	Eigen::Matrix<float, 4, 1> gradient_f = J_gb*f_gb;
	Eigen::Quaternionf SEqHatDot =
		Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0));

	// normalize the gradient to estimate direction of the gyroscope error
	psmove_quaternion_normalize_with_default(SEqHatDot, *k_psmove_quaternion_zero);

	// Eqn 47) omega_err= 2*SEq*SEqHatDot
	// compute angular estimated direction of the gyroscope error
	Eigen::Quaternionf omega_err = Eigen::Quaternionf(SEq.coeffs()*2.f) * SEqHatDot;

	// Eqn 48) net_omega_bias+= zeta*omega_err
	// Compute the net accumulated gyroscope bias
	marg_state->omega_bias = Eigen::Quaternionf(marg_state->omega_bias.coeffs() + omega_err.coeffs()*zeta*delta_t);
	marg_state->omega_bias.w() = 0.f; // no bias should accumulate on the w-component

	// Eqn 49) omega_corrected = omega - net_omega_bias
	Eigen::Quaternionf omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z());
	Eigen::Quaternionf corrected_omega = Eigen::Quaternionf(omega.coeffs() - marg_state->omega_bias.coeffs());

	// Compute the rate of change of the orientation purely from the gyroscope
	// Eqn 12) q_dot = 0.5*q*omega
	Eigen::Quaternionf SEqDot_omega = Eigen::Quaternionf(SEq.coeffs() * 0.5f) * corrected_omega;

	// Compute the estimated quaternion rate of change
	// Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot
	Eigen::Quaternionf SEqDot_est = Eigen::Quaternionf(SEqDot_omega.coeffs() - SEqHatDot.coeffs()*beta);

	// Compute then integrate the estimated quaternion rate
	// Eqn 42) SEq_new = SEq + SEqDot_est*delta_t
	Eigen::Quaternionf SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_est.coeffs()*delta_t);

	// Make sure the net quaternion is a pure rotation quaternion
	SEq_new.normalize();

//.........这里部分代码省略.........

void Streamer::performPlayerUpdate(Player &player, bool automatic)
{
	Eigen::Vector3f delta = Eigen::Vector3f::Zero(), position = player.position;
	int state = GetPlayerState(player.playerID);
	bool update = true;
	if (automatic)
	{
		player.interiorID = GetPlayerInterior(player.playerID);
		player.worldID = GetPlayerVirtualWorld(player.playerID);
		GetPlayerPos(player.playerID, &player.position[0], &player.position[1], &player.position[2]);
		if (state != PLAYER_STATE_NONE && state != PLAYER_STATE_WASTED)
		{
			if (player.position != position)
			{
				position = player.position;
				Eigen::Vector3f velocity = Eigen::Vector3f::Zero();
				if (state == PLAYER_STATE_ONFOOT)
				{
					GetPlayerVelocity(player.playerID, &velocity[0], &velocity[1], &velocity[2]);
				}
				else if (state == PLAYER_STATE_DRIVER || state == PLAYER_STATE_PASSENGER)
				{
					GetVehicleVelocity(GetPlayerVehicleID(player.playerID), &velocity[0], &velocity[1], &velocity[2]);
				}
				float velocityNorm = velocity.squaredNorm();
				if (velocityNorm >= velocityBoundaries.get<0>() && velocityNorm <= velocityBoundaries.get<1>())
				{
					delta = velocity * averageUpdateTime;
					player.position += delta;
				}
			}
			else
			{
				update = player.updateWhenIdle;
			}
		}
		else
		{
			update = false;
		}
	}
	std::vector<SharedCell> cells;
	if (update)
	{
		core->getGrid()->findAllCells(player, cells);
		if (!cells.empty())
		{
			if (!core->getData()->objects.empty() && player.enabledItems[STREAMER_TYPE_OBJECT] && !IsPlayerNPC(player.playerID))
			{
				processObjects(player, cells);
			}
			if (!core->getData()->checkpoints.empty() && player.enabledItems[STREAMER_TYPE_CP])
			{
				processCheckpoints(player, cells);
			}
			if (!core->getData()->raceCheckpoints.empty() && player.enabledItems[STREAMER_TYPE_RACE_CP])
			{
				processRaceCheckpoints(player, cells);
			}
			if (!core->getData()->mapIcons.empty() && player.enabledItems[STREAMER_TYPE_MAP_ICON] && !IsPlayerNPC(player.playerID))
			{
				processMapIcons(player, cells);
			}
			if (!core->getData()->textLabels.empty() && player.enabledItems[STREAMER_TYPE_3D_TEXT_LABEL] && !IsPlayerNPC(player.playerID))
			{
				processTextLabels(player, cells);
			}
			if (!core->getData()->areas.empty() && player.enabledItems[STREAMER_TYPE_AREA])
			{
				if (!delta.isZero())
				{
					player.position = position;
				}
				processAreas(player, cells);
				if (!delta.isZero())
				{
					player.position += delta;
				}
			}
		}
	}
	if (automatic)
	{
		if (!core->getData()->pickups.empty())
		{
			if (!update)
			{
				core->getGrid()->findMinimalCells(player, cells);
			}
			processPickups(player, cells);
		}
		if (!delta.isZero())
		{
			player.position = position;
		}
		executeCallbacks();
	}
}