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);
149#define VECRROTATE(ro,c) VECROTATE((ro).c[0],(ro).c[1],(ro).c[2],(ro).c[3],c)
152#define calc_vector_length(pt) veclength(pt)
159#define normalize_vector(pt) vecnormal(pt,pt)
162float calc_angle_between_two_vectors3f(
float * a,
float * b);
166#define calc_vector_product(a,b,c) veccross(c,a,b);
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);
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);
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);
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);
237GLDOUBLE det3x3(GLDOUBLE* data);
240float* transformf(
float* r,
const float* a,
const GLDOUBLE* b);
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 );
249float* mat423f(
float *out3x3,
float *in4x4);
250float* matinverse3f(
float *out3x3,
float *in3x3);
251float* transform3x3f(
float *out3,
float *in3,
float *mat3x3);
254GLDOUBLE* pointxyz2double(
double* r,
struct point_XYZ *p);
256double *transformAFFINEd(
double *r,
double *a,
const GLDOUBLE* mat);
257double * matrixAFFINE2RotationMatrix(
double* rotmat,
double *fullmat);
258double *transformUPPER3X3d(
double *r,
double *a,
const GLDOUBLE* mat);
259double *transformFULL4d(
double *r4,
double *a4,
double *mat);
269#define calc_vector_scalar_product(a,b) vecdot(&(a),&(b))
271double closest_point_of_segment_to_y_axis_segment(
double y1,
double y2,
struct point_XYZ p1,
struct point_XYZ p2);
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);
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);
297GLDOUBLE* matrotate(GLDOUBLE* Result,
double Theta,
double x,
double y,
double z);
301double matrotate2vd(GLDOUBLE* res,
double * iv,
double * dv);
302void rotate_v2v_axisAngled(
double* axis,
double* angle,
double *orig,
double *result);
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);
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);
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);