/*!
  \file
  \brief WiiJoystick を用いた補助関数

  \author Satofumi KAMIMURA

  $Id: WiiUtils.cpp 774 2009-05-05 08:02:57Z satofumi $

  \todo namespace の使い方を見直す
*/

#include "WiiUtils.h"
#include "Angle.h"
#include "MathUtils.h"

using namespace qrk;


void qrk::estimateWiiPose(Angle& vertical_direction, Angle& rotation,
                          const Point3d<double>& acceleration)
{
  // 描画情報の更新
  double length = sqrt((acceleration.x * acceleration.x) +
                       (acceleration.z * acceleration.z));
  double x_rad = atan2(-acceleration.y, length);
  double z_rad = atan2(acceleration.z, acceleration.x) - (M_PI / 2.0);

  vertical_direction = rad(x_rad);
  rotation = rad(z_rad);
}
