source: GTP/trunk/Lib/Geom/OgreStuff/include/ode/odemath.h @ 1809

Revision 1809, 9.9 KB checked in by gumbau, 18 years ago (diff)
Line 
1/*************************************************************************
2 *                                                                       *
3 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
4 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
5 *                                                                       *
6 * This library is free software; you can redistribute it and/or         *
7 * modify it under the terms of EITHER:                                  *
8 *   (1) The GNU Lesser General Public License as published by the Free  *
9 *       Software Foundation; either version 2.1 of the License, or (at  *
10 *       your option) any later version. The text of the GNU Lesser      *
11 *       General Public License is included with this library in the     *
12 *       file LICENSE.TXT.                                               *
13 *   (2) The BSD-style license that is included with this library in     *
14 *       the file LICENSE-BSD.TXT.                                       *
15 *                                                                       *
16 * This library is distributed in the hope that it will be useful,       *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
20 *                                                                       *
21 *************************************************************************/
22
23#ifndef _ODE_ODEMATH_H_
24#define _ODE_ODEMATH_H_
25
26#include <ode/common.h>
27
28#ifdef __GNUC__
29#define PURE_INLINE extern inline
30#else
31#define PURE_INLINE inline
32#endif
33
34/*
35 * macro to access elements i,j in an NxM matrix A, independent of the
36 * matrix storage convention.
37 */
38#define dACCESS33(A,i,j) ((A)[(i)*4+(j)])
39
40
41/*
42 * 3-way dot product. dDOTpq means that elements of `a' and `b' are spaced
43 * p and q indexes apart respectively. dDOT() means dDOT11.
44 * in C++ we could use function templates to get all the versions of these
45 * functions - but on some compilers this will result in sub-optimal code.
46 */
47
48#define dDOTpq(a,b,p,q) ((a)[0]*(b)[0] + (a)[p]*(b)[q] + (a)[2*(p)]*(b)[2*(q)])
49
50#ifdef __cplusplus
51
52PURE_INLINE dReal dDOT   (const dReal *a, const dReal *b) { return dDOTpq(a,b,1,1); }
53PURE_INLINE dReal dDOT13 (const dReal *a, const dReal *b) { return dDOTpq(a,b,1,3); }
54PURE_INLINE dReal dDOT31 (const dReal *a, const dReal *b) { return dDOTpq(a,b,3,1); }
55PURE_INLINE dReal dDOT33 (const dReal *a, const dReal *b) { return dDOTpq(a,b,3,3); }
56PURE_INLINE dReal dDOT14 (const dReal *a, const dReal *b) { return dDOTpq(a,b,1,4); }
57PURE_INLINE dReal dDOT41 (const dReal *a, const dReal *b) { return dDOTpq(a,b,4,1); }
58PURE_INLINE dReal dDOT44 (const dReal *a, const dReal *b) { return dDOTpq(a,b,4,4); }
59
60#else
61
62#define dDOT(a,b)   dDOTpq(a,b,1,1)
63#define dDOT13(a,b) dDOTpq(a,b,1,3)
64#define dDOT31(a,b) dDOTpq(a,b,3,1)
65#define dDOT33(a,b) dDOTpq(a,b,3,3)
66#define dDOT14(a,b) dDOTpq(a,b,1,4)
67#define dDOT41(a,b) dDOTpq(a,b,4,1)
68#define dDOT44(a,b) dDOTpq(a,b,4,4)
69
70#endif /* __cplusplus */
71
72
73/*
74 * cross product, set a = b x c. dCROSSpqr means that elements of `a', `b'
75 * and `c' are spaced p, q and r indexes apart respectively.
76 * dCROSS() means dCROSS111. `op' is normally `=', but you can set it to
77 * +=, -= etc to get other effects.
78 */
79
80#define dCROSS(a,op,b,c) \
81  (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \
82  (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \
83  (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]);
84#define dCROSSpqr(a,op,b,c,p,q,r) \
85  (a)[  0] op ((b)[  q]*(c)[2*r] - (b)[2*q]*(c)[  r]); \
86  (a)[  p] op ((b)[2*q]*(c)[  0] - (b)[  0]*(c)[2*r]); \
87  (a)[2*p] op ((b)[  0]*(c)[  r] - (b)[  q]*(c)[  0]);
88#define dCROSS114(a,op,b,c) dCROSSpqr(a,op,b,c,1,1,4)
89#define dCROSS141(a,op,b,c) dCROSSpqr(a,op,b,c,1,4,1)
90#define dCROSS144(a,op,b,c) dCROSSpqr(a,op,b,c,1,4,4)
91#define dCROSS411(a,op,b,c) dCROSSpqr(a,op,b,c,4,1,1)
92#define dCROSS414(a,op,b,c) dCROSSpqr(a,op,b,c,4,1,4)
93#define dCROSS441(a,op,b,c) dCROSSpqr(a,op,b,c,4,4,1)
94#define dCROSS444(a,op,b,c) dCROSSpqr(a,op,b,c,4,4,4)
95
96
97/*
98 * set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b.
99 * A is stored by rows, and has `skip' elements per row. the matrix is
100 * assumed to be already zero, so this does not write zero elements!
101 * if (plus,minus) is (+,-) then a positive version will be written.
102 * if (plus,minus) is (-,+) then a negative version will be written.
103 */
104
105#define dCROSSMAT(A,a,skip,plus,minus) \
106  (A)[1] = minus (a)[2]; \
107  (A)[2] = plus (a)[1]; \
108  (A)[(skip)+0] = plus (a)[2]; \
109  (A)[(skip)+2] = minus (a)[0]; \
110  (A)[2*(skip)+0] = minus (a)[1]; \
111  (A)[2*(skip)+1] = plus (a)[0];
112
113
114/*
115 * compute the distance between two 3-vectors
116 */
117
118#ifdef __cplusplus
119PURE_INLINE float dDISTANCE (const float a[3], const float b[3])
120        { return (float) dSqrt( (a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) + (a[2]-b[2])*(a[2]-b[2]) ); }
121PURE_INLINE double dDISTANCE (const double a[3], const double b[3])
122        { return dSqrt( (a[0]-b[0])*(a[0]-b[0]) + (a[1]-b[1])*(a[1]-b[1]) + (a[2]-b[2])*(a[2]-b[2]) ); }
123#else
124#define dDISTANCE(a,b) \
125        (dSqrt( ((a)[0]-(b)[0])*((a)[0]-(b)[0]) + ((a)[1]-(b)[1])*((a)[1]-(b)[1]) + ((a)[2]-(b)[2])*((a)[2]-(b)[2]) ))
126#endif
127
128
129/*
130 * special case matrix multipication, with operator selection
131 */
132
133#define dMULTIPLYOP0_331(A,op,B,C) \
134  (A)[0] op dDOT((B),(C)); \
135  (A)[1] op dDOT((B+4),(C)); \
136  (A)[2] op dDOT((B+8),(C));
137#define dMULTIPLYOP1_331(A,op,B,C) \
138  (A)[0] op dDOT41((B),(C)); \
139  (A)[1] op dDOT41((B+1),(C)); \
140  (A)[2] op dDOT41((B+2),(C));
141#define dMULTIPLYOP0_133(A,op,B,C) \
142  (A)[0] op dDOT14((B),(C)); \
143  (A)[1] op dDOT14((B),(C+1)); \
144  (A)[2] op dDOT14((B),(C+2));
145#define dMULTIPLYOP0_333(A,op,B,C) \
146  (A)[0] op dDOT14((B),(C)); \
147  (A)[1] op dDOT14((B),(C+1)); \
148  (A)[2] op dDOT14((B),(C+2)); \
149  (A)[4] op dDOT14((B+4),(C)); \
150  (A)[5] op dDOT14((B+4),(C+1)); \
151  (A)[6] op dDOT14((B+4),(C+2)); \
152  (A)[8] op dDOT14((B+8),(C)); \
153  (A)[9] op dDOT14((B+8),(C+1)); \
154  (A)[10] op dDOT14((B+8),(C+2));
155#define dMULTIPLYOP1_333(A,op,B,C) \
156  (A)[0] op dDOT44((B),(C)); \
157  (A)[1] op dDOT44((B),(C+1)); \
158  (A)[2] op dDOT44((B),(C+2)); \
159  (A)[4] op dDOT44((B+1),(C)); \
160  (A)[5] op dDOT44((B+1),(C+1)); \
161  (A)[6] op dDOT44((B+1),(C+2)); \
162  (A)[8] op dDOT44((B+2),(C)); \
163  (A)[9] op dDOT44((B+2),(C+1)); \
164  (A)[10] op dDOT44((B+2),(C+2));
165#define dMULTIPLYOP2_333(A,op,B,C) \
166  (A)[0] op dDOT((B),(C)); \
167  (A)[1] op dDOT((B),(C+4)); \
168  (A)[2] op dDOT((B),(C+8)); \
169  (A)[4] op dDOT((B+4),(C)); \
170  (A)[5] op dDOT((B+4),(C+4)); \
171  (A)[6] op dDOT((B+4),(C+8)); \
172  (A)[8] op dDOT((B+8),(C)); \
173  (A)[9] op dDOT((B+8),(C+4)); \
174  (A)[10] op dDOT((B+8),(C+8));
175
176#ifdef __cplusplus
177
178#define DECL template <class TA, class TB, class TC> PURE_INLINE void
179
180DECL dMULTIPLY0_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_331(A,=,B,C) }
181DECL dMULTIPLY1_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_331(A,=,B,C) }
182DECL dMULTIPLY0_133(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_133(A,=,B,C) }
183DECL dMULTIPLY0_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_333(A,=,B,C) }
184DECL dMULTIPLY1_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_333(A,=,B,C) }
185DECL dMULTIPLY2_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP2_333(A,=,B,C) }
186
187DECL dMULTIPLYADD0_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_331(A,+=,B,C) }
188DECL dMULTIPLYADD1_331(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_331(A,+=,B,C) }
189DECL dMULTIPLYADD0_133(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_133(A,+=,B,C) }
190DECL dMULTIPLYADD0_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP0_333(A,+=,B,C) }
191DECL dMULTIPLYADD1_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP1_333(A,+=,B,C) }
192DECL dMULTIPLYADD2_333(TA *A, const TB *B, const TC *C) { dMULTIPLYOP2_333(A,+=,B,C) }
193
194#undef DECL
195
196#else
197
198#define dMULTIPLY0_331(A,B,C) dMULTIPLYOP0_331(A,=,B,C)
199#define dMULTIPLY1_331(A,B,C) dMULTIPLYOP1_331(A,=,B,C)
200#define dMULTIPLY0_133(A,B,C) dMULTIPLYOP0_133(A,=,B,C)
201#define dMULTIPLY0_333(A,B,C) dMULTIPLYOP0_333(A,=,B,C)
202#define dMULTIPLY1_333(A,B,C) dMULTIPLYOP1_333(A,=,B,C)
203#define dMULTIPLY2_333(A,B,C) dMULTIPLYOP2_333(A,=,B,C)
204
205#define dMULTIPLYADD0_331(A,B,C) dMULTIPLYOP0_331(A,+=,B,C)
206#define dMULTIPLYADD1_331(A,B,C) dMULTIPLYOP1_331(A,+=,B,C)
207#define dMULTIPLYADD0_133(A,B,C) dMULTIPLYOP0_133(A,+=,B,C)
208#define dMULTIPLYADD0_333(A,B,C) dMULTIPLYOP0_333(A,+=,B,C)
209#define dMULTIPLYADD1_333(A,B,C) dMULTIPLYOP1_333(A,+=,B,C)
210#define dMULTIPLYADD2_333(A,B,C) dMULTIPLYOP2_333(A,+=,B,C)
211
212#endif
213
214// Terrain callback mod (start)
215#define dOP(a,op,b,c) \
216 (a)[0] = ((b)[0]) op ((c)[0]); \
217 (a)[1] = ((b)[1]) op ((c)[1]); \
218 (a)[2] = ((b)[2]) op ((c)[2]);
219#define dOPC(a,op,b,c) \
220 (a)[0] = ((b)[0]) op (c); \
221 (a)[1] = ((b)[1]) op (c); \
222 (a)[2] = ((b)[2]) op (c);
223#define dOPE(a,op,b) \
224 (a)[0] op ((b)[0]); \
225 (a)[1] op ((b)[1]); \
226 (a)[2] op ((b)[2]);
227#define dOPEC(a,op,c) \
228 (a)[0] op (c); \
229 (a)[1] op (c); \
230 (a)[2] op (c);
231#define dLENGTH(a) \
232 (dSqrt( ((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2]) ));
233#define dLENGTHSQUARED(a) \
234 (((a)[0])*((a)[0]) + ((a)[1])*((a)[1]) + ((a)[2])*((a)[2]));
235// Terrain callback mod (end)
236
237#ifdef __cplusplus
238extern "C" {
239#endif
240
241/*
242 * normalize 3x1 and 4x1 vectors (i.e. scale them to unit length)
243 */
244void dNormalize3 (dVector3 a);
245void dNormalize4 (dVector4 a);
246
247
248/*
249 * given a unit length "normal" vector n, generate vectors p and q vectors
250 * that are an orthonormal basis for the plane space perpendicular to n.
251 * i.e. this makes p,q such that n,p,q are all perpendicular to each other.
252 * q will equal n x p. if n is not unit length then p will be unit length but
253 * q wont be.
254 */
255
256void dPlaneSpace (const dVector3 n, dVector3 p, dVector3 q);
257
258#ifdef __cplusplus
259}
260#endif
261
262#endif
Note: See TracBrowser for help on using the repository browser.