FreeWRL/FreeX3D  3.0.0
LinearAlgebra.h
1 /*
2 
3 
4 Linear algebra.
5 
6 */
7 
8 /****************************************************************************
9  This file is part of the FreeWRL/FreeX3D Distribution.
10 
11  Copyright 2009 CRC Canada. (http://www.crc.gc.ca)
12 
13  FreeWRL/FreeX3D is free software: you can redistribute it and/or modify
14  it under the terms of the GNU Lesser Public License as published by
15  the Free Software Foundation, either version 3 of the License, or
16  (at your option) any later version.
17 
18  FreeWRL/FreeX3D is distributed in the hope that it will be useful,
19  but WITHOUT ANY WARRANTY; without even the implied warranty of
20  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21  GNU General Public License for more details.
22 
23  You should have received a copy of the GNU General Public License
24  along with FreeWRL/FreeX3D. If not, see <http://www.gnu.org/licenses/>.
25 ****************************************************************************/
26 
27 
28 #ifndef __FREEWRL_LINEAR_ALGEBRA_H__
29 #define __FREEWRL_LINEAR_ALGEBRA_H__
30 
31 #define VECSQ(a) VECPT(a,a)
32 #define VECPT(a,b) ((a).x*(b).x + (a).y*(b).y + (a).z*(b).z)
33 #define VECDIFF(a,b,c) {(c).x = (a).x-(b).x;(c).y = (a).y-(b).y;(c).z = (a).z-(b).z;}
34 #define VECADD(a,b) {(a).x += (b).x; (a).y += (b).y; (a).z += (b).z;}
35 #define VEC_FROM_CDIFF(a,b,r) {(r).x = (a).c[0]-(b).c[0];(r).y = (a).c[1]-(b).c[1];(r).z = (a).c[2]-(b).c[2];}
36 #define VECCP(a,b,c) {(c).x = (a).y*(b).z-(b).y*(a).z; (c).y = -((a).x*(b).z-(b).x*(a).z); (c).z = (a).x*(b).y-(b).x*(a).y;}
37 #define VECSCALE(a,c) {(a).x *= c; (a).y *= c; (a).z *= c;}
38 #define VECCOPY(a,b) {(a).x = (b).x; (a).y = (b).y; (a).z = (b).z;}
39 
40 /*special case ; used in Extrusion.GenPolyRep and ElevationGrid.GenPolyRep:
41  * Calc diff vec from 2 coordvecs which must be in the same field */
42 #define VEC_FROM_COORDDIFF(f,a,g,b,v) {\
43  (v).x= (f)[(a)*3]-(g)[(b)*3]; \
44  (v).y= (f)[(a)*3+1]-(g)[(b)*3+1]; \
45  (v).z= (f)[(a)*3+2]-(g)[(b)*3+2]; \
46 }
47 
48 /* rotate a vector along one axis */
49 #define VECROTATE_X(c,angle) { \
50  /*(c).x = (c).x */ \
51  (c).y = cos(angle) * (c).y - sin(angle) * (c).z; \
52  (c).z = sin(angle) * (c).y + cos(angle) * (c).z; \
53  }
54 #define VECROTATE_Y(c,angle) { \
55  (c).x = cos(angle)*(c).x + + sin(angle) * (c).z; \
56  /*(c).y = (c).y */ \
57  (c).z = -sin(angle)*(c).x + cos(angle) * (c).z; \
58  }
59 #define VECROTATE_Z(c,angle) { \
60  (c).x = cos(angle)*(c).x - sin(angle) * (c).y; \
61  (c).y = sin(angle)*(c).x + cos(angle) * (c).y; \
62  /*(c).z = s (c).z; */ \
63  }
64 
65 #define MATRIX_ROTATION_X(angle,m) {\
66  m[0][0]=1; m[0][1]=0; m[0][2]=0; \
67  m[1][0]=0; m[1][1]=cos(angle); m[1][2]=- sin(angle); \
68  m[2][0]=0; m[2][1]=sin(angle); m[2][2]=cos(angle); \
69 }
70 #define MATRIX_ROTATION_Y(angle,m) {\
71  m[0][0]=cos(angle); m[0][1]=0; m[0][2]=sin(angle); \
72  m[1][0]=0; m[1][1]=1; m[1][2]=0; \
73  m[2][0]=-sin(angle); m[2][1]=0; m[2][2]=cos(angle); \
74 }
75 #define MATRIX_ROTATION_Z(angle,m) {\
76  m[0][0]=cos(angle); m[0][1]=- sin(angle); m[0][2]=0; \
77  m[1][0]=sin(angle); m[1][1]=cos(angle); m[1][2]=0; \
78  m[2][0]=0; m[2][1]=0; m[2][2]=1; \
79 }
80 
81 /* next matrix calculation comes from comp.graphics.algorithms FAQ */
82 /* the axis vector has to be normalized */
83 #define MATRIX_FROM_ROTATION(ro,m) { \
84  struct { double x,y,z,w ; } __q; \
85  double sinHalfTheta = sin(0.5*(ro.c[3]));\
86  double xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;\
87  __q.x = (ro.c[0])*sinHalfTheta;\
88  __q.y = (ro.c[1])*sinHalfTheta;\
89  __q.z = (ro.c[2])*sinHalfTheta;\
90  __q.w = cos(0.5*(ro.c[3]));\
91  xs = 2*__q.x; ys = 2*__q.y; zs = 2*__q.z;\
92  wx = __q.w*xs; wy = __q.w*ys; wz = __q.w*zs;\
93  xx = __q.x*xs; xy = __q.x*ys; xz = __q.x*zs;\
94  yy = __q.y*ys; yz = __q.y*zs; zz = __q.z*zs;\
95  m[0][0] = 1 - (yy + zz); m[0][1] = xy - wz; m[0][2] = xz + wy;\
96  m[1][0] = xy + wz; m[1][1] = 1 - (xx + zz);m[1][2] = yz - wx;\
97  m[2][0] = xz - wy; m[2][1] = yz + wx; m[2][2] = 1-(xx + yy);\
98 }
99 
100 /* matrix multiplication */
101 #define VECMM(m,c) { \
102  double ___x=(c).x,___y=(c).y,___z=(c).z; \
103  (c).x= m[0][0]*___x + m[0][1]*___y + m[0][2]*___z; \
104  (c).y= m[1][0]*___x + m[1][1]*___y + m[1][2]*___z; \
105  (c).z= m[2][0]*___x + m[2][1]*___y + m[2][2]*___z; \
106 }
107 
108 
109 /* next define rotates vector c with rotation vector r and angle */
110 /* after section 5.8 of the VRML`97 spec */
111 
112 #define VECROTATE(rx,ry,rz,angle,nc) { \
113  double ___x=(nc).x,___y=(nc).y,___z=(nc).z; \
114  double ___c=cos(angle), ___s=sin(angle), ___t=1-___c; \
115  (nc).x= (___t*((rx)*(rx))+___c) *___x \
116  + (___t*(rx)*(ry) -___s*(rz))*___y \
117  + (___t*(rx)*(rz) +___s*(ry))*___z ; \
118  (nc).y= (___t*(rx)*(ry) +___s*(rz))*___x \
119  + (___t*((ry)*(ry))+___c) *___y \
120  + (___t*(ry)*(rz) -___s*(rx))*___z ; \
121  (nc).z= (___t*(rx)*(rz) -___s*(ry))*___x \
122  + (___t*(ry)*(rz) +___s*(rx))*___y \
123  + (___t*((rz)*(rz))+___c) *___z ; \
124  }
125 
126 
127 /*
128 #define VECROTATE(rx,ry,rz,angle,c) { \
129  double ___c=cos(angle), ___s=sin(angle), ___t=1-___c; \
130  (c).x= (___t*((rx)*(rx))+___c) *(c).x \
131  + (___t*(rx)*(ry) +___s*(rz))*(c).y \
132  + (___t*(rx)*(rz) -___s*(ry))*(c).z ; \
133  (c).y= (___t*(rx)*(ry) -___s*(rz))*(c).x \
134  + (___t*((ry)*(ry))+___c) *(c).y \
135  + (___t*(ry)*(rz) +___s*(rx))*(c).z ; \
136  (c).z= (___t*(rx)*(rz) +___s*(ry))*(c).x \
137  + (___t*(ry)*(rz) -___s*(rx))*(c).y \
138  + (___t*((rz)*(rz))+ ___c) *(c).z ; \
139  }
140 
141 */
142 
143 float *double2float(float *b, const double *a, int n);
144 double *float2double(double *b, float *a, int n);
145 
146 /* next define abbreviates VECROTATE with use of the SFRotation struct */
147 #define VECRROTATE(ro,c) VECROTATE((ro).c[0],(ro).c[1],(ro).c[2],(ro).c[3],c)
148 
149 
150 #define calc_vector_length(pt) veclength(pt)
151 
152 float veclength( struct point_XYZ p );
153 
154 /* returns vector length, too */
155 GLDOUBLE vecnormal(struct point_XYZ*r, struct point_XYZ* v);
156 
157 #define normalize_vector(pt) vecnormal(pt,pt)
158 
159 float calc_angle_between_two_vectors(struct point_XYZ a, struct point_XYZ b);
160 
161 double vecangle(struct point_XYZ* V1, struct point_XYZ* V2);
162 
163 
164 #define calc_vector_product(a,b,c) veccross(c,a,b);
165 
166 void veccross(struct point_XYZ *c , struct point_XYZ a, struct point_XYZ b);
167 
168 double signd(double val);
169 double * vecsignd(double *b, double *a);
170 double *vecsetd(double *b, double x, double y, double z);
171 double * vecmuld(double *c, double *a, double *b);
172 double * vecaddd(double *c, double *a, double *b);
173 double *vecdifd(double *c, double* a, double *b);
174 double * veccrossd(double *c, double *a, double *b);
175 double veclengthd( double *p );
176 double vecdotd(double *a, double *b);
177 double* vecscaled(double* r, double* v, double s);
178 double vecnormald(double *r, double *v);
179 double *veccopyd(double *c, double *a);
180 double *vecnegated(double *b, double *a);
181 
182 double * vecadd2d(double *c, double *a, double *b);
183 double *vecdif2d(double *c, double* a, double *b);
184 double veclength2d( double *p );
185 double vecdot2d(double *a, double *b);
186 double* vecscale2d(double* r, double* v, double s);
187 double vecnormal2d(double *r, double *v);
188 
189 int vecsame2f(float *a, float *b);
190 float *vecset2f(float *b, float x, float y);
191 float *veccopy2f(float *b, float *a);
192 float * vecadd2f(float *c, float *a, float *b);
193 float *vecdif2f(float *c, float* a, float *b);
194 float veclength2f( float *p );
195 float vecdot2f(float *a, float *b);
196 float* vecscale2f(float* r, float* v, float s);
197 float vecnormal2f(float *r, float *v);
198 float *vecmult2f(float *c, float *a, float *b);
199 
200 int vecsame3f(float *a, float *b);
201 float *veccopy3f(float *b, float *a);
202 float *vecset3f(float *b, float x, float y, float z);
203 float *vecadd3f(float *c, float *a, float *b);
204 float *vecdif3f(float *c, float *a, float *b);
205 float vecdot3f(float *a, float *b);
206 float *veccross3f(float *c, float *a, float *b);
207 float *vecscale3f(float *b, float *a, float scale);
208 float *vecmult3f(float *c, float *a, float *b);
209 float veclength3f(float *a);
210 float *vecnormalize3f(float *b, float *a);
211 float det3f(float *a, float *b, float *c);
212 float *axisangle_rotate3f(float* b, float *a, float *axisangle);
213 BOOL line_intersect_line_3f(float *p1, float *v1, float *p2, float *v2, float *t, float *s, float *x1, float *x2);
214 BOOL line_intersect_planed_3f(float *p, float *v, float *N, float d, float *pi, float *t);
215 BOOL line_intersect_plane_3f(float *p, float *v, float *N, float *pp, float *pi, float *t);
216 BOOL line_intersect_cylinder_3f(float *p, float *v, float radius, float *pi);
217 
218 
219 float vecdot4f( float *a, float *b );
220 float *vecscale4f(float *b, float *a, float scale);
221 float *veccopy4f(float *b, float *a);
222 int vecsame4f(float *a, float *b);
223 
224 GLDOUBLE det3x3(GLDOUBLE* data);
225 
226 struct point_XYZ* transform(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b);
227 float* transformf(float* r, const float* a, const GLDOUBLE* b);
228 
229 float* matmultvec3f(float* r3, float *mat3, float* a3 );
230 float* vecmultmat3f(float* r3, float* a3, float *mat3 );
231 BOOL matrix3x3_inverse_float(float *inn, float *outt);
232 float* vecmultmat4f(float* r4, float *a4, float *mat4);
233 float* matmultvec4f(float* r4, float *mat4, float* a4 );
234 
235 
236 float* mat423f(float *out3x3, float *in4x4);
237 float* matinverse3f(float *out3x3, float *in3x3);
238 float* transform3x3f(float *out3, float *in3, float *mat3x3);
239 
240 struct point_XYZ* transformAFFINE(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b);
241 GLDOUBLE* pointxyz2double(double* r, struct point_XYZ *p); /* instead of casting struct to array, this is more rigorous */
242 struct point_XYZ* double2pointxyz(struct point_XYZ* r, double* p); /* ditto */
243 double *transformAFFINEd(double *r, double *a, const GLDOUBLE* mat); /* same as transformAFFINE which is the same as transform() - just different parameter types */
244 double *transformUPPER3X3d(double *r, double *a, const GLDOUBLE* mat);
245 double *transformFULL4d(double *r4, double *a4, double *mat);
246 
247 /*only transforms using the rotation component.
248  Usefull for transforming normals, and optimizing when you know there's no translation */
249 struct point_XYZ* transform3x3(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b);
250 
251 struct point_XYZ* vecscale(struct point_XYZ* r, struct point_XYZ* v, GLDOUBLE s);
252 
253 double vecdot(struct point_XYZ* a, struct point_XYZ* b);
254 
255 #define calc_vector_scalar_product(a,b) vecdot(&(a),&(b))
256 
257 double closest_point_of_segment_to_y_axis_segment(double y1, double y2, struct point_XYZ p1, struct point_XYZ p2);
258 
259 struct point_XYZ* vecadd(struct point_XYZ* r, struct point_XYZ* v, struct point_XYZ* v2);
260 
261 struct point_XYZ* vecdiff(struct point_XYZ* r, struct point_XYZ* v, struct point_XYZ* v2);
262 
263 /*specify a direction "n", and you get two vectors i, and j, perpendicular to n and themselfs. */
264 void make_orthogonal_vector_space(struct point_XYZ* i, struct point_XYZ* j, struct point_XYZ n);
265 
266 GLDOUBLE* matinverse(GLDOUBLE* res, GLDOUBLE* m);
267 GLDOUBLE* matinverseFULL(GLDOUBLE* res, GLDOUBLE* m);
268 GLDOUBLE* matinverseAFFINE(GLDOUBLE* res, GLDOUBLE* m);
269 
270 float* matinverse4f(float* res, float* mm);
271 GLDOUBLE* mattranspose(GLDOUBLE* res, GLDOUBLE* m);
272 float* mattranspose4f(float* res, float* mm);
273 float *matidentity3f(float *b);
274 float* matmultiply3f(float* r, float* mm , float* nn);
275 float* mattranspose3f(float* res, float* mm);
276 
277 struct point_XYZ* polynormal(struct point_XYZ* r, struct point_XYZ* p1, struct point_XYZ* p2, struct point_XYZ* p3);
278 /*simple wrapper for now. optimize later */
279 struct point_XYZ* polynormalf(struct point_XYZ* r, float* p1, float* p2, float* p3);
280 
281 GLDOUBLE* matrotate(GLDOUBLE* Result, double Theta, double x, double y, double z);
282 
283 /*rotates dv back on iv*/
284 double matrotate2v(GLDOUBLE* res, struct point_XYZ iv/*original*/, struct point_XYZ dv/*result*/);
285 void rotate_v2v_axisAngled(double* axis, double* angle, double *orig, double *result);
286 
287 GLDOUBLE* mattranslate(GLDOUBLE* r, double dx, double dy, double dz);
288 
289 GLDOUBLE* matmultiply(GLDOUBLE* r, GLDOUBLE* m , GLDOUBLE* n);
290 GLDOUBLE* matmultiplyFULL(GLDOUBLE* r, GLDOUBLE* m , GLDOUBLE* n);
291 GLDOUBLE* matmultiplyAFFINE(GLDOUBLE* r, GLDOUBLE* m , GLDOUBLE* n);
292 float* matmultiply4f(float* r, float* mm , float* nn);
293 float *axisangle2matrix4f(float *b, float *axisangle);
294 float *matidentity4f(float *b);
295 void matrixFromAxisAngle4d(double *mat, double rangle, double x, double y, double z);
296 
297 void scale_to_matrix (double *mat, struct point_XYZ *scale);
298 void loadIdentityMatrix (double *mat);
299 double *matcopy(double *r, double*mat);
300 float *matdouble2float4(float *rmat4, double *dmat4);
301 void printmatrix2(GLDOUBLE* mat,char* description );
302 void printmatrix3(GLDOUBLE *mat, char *description, int row_major);
303 void general_slerp(double *ret, double *p1, double *p2, int size, const double t);
304 void point_XYZ_slerp(struct point_XYZ *ret, struct point_XYZ *p1, struct point_XYZ *p2, const double t);
305 #endif /* __FREEWRL_LINEAR_ALGEBRA_H__ */
point_XYZ
Definition: Structs.h:34