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
148/* next define abbreviates VECROTATE with use of the SFRotation struct */
149#define VECRROTATE(ro,c) VECROTATE((ro).c[0],(ro).c[1],(ro).c[2],(ro).c[3],c)
150
151
152#define calc_vector_length(pt) veclength(pt)
153
154float veclength( struct point_XYZ p );
155
156/* returns vector length, too */
157GLDOUBLE vecnormal(struct point_XYZ*r, struct point_XYZ* v);
158
159#define normalize_vector(pt) vecnormal(pt,pt)
160
161float calc_angle_between_two_vectors(struct point_XYZ a, struct point_XYZ b);
162float calc_angle_between_two_vectors3f(float * a, float * b);
163double vecangle(struct point_XYZ* V1, struct point_XYZ* V2);
164
165
166#define calc_vector_product(a,b,c) veccross(c,a,b);
167
168void veccross(struct point_XYZ *c , struct point_XYZ a, struct point_XYZ b);
169
170int vecsamed(double *a, double *b);
171double signd(double val);
172double * vecsignd(double *b, double *a);
173double *vecsetd(double *b, double x, double y, double z);
174double *vecset4d(double *b, double x, double y, double z, double a);
175double * vecmuld(double *c, double *a, double *b);
176double * vecaddd(double *c, double *a, double *b);
177double *vecdifd(double *c, double* a, double *b);
178double * veccrossd(double *c, double *a, double *b);
179double veclengthd( double *p );
180double vecdotd(double *a, double *b);
181double* vecscaled(double* r, double* v, double s);
182double vecnormald(double *r, double *v);
183double *veccopyd(double *c, double *a);
184double *vecnegated(double *b, double *a);
185double *vecswizzle2d(double *inout );
186double * vecadd2d(double *c, double *a, double *b);
187double *vecdif2d(double *c, double* a, double *b);
188double veclength2d( double *p );
189double vecdot2d(double *a, double *b);
190double* vecscale2d(double* r, double* v, double s);
191double vecnormal2d(double *r, double *v);
192void vecprint3db(char *name, double *p, char *eol);
193void vecprint4db(char *name, double *p, char *eol);
194double *veccopy4d(double *c, double *a);
195double veclength4d( double *p );
196double *vecdif4d(double *c, double* a, double *b);
197
198int vecsame2f(float *a, float *b);
199float *vecset2f(float *b, float x, float y);
200float *veccopy2f(float *b, float *a);
201float * vecadd2f(float *c, float *a, float *b);
202float *vecdif2f(float *c, float* a, float *b);
203float veclength2f( float *p );
204float vecdot2f(float *a, float *b);
205float* vecscale2f(float* r, float* v, float s);
206float vecnormal2f(float *r, float *v);
207float *vecmult2f(float *c, float *a, float *b);
208
209void vecprint3fb(char *name, float *p, char *eol);
210int vecsame3f(float *a, float *b);
211float *veccopy3f(float *b, float *a);
212float *vecset3f(float *b, float x, float y, float z);
213float *vecadd3f(float *c, float *a, float *b);
214float *vecdif3f(float *c, float *a, float *b);
215float vecdot3f(float *a, float *b);
216float *veccross3f(float *c, float *a, float *b);
217float *vecscale3f(float *b, float *a, float scale);
218float *vecmult3f(float *c, float *a, float *b);
219float veclength3f(float *a);
220float *vecnormalize3f(float *b, float *a);
221float *vecnegate3f(float *b, float *a);
222float det3f(float *a, float *b, float *c);
223float *axisangle_rotate3f(float* b, float *a, float *axisangle);
224float *axisangle_rotate4f(float* axisAngleC, float *axisAngleA, float *axisAngleB);
225BOOL line_intersect_line_3f(float *p1, float *v1, float *p2, float *v2, float *t, float *s, float *x1, float *x2);
226BOOL line_intersect_planed_3f(float *p, float *v, float *N, float d, float *pi, float *t);
227BOOL line_intersect_plane_3f(float *p, float *v, float *N, float *pp, float *pi, float *t);
228BOOL line_intersect_cylinder_3f(float *p, float *v, float radius, float *pi);
229
230void vecprint4fb(char *name, float *p, char *eol);
231float vecdot4f( float *a, float *b );
232float *vecscale4f(float *b, float *a, float scale);
233float *veccopy4f(float *b, float *a);
234int vecsame4f(float *a, float *b);
235float *vecset4f(float *b, float x, float y, float z, float a);
236
237GLDOUBLE det3x3(GLDOUBLE* data);
238
239struct point_XYZ* transform(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b);
240float* transformf(float* r, const float* a, const GLDOUBLE* b);
241
242float* matmultvec3f(float* r3, float *mat3, float* a3 );
243float* vecmultmat3f(float* r3, float* a3, float *mat3 );
244BOOL matrix3x3_inverse_float(float *inn, float *outt);
245float* vecmultmat4f(float* r4, float *a4, float *mat4);
246float* matmultvec4f(float* r4, float *mat4, float* a4 );
247
248
249float* mat423f(float *out3x3, float *in4x4);
250float* matinverse3f(float *out3x3, float *in3x3);
251float* transform3x3f(float *out3, float *in3, float *mat3x3);
252
253struct point_XYZ* transformAFFINE(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b);
254GLDOUBLE* pointxyz2double(double* r, struct point_XYZ *p); /* instead of casting struct to array, this is more rigorous */
255struct point_XYZ* double2pointxyz(struct point_XYZ* r, double* p); /* ditto */
256double *transformAFFINEd(double *r, double *a, const GLDOUBLE* mat); /* same as transformAFFINE which is the same as transform() - just different parameter types */
257double * matrixAFFINE2RotationMatrix(double* rotmat, double *fullmat);
258double *transformUPPER3X3d(double *r, double *a, const GLDOUBLE* mat);
259double *transformFULL4d(double *r4, double *a4, double *mat);
260
261/*only transforms using the rotation component.
262 Usefull for transforming normals, and optimizing when you know there's no translation */
263struct point_XYZ* transform3x3(struct point_XYZ* r, const struct point_XYZ* a, const GLDOUBLE* b);
264
265struct point_XYZ* vecscale(struct point_XYZ* r, struct point_XYZ* v, GLDOUBLE s);
266
267double vecdot(struct point_XYZ* a, struct point_XYZ* b);
268
269#define calc_vector_scalar_product(a,b) vecdot(&(a),&(b))
270
271double closest_point_of_segment_to_y_axis_segment(double y1, double y2, struct point_XYZ p1, struct point_XYZ p2);
272
273struct point_XYZ* vecadd(struct point_XYZ* r, struct point_XYZ* v, struct point_XYZ* v2);
274
275struct point_XYZ* vecdiff(struct point_XYZ* r, struct point_XYZ* v, struct point_XYZ* v2);
276
277/*specify a direction "n", and you get two vectors i, and j, perpendicular to n and themselfs. */
278void make_orthogonal_vector_space(struct point_XYZ* i, struct point_XYZ* j, struct point_XYZ n);
279
280GLDOUBLE* matinverse(GLDOUBLE* res, GLDOUBLE* m);
281GLDOUBLE* matinverseFULL(GLDOUBLE* res, GLDOUBLE* m);
282GLDOUBLE* matinverseAFFINE(GLDOUBLE* res, GLDOUBLE* m);
283double *matidentity4d(double *b);
284double *mattranslate4d(double *mat, double* xyz);
285
286float* matinverse4f(float* res, float* mm);
287GLDOUBLE* mattranspose(GLDOUBLE* res, GLDOUBLE* m);
288float* mattranspose4f(float* res, float* mm);
289float *matidentity3f(float *b);
290float* matmultiply3f(float* r, float* mm , float* nn);
291float* mattranspose3f(float* res, float* mm);
292
293struct point_XYZ* polynormal(struct point_XYZ* r, struct point_XYZ* p1, struct point_XYZ* p2, struct point_XYZ* p3);
294/*simple wrapper for now. optimize later */
295struct point_XYZ* polynormalf(struct point_XYZ* r, float* p1, float* p2, float* p3);
296
297GLDOUBLE* matrotate(GLDOUBLE* Result, double Theta, double x, double y, double z);
298
299/*rotates dv back on iv*/
300double matrotate2v(GLDOUBLE* res, struct point_XYZ iv/*original*/, struct point_XYZ dv/*result*/);
301double matrotate2vd(GLDOUBLE* res, double * iv/*original*/, double * dv/*result*/);
302void rotate_v2v_axisAngled(double* axis, double* angle, double *orig, double *result);
303
304GLDOUBLE* mattranslate(GLDOUBLE* r, double dx, double dy, double dz);
305GLDOUBLE* matscale(GLDOUBLE* r, double sx, double sy, double sz);
306GLDOUBLE* matmultiply(GLDOUBLE* r, GLDOUBLE* m , GLDOUBLE* n);
307GLDOUBLE* matmultiplyFULL(GLDOUBLE* r, GLDOUBLE* m , GLDOUBLE* n);
308GLDOUBLE* matmultiplyAFFINE(GLDOUBLE* r, GLDOUBLE* m , GLDOUBLE* n);
309float* matmultiply4f(float* r, float* mm , float* nn);
310float *axisangle2matrix4f(float *b, float *axisangle);
311float *matidentity4f(float *b);
312void matrixFromAxisAngle4d(double *mat, double rangle, double x, double y, double z);
313
314void scale_to_matrix (double *mat, struct point_XYZ *scale);
315void loadIdentityMatrix (double *mat);
316double *matcopy(double *r, double*mat);
317float *matdouble2float4(float *rmat4, double *dmat4);
318void printmatrix2(GLDOUBLE* mat,char* description );
319void printmatrix3(GLDOUBLE *mat, char *description, int row_major);
320void general_slerp(double *ret, double *p1, double *p2, int size, const double t);
321void point_XYZ_slerp(struct point_XYZ *ret, struct point_XYZ *p1, struct point_XYZ *p2, const double t);
322
323float *veclerp3f(float *T, float *A, float *B, float alpha);
324float *veclerp2f(float *T, float *A, float *B, float alpha);
325double *veclerpd(double *T, double *A, double *B, double alpha);
326#endif /* __FREEWRL_LINEAR_ALGEBRA_H__ */