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 ¤t_omega,
const Eigen::Vector3f ¤t_g,
const Eigen::Vector3f ¤t_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();
// Save the new quaternion back into the orientation state
orientation_state->quaternion = SEq_new;
}
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();
}
}