TQuaternion MakeRotationQuaternion (TVector3 s, TVector3 t){
TQuaternion res;
TVector3 u;
double cos2phi, sin2phi;
double cosphi, sinphi;
u = CrossProduct (s, t);
sin2phi = NormVector (&u);
if (sin2phi < EPS) {
res = MakeQuaternion (0., 0., 0., 1.);
} else {
cos2phi = DotProduct (s, t);
sinphi = sqrt ( (1 - cos2phi) / 2.0);
cosphi = sqrt ( (1 + cos2phi) / 2.0);
res.x = sinphi * u.x;
res.y = sinphi * u.y;
res.z = sinphi * u.z;
res.w = cosphi;
}
return res;
}
void MotionEmuThread() {
auto update_time = std::chrono::steady_clock::now();
Math::Quaternion<float> q = MakeQuaternion(Math::Vec3<float>(), 0);
Math::Quaternion<float> old_q;
while (!shutdown_event.WaitUntil(update_time)) {
update_time += update_duration;
old_q = q;
{
std::lock_guard<std::mutex> guard(tilt_mutex);
// Find the quaternion describing current 3DS tilting
q = MakeQuaternion(Math::MakeVec(-tilt_direction.y, 0.0f, tilt_direction.x),
tilt_angle);
}
auto inv_q = q.Inverse();
// Set the gravity vector in world space
auto gravity = Math::MakeVec(0.0f, -1.0f, 0.0f);
// Find the angular rate vector in world space
auto angular_rate = ((q - old_q) * inv_q).xyz * 2;
angular_rate *= 1000 / update_millisecond / MathUtil::PI * 180;
// Transform the two vectors from world space to 3DS space
gravity = QuaternionRotate(inv_q, gravity);
angular_rate = QuaternionRotate(inv_q, angular_rate);
// Update the sensor state
{
std::lock_guard<std::mutex> guard(status_mutex);
status = std::make_tuple(gravity, angular_rate);
}
}
}
