SCL  1.0
Standard Control Library : Control, dynamics, physics, and simulation
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Macros Groups Pages
TaoDeMatrix3f.h
1 /* Copyright (c) 2005 Arachi, Inc. and Stanford University. All rights reserved.
2  *
3  * Permission is hereby granted, free of charge, to any person obtaining
4  * a copy of this software and associated documentation files (the
5  * "Software"), to deal in the Software without restriction, including
6  * without limitation the rights to use, copy, modify, merge, publish,
7  * distribute, sublicense, and/or sell copies of the Software, and to
8  * permit persons to whom the Software is furnished to do so, subject
9  * to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included
12  * in all copies or substantial portions of the Software.
13  *
14  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
15  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
16  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
17  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
18  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
19  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
20  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
21  */
22 
23 #ifndef _deMatrix3f_h
24 #define _deMatrix3f_h
25 
26 #ifdef __cplusplus
27 extern "C" {
28 #endif
29 
30 extern void deSetQ4M3(deFloat* resq, const deFloat (*m1)[DE_MATRIX3_COL]);
31 /* Euler XYZ */
32 extern void deSetV3M3xyz(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL]);
33 /* Euler ZYX */
34 extern void deSetV3M3zyx(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL]);
35 extern void deSetV3M3zyxV3(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL], const deFloat* last);
36 
37 extern void deLUdecomposeM3M3(deFloat (*lu)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL]);
38 extern void deBackSubstituteV3M3V3(deFloat* x, const deFloat (*lu)[DE_MATRIX3_COL], const deFloat* y);
39 extern void deSetM3S2(deFloat (*res)[DE_MATRIX3_COL], const int axis, const deFloat angle);
40 
41 /* inlines */
42 
43 DE_MATH_API void deSetM3S9(deFloat (*res)[DE_MATRIX3_COL],
44  const deFloat a0, const deFloat a1, const deFloat a2,
45  const deFloat a3, const deFloat a4, const deFloat a5,
46  const deFloat a6, const deFloat a7, const deFloat a8)
47 {
48  res[0][0] = a0; res[0][1] = a1; res[0][2] = a2;
49  res[1][0] = a3; res[1][1] = a4; res[1][2] = a5;
50  res[2][0] = a6; res[2][1] = a7; res[2][2] = a8;
51 }
52 
53 DE_MATH_API void deZeroM3(deFloat (*res)[DE_MATRIX3_COL])
54 {
55  res[0][0] = 0; res[0][1] = 0; res[0][2] = 0;
56  res[1][0] = 0; res[1][1] = 0; res[1][2] = 0;
57  res[2][0] = 0; res[2][1] = 0; res[2][2] = 0;
58 }
59 
60 DE_MATH_API void deIdentityM3(deFloat (*res)[DE_MATRIX3_COL])
61 {
62  res[0][0] = 1; res[0][1] = 0; res[0][2] = 0;
63  res[1][0] = 0; res[1][1] = 1; res[1][2] = 0;
64  res[2][0] = 0; res[2][1] = 0; res[2][2] = 1;
65 }
66 
67 DE_MATH_API void deMulM3S1(deFloat (*res)[DE_MATRIX3_COL], const deFloat s)
68 {
69  res[0][0] *= s; res[0][1] *= s; res[0][2] *= s;
70  res[1][0] *= s; res[1][1] *= s; res[1][2] *= s;
71  res[2][0] *= s; res[2][1] *= s; res[2][2] *= s;
72 }
73 
74 DE_MATH_API void deNegateM3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL])
75 {
76  res[0][0] = -m1[0][0]; res[0][1] = -m1[0][1]; res[0][2] = -m1[0][2];
77  res[1][0] = -m1[1][0]; res[1][1] = -m1[1][1]; res[1][2] = -m1[1][2];
78  res[2][0] = -m1[2][0]; res[2][1] = -m1[2][1]; res[2][2] = -m1[2][2];
79 }
80 
81 DE_MATH_API void deSetM3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL])
82 {
83  res[0][0] = m1[0][0]; res[0][1] = m1[0][1]; res[0][2] = m1[0][2];
84  res[1][0] = m1[1][0]; res[1][1] = m1[1][1]; res[1][2] = m1[1][2];
85  res[2][0] = m1[2][0]; res[2][1] = m1[2][1]; res[2][2] = m1[2][2];
86 }
87 
88 DE_MATH_API void deAddM3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL])
89 {
90  res[0][0] += m1[0][0]; res[0][1] += m1[0][1]; res[0][2] += m1[0][2];
91  res[1][0] += m1[1][0]; res[1][1] += m1[1][1]; res[1][2] += m1[1][2];
92  res[2][0] += m1[2][0]; res[2][1] += m1[2][1]; res[2][2] += m1[2][2];
93 }
94 
95 DE_MATH_API void deSubM3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL])
96 {
97  res[0][0] -= m1[0][0]; res[0][1] -= m1[0][1]; res[0][2] -= m1[0][2];
98  res[1][0] -= m1[1][0]; res[1][1] -= m1[1][1]; res[1][2] -= m1[1][2];
99  res[2][0] -= m1[2][0]; res[2][1] -= m1[2][1]; res[2][2] -= m1[2][2];
100 }
101 
102 
103 DE_MATH_API void deAddM3M3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL], const deFloat (*m2)[DE_MATRIX3_COL])
104 {
105  res[0][0] = m1[0][0] + m2[0][0]; res[0][1] = m1[0][1] + m2[0][1]; res[0][2] = m1[0][2] + m2[0][2];
106  res[1][0] = m1[1][0] + m2[1][0]; res[1][1] = m1[1][1] + m2[1][1]; res[1][2] = m1[1][2] + m2[1][2];
107  res[2][0] = m1[2][0] + m2[2][0]; res[2][1] = m1[2][1] + m2[2][1]; res[2][2] = m1[2][2] + m2[2][2];
108 }
109 
110 DE_MATH_API void deSubM3M3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL], const deFloat (*m2)[DE_MATRIX3_COL])
111 {
112  res[0][0] = m1[0][0] - m2[0][0]; res[0][1] = m1[0][1] - m2[0][1]; res[0][2] = m1[0][2] - m2[0][2];
113  res[1][0] = m1[1][0] - m2[1][0]; res[1][1] = m1[1][1] - m2[1][1]; res[1][2] = m1[1][2] - m2[1][2];
114  res[2][0] = m1[2][0] - m2[2][0]; res[2][1] = m1[2][1] - m2[2][1]; res[2][2] = m1[2][2] - m2[2][2];
115 }
116 
117 DE_MATH_API void deMulM3M3S1(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL], const deFloat s)
118 {
119  res[0][0] = m1[0][0] * s; res[0][1] = m1[0][1] * s; res[0][2] = m1[0][2] * s;
120  res[1][0] = m1[1][0] * s; res[1][1] = m1[1][1] * s; res[1][2] = m1[1][2] * s;
121  res[2][0] = m1[2][0] * s; res[2][1] = m1[2][1] * s; res[2][2] = m1[2][2] * s;
122 }
123 
124 DE_MATH_API void deMulM3M3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL], const deFloat (*m2)[DE_MATRIX3_COL])
125 {
126  res[0][0] = m1[0][0] * m2[0][0] + m1[0][1] * m2[1][0] + m1[0][2] * m2[2][0];
127  res[0][1] = m1[0][0] * m2[0][1] + m1[0][1] * m2[1][1] + m1[0][2] * m2[2][1];
128  res[0][2] = m1[0][0] * m2[0][2] + m1[0][1] * m2[1][2] + m1[0][2] * m2[2][2];
129  res[1][0] = m1[1][0] * m2[0][0] + m1[1][1] * m2[1][0] + m1[1][2] * m2[2][0];
130  res[1][1] = m1[1][0] * m2[0][1] + m1[1][1] * m2[1][1] + m1[1][2] * m2[2][1];
131  res[1][2] = m1[1][0] * m2[0][2] + m1[1][1] * m2[1][2] + m1[1][2] * m2[2][2];
132  res[2][0] = m1[2][0] * m2[0][0] + m1[2][1] * m2[1][0] + m1[2][2] * m2[2][0];
133  res[2][1] = m1[2][0] * m2[0][1] + m1[2][1] * m2[1][1] + m1[2][2] * m2[2][1];
134  res[2][2] = m1[2][0] * m2[0][2] + m1[2][1] * m2[1][2] + m1[2][2] * m2[2][2];
135 }
136 
137 /* res = m1^T * m2 */
138 DE_MATH_API void deMulM3M3tM3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL], const deFloat (*m2)[DE_MATRIX3_COL])
139 {
140  res[0][0] = m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0];
141  res[0][1] = m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1];
142  res[0][2] = m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2];
143  res[1][0] = m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0];
144  res[1][1] = m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1];
145  res[1][2] = m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2];
146  res[2][0] = m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0];
147  res[2][1] = m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1];
148  res[2][2] = m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2];
149 }
150 
151 /* res = m1 * m2^T */
152 DE_MATH_API void deMulM3M3M3t(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL], const deFloat (*m2)[DE_MATRIX3_COL])
153 {
154  res[0][0] = m1[0][0] * m2[0][0] + m1[0][1] * m2[0][1] + m1[0][2] * m2[0][2];
155  res[0][1] = m1[0][0] * m2[1][0] + m1[0][1] * m2[1][1] + m1[0][2] * m2[1][2];
156  res[0][2] = m1[0][0] * m2[2][0] + m1[0][1] * m2[2][1] + m1[0][2] * m2[2][2];
157  res[1][0] = m1[1][0] * m2[0][0] + m1[1][1] * m2[0][1] + m1[1][2] * m2[0][2];
158  res[1][1] = m1[1][0] * m2[1][0] + m1[1][1] * m2[1][1] + m1[1][2] * m2[1][2];
159  res[1][2] = m1[1][0] * m2[2][0] + m1[1][1] * m2[2][1] + m1[1][2] * m2[2][2];
160  res[2][0] = m1[2][0] * m2[0][0] + m1[2][1] * m2[0][1] + m1[2][2] * m2[0][2];
161  res[2][1] = m1[2][0] * m2[1][0] + m1[2][1] * m2[1][1] + m1[2][2] * m2[1][2];
162  res[2][2] = m1[2][0] * m2[2][0] + m1[2][1] * m2[2][1] + m1[2][2] * m2[2][2];
163 }
164 
165 DE_MATH_API void deMulV3M3V3(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL], const deFloat* v2)
166 {
167  resv[0] = m1[0][0] * v2[0] + m1[0][1] * v2[1] + m1[0][2] * v2[2];
168  resv[1] = m1[1][0] * v2[0] + m1[1][1] * v2[1] + m1[1][2] * v2[2];
169  resv[2] = m1[2][0] * v2[0] + m1[2][1] * v2[1] + m1[2][2] * v2[2];
170 }
171 
172 /* resv = m2^T * v2 */
173 DE_MATH_API void deMulV3M3tV3(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL], const deFloat* v2)
174 {
175  resv[0] = m1[0][0] * v2[0] + m1[1][0] * v2[1] + m1[2][0] * v2[2];
176  resv[1] = m1[0][1] * v2[0] + m1[1][1] * v2[1] + m1[2][1] * v2[2];
177  resv[2] = m1[0][2] * v2[0] + m1[1][2] * v2[1] + m1[2][2] * v2[2];
178 }
179 
180 /* res = m1 * (v2 x) */
181 DE_MATH_API void deMulM3M3V3x(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL], const deFloat* v2)
182 {
183  res[0][0] = m1[0][1] * v2[2] - m1[0][2] * v2[1];
184  res[0][1] = -m1[0][0] * v2[2] + m1[0][2] * v2[0];
185  res[0][2] = m1[0][0] * v2[1] - m1[0][1] * v2[0];
186  res[1][0] = m1[1][1] * v2[2] - m1[1][2] * v2[1];
187  res[1][1] = -m1[1][0] * v2[2] + m1[1][2] * v2[0];
188  res[1][2] = m1[1][0] * v2[1] - m1[1][1] * v2[0];
189  res[2][0] = m1[2][1] * v2[2] - m1[2][2] * v2[1];
190  res[2][1] = -m1[2][0] * v2[2] + m1[2][2] * v2[0];
191  res[2][2] = m1[2][0] * v2[1] - m1[2][1] * v2[0];
192 }
193 
194 /* [0 1 2;3 4 5;6 7 8]^T = [0 3 6;1 4 7;2 5 8] */
195 DE_MATH_API void deTransposeM3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL])
196 {
197  res[0][0] = m1[0][0]; res[0][1] = m1[1][0]; res[0][2] = m1[2][0];
198  res[1][0] = m1[0][1]; res[1][1] = m1[1][1]; res[1][2] = m1[2][1];
199  res[2][0] = m1[0][2]; res[2][1] = m1[1][2]; res[2][2] = m1[2][2];
200 }
201 
202 DE_MATH_API void deColumnV3M3S1(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL], const int col)
203 {
204  resv[0] = m1[0][col];
205  resv[1] = m1[1][col];
206  resv[2] = m1[2][col];
207 }
208 
209 DE_MATH_API void deDiagonalM3V3(deFloat (*res)[DE_MATRIX3_COL], const deFloat* v1)
210 {
211  res[0][0] = v1[0]; res[0][1] = 0; res[0][2] = 0;
212  res[1][0] = 0; res[1][1] = v1[1]; res[1][2] = 0;
213  res[2][0] = 0; res[2][1] = 0; res[2][2] = v1[2];
214 }
215 
216 DE_MATH_API void deDiagonalM3S3(deFloat (*res)[DE_MATRIX3_COL], const deFloat x, const deFloat y, const deFloat z)
217 {
218  res[0][0] = x; res[0][1] = 0; res[0][2] = 0;
219  res[1][0] = 0; res[1][1] = y; res[1][2] = 0;
220  res[2][0] = 0; res[2][1] = 0; res[2][2] = z;
221 }
222 
223 DE_MATH_API void deDiagonalV3M3(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL])
224 {
225  resv[0] = m1[0][0];
226  resv[1] = m1[1][1];
227  resv[2] = m1[2][2];
228 }
229 
230 DE_MATH_API deFloat dedetM3(const deFloat (*m1)[DE_MATRIX3_COL])
231 {
232  return ( m1[0][0] * (m1[1][1] * m1[2][2] - m1[1][2] * m1[2][1])
233  - m1[0][1] * (m1[1][0] * m1[2][2] - m1[1][2] * m1[2][0])
234  + m1[0][2] * (m1[1][0] * m1[2][1] - m1[1][1] * m1[2][0]));
235 }
236 
237 DE_MATH_API void deInvertDetSPDM3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL])
238 {
239  deFloat ood = 1 / dedetM3(m1);
240 
241  res[0][0] = ood * (m1[1][1] * m1[2][2] - m1[1][2] * m1[2][1]);
242  res[0][1] = -ood * (m1[0][1] * m1[2][2] - m1[0][2] * m1[2][1]);
243  res[0][2] = ood * (m1[0][1] * m1[1][2] - m1[0][2] * m1[1][1]);
244  res[1][0] = res[0][1];
245  res[1][1] = ood * (m1[0][0] * m1[2][2] - m1[0][2] * m1[2][0]);
246  res[1][2] = -ood * (m1[0][0] * m1[1][2] - m1[0][2] * m1[1][0]);
247  res[2][0] = res[0][2];
248  res[2][1] = res[1][2];
249  res[2][2] = ood * (m1[0][0] * m1[1][1] - m1[0][1] * m1[1][0]);
250 }
251 
252 DE_MATH_API void deInvertDetM3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL])
253 {
254  deFloat ood = 1 / dedetM3(m1);
255 
256  res[0][0] = ood * (m1[1][1] * m1[2][2] - m1[1][2] * m1[2][1]);
257  res[0][1] = -ood * (m1[0][1] * m1[2][2] - m1[0][2] * m1[2][1]);
258  res[0][2] = ood * (m1[0][1] * m1[1][2] - m1[0][2] * m1[1][1]);
259  res[1][0] = -ood * (m1[1][0] * m1[2][2] - m1[1][2] * m1[2][0]);
260  res[1][1] = ood * (m1[0][0] * m1[2][2] - m1[0][2] * m1[2][0]);
261  res[1][2] = -ood * (m1[0][0] * m1[1][2] - m1[0][2] * m1[1][0]);
262  res[2][0] = ood * (m1[1][0] * m1[2][1] - m1[1][1] * m1[2][0]);
263  res[2][1] = -ood * (m1[0][0] * m1[2][1] - m1[0][1] * m1[2][0]);
264  res[2][2] = ood * (m1[0][0] * m1[1][1] - m1[0][1] * m1[1][0]);
265 }
266 
267 /* resv = m1 - m2 */
268 DE_MATH_API void deAngularErrorV3M3M3(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL], const deFloat (*m2)[DE_MATRIX3_COL])
269 {
270  resv[0] = -(deFloat)0.5*((m1[1][0] * m2[2][0] + m1[1][1] * m2[2][1] + m1[1][2] * m2[2][2])
271  - (m1[2][0] * m2[1][0] + m1[2][1] * m2[1][1] + m1[2][2] * m2[1][2]));
272  resv[1] = -(deFloat)0.5*((m1[2][0] * m2[0][0] + m1[2][1] * m2[0][1] + m1[2][2] * m2[0][2])
273  - (m1[0][0] * m2[2][0] + m1[0][1] * m2[2][1] + m1[0][2] * m2[2][2]));
274  resv[2] = -(deFloat)0.5*((m1[0][0] * m2[1][0] + m1[0][1] * m2[1][1] + m1[0][2] * m2[1][2])
275  - (m1[1][0] * m2[0][0] + m1[1][1] * m2[0][1] + m1[1][2] * m2[0][2]));
276 }
277 
278 /*
279  * 27 mult + 12 add
280  * Intro. to Robotics by J. Craig: p. 55
281  */
282 DE_MATH_API void deSetM3Q4(deFloat (*res)[DE_MATRIX3_COL], const deFloat* q1)
283 {
284  res[0][0] = 1 - 2 * (q1[1] * q1[1] + q1[2] * q1[2]);
285  res[0][1] = 2 * (q1[0] * q1[1] - q1[3] * q1[2]);
286  res[0][2] = 2 * (q1[0] * q1[2] + q1[3] * q1[1]);
287  res[1][0] = 2 * (q1[0] * q1[1] + q1[3] * q1[2]);
288  res[1][1] = 1 - 2 * (q1[0] * q1[0] + q1[2] * q1[2]);
289  res[1][2] = 2 * (q1[1] * q1[2] - q1[3] * q1[0]);
290  res[2][0] = 2 * (q1[0] * q1[2] - q1[3] * q1[1]);
291  res[2][1] = 2 * (q1[1] * q1[2] + q1[3] * q1[0]);
292  res[2][2] = 1 - 2 * (q1[0] * q1[0] + q1[1] * q1[1]);
293 }
294 
295 /* res = v1 * v2^T */
296 DE_MATH_API void deMulM3V3V3t(deFloat (*res)[DE_MATRIX3_COL], const deFloat* v1, const deFloat* v2)
297 {
298  res[0][0] = v1[0] * v2[0];
299  res[0][1] = v1[0] * v2[1];
300  res[0][2] = v1[0] * v2[2];
301  res[1][0] = v1[1] * v2[0];
302  res[1][1] = v1[1] * v2[1];
303  res[1][2] = v1[1] * v2[2];
304  res[2][0] = v1[2] * v2[0];
305  res[2][1] = v1[2] * v2[1];
306  res[2][2] = v1[2] * v2[2];
307 }
308 
309 /* ([x y z] x) = [0 -z y; z 0 -x; -y x 0] */
310 DE_MATH_API void deSetM3V3x(deFloat (*res)[DE_MATRIX3_COL], const deFloat* v1)
311 {
312  res[0][0]= 0; res[0][1]= -v1[2]; res[0][2]= v1[1];
313  res[1][0]= v1[2]; res[1][1]= 0; res[1][2]= -v1[0];
314  res[2][0]= -v1[1]; res[2][1]= v1[0]; res[2][2]= 0;
315 }
316 
317 /* res = (v1 x)*m2 */
318 DE_MATH_API void deMulM3V3xM3(deFloat (*res)[DE_MATRIX3_COL], const deFloat* v1, const deFloat (*m2)[DE_MATRIX3_COL])
319 {
320  res[0][0]= -v1[2] * m2[1][0] + v1[1] * m2[2][0];
321  res[0][1]= -v1[2] * m2[1][1] + v1[1] * m2[2][1];
322  res[0][2]= -v1[2] * m2[1][2] + v1[1] * m2[2][2];
323  res[1][0]= v1[2] * m2[0][0] - v1[0] * m2[2][0];
324  res[1][1]= v1[2] * m2[0][1] - v1[0] * m2[2][1];
325  res[1][2]= v1[2] * m2[0][2] - v1[0] * m2[2][2];
326  res[2][0]= -v1[1] * m2[0][0] + v1[0] * m2[1][0];
327  res[2][1]= -v1[1] * m2[0][1] + v1[0] * m2[1][1];
328  res[2][2]= -v1[1] * m2[0][2] + v1[0] * m2[1][2];
329 }
330 
331 /* setting rotation matrices */
332 
333 /* axis angle */
334 DE_MATH_API void deSetM3V3S1(deFloat (*res)[DE_MATRIX3_COL], const deFloat* axis, const deFloat angle)
335 {
336  /* also, see Intro. to Robotics by J. Craig: p. 52 */
337 
338  deFloat c = deCos(angle);
339  deFloat v = 1 - c;
340  deFloat s = deSin(angle);
341 
342  res[0][0] = axis[0] * axis[0] * v + c;
343  res[0][1] = axis[0] * axis[1] * v - axis[2] * s;
344  res[0][2] = axis[0] * axis[2] * v + axis[1] * s;
345  res[1][0] = axis[1] * axis[0] * v + axis[2] * s;
346  res[1][1] = axis[1] * axis[1] * v + c;
347  res[1][2] = axis[1] * axis[2] * v - axis[0] * s;
348  res[2][0] = axis[2] * axis[0] * v - axis[1] * s;
349  res[2][1] = axis[2] * axis[1] * v + axis[0] * s;
350  res[2][2] = axis[2] * axis[2] * v + c;
351 }
352 
353 DE_MATH_API void deSetM3xyzS3(deFloat (*res)[DE_MATRIX3_COL], const deFloat x, const deFloat y, const deFloat z)
354 {
355  deFloat cx = deCos(x);
356  deFloat sx = deSin(x);
357  deFloat cy = deCos(y);
358  deFloat sy = deSin(y);
359  deFloat cz = deCos(z);
360  deFloat sz = deSin(z);
361 
362  res[0][0] = cy * cz;
363  res[0][1] = -cy * sz;
364  res[0][2] = sy;
365 
366  res[1][0] = sx * sy * cz + cx * sz;
367  res[1][1] = -sx * sy * sz + cx * cz;
368  res[1][2] = -sx * cy;
369 
370  res[2][0] = -cx * sy * cz + sx * sz;
371  res[2][1] = cx * sy * sz + sx * cz;
372  res[2][2] = cx * cy;
373 }
374 
375 DE_MATH_API void deSetM3zyxS3(deFloat (*res)[DE_MATRIX3_COL], const deFloat x, const deFloat y, const deFloat z)
376 {
377  deFloat cx = deCos(x);
378  deFloat sx = deSin(x);
379  deFloat cy = deCos(y);
380  deFloat sy = deSin(y);
381  deFloat cz = deCos(z);
382  deFloat sz = deSin(z);
383 
384  res[0][0] = cy * cz;
385  res[1][0] = sx * sy * cz - cx * sz;
386  res[2][0] = cx * sy * cz + sx * sz;
387 
388  res[0][1] = cy * sz;
389  res[1][1] = sx * sy * sz + cx * cz;
390  res[2][1] = cx * sy * sz - sx * cz;
391 
392  res[2][0] = -sy;
393  res[2][1] = sx * cy;
394  res[2][2] = cx * cy;
395 }
396 
397 #ifdef __cplusplus
398 }
399 #endif
400 
401 #endif /* _deMatrix3f_h */
Generated on Sun Sep 14 2014 02:35:55 for SCL by   doxygen 1.8.6