TAO: deMatrix3f.h Source File

00001 /* Copyright (c) 2005 Arachi, Inc. and Stanford University.
00002  *
00003  * Permission is hereby granted, free of charge, to any person obtaining
00004  * a copy of this software and associated documentation files (the
00005  * "Software"), to deal in the Software without restriction, including
00006  * without limitation the rights to use, copy, modify, merge, publish,
00007  * distribute, sublicense, and/or sell copies of the Software, and to
00008  * permit persons to whom the Software is furnished to do so, subject
00009  * to the following conditions:
00010  *
00011  * The above copyright notice and this permission notice shall be included
00012  * in all copies or substantial portions of the Software.
00013  *
00014  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
00015  * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
00016  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
00017  * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
00018  * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
00019  * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
00020  * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00021  */
00022 
00023 #ifndef _deMatrix3f_h
00024 #define _deMatrix3f_h
00025 
00026 #ifdef __cplusplus
00027 extern "C" {
00028 #endif
00029 
00030 extern void deSetQ4M3(deFloat* resq, const deFloat (*m1)[DE_MATRIX3_COL]);
00031 /* Euler XYZ */
00032 extern void deSetV3M3xyz(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL]);
00033 /* Euler ZYX */
00034 extern void deSetV3M3zyx(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL]);
00035 extern void deSetV3M3zyxV3(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL], const deFloat* last);
00036 
00037 extern void deLUdecomposeM3M3(deFloat (*lu)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL]);
00038 extern void deBackSubstituteV3M3V3(deFloat* x, const deFloat (*lu)[DE_MATRIX3_COL], const deFloat* y);
00039 extern void deSetM3S2(deFloat (*res)[DE_MATRIX3_COL], const int axis, const deFloat angle);
00040 
00041 /* inlines */
00042 
00043 DE_MATH_API void deSetM3S9(deFloat (*res)[DE_MATRIX3_COL],
00044                                                    const deFloat a0, const deFloat a1, const deFloat a2,
00045                                                    const deFloat a3, const deFloat a4, const deFloat a5,
00046                                                    const deFloat a6, const deFloat a7, const deFloat a8)
00047 {
00048         res[0][0] = a0; res[0][1] = a1; res[0][2] = a2;
00049         res[1][0] = a3; res[1][1] = a4; res[1][2] = a5;
00050         res[2][0] = a6; res[2][1] = a7; res[2][2] = a8;
00051 }
00052 
00053 DE_MATH_API void deZeroM3(deFloat (*res)[DE_MATRIX3_COL])
00054 {
00055         res[0][0] = 0;  res[0][1] = 0;  res[0][2] = 0;
00056         res[1][0] = 0;  res[1][1] = 0;  res[1][2] = 0;
00057         res[2][0] = 0;  res[2][1] = 0;  res[2][2] = 0;
00058 }
00059 
00060 DE_MATH_API void deIdentityM3(deFloat (*res)[DE_MATRIX3_COL])
00061 {
00062         res[0][0] = 1;  res[0][1] = 0;  res[0][2] = 0;
00063         res[1][0] = 0;  res[1][1] = 1;  res[1][2] = 0;
00064         res[2][0] = 0;  res[2][1] = 0;  res[2][2] = 1;
00065 }
00066 
00067 DE_MATH_API void deMulM3S1(deFloat (*res)[DE_MATRIX3_COL], const deFloat s)
00068 {
00069         res[0][0] *= s; res[0][1] *= s; res[0][2] *= s;
00070         res[1][0] *= s; res[1][1] *= s; res[1][2] *= s;
00071         res[2][0] *= s; res[2][1] *= s; res[2][2] *= s;
00072 }
00073 
00074 DE_MATH_API void deNegateM3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL])
00075 {
00076         res[0][0] = -m1[0][0];  res[0][1] = -m1[0][1];  res[0][2] = -m1[0][2];
00077         res[1][0] = -m1[1][0];  res[1][1] = -m1[1][1];  res[1][2] = -m1[1][2];
00078         res[2][0] = -m1[2][0];  res[2][1] = -m1[2][1];  res[2][2] = -m1[2][2];
00079 }
00080 
00081 DE_MATH_API void deSetM3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL])
00082 {
00083         res[0][0] = m1[0][0];   res[0][1] = m1[0][1];   res[0][2] = m1[0][2];
00084         res[1][0] = m1[1][0];   res[1][1] = m1[1][1];   res[1][2] = m1[1][2];
00085         res[2][0] = m1[2][0];   res[2][1] = m1[2][1];   res[2][2] = m1[2][2];
00086 }
00087 
00088 DE_MATH_API void deAddM3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL])
00089 {
00090         res[0][0] += m1[0][0];  res[0][1] += m1[0][1];  res[0][2] += m1[0][2];
00091         res[1][0] += m1[1][0];  res[1][1] += m1[1][1];  res[1][2] += m1[1][2];
00092         res[2][0] += m1[2][0];  res[2][1] += m1[2][1];  res[2][2] += m1[2][2];
00093 }
00094 
00095 DE_MATH_API void deSubM3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL])
00096 {
00097         res[0][0] -= m1[0][0];  res[0][1] -= m1[0][1];  res[0][2] -= m1[0][2];
00098         res[1][0] -= m1[1][0];  res[1][1] -= m1[1][1];  res[1][2] -= m1[1][2];
00099         res[2][0] -= m1[2][0];  res[2][1] -= m1[2][1];  res[2][2] -= m1[2][2];
00100 }
00101 
00102 
00103 DE_MATH_API void deAddM3M3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL], const deFloat (*m2)[DE_MATRIX3_COL])
00104 {
00105         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];
00106         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];
00107         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];
00108 }
00109 
00110 DE_MATH_API void deSubM3M3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL], const deFloat (*m2)[DE_MATRIX3_COL])
00111 {
00112         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];
00113         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];
00114         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];
00115 }
00116 
00117 DE_MATH_API void deMulM3M3S1(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL], const deFloat s)
00118 {
00119         res[0][0] = m1[0][0] * s;       res[0][1] = m1[0][1] * s;       res[0][2] = m1[0][2] * s;
00120         res[1][0] = m1[1][0] * s;       res[1][1] = m1[1][1] * s;       res[1][2] = m1[1][2] * s;
00121         res[2][0] = m1[2][0] * s;       res[2][1] = m1[2][1] * s;       res[2][2] = m1[2][2] * s;
00122 }
00123 
00124 DE_MATH_API void deMulM3M3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL], const deFloat (*m2)[DE_MATRIX3_COL])
00125 {
00126     res[0][0] = m1[0][0] * m2[0][0] + m1[0][1] * m2[1][0] + m1[0][2] * m2[2][0];
00127     res[0][1] = m1[0][0] * m2[0][1] + m1[0][1] * m2[1][1] + m1[0][2] * m2[2][1];
00128     res[0][2] = m1[0][0] * m2[0][2] + m1[0][1] * m2[1][2] + m1[0][2] * m2[2][2];
00129     res[1][0] = m1[1][0] * m2[0][0] + m1[1][1] * m2[1][0] + m1[1][2] * m2[2][0];
00130     res[1][1] = m1[1][0] * m2[0][1] + m1[1][1] * m2[1][1] + m1[1][2] * m2[2][1];
00131     res[1][2] = m1[1][0] * m2[0][2] + m1[1][1] * m2[1][2] + m1[1][2] * m2[2][2];
00132     res[2][0] = m1[2][0] * m2[0][0] + m1[2][1] * m2[1][0] + m1[2][2] * m2[2][0];
00133     res[2][1] = m1[2][0] * m2[0][1] + m1[2][1] * m2[1][1] + m1[2][2] * m2[2][1];
00134     res[2][2] = m1[2][0] * m2[0][2] + m1[2][1] * m2[1][2] + m1[2][2] * m2[2][2];
00135 }
00136 
00137 /* res = m1^T * m2 */
00138 DE_MATH_API void deMulM3M3tM3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL], const deFloat (*m2)[DE_MATRIX3_COL])
00139 {
00140     res[0][0] = m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0];
00141     res[0][1] = m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1];
00142     res[0][2] = m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2];
00143     res[1][0] = m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0];
00144     res[1][1] = m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1];
00145     res[1][2] = m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2];
00146     res[2][0] = m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0];
00147     res[2][1] = m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1];
00148     res[2][2] = m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2];
00149 }
00150 
00151 /* res = m1 * m2^T */
00152 DE_MATH_API void deMulM3M3M3t(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL], const deFloat (*m2)[DE_MATRIX3_COL])
00153 {
00154     res[0][0] = m1[0][0] * m2[0][0] + m1[0][1] * m2[0][1] + m1[0][2] * m2[0][2];
00155     res[0][1] = m1[0][0] * m2[1][0] + m1[0][1] * m2[1][1] + m1[0][2] * m2[1][2];
00156     res[0][2] = m1[0][0] * m2[2][0] + m1[0][1] * m2[2][1] + m1[0][2] * m2[2][2];
00157     res[1][0] = m1[1][0] * m2[0][0] + m1[1][1] * m2[0][1] + m1[1][2] * m2[0][2];
00158     res[1][1] = m1[1][0] * m2[1][0] + m1[1][1] * m2[1][1] + m1[1][2] * m2[1][2];
00159     res[1][2] = m1[1][0] * m2[2][0] + m1[1][1] * m2[2][1] + m1[1][2] * m2[2][2];
00160     res[2][0] = m1[2][0] * m2[0][0] + m1[2][1] * m2[0][1] + m1[2][2] * m2[0][2];
00161     res[2][1] = m1[2][0] * m2[1][0] + m1[2][1] * m2[1][1] + m1[2][2] * m2[1][2];
00162     res[2][2] = m1[2][0] * m2[2][0] + m1[2][1] * m2[2][1] + m1[2][2] * m2[2][2];
00163 }
00164 
00165 DE_MATH_API void deMulV3M3V3(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL], const deFloat* v2)
00166 {
00167     resv[0] = m1[0][0] * v2[0] + m1[0][1] * v2[1] + m1[0][2] * v2[2];
00168     resv[1] = m1[1][0] * v2[0] + m1[1][1] * v2[1] + m1[1][2] * v2[2];
00169     resv[2] = m1[2][0] * v2[0] + m1[2][1] * v2[1] + m1[2][2] * v2[2];
00170 }
00171 
00172 /* resv = m2^T * v2 */
00173 DE_MATH_API void deMulV3M3tV3(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL], const deFloat* v2)
00174 {
00175     resv[0] = m1[0][0] * v2[0] + m1[1][0] * v2[1] + m1[2][0] * v2[2];
00176     resv[1] = m1[0][1] * v2[0] + m1[1][1] * v2[1] + m1[2][1] * v2[2];
00177     resv[2] = m1[0][2] * v2[0] + m1[1][2] * v2[1] + m1[2][2] * v2[2];
00178 }
00179 
00180 /* res = m1 * (v2 x) */
00181 DE_MATH_API void deMulM3M3V3x(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL], const deFloat* v2)
00182 {
00183     res[0][0] =                           m1[0][1] * v2[2] - m1[0][2] * v2[1];
00184     res[0][1] = -m1[0][0] * v2[2]                 + m1[0][2] * v2[0];
00185     res[0][2] =  m1[0][0] * v2[1]       - m1[0][1] * v2[0];
00186     res[1][0] =                  m1[1][1] * v2[2] - m1[1][2] * v2[1];
00187     res[1][1] = -m1[1][0] * v2[2]                 + m1[1][2] * v2[0];
00188     res[1][2] =  m1[1][0] * v2[1]       - m1[1][1] * v2[0];
00189     res[2][0] =                  m1[2][1] * v2[2] - m1[2][2] * v2[1];
00190     res[2][1] = -m1[2][0] * v2[2]                 + m1[2][2] * v2[0];
00191     res[2][2] =  m1[2][0] * v2[1]       - m1[2][1] * v2[0];
00192 }
00193 
00194 /* [0 1 2;3 4 5;6 7 8]^T = [0 3 6;1 4 7;2 5 8]  */
00195 DE_MATH_API void deTransposeM3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL])
00196 {
00197     res[0][0] = m1[0][0];    res[0][1] = m1[1][0];    res[0][2] = m1[2][0];
00198     res[1][0] = m1[0][1];    res[1][1] = m1[1][1];    res[1][2] = m1[2][1];
00199     res[2][0] = m1[0][2];    res[2][1] = m1[1][2];    res[2][2] = m1[2][2];
00200 }
00201 
00202 DE_MATH_API void deColumnV3M3S1(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL], const int col)
00203 {
00204         resv[0] = m1[0][col];
00205         resv[1] = m1[1][col];
00206         resv[2] = m1[2][col];
00207 }
00208 
00209 DE_MATH_API void deDiagonalM3V3(deFloat (*res)[DE_MATRIX3_COL], const deFloat* v1)
00210 {
00211         res[0][0] = v1[0];      res[0][1] = 0;          res[0][2] = 0;
00212         res[1][0] = 0;          res[1][1] = v1[1];      res[1][2] = 0;
00213         res[2][0] = 0;          res[2][1] = 0;          res[2][2] = v1[2];
00214 }
00215 
00216 DE_MATH_API void deDiagonalM3S3(deFloat (*res)[DE_MATRIX3_COL], const deFloat x, const deFloat y, const deFloat z)
00217 {
00218         res[0][0] = x;          res[0][1] = 0;          res[0][2] = 0;
00219         res[1][0] = 0;          res[1][1] = y;          res[1][2] = 0;
00220         res[2][0] = 0;          res[2][1] = 0;          res[2][2] = z;
00221 }
00222 
00223 DE_MATH_API void deDiagonalV3M3(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL])
00224 {
00225     resv[0] = m1[0][0];
00226     resv[1] = m1[1][1];
00227     resv[2] = m1[2][2];
00228 }
00229 
00230 DE_MATH_API deFloat dedetM3(const deFloat (*m1)[DE_MATRIX3_COL])
00231 {
00232         return (  m1[0][0] * (m1[1][1] * m1[2][2] - m1[1][2] * m1[2][1])
00233                         - m1[0][1] * (m1[1][0] * m1[2][2] - m1[1][2] * m1[2][0])
00234                         + m1[0][2] * (m1[1][0] * m1[2][1] - m1[1][1] * m1[2][0]));
00235 }
00236 
00237 DE_MATH_API void deInvertDetSPDM3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL])
00238 {
00239      deFloat ood = 1 / dedetM3(m1);
00240         
00241      res[0][0] =  ood * (m1[1][1] * m1[2][2] - m1[1][2] * m1[2][1]);
00242      res[0][1] = -ood * (m1[0][1] * m1[2][2] - m1[0][2] * m1[2][1]);
00243      res[0][2] =  ood * (m1[0][1] * m1[1][2] - m1[0][2] * m1[1][1]);
00244      res[1][0] =  res[0][1];
00245      res[1][1] =  ood * (m1[0][0] * m1[2][2] - m1[0][2] * m1[2][0]);
00246      res[1][2] = -ood * (m1[0][0] * m1[1][2] - m1[0][2] * m1[1][0]);
00247      res[2][0] =  res[0][2];
00248      res[2][1] =  res[1][2];
00249      res[2][2] =  ood * (m1[0][0] * m1[1][1] - m1[0][1] * m1[1][0]);
00250 }
00251 
00252 DE_MATH_API void deInvertDetM3M3(deFloat (*res)[DE_MATRIX3_COL], const deFloat (*m1)[DE_MATRIX3_COL])
00253 {
00254      deFloat ood = 1 / dedetM3(m1);
00255         
00256      res[0][0] =  ood * (m1[1][1] * m1[2][2] - m1[1][2] * m1[2][1]);
00257      res[0][1] = -ood * (m1[0][1] * m1[2][2] - m1[0][2] * m1[2][1]);
00258      res[0][2] =  ood * (m1[0][1] * m1[1][2] - m1[0][2] * m1[1][1]);
00259      res[1][0] = -ood * (m1[1][0] * m1[2][2] - m1[1][2] * m1[2][0]);
00260      res[1][1] =  ood * (m1[0][0] * m1[2][2] - m1[0][2] * m1[2][0]);
00261      res[1][2] = -ood * (m1[0][0] * m1[1][2] - m1[0][2] * m1[1][0]);
00262      res[2][0] =  ood * (m1[1][0] * m1[2][1] - m1[1][1] * m1[2][0]);
00263      res[2][1] = -ood * (m1[0][0] * m1[2][1] - m1[0][1] * m1[2][0]);
00264      res[2][2] =  ood * (m1[0][0] * m1[1][1] - m1[0][1] * m1[1][0]);
00265 }
00266 
00267 /* resv = m1 - m2 */
00268 DE_MATH_API void deAngularErrorV3M3M3(deFloat* resv, const deFloat (*m1)[DE_MATRIX3_COL], const deFloat (*m2)[DE_MATRIX3_COL])
00269 {
00270         resv[0] = -(deFloat)0.5*((m1[1][0] * m2[2][0] + m1[1][1] * m2[2][1] + m1[1][2] * m2[2][2]) 
00271                                  - (m1[2][0] * m2[1][0] + m1[2][1] * m2[1][1] + m1[2][2] * m2[1][2]));
00272         resv[1] = -(deFloat)0.5*((m1[2][0] * m2[0][0] + m1[2][1] * m2[0][1] + m1[2][2] * m2[0][2])
00273                          - (m1[0][0] * m2[2][0] + m1[0][1] * m2[2][1] + m1[0][2] * m2[2][2]));
00274         resv[2] = -(deFloat)0.5*((m1[0][0] * m2[1][0] + m1[0][1] * m2[1][1] + m1[0][2] * m2[1][2])
00275                          - (m1[1][0] * m2[0][0] + m1[1][1] * m2[0][1] + m1[1][2] * m2[0][2]));
00276 }
00277 
00278 /*
00279  * 27 mult + 12 add
00280  * Intro. to Robotics by J. Craig: p. 55
00281  */
00282 DE_MATH_API void deSetM3Q4(deFloat (*res)[DE_MATRIX3_COL], const deFloat* q1)
00283 {
00284     res[0][0] = 1 - 2 * (q1[1] * q1[1] + q1[2] * q1[2]);
00285     res[0][1] =     2 * (q1[0] * q1[1] - q1[3] * q1[2]);
00286     res[0][2] =     2 * (q1[0] * q1[2] + q1[3] * q1[1]);
00287     res[1][0] =     2 * (q1[0] * q1[1] + q1[3] * q1[2]);
00288     res[1][1] = 1 - 2 * (q1[0] * q1[0] + q1[2] * q1[2]);
00289     res[1][2] =     2 * (q1[1] * q1[2] - q1[3] * q1[0]);
00290     res[2][0] =     2 * (q1[0] * q1[2] - q1[3] * q1[1]);
00291     res[2][1] =     2 * (q1[1] * q1[2] + q1[3] * q1[0]);
00292     res[2][2] = 1 - 2 * (q1[0] * q1[0] + q1[1] * q1[1]);
00293 }
00294 
00295 /* res = v1 * v2^T */
00296 DE_MATH_API void deMulM3V3V3t(deFloat (*res)[DE_MATRIX3_COL], const deFloat* v1, const deFloat* v2)
00297 {
00298     res[0][0] = v1[0] * v2[0];
00299     res[0][1] = v1[0] * v2[1];
00300     res[0][2] = v1[0] * v2[2];
00301     res[1][0] = v1[1] * v2[0];
00302     res[1][1] = v1[1] * v2[1];
00303     res[1][2] = v1[1] * v2[2];
00304     res[2][0] = v1[2] * v2[0];
00305     res[2][1] = v1[2] * v2[1];
00306     res[2][2] = v1[2] * v2[2];
00307 }
00308 
00309 /* ([x y z] x) = [0 -z y; z 0 -x; -y x 0] */
00310 DE_MATH_API void deSetM3V3x(deFloat (*res)[DE_MATRIX3_COL], const deFloat* v1)
00311 {
00312     res[0][0]=      0;  res[0][1]= -v1[2]; res[0][2]=  v1[1];
00313     res[1][0]=  v1[2]; res[1][1]=      0; res[1][2]= -v1[0];
00314     res[2][0]= -v1[1]; res[2][1]=  v1[0]; res[2][2]=      0;
00315 }
00316 
00317 /* res = (v1 x)*m2 */
00318 DE_MATH_API void deMulM3V3xM3(deFloat (*res)[DE_MATRIX3_COL], const deFloat* v1, const deFloat (*m2)[DE_MATRIX3_COL])
00319 {
00320     res[0][0]= -v1[2] * m2[1][0] + v1[1] * m2[2][0];
00321     res[0][1]= -v1[2] * m2[1][1] + v1[1] * m2[2][1];
00322     res[0][2]= -v1[2] * m2[1][2] + v1[1] * m2[2][2];
00323     res[1][0]=  v1[2] * m2[0][0] - v1[0] * m2[2][0];
00324     res[1][1]=  v1[2] * m2[0][1] - v1[0] * m2[2][1];
00325     res[1][2]=  v1[2] * m2[0][2] - v1[0] * m2[2][2];
00326     res[2][0]= -v1[1] * m2[0][0] + v1[0] * m2[1][0];
00327     res[2][1]= -v1[1] * m2[0][1] + v1[0] * m2[1][1];
00328     res[2][2]= -v1[1] * m2[0][2] + v1[0] * m2[1][2];
00329 }
00330 
00331 /* setting rotation matrices */
00332 
00333 /* axis angle */
00334 DE_MATH_API void deSetM3V3S1(deFloat (*res)[DE_MATRIX3_COL], const deFloat* axis, const deFloat angle)
00335 {
00336         /* also, see Intro. to Robotics by J. Craig: p. 52 */
00337 
00338         deFloat c = deCos(angle);
00339         deFloat v = 1 - c;
00340         deFloat s = deSin(angle);
00341 
00342         res[0][0] = axis[0] * axis[0] * v + c;
00343         res[0][1] = axis[0] * axis[1] * v - axis[2] * s;
00344         res[0][2] = axis[0] * axis[2] * v + axis[1] * s;
00345         res[1][0] = axis[1] * axis[0] * v + axis[2] * s;
00346         res[1][1] = axis[1] * axis[1] * v + c;
00347         res[1][2] = axis[1] * axis[2] * v - axis[0] * s;
00348         res[2][0] = axis[2] * axis[0] * v - axis[1] * s;
00349         res[2][1] = axis[2] * axis[1] * v + axis[0] * s;
00350         res[2][2] = axis[2] * axis[2] * v + c;
00351 }
00352 
00353 DE_MATH_API void deSetM3xyzS3(deFloat (*res)[DE_MATRIX3_COL], const deFloat x, const deFloat y, const deFloat z)
00354 {
00355         deFloat cx = deCos(x);
00356         deFloat sx = deSin(x);
00357         deFloat cy = deCos(y);
00358         deFloat sy = deSin(y);
00359         deFloat cz = deCos(z);
00360         deFloat sz = deSin(z);
00361 
00362         res[0][0] =  cy * cz;
00363         res[0][1] = -cy * sz;
00364         res[0][2] =  sy;
00365 
00366         res[1][0] =  sx * sy * cz + cx * sz;
00367         res[1][1] = -sx * sy * sz + cx * cz;
00368         res[1][2] = -sx * cy;
00369 
00370         res[2][0] = -cx * sy * cz + sx * sz;
00371         res[2][1] =  cx * sy * sz + sx * cz;
00372         res[2][2] =  cx * cy;
00373 }
00374 
00375 DE_MATH_API void deSetM3zyxS3(deFloat (*res)[DE_MATRIX3_COL], const deFloat x, const deFloat y, const deFloat z)
00376 {
00377         deFloat cx = deCos(x);
00378         deFloat sx = deSin(x);
00379         deFloat cy = deCos(y);
00380         deFloat sy = deSin(y);
00381         deFloat cz = deCos(z);
00382         deFloat sz = deSin(z);
00383 
00384         res[0][0] =  cy * cz;
00385         res[1][0] =  sx * sy * cz - cx * sz;
00386         res[2][0] =  cx * sy * cz + sx * sz;
00387 
00388         res[0][1] =  cy * sz;
00389         res[1][1] =  sx * sy * sz + cx * cz;
00390         res[2][1] =  cx * sy * sz - sx * cz;
00391 
00392         res[2][0] = -sy;
00393         res[2][1] =  sx * cy;
00394         res[2][2] =  cx * cy;
00395 }
00396 
00397 #ifdef __cplusplus
00398 }
00399 #endif
00400 
00401 #endif /* _deMatrix3f_h */