6 vector_sub(Vector *res, Vector *a, Vector *b) {
13 vector_add(Vector *res, Vector *a, Vector *b) {
20 vector_vmul(Vector *res, Vector *a, Vector *b) {
21 res->x = a->y * b->z - b->y * a->z;
22 res->y = a->z * b->x - b->z * a->x;
23 res->z = a->x * b->y - b->x * a->y;
27 vector_smul(Vector *a, Vector *b) {
36 vector_length(Vector *v) {
37 return sqrt( v->x*v->x + v->y*v->y + v->z*v->z );
41 vector_mul(Vector *res, Vector *a, double b) {
48 quater_vmul(Quaternion *res, Quaternion *a, Quaternion *b) {
51 vector_vmul(&aXb, &(a->d), &(b->d));
52 vector_mul(&wb, &(b->d), a->w);
53 vector_mul(&Wa, &(a->d), b->w);
55 res->d.x = aXb.x + wb.x + Wa.x;
56 res->d.y = aXb.y + wb.y + Wa.y;
57 res->d.z = aXb.z + wb.z + Wa.z;
60 res->w = a->w * b->w - vector_smul( &(a->d), &(b->d) );
64 quater_length(Quaternion *a) {
65 return sqrt(a->d.x*a->d.x + a->d.y*a->d.y + a->d.z*a->d.z + a->w * a->w);
69 quater_inverse(Quaternion *res, Quaternion *a) {
70 double norm = a->d.x*a->d.x + a->d.y*a->d.y + a->d.z*a->d.z + a->w * a->w;
71 res->d.x = -a->d.x/norm;
72 res->d.y = -a->d.y/norm;
73 res->d.z = -a->d.z/norm;
78 rotate(Vector *res, Vector *v, Quaternion *q) {
79 Quaternion V, qV, Q, R;
81 memcpy( &(V.d), v, sizeof(Vector) );
84 quater_vmul(&qV, q, &V);
85 quater_inverse(&Q, q);
86 quater_vmul(&R, &qV, &Q);
88 memcpy( res, &(R.d), sizeof(Vector) );
92 quater_normalize(Quaternion *q) {
95 mag = sqrt(q->d.x*q->d.x + q->d.y*q->d.y + q->d.z*q->d.z + q->w*q->w);
103 vector_normal(Vector *v) {
104 vector_mul( v, v, 1.0/ vector_length(v) );
108 quater_to_matrix( Quaternion *q, double m[3][3] ) {
109 double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;
110 double s = 2.0/(q->d.x*q->d.x + q->d.y*q->d.y + q->d.z*q->d.z + q->w*q->w); // 4 mul 3 add 1 div
111 x2 = q->d.x * s; y2 = q->d.y * s; z2 = q->d.z * s;
112 xx = q->d.x * x2; xy = q->d.x * y2; xz = q->d.x * z2;
113 yy = q->d.y * y2; yz = q->d.y * z2; zz = q->d.z * z2;
114 wx = q->w * x2; wy = q->w * y2; wz = q->w * z2;
116 m[0][0] = 1.0 - (yy + zz);
121 m[1][1] = 1.0 - (xx + zz);
126 m[2][2] = 1.0 - (xx + yy);
130 matrix_to_quater( double m[3][3], Quaternion *q ) {
131 double tr = m[0][0] + m[1][1] + m[2][2]; // trace of martix
132 if (tr > 0.0){ // if trace positive than "w" is biggest component
133 q->d.x = m[1][2] - m[2][1];
134 q->d.y = m[2][0] - m[0][2];
135 q->d.z = m[0][1] - m[1][0];
137 }else if( (m[0][0] > m[1][1] ) && ( m[0][0] > m[2][2]) ) {
138 q->d.x = 1.0 + m[0][0] - m[1][1] - m[2][2];
139 q->d.y = m[1][0] + m[0][1];
140 q->d.z = m[2][0] + m[0][2];
141 q->w = m[1][2] - m[2][1];
142 } else if ( m[1][1] > m[2][2] ){
143 q->d.x = m[1][0] + m[0][1];
144 q->d.y = 1.0 + m[1][1] - m[0][0] - m[2][2];
145 q->d.z = m[2][1] + m[1][2];
146 q->w = m[2][0] - m[0][2];
148 q->d.x = m[2][0] + m[0][2];
149 q->d.y = m[2][1] + m[1][2];
150 q->d.z = 1.0 + m[2][2] - m[0][0] - m[1][1];
151 q->w = m[0][1] - m[1][0];
157 quater_to_angles( Quaternion *q, Vector *a) {
161 quater_to_matrix(q, m);
163 D = a->y = asin(m[2][0]);
166 if ( fabs(C) > 1e-8 ) {
169 a->x = atan2( try, trx );
173 a->z = atan2( try, trx );
179 a->z = atan2( try, trx );