28#ifndef __FREEWRL_LINEAR_ALGEBRA_H__
29#define __FREEWRL_LINEAR_ALGEBRA_H__
31double angleNormalized(
double angle);
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;}
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]; \
51#define VECROTATE_X(c,angle) { \
53 (c).y = cos(angle) * (c).y - sin(angle) * (c).z; \
54 (c).z = sin(angle) * (c).y + cos(angle) * (c).z; \
56#define VECROTATE_Y(c,angle) { \
57 (c).x = cos(angle)*(c).x + + sin(angle) * (c).z; \
59 (c).z = -sin(angle)*(c).x + cos(angle) * (c).z; \
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; \
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); \
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); \
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; \
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);\
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; \
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 ; \
145float *double2float(
float *b,
const double *a,
int n);
146double *float2double(
double *b,
float *a,
int n);
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);
153int approx3f(
float *a,
float *b);
154int approx4f(
float *a,
float *b);
158#define VECRROTATE(ro,c) VECROTATE((ro).c[0],(ro).c[1],(ro).c[2],(ro).c[3],c)
161#define calc_vector_length(pt) veclength(pt)
168#define normalize_vector(pt) vecnormal(pt,pt)
171float calc_angle_between_two_vectors3f(
float * a,
float * b);
173float vecangle2f(
float * V1,
float * V2);
175#define calc_vector_product(a,b,c) veccross(c,a,b);
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);
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);
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);
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);
259double det3x3(
double* data);
262float* transformf(
float* r,
const float* a,
const double* b);
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);
273float* mat423f(
float *out3x3,
float *in4x4);
274float* matinverse3f(
float *out3x3,
float *in3x3);
276float* transform3x3f(
float *out3,
float *in3,
float *mat3x3);
279double* pointxyz2double(
double* r,
struct point_XYZ *p);
281double *transformAFFINEd(
double *r,
double *a,
const double* mat);
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);
296#define calc_vector_scalar_product(a,b) vecdot(&(a),&(b))
298double closest_point_of_segment_to_y_axis_segment(
double y1,
double y2,
struct point_XYZ p1,
struct point_XYZ p2);
307double* matinverse(
double* res,
double* m);
308double* matinverseFULL(
double* res,
double* m);
309double* matinverseAFFINE(
double* res,
double* m);
310double* mattranspose(
double* res,
double* m);
312double *matidentity4d(
double *b);
313double *mattranslate4d(
double *mat,
double* xyz);
314double* matscale4d(
double* mat,
double* sxyz);
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);
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);
330float *matidentity3f(
float *b);
331float* matmultiply3f(
float* r,
float* mm ,
float* nn);
332float* mattranspose3f(
float* res,
float* mm);
334double* matrotate(
double* Result,
double Theta,
double x,
double y,
double z);
338double matrotate2vd(
double* res,
double * iv,
double * dv);
339void rotate_v2v_axisAngled(
double* axis,
double* angle,
double *orig,
double *result);
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);
354void general_slerp(
double *ret,
double *p1,
double *p2,
int size,
const double t);
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);