#include"libps2.h"
#include<math.h>
#include"mytype.h"

/*---------------------------
  クォータニオンを求める
  axis  : 回転軸方向の単位ベクトル
  angle : 回転量
  ---------------------------*/
void
quotanion(FVECTOR q, FVECTOR axis, float angle)
{
      float s=sin(angle/2);
      q[0] = axis[0]*s;
      q[1] = axis[1]*s;
      q[2] = axis[2]*s;
      q[3] = cos(angle/2);
}

/*-----------------------
  クォータニオンqから
  変換行列rotを生成する
  -----------------------*/
void
quotanion_rotmatrix(FMATRIX rot, FVECTOR q)
{
    rot[0][0] = q[3]*q[3] + q[0]*q[0] - q[1]*q[1] - q[2]*q[2];
    rot[0][1] = 2*(q[0]*q[1] - q[3]*q[2]);
    rot[0][2] = 2*(q[0]*q[2] + q[3]*q[1]);
    rot[0][3] = 0;
    rot[1][0] = 2*(q[0]*q[1] + q[3]*q[2]);
    rot[1][1] = q[3]*q[3] - q[0]*q[0] + q[1]*q[1] - q[2]*q[2];
    rot[1][2] = 2*(q[1]*q[2] - q[3]*q[0]);
    rot[1][3] = 0;
    rot[2][0] = 2*(q[0]*q[2] - q[3]*q[1]);
    rot[2][1] = 2*(q[1]*q[2] + q[3]*q[0]);
    rot[2][2] = q[3]*q[3] - q[0]*q[0] - q[1]*q[1] + q[2]*q[2];
    rot[2][3] = 0;
    rot[3][0] = 0;
    rot[3][1] = 0;
    rot[3][2] = 0;
    rot[3][3] = 1;
  
}

float
degree2radian(float digree)
{
    float radian = digree * M_PI/180.0;
    radian += (radian < -M_PI) ? 2*M_PI : 0;
    radian += (radian > M_PI) ? -2*M_PI : 0;
    return radian;
}