FreeWRL / FreeX3D 4.3.0
LinearAlgebra.h
1/*
2
3
4Linear 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
31double angleNormalized(double angle);
32
33#define VECSQ(a) VECPT(a,a)
34#define VECPT(a,b) ((a).x*(b).x + (a).y*(b).y + (a).z*(b).z)
35#define VECDIFF(a,b,c) {(c).x = (a).x-(b).x;(c).y = (a).y-(b).y;(c).z = (a).z-(b).z;}
36#define VECADD(a,b) {(a).x += (b).x; (a).y += (b).y; (a).z += (b).z;}
37#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];}
38#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;}
39#define VECSCALE(a,c) {(a).x *= c; (a).y *= c; (a).z *= c;}
40#define VECCOPY(a,b) {(a).x = (b).x; (a).y = (b).y; (a).z = (b).z;}
41
42/*special case ; used in Extrusion.GenPolyRep and ElevationGrid.GenPolyRep:
43 * Calc diff vec from 2 coordvecs which must be in the same field */
44#define VEC_FROM_COORDDIFF(f,a,g,b,v) {\
45 (v).x= (f)[(a)*3]-(g)[(b)*3]; \
46 (v).y= (f)[(a)*3+1]-(g)[(b)*3+1]; \
47 (v).z= (f)[(a)*3+2]-(g)[(b)*3+2]; \
48}
49
50/* rotate a vector along one axis */
51#define VECROTATE_X(c,angle) { \
52 /*(c).x = (c).x */ \
53 (c).y = cos(angle) * (c).y - sin(angle) * (c).z; \
54 (c).z = sin(angle) * (c).y + cos(angle) * (c).z; \
55 }
56#define VECROTATE_Y(c,angle) { \
57 (c).x = cos(angle)*(c).x + + sin(angle) * (c).z; \
58 /*(c).y = (c).y */ \
59 (c).z = -sin(angle)*(c).x + cos(angle) * (c).z; \
60 }
61#define VECROTATE_Z(c,angle) { \
62 (c).x = cos(angle)*(c).x - sin(angle) * (c).y; \
63 (c).y = sin(angle)*(c).x + cos(angle) * (c).y; \
64 /*(c).z = s (c).z; */ \
65 }
66
67#define MATRIX_ROTATION_X(angle,m) {\
68 m[0][0]=1; m[0][1]=0; m[0][2]=0; \
69 m[1][0]=0; m[1][1]=cos(angle); m[1][2]=- sin(angle); \
70 m[2][0]=0; m[2][1]=sin(angle); m[2][2]=cos(angle); \
71}
72#define MATRIX_ROTATION_Y(angle,m) {\
73 m[0][0]=cos(angle); m[0][1]=0; m[0][2]=sin(angle); \
74 m[1][0]=0; m[1][1]=1; m[1][2]=0; \
75 m[2][0]=-sin(angle); m[2][1]=0; m[2][2]=cos(angle); \
76}
77#define MATRIX_ROTATION_Z(angle,m) {\
78 m[0][0]=cos(angle); m[0][1]=- sin(angle); m[0][2]=0; \
79 m[1][0]=sin(angle); m[1][1]=cos(angle); m[1][2]=0; \
80 m[2][0]=0; m[2][1]=0; m[2][2]=1; \
81}
82
83/* next matrix calculation comes from comp.graphics.algorithms FAQ */
84/* the axis vector has to be normalized */
85#define MATRIX_FROM_ROTATION(ro,m) { \
86 struct { double x,y,z,w ; } __q; \
87 double sinHalfTheta = sin(0.5*(ro.c[3]));\
88 double xs, ys, zs, wx, wy, wz, xx, xy, xz, yy, yz, zz;\
89 __q.x = (ro.c[0])*sinHalfTheta;\
90 __q.y = (ro.c[1])*sinHalfTheta;\
91 __q.z = (ro.c[2])*sinHalfTheta;\
92 __q.w = cos(0.5*(ro.c[3]));\
93 xs = 2*__q.x; ys = 2*__q.y; zs = 2*__q.z;\
94 wx = __q.w*xs; wy = __q.w*ys; wz = __q.w*zs;\
95 xx = __q.x*xs; xy = __q.x*ys; xz = __q.x*zs;\
96 yy = __q.y*ys; yz = __q.y*zs; zz = __q.z*zs;\
97 m[0][0] = 1 - (yy + zz); m[0][1] = xy - wz; m[0][2] = xz + wy;\
98 m[1][0] = xy + wz; m[1][1] = 1 - (xx + zz);m[1][2] = yz - wx;\
99 m[2][0] = xz - wy; m[2][1] = yz + wx; m[2][2] = 1-(xx + yy);\
100}
101
102/* matrix multiplication */
103#define VECMM(m,c) { \
104 double ___x=(c).x,___y=(c).y,___z=(c).z; \
105 (c).x= m[0][0]*___x + m[0][1]*___y + m[0][2]*___z; \
106 (c).y= m[1][0]*___x + m[1][1]*___y + m[1][2]*___z; \
107 (c).z= m[2][0]*___x + m[2][1]*___y + m[2][2]*___z; \
108}
109
110
111/* next define rotates vector c with rotation vector r and angle */
112/* after section 5.8 of the VRML`97 spec */
113
114#define VECROTATE(rx,ry,rz,angle,nc) { \
115 double ___x=(nc).x,___y=(nc).y,___z=(nc).z; \
116 double ___c=cos(angle), ___s=sin(angle), ___t=1-___c; \
117 (nc).x= (___t*((rx)*(rx))+___c) *___x \
118 + (___t*(rx)*(ry) -___s*(rz))*___y \
119 + (___t*(rx)*(rz) +___s*(ry))*___z ; \
120 (nc).y= (___t*(rx)*(ry) +___s*(rz))*___x \
121 + (___t*((ry)*(ry))+___c) *___y \
122 + (___t*(ry)*(rz) -___s*(rx))*___z ; \
123 (nc).z= (___t*(rx)*(rz) -___s*(ry))*___x \
124 + (___t*(ry)*(rz) +___s*(rx))*___y \
125 + (___t*((rz)*(rz))+___c) *___z ; \
126 }
127
128
129/*
130#define VECROTATE(rx,ry,rz,angle,c) { \
131 double ___c=cos(angle), ___s=sin(angle), ___t=1-___c; \
132 (c).x= (___t*((rx)*(rx))+___c) *(c).x \
133 + (___t*(rx)*(ry) +___s*(rz))*(c).y \
134 + (___t*(rx)*(rz) -___s*(ry))*(c).z ; \
135 (c).y= (___t*(rx)*(ry) -___s*(rz))*(c).x \
136 + (___t*((ry)*(ry))+___c) *(c).y \
137 + (___t*(ry)*(rz) +___s*(rx))*(c).z ; \
138 (c).z= (___t*(rx)*(rz) +___s*(ry))*(c).x \
139 + (___t*(ry)*(rz) -___s*(rx))*(c).y \
140 + (___t*((rz)*(rz))+ ___c) *(c).z ; \
141 }
142
143*/
144
145float *double2float(float *b, const double *a, int n);
146double *float2double(double *b, float *a, int n);
147
148float fclamp(float fval, float fstart, float fend);
149float *vecclamp2f(float *fval, float *fstart, float *fend);
150float *vecclamp3f(float *fval, float *fstart, float *fend);
151float *fvecclamp3f(float *fval, float fstart, float fend);
152// #define APPROX(a,b) (fabs((a)-(b))<0.00000001)
153int approx3f(float *a, float *b);
154int approx4f(float *a, float *b);
155
156
157/* next define abbreviates VECROTATE with use of the SFRotation struct */
158#define VECRROTATE(ro,c) VECROTATE((ro).c[0],(ro).c[1],(ro).c[2],(ro).c[3],c)
159
160
161#define calc_vector_length(pt) veclength(pt)
162
163float veclength( struct point_XYZ p );
164
165/* returns vector length, too */
166double vecnormal(struct point_XYZ*r, struct point_XYZ* v);
167
168#define normalize_vector(pt) vecnormal(pt,pt)
169
170float calc_angle_between_two_vectors(struct point_XYZ a, struct point_XYZ b);
171float calc_angle_between_two_vectors3f(float * a, float * b);
172double vecangle(struct point_XYZ* V1, struct point_XYZ* V2);
173float vecangle2f(float * V1, float * V2);
174
175#define calc_vector_product(a,b,c) veccross(c,a,b);
176
177void veccross(struct point_XYZ *c , struct point_XYZ a, struct point_XYZ b);
178
179int vecsamed(double *a, double *b);
180double signd(double val);
181double * vecsignd(double *b, double *a);
182double *vecsetd(double *b, double x, double y, double z);
183double* vecset2d(double* b, double x, double y);
184double *vecset4d(double *b, double x, double y, double z, double a);
185double * vecmuld(double *c, double *a, double *b);
186double * vecaddd(double *c, double *a, double *b);
187double *vecdifd(double *c, double* a, double *b);
188double * veccrossd(double *c, double *a, double *b);
189double veclengthd( double *p );
190double vecdotd(double *a, double *b);
191double* vecscaled(double* r, double* v, double s);
192double vecnormald(double *r, double *v);
193double *veccopyd(double *c, double *a);
194double *vecnegated(double *b, double *a);
195double *vecswizzle2d(double *inout );
196double * vecadd2d(double *c, double *a, double *b);
197double *vecdif2d(double *c, double* a, double *b);
198double* veccopy2d(double* c, double* a);
199double veclength2d( double *p );
200double vecdot2d(double *a, double *b);
201double* vecscale2d(double* r, double* v, double s);
202double vecnormal2d(double *r, double *v);
203void vecprint3db(char *name, double *p, char *eol);
204void vecprint4db(char *name, double *p, char *eol);
205double *veccopy4d(double *c, double *a);
206double veclength4d( double *p );
207double *vecdif4d(double *c, double* a, double *b);
208double det3d(double *a, double *b, double *c);
209double* vecmult2d(double* c, double* a, double* b);
210
211int vecsame2f(float *a, float *b);
212float *vecset2f(float *b, float x, float y);
213float *veccopy2f(float *b, float *a);
214float * vecadd2f(float *c, float *a, float *b);
215float *vecdif2f(float *c, float* a, float *b);
216float veclength2f( float *p );
217float vecdot2f(float *a, float *b);
218float* vecscale2f(float* r, float* v, float s);
219float vecnormal2f(float *r, float *v);
220float *vecmult2f(float *c, float *a, float *b);
221
222void vecprint3fb(char *name, float *p, char *eol);
223int vecsame3f(float *a, float *b);
224int vecclose3f(float* a, float* b, float tol);
225int vecapprox3f(float *a, float *b, float tol);
226int vecapprox2f(float *a, float *b, float tol);
227float *veccopy3f(float *b, float *a);
228float *vecset3f(float *b, float x, float y, float z);
229float *vecadd3f(float *c, float *a, float *b);
230float *vecdif3f(float *c, float *a, float *b);
231float vecdot3f(float *a, float *b);
232float *veccross3f(float *c, float *a, float *b);
233float *vecscale3f(float *b, float *a, float scale);
234float *vecmult3f(float *c, float *a, float *b);
235double* vecmult3d(double* c, double* a, double* b);
236float veclength3f(float *a);
237float *vecnormalize3f(float *b, float *a);
238float *vecnegate3f(float *b, float *a);
239float det3f(float *a, float *b, float *c);
240float *axisangle_rotate3f(float* b, float *a, float *axisangle);
241double* axisangle_rotate3d(double* b, double* a, float* axisangle);
242float *axisangle_rotate4f(float* axisAngleC, float *axisAngleA, float *axisAngleB);
243int line_intersect_line_3f(float *p1, float *v1, float *p2, float *v2, float *t, float *s, float *x1, float *x2);
244int line_intersect_planed_3f(float *p, float *v, float *N, float d, float *pi, float *t);
245int line_intersect_plane_3f(float *p, float *v, float *N, float *pp, float *pi, float *t);
246int line_intersect_planed_3d(double *p, double *v, double *N, double d, double *pi, double *t);
247int line_intersect_plane_3d(double *p, double *v, double *N, double *pp, double *pi, double *t);
248int line_intersect_cylinder_3f(float *p, float *v, float radius, float *pi);
249
250void vecprint4fb(char *name, float *p, char *eol);
251float vecdot4f( float *a, float *b );
252double vecdot4d(double* a, double* b);
253float *vecscale4f(float *b, float *a, float scale);
254double* vecscale4d(double* b, double* a, double scale);
255float *veccopy4f(float *b, float *a);
256int vecsame4f(float *a, float *b);
257float *vecset4f(float *b, float x, float y, float z, float a);
258
259double det3x3(double* data);
260
261struct point_XYZ* transform(struct point_XYZ* r, const struct point_XYZ* a, const double* b);
262float* transformf(float* r, const float* a, const double* b);
263
264float* matmultvec3f(float* r3, float *mat3, float* a3 );
265float* vecmultmat3f(float* r3, float* a3, float *mat3 );
266int matrix3x3_inverse_float(float *inn, float *outt);
267float* vecmultmat4f(float* r4, float *a4, float *mat4);
268double* vecmultmat4d(double* r4, double* a4, double* mat4);
269float* matmultvec4f(float* r4, float *mat4, float* a4 );
270double* matmultvec4d(double* r4, double* mat4, double* a4);
271
272
273float* mat423f(float *out3x3, float *in4x4);
274float* matinverse3f(float *out3x3, float *in3x3);
275
276float* transform3x3f(float *out3, float *in3, float *mat3x3);
277
278struct point_XYZ* transformAFFINE(struct point_XYZ* r, const struct point_XYZ* a, const double* b);
279double* pointxyz2double(double* r, struct point_XYZ *p); /* instead of casting struct to array, this is more rigorous */
280struct point_XYZ* double2pointxyz(struct point_XYZ* r, double* p); /* ditto */
281double *transformAFFINEd(double *r, double *a, const double* mat); /* same as transformAFFINE which is the same as transform() - just different parameter types */
282double * matrixAFFINE2RotationMatrix(double* rotmat, double *fullmat);
283void AFFINEmatrix2axisangle(double* axisangle, double* matrix4);
284void AFFINEmatrix2axisangled(double* axisangle, double* matrix4);
285double *transformUPPER3X3d(double *r, double *a, const double* mat);
286double *transformFULL4d(double *r4, double *a4, double *mat);
287
288/*only transforms using the rotation component.
289 Usefull for transforming normals, and optimizing when you know there's no translation */
290struct point_XYZ* transform3x3(struct point_XYZ* r, const struct point_XYZ* a, const double* b);
291
292struct point_XYZ* vecscale(struct point_XYZ* r, struct point_XYZ* v, double s);
293
294double vecdot(struct point_XYZ* a, struct point_XYZ* b);
295
296#define calc_vector_scalar_product(a,b) vecdot(&(a),&(b))
297
298double closest_point_of_segment_to_y_axis_segment(double y1, double y2, struct point_XYZ p1, struct point_XYZ p2);
299
300struct point_XYZ* vecadd(struct point_XYZ* r, struct point_XYZ* v, struct point_XYZ* v2);
301
302struct point_XYZ* vecdiff(struct point_XYZ* r, struct point_XYZ* v, struct point_XYZ* v2);
303
304/*specify a direction "n", and you get two vectors i, and j, perpendicular to n and themselfs. */
305void make_orthogonal_vector_space(struct point_XYZ* i, struct point_XYZ* j, struct point_XYZ n);
306
307double* matinverse(double* res, double* m);
308double* matinverseFULL(double* res, double* m);
309double* matinverseAFFINE(double* res, double* m);
310double* mattranspose(double* res, double* m);
311
312double *matidentity4d(double *b);
313double *mattranslate4d(double *mat, double* xyz);
314double* matscale4d(double* mat, double* sxyz);
315
316float* matinverse4f(float* res, float* mm);
317float* mattranspose4f(float* res, float* mm);
318float* matmultiply4f(float* r, float* mm, float* nn);
319float* axisangle2matrix4f(float* b, float* axisangle);
320float* matidentity4f(float* b);
321
322double* matidentity3d(double* b);
323double* matmultiply3d(double* r, double* mm, double* nn);
324double* matinverse3d(double* out3x3, double* in3x3);
325double* mattranspose3d(double* res, double* mm);
326double* matmultvec3d(double* r3, double* mat3, double* a3);
327double* vecmultmat3d(double* r3, double* a3, double* mat3);
328
329
330float *matidentity3f(float *b);
331float* matmultiply3f(float* r, float* mm , float* nn);
332float* mattranspose3f(float* res, float* mm);
333
334double* matrotate(double* Result, double Theta, double x, double y, double z);
335
336/*rotates dv back on iv*/
337double matrotate2v(double* res, struct point_XYZ iv/*original*/, struct point_XYZ dv/*result*/);
338double matrotate2vd(double* res, double * iv/*original*/, double * dv/*result*/);
339void rotate_v2v_axisAngled(double* axis, double* angle, double *orig, double *result);
340
341double* mattranslate(double* r, double dx, double dy, double dz);
342double* matscale(double* r, double sx, double sy, double sz);
343double* matmultiply(double* r, double* m , double* n);
344double* matmultiplyFULL(double* r, double* m , double* n);
345double* matmultiplyAFFINE(double* r, double* m , double* n);
346void matrixFromAxisAngle4d(double *mat, double rangle, double x, double y, double z);
347void scale_to_matrix (double *mat, struct point_XYZ *scale);
348void loadIdentityMatrix (double *mat);
349double *matcopy(double *r, double*mat);
350float *matdouble2float4(float *rmat4, double *dmat4);
351void printmatrix2(double* mat,char* description );
352void printmatrix3(double *mat, char *description, int row_major);
353
354void general_slerp(double *ret, double *p1, double *p2, int size, const double t);
355void point_XYZ_slerp(struct point_XYZ *ret, struct point_XYZ *p1, struct point_XYZ *p2, const double t);
356
357float *veclerp3f(float *T, float *A, float *B, float alpha);
358float *veclerp2f(float *T, float *A, float *B, float alpha);
359double *veclerpd(double *T, double *A, double *B, double alpha);
360
361struct point_XYZ* polynormal(struct point_XYZ* r, struct point_XYZ* p1, struct point_XYZ* p2, struct point_XYZ* p3);
362/*simple wrapper for now. optimize later */
363struct point_XYZ* polynormalf(struct point_XYZ* r, float* p1, float* p2, float* p3);
364
365
366#endif /* __FREEWRL_LINEAR_ALGEBRA_H__ */