|
0
|
1 /*
|
|
|
2 * $Id$
|
|
|
3 */
|
|
|
4 #include<libps2.h>
|
|
|
5 #include<math.h>
|
|
|
6 #include"mytype.h"
|
|
|
7
|
|
|
8 /*---------------------------
|
|
|
9 $B%/%)!<%?%K%*%s$r5a$a$k(B
|
|
|
10 axis : $B2sE><4J}8~$NC10L%Y%/%H%k(B
|
|
|
11 angle : $B2sE>NL(B
|
|
|
12 ---------------------------*/
|
|
|
13 void
|
|
|
14 quotanion(FVECTOR q, FVECTOR axis, float angle)
|
|
|
15 {
|
|
|
16 float s=sin(angle/2);
|
|
|
17 q[0] = axis[0]*s;
|
|
|
18 q[1] = axis[1]*s;
|
|
|
19 q[2] = axis[2]*s;
|
|
|
20 q[3] = cos(angle/2);
|
|
|
21 }
|
|
|
22
|
|
|
23 /*-----------------------
|
|
|
24 $B%/%)!<%?%K%*%s(Bq$B$+$i(B
|
|
|
25 $BJQ499TNs(Brot$B$r@8@.$9$k(B
|
|
|
26 -----------------------*/
|
|
|
27 void
|
|
|
28 quotanion_rotmatrix(FMATRIX rot, FVECTOR q)
|
|
|
29 {
|
|
|
30 rot[0][0] = q[3]*q[3] + q[0]*q[0] - q[1]*q[1] - q[2]*q[2];
|
|
|
31 rot[0][1] = 2*(q[0]*q[1] - q[3]*q[2]);
|
|
|
32 rot[0][2] = 2*(q[0]*q[2] + q[3]*q[1]);
|
|
|
33 rot[0][3] = 0;
|
|
|
34 rot[1][0] = 2*(q[0]*q[1] + q[3]*q[2]);
|
|
|
35 rot[1][1] = q[3]*q[3] - q[0]*q[0] + q[1]*q[1] - q[2]*q[2];
|
|
|
36 rot[1][2] = 2*(q[1]*q[2] - q[3]*q[0]);
|
|
|
37 rot[1][3] = 0;
|
|
|
38 rot[2][0] = 2*(q[0]*q[2] - q[3]*q[1]);
|
|
|
39 rot[2][1] = 2*(q[1]*q[2] + q[3]*q[0]);
|
|
|
40 rot[2][2] = q[3]*q[3] - q[0]*q[0] - q[1]*q[1] + q[2]*q[2];
|
|
|
41 rot[2][3] = 0;
|
|
|
42 rot[3][0] = 0;
|
|
|
43 rot[3][1] = 0;
|
|
|
44 rot[3][2] = 0;
|
|
|
45 rot[3][3] = 1;
|
|
|
46 }
|
|
|
47
|
|
|
48 float
|
|
|
49 degree2radian(float digree)
|
|
|
50 {
|
|
|
51 float radian = digree * M_PI/180.0;
|
|
|
52 radian += (radian < -M_PI) ? 2*M_PI : 0;
|
|
|
53 radian += (radian > M_PI) ? -2*M_PI : 0;
|
|
|
54 return radian;
|
|
|
55 }
|
