1 static void vneg (float *res
, float *v
)
8 static void vadd (float *res
, float *v1
, float *v2
)
10 res
[0] = v1
[0] + v2
[0];
11 res
[1] = v1
[1] + v2
[1];
12 res
[2] = v1
[2] + v2
[2];
15 static void qconjugate (float *res
, float *q
)
21 static void qapply (float *res
, float *q
, float *v
)
30 float t2
, t3
, t4
, t5
, t6
, t7
, t8
, t9
, t10
;
40 res
[0] = 2*( (t8
+ t10
)*v1
+ (t6
- t4
)*v2
+ (t3
+ t7
)*v3
) + v1
;
41 res
[1] = 2*( (t4
+ t6
)*v1
+ (t5
+ t10
)*v2
+ (t9
- t2
)*v3
) + v2
;
42 res
[2] = 2*( (t7
- t3
)*v1
+ (t2
+ t9
)*v2
+ (t5
+ t8
)*v3
) + v3
;
45 static void qcompose (float *res
, float *q1
, float *q2
)
47 res
[0] = q1
[3]*q2
[0] + q1
[0]*q2
[3] + q1
[1]*q2
[2] - q1
[2]*q2
[1];
48 res
[1] = q1
[3]*q2
[1] + q1
[1]*q2
[3] + q1
[2]*q2
[0] - q1
[0]*q2
[2];
49 res
[2] = q1
[3]*q2
[2] + q1
[2]*q2
[3] + q1
[0]*q2
[1] - q1
[1]*q2
[0];
50 res
[3] = q1
[3]*q2
[3] - q1
[0]*q2
[0] - q1
[1]*q2
[1] - q1
[2]*q2
[2];
54 static void q2matrixt (float *mat
, float *q
, float *v
)
73 mat
[0] = 1 - 2 * ( yy
+ zz
);
74 mat
[1] = 2 * ( xy
- zw
);
75 mat
[2] = 2 * ( xz
+ yw
);
77 mat
[4] = 2 * ( xy
+ zw
);
78 mat
[5] = 1 - 2 * ( xx
+ zz
);
79 mat
[6] = 2 * ( yz
- xw
);
81 mat
[8] = 2 * ( xz
- yw
);
82 mat
[9] = 2 * ( yz
+ xw
);
83 mat
[10] = 1 - 2 * ( xx
+ yy
);
97 static void mscale (float *res
, float *m
, float s
)
100 for (i
= 0; i
< 12; ++i
) *res
++ = *m
++ * s
;
102 static void mapply_to_point (float *res
, float *m
, float *v
)
107 res
[0] = x
*m
[0] + y
*m
[4] + z
*m
[8] + m
[3];
108 res
[1] = x
*m
[1] + y
*m
[5] + z
*m
[9] + m
[7];
109 res
[2] = x
*m
[2] + y
*m
[6] + z
*m
[10] + m
[11];
112 static void mapply_to_vector (float *res
, float *m
, float *v
)
117 res
[0] = x
*m
[0] + y
*m
[4] + z
*m
[8];
118 res
[1] = x
*m
[1] + y
*m
[5] + z
*m
[9];
119 res
[2] = x
*m
[2] + y
*m
[6] + z
*m
[10];
122 static void vaddto (float *v1
, float *v2
)
129 static void vcopy (float *res
, float *v
)
136 static void vsub (float *res
, float *v1
, float *v2
)
138 res
[0] = v1
[0] - v2
[0];
139 res
[1] = v1
[1] - v2
[1];
140 res
[2] = v1
[2] - v2
[2];
146 static void q2matrixt (float *mat
, float *q
, float *v
)
165 mat
[0] = 1 - 2 * ( yy
+ zz
);
166 mat
[4] = 2 * ( xy
- zw
);
167 mat
[8] = 2 * ( xz
+ yw
);
169 mat
[1] = 2 * ( xy
+ zw
);
170 mat
[5] = 1 - 2 * ( xx
+ zz
);
171 mat
[9] = 2 * ( yz
- xw
);
173 mat
[2] = 2 * ( xz
- yw
);
174 mat
[6] = 2 * ( yz
+ xw
);
175 mat
[10] = 1 - 2 * ( xx
+ yy
);