/*
 * $Id$
 */
#include<libps2.h>
#include<math.h>
#include"mytype.h"

/*---------------------------
  $B%/%)!<%?%K%*%s$r5a$a$k(B
  axis  : $B2sE><4J}8~$NC10L%Y%/%H%k(B
  angle : $B2sE>NL(B
  ---------------------------*/
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);
}

/*-----------------------
  $B%/%)!<%?%K%*%s(Bq$B$+$i(B
  $BJQ499TNs(Brot$B$r@8@.$9$k(B
  -----------------------*/
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;
}