void MAIN(void) {
Quaternion lage(1, 0,0,0);
Vector3D vx(1,0,0), v;
double giroX, giroY, giroZ;
Quaternion deltaLage;
YPR ypr;
for(int i = 0; i <100; i++) { // shall be: while(1)
ypr = lage.toYPR();
v = vx.qRotate(lage);
PRINTF("------------ Step %d\n", i);
PRINTF("Currten attitude quat:");
lage.print();
PRINTF("Currten attitude v :");
v.print();
PRINTF("Currten attitude ypr :");
ypr.print();
ypr = lage.toYPRnils();
PRINTF("Currten attitude yprN:");
ypr.print();
readGyro(giroX, giroY, giroZ);
ypr = YPR(giroX, giroY, giroZ);
PRINTF("gyro-ypr :\t\t ");
ypr.print();
deltaLage = ypr.toQuaternion().normalize();
PRINTF("gyro-quat:");
deltaLage.print();
lage = deltaLage * lage; // operator* for quaternions
}
PRINTF("----------- Test END -------------------\n");
}
void MAIN(void) {
Vector3D v(1 ,2 ,3); // from vector
Vector3D w(0 , -1 , -4); // to vector
double alpha ;
Vector3D u;
alpha = v.getAngle(w); // liefert winkel in rad
u = v.cross (w).normalize();
AngleAxis u_phi(alpha, u);
Quaternion q(u_phi);
YPR ypr(q);
PRINTF("Rotation q: "); q.print();
PRINTF("Rotation ypr: "); ypr.print();
PRINTF("----------- again --------------- \n");
q = Quaternion(cos(alpha/2), u.scale(sin(alpha/2)));
ypr = YPR(q);
PRINTF("Rotation q: "); q.print();
PRINTF("Rotation ypr: "); ypr.print();
}
//=============================================================================
// 更新処理
//=============================================================================
void CWiiController::update()
{
// 前回の状態を保存
buttonStatePrev = buttonState;
accelPrev = accel;
rotSpeedPrev = rotSpeed;
rotPrev = rot;
joystickPrev = joystick;
IRPrev = IR;
motionConnectPrev = motionConnect;
// モーションセンサーが認識されたら有効化
motionConnect = wiiRemote->MotionPlusConnected();
if(motionConnect == true && motionConnectPrev ==false)
{
wiiRemote->EnableMotionPlus();
rotResetFlag = true;
}
// LED点灯
batteryLightingLED();
// wiiリモコンの状態を取得...というかリセット
// これやらないとステータスが更新されない
wiiRemote->RefreshState();
// ボタンの押下状態を保存
buttonState = wiiRemote->Button.Bits;
// ヌンチャクの押下状態を保存
buttonState += wiiRemote->Nunchuk.C * 0x0020;
buttonState += wiiRemote->Nunchuk.Z * 0x0040;
// リピートカウントの更新
for (int count = 0; count < WC_BUTTON_MAX; count++)
{
if (buttonState & BUTTON_STATE_BITS[count])
(repeatCount[count] < REPEAT_COUNT_MAX) ? repeatCount[count]++ : repeatCount[count] = REPEAT_COUNT_MAX;
else
repeatCount[count] = 0;
}
// 加速度を保存
accel = D3DXVECTOR3(wiiRemote->Acceleration.X, wiiRemote->Acceleration.Y, wiiRemote->Acceleration.Z);
// ※モーションプラスの接続状況によって処理を変更
// モーションプラス接続時の処理
if(motionConnect == true)
{
// 角速度を保存
//rotSpeed = D3DXVECTOR3(wiiRemote->MotionPlus.Speed.Pitch, wiiRemote->MotionPlus.Speed.Yaw, wiiRemote->MotionPlus.Speed.Roll);
rotSpeed = D3DXVECTOR3(wiiRemote->MotionPlus.Speed.Pitch + cutOffHZ_X, wiiRemote->MotionPlus.Speed.Yaw + cutOffHZ_Y, wiiRemote->MotionPlus.Speed.Roll + cutOffHZ_Z);
// 調整
adJustmentRotSpeed();
// 算出角度(Yaw Pitch Roll)
D3DXVECTOR3 YPR(0.0f, 0.0f, 0.0f);
// オフセット値(初期値を代入しておく)
// ※本来はキャリブレーション（水平なとこに一定時間置いてってやつ）の値を入れること
// この値はwiiリモコン赤専用の値
static D3DXVECTOR3 offset_YPR(168.0f, 168.0f, 66.0f);
// 一時変数(キャリブレーションの際に使う)
static D3DXVECTOR3 temp_YPR(0.0f, 0.0f, 0.0f);
// キャリブレーション計算を行う
if(rotSpeedCalibrationFlag == true)
{
temp_YPR += rotSpeed;
rotSpeedCalibrationCount++;
// 指定の回数サンプリングしたら
if(rotSpeedCalibrationCount >= rotSpeedCalibrationCountMax)
{
// オフセット値を算出
offset_YPR = temp_YPR / (float)rotSpeedCalibrationCountMax;
// 変数初期化
temp_YPR = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
rotSpeedCalibrationCount = 0;
rotSpeedCalibrationFlag = false;
}
}
// 角速度補正
rotSpeed -= temp_YPR;
// フレームレート計測
static DWORD time = timeGetTime();
// 1フレームあたりの時間計測
DWORD frametime = timeGetTime() - time;
time = timeGetTime();
// 角度を計算(単純積分)
// 角度 = 角速度[deg/sec] * 時間[sec]
/*if( rotSpeed.x > cutOffHZ_X || rotSpeed.x < -cutOffHZ_X )
{
YPR.x = rotSpeed.x * frametime / 1000.0f;
rot.x += YPR.x;
}
if( rotSpeed.y > cutOffHZ_Y || rotSpeed.y < -cutOffHZ_Y )
{
YPR.y = rotSpeed.y * frametime / 1000.0f;
rot.y += YPR.y;
}
if( rotSpeed.z > cutOffHZ_Z || rotSpeed.z < -cutOffHZ_Z )
{
YPR.z = rotSpeed.z * frametime / 1000.0f;
rot.z += YPR.z;
}*/
// 角度を計算(台形補間)
// 角度 = ((１つ前の角速度[deg/sec] + 今の角速度[deg/sec]) * 0.5) * 時間[sec]
// たいして補間されねーじゃん・・・
// cutOffHZを調整する必要あり
if( rotSpeed.x > cuttOffHZ || rotSpeed.x < -cuttOffHZ )
{
YPR.x = ((rotSpeed.x + rotSpeedPrev.x) * 0.5f) * (frametime / 1000.0f);
rot.x += YPR.x;
}
if( rotSpeed.y > cuttOffHZ || rotSpeed.y < -cuttOffHZ )
{
YPR.y = ((rotSpeed.y + rotSpeedPrev.y) * 0.5f) * (frametime / 1000.0f);
rot.y += YPR.y;
}
if( rotSpeed.z > cuttOffHZ || rotSpeed.z < -cuttOffHZ )
{
YPR.z = ((rotSpeed.z + rotSpeedPrev.z) * 0.5f) * (frametime / 1000.0f);
rot.z += YPR.z;
}
// 姿勢をリセット
if(rotResetFlag == true)
{
rot = D3DXVECTOR3(0.0f, 0.0f, 0.0f);
rotResetFlag = false;
}
// オーバーフロー対策
if(rot.x <= -180.0f)
rot.x = 180.0f;
if(rot.x > 180.0f)
rot.x = -180.0f;
if(rot.y <= -180.0f)
rot.y = 180.0f;
if(rot.y > 180.0f)
rot.y = -180.0f;
if(rot.z <= -180.0f)
rot.z = 180.0f;
if(rot.z > 180.0f)
rot.z = -180.0f;
}
// モーションプラス未接続時の処理
else
{
// 回転角を保存（本体のみ）
// 加速度センサーで計算しているため精度低い・・・というか間違ってるんじゃね？
rot = D3DXVECTOR3(wiiRemote->Acceleration.Orientation.X, wiiRemote->Acceleration.Orientation.Y, wiiRemote->Acceleration.Orientation.Z);
}
// IR情報の取得
IR = D3DXVECTOR2((wiiRemote->IR.Dot[0].X - 0.5f) * 2.0f, (wiiRemote->IR.Dot[0].Y - 0.5f) * 2.0f);
// ヌンチャクが接続されていたら
if(wiiRemote->NunchukConnected())
{
// 加速度と加速度から算出した角度を保存
accelN = D3DXVECTOR3(wiiRemote->Nunchuk.Acceleration.X, wiiRemote->Nunchuk.Acceleration.Y, wiiRemote->Nunchuk.Acceleration.Z);
rotN = D3DXVECTOR3(wiiRemote->Nunchuk.Acceleration.Orientation.X, wiiRemote->Nunchuk.Acceleration.Orientation.Y, wiiRemote->Nunchuk.Acceleration.Orientation.Z);
// ヌンチャクのジョイスティック情報取得
joystick = D3DXVECTOR2(wiiRemote->Nunchuk.Joystick.X, wiiRemote->Nunchuk.Joystick.Y);
// 下限と上限の調整
if((joystick.x < joyStickDeadZone) && (joystick.x > -joyStickDeadZone))
joystick.x = 0.0f;
if((joystick.y < joyStickDeadZone) && (joystick.y > -joyStickDeadZone))
joystick.y = 0.0f;
if(joystick.x > joyStickMax)
joystick.x = joyStickMax;
if(joystick.x < -joyStickMax)
joystick.x = -joyStickMax;
if(joystick.y > joyStickMax)
joystick.y = joyStickMax;
if(joystick.y < -joyStickMax)
joystick.y = -joyStickMax;
}
}