00001 #ifndef MATVEC_MACROS_H
00002 #define MATVEC_MACROS_H
00003
00004
00012 #include <math.h>
00013
00014
00017 #define MATVEC_BEGIN do{
00018 #define MATVEC_END } while(0)
00019
00023
00048 #define MATVEC_MAT3X3_COMP(matrix,list) \
00049 MATVEC_MAT3X3_COMP_EVAL(matrix,list)
00050
00070 #define MATVEC_MAT3X3_COMP_EVAL( matrix, \
00071 m00, m01, m02, \
00072 m10, m11, m12, \
00073 m20, m21, m22) \
00074 (matrix)m00,(matrix)m01,(matrix)m02, \
00075 (matrix)m10,(matrix)m11,(matrix)m12, \
00076 (matrix)m20,(matrix)m21,(matrix)m22
00077
00078
00091 #define MATVEC_MAT4X4_COMP(matrix,list) \
00092 MATVEC_MAT4X4_COMP_EVAL(matrix,list)
00093
00120 #define MATVEC_MAT4X4_COMP_EVAL( \
00121 matrix, \
00122 m00, m01, m02, m03, \
00123 m10, m11, m12, m13, \
00124 m20, m21, m22, m23, \
00125 m30, m31, m32, m33 \
00126 ) \
00127 (matrix)m00,(matrix)m01,(matrix)m02,(matrix)m03, \
00128 (matrix)m10,(matrix)m11,(matrix)m12,(matrix)m13, \
00129 (matrix)m20,(matrix)m21,(matrix)m22,(matrix)m23, \
00130 (matrix)m30,(matrix)m31,(matrix)m32,(matrix)m33
00131
00135
00136
00137
00138
00139
00148 #define MATVEC_MAT3X3_AS_MDARRAY_COMP \
00149 [0][0],[0][1],[0][2], \
00150 [1][0],[1][1],[1][2], \
00151 [2][0],[2][1],[2][2]
00152
00159 #define MATVEC_MAT4X4_AS_MDARRAY_COMP \
00160 [0][0],[0][1],[0][2],[0][3], \
00161 [1][0],[1][1],[1][2],[1][3], \
00162 [2][0],[2][1],[2][2],[2][3], \
00163 [3][0],[3][1],[3][2],[3][3]
00164
00173 #define MATVEC_MAT3X3_AS_ARRAY_COMP(ncols) \
00174 [0*(ncols)+0],[0*(ncols)+1],[0*(ncols)+2], \
00175 [1*(ncols)+0],[1*(ncols)+1],[1*(ncols)+2], \
00176 [2*(ncols)+0],[2*(ncols)+1],[2*(ncols)+2]
00177
00178
00187 #define MATVEC_MAT4X4_AS_ARRAY_COMP(ncols) \
00188 [0*(ncols)+0],[0*(ncols)+1],[0*(ncols)+2],[0*(ncols)+3], \
00189 [1*(ncols)+0],[1*(ncols)+1],[1*(ncols)+2],[1*(ncols)+3], \
00190 [2*(ncols)+0],[2*(ncols)+1],[2*(ncols)+2],[2*(ncols)+3], \
00191 [3*(ncols)+0],[3*(ncols)+1],[3*(ncols)+2],[3*(ncols)+3]
00192
00193
00197
00198
00199
00200
00201
00202
00203
00204
00226 #define MATVEC_MAT3X3_AS_MDARRAY(matrix) \
00227 MATVEC_MAT3X3_COMP(matrix,MATVEC_MAT3X3_AS_MDARRAY_COMP)
00228
00229
00251 #define MATVEC_MAT4X4_AS_MDARRAY(matrix) \
00252 MATVEC_MAT4X4_COMP(matrix,MATVEC_MAT4X4_AS_MDARRAY_COMP)
00253
00254
00255
00256
00257
00291 #define MATVEC_MAT3X3_AS_ARRAY(matrix,ncols) \
00292 MATVEC_MAT3X3_COMP(matrix,MATVEC_MAT3X3_AS_ARRAY_COMP(ncols))
00293
00294
00295
00329 #define MATVEC_MAT4X4_AS_ARRAY(matrix,ncols) \
00330 MATVEC_MAT4X4_COMP(matrix,MATVEC_MAT4X4_AS_ARRAY_COMP(ncols))
00331
00332
00333
00334
00335
00336
00337
00347 #define MATVEC_VEC_AS_ARRAY(vector,stride) \
00348 (vector)[0],(vector)[stride],(vector)[2*stride]
00349
00370 #define MATVEC_VEC_AS_STRUCT(vector,x,y,z) \
00371 (vector).x,(vector).y,(vector).z
00372
00379
00380
00399 #define MATVEC_MAT3X3_ASSIGN_COMP( \
00400 d00, d01, d02, \
00401 d10, d11, d12, \
00402 d20, d21, d22, \
00403 s00, s01, s02, \
00404 s10, s11, s12, \
00405 s20, s21, s22 \
00406 ) \
00407 MATVEC_BEGIN \
00408 (d00) = (s00); \
00409 (d01) = (s01); \
00410 (d02) = (s02); \
00411 (d10) = (s10); \
00412 (d11) = (s11); \
00413 (d12) = (s12); \
00414 (d20) = (s20); \
00415 (d21) = (s21); \
00416 (d22) = (s22); \
00417 MATVEC_END
00418
00430 #define MATVEC_MAT4X4_ASSIGN_COMP( \
00431 d00, d01, d02, d03, \
00432 d10, d11, d12, d13, \
00433 d20, d21, d22, d23, \
00434 d30, d31, d32, d33, \
00435 s00, s01, s02, s03, \
00436 s10, s11, s12, s13, \
00437 s20, s21, s22, s23, \
00438 s30, s31, s32, s33 \
00439 ) \
00440 MATVEC_BEGIN \
00441 (d00) = (s00); \
00442 (d01) = (s01); \
00443 (d02) = (s02); \
00444 (d03) = (s03); \
00445 (d10) = (s10); \
00446 (d11) = (s11); \
00447 (d12) = (s12); \
00448 (d13) = (s13); \
00449 (d20) = (s20); \
00450 (d21) = (s21); \
00451 (d22) = (s22); \
00452 (d23) = (s23); \
00453 (d30) = (s30); \
00454 (d31) = (s31); \
00455 (d32) = (s32); \
00456 (d33) = (s33); \
00457 MATVEC_END
00458
00481 #define MATVEC_MAT3X3_ASSIGN(dst,src) \
00482 MATVEC_MAT3X3_ASSIGN_COMP(dst,src)
00483
00484
00503 #define MATVEC_MAT4X4_ASSIGN(dst,src) \
00504 MATVEC_MAT4X4_ASSIGN_COMP(dst,src)
00505
00506
00507
00518 #define MATVEC_MAT3X3_SET(matrix,value) \
00519 MATVEC_MAT3X3_ASSIGN_COMP( \
00520 matrix, \
00521 (value),(value),(value), \
00522 (value),(value),(value), \
00523 (value),(value),(value) \
00524 )
00525
00526
00537 #define MATVEC_MAT4X4_SET(matrix,value) \
00538 MATVEC_MAT4X4_ASSIGN_COMP( \
00539 matrix, \
00540 (value),(value),(value),(value), \
00541 (value),(value),(value),(value), \
00542 (value),(value),(value),(value), \
00543 (value),(value),(value),(value), \
00544 )
00545
00546
00554 #define MATVEC_MAT3X3_IDENTITY(matrix) \
00555 MATVEC_MAT3X3_ASSIGN_COMP( \
00556 matrix, \
00557 1,0,0, \
00558 0,1,0, \
00559 0,0,1 \
00560 )
00561
00562
00570 #define MATVEC_MAT4X4_IDENTITY(matrix) \
00571 MATVEC_MAT4X4_ASSIGN_COMP( \
00572 matrix, \
00573 1,0,0,0, \
00574 0,1,0,0, \
00575 0,0,1,0, \
00576 0,0,0,1 \
00577 )
00578
00579
00580
00593 #define MATVEC_MAT3X3_TRANSPOSE_COMP( \
00594 d00, d01, d02, \
00595 d10, d11, d12, \
00596 d20, d21, d22, \
00597 s00, s01, s02, \
00598 s10, s11, s12, \
00599 s20, s21, s22 \
00600 ) \
00601 MATVEC_BEGIN \
00602 (d00) = (s00); \
00603 (d01) = (s10); \
00604 (d02) = (s20); \
00605 (d10) = (s01); \
00606 (d11) = (s11); \
00607 (d12) = (s21); \
00608 (d20) = (s02); \
00609 (d21) = (s12); \
00610 (d22) = (s22); \
00611 MATVEC_END
00612
00624 #define MATVEC_MAT4X4_TRANSPOSE_COMP( \
00625 d00, d01, d02, d03, \
00626 d10, d11, d12, d13, \
00627 d20, d21, d22, d23, \
00628 d30, d31, d32, d33, \
00629 s00, s01, s02, s03, \
00630 s10, s11, s12, s13, \
00631 s20, s21, s22, s23, \
00632 s30, s31, s32, s33 \
00633 ) \
00634 MATVEC_BEGIN \
00635 (d00) = (s00); \
00636 (d01) = (s10); \
00637 (d02) = (s20); \
00638 (d03) = (s30); \
00639 (d10) = (s01); \
00640 (d11) = (s11); \
00641 (d12) = (s21); \
00642 (d13) = (s31); \
00643 (d20) = (s02); \
00644 (d21) = (s12); \
00645 (d22) = (s22); \
00646 (d23) = (s32); \
00647 (d30) = (s03); \
00648 (d31) = (s13); \
00649 (d32) = (s23); \
00650 (d33) = (s33); \
00651 MATVEC_END
00652
00666 #define MATVEC_MAT3X3_TRANSPOSE(res,src) \
00667 MATVEC_MAT3X3_TRANSPOSE_COMP(res,src)
00668
00680 #define MATVEC_MAT4X4_TRANSPOSE(res,src) \
00681 MATVEC_MAT4X4_TRANSPOSE_COMP(res,src)
00682
00683
00697 #define MATVEC_MAT3X3_MULT_COMP( \
00698 d00, d01, d02, \
00699 d10, d11, d12, \
00700 d20, d21, d22, \
00701 a00, a01, a02, \
00702 a10, a11, a12, \
00703 a20, a21, a22, \
00704 b00, b01, b02, \
00705 b10, b11, b12, \
00706 b20, b21, b22 \
00707 ) \
00708 MATVEC_BEGIN \
00709 (d00) = (a00) * (b00) + (a01) * (b10) + (a02) * (b20); \
00710 (d01) = (a00) * (b01) + (a01) * (b11) + (a02) * (b21); \
00711 (d02) = (a00) * (b02) + (a01) * (b12) + (a02) * (b22); \
00712 (d10) = (a10) * (b00) + (a11) * (b10) + (a12) * (b20); \
00713 (d11) = (a10) * (b01) + (a11) * (b11) + (a12) * (b21); \
00714 (d12) = (a10) * (b02) + (a11) * (b12) + (a12) * (b22); \
00715 (d20) = (a20) * (b00) + (a21) * (b10) + (a22) * (b20); \
00716 (d21) = (a20) * (b01) + (a21) * (b11) + (a22) * (b21); \
00717 (d22) = (a20) * (b02) + (a21) * (b12) + (a22) * (b22); \
00718 MATVEC_END
00719
00720
00721
00734 #define MATVEC_MAT4X4_MULT_COMP( \
00735 d00, d01, d02, d03, \
00736 d10, d11, d12, d13, \
00737 d20, d21, d22, d23, \
00738 d30, d31, d32, d33, \
00739 a00, a01, a02, a03, \
00740 a10, a11, a12, a13, \
00741 a20, a21, a22, a23, \
00742 a30, a31, a32, a33, \
00743 b00, b01, b02, b03, \
00744 b10, b11, b12, b13, \
00745 b20, b21, b22, b23, \
00746 b30, b31, b32, b33 \
00747 ) \
00748 MATVEC_BEGIN \
00749 (d00) = (a00) * (b00) + (a01) * (b10) + (a02) * (b20) + (a03) * (b30); \
00750 (d01) = (a00) * (b01) + (a01) * (b11) + (a02) * (b21) + (a03) * (b31); \
00751 (d02) = (a00) * (b02) + (a01) * (b12) + (a02) * (b22) + (a03) * (b32); \
00752 (d03) = (a00) * (b03) + (a01) * (b13) + (a02) * (b23) + (a03) * (b33); \
00753 (d10) = (a10) * (b00) + (a11) * (b10) + (a12) * (b20) + (a13) * (b30); \
00754 (d11) = (a10) * (b01) + (a11) * (b11) + (a12) * (b21) + (a13) * (b31); \
00755 (d12) = (a10) * (b02) + (a11) * (b12) + (a12) * (b22) + (a13) * (b32); \
00756 (d13) = (a10) * (b03) + (a11) * (b13) + (a12) * (b23) + (a13) * (b33); \
00757 (d20) = (a20) * (b00) + (a21) * (b10) + (a22) * (b20) + (a23) * (b30); \
00758 (d21) = (a20) * (b01) + (a21) * (b11) + (a22) * (b21) + (a23) * (b31); \
00759 (d22) = (a20) * (b02) + (a21) * (b12) + (a22) * (b22) + (a23) * (b32); \
00760 (d23) = (a20) * (b03) + (a21) * (b13) + (a22) * (b23) + (a23) * (b33); \
00761 (d30) = (a30) * (b00) + (a31) * (b10) + (a32) * (b20) + (a33) * (b30); \
00762 (d31) = (a30) * (b01) + (a31) * (b11) + (a32) * (b21) + (a33) * (b31); \
00763 (d32) = (a30) * (b02) + (a31) * (b12) + (a32) * (b22) + (a33) * (b32); \
00764 (d33) = (a30) * (b03) + (a31) * (b13) + (a32) * (b23) + (a33) * (b33); \
00765 MATVEC_END
00766
00767
00782 #define MATVEC_MAT3X3_MULT(res,a,b) \
00783 MATVEC_MAT3X3_MULT_COMP(res,a,b)
00784
00797 #define MATVEC_MAT4X4_MULT(res,a,b) \
00798 MATVEC_MAT4X4_MULT_COMP(res,a,b)
00799
00800
00801
00804
00805
00832 #define MATVEC_MAT3X3_ROT_X(matrix,theta) \
00833 MATVEC_BEGIN \
00834 double cos_theta = cos(theta); \
00835 double sin_theta = sin(theta); \
00836 MATVEC_MAT3X3_ASSIGN_COMP( \
00837 matrix, \
00838 1, 0, 0, \
00839 0, cos_theta, sin_theta, \
00840 0, -sin_theta, cos_theta \
00841 ); \
00842 MATVEC_END
00843
00864 #define MATVEC_MAT3X3_ROT_Y(matrix,theta) \
00865 MATVEC_BEGIN \
00866 double cos_theta = cos(theta); \
00867 double sin_theta = sin(theta); \
00868 MATVEC_MAT3X3_ASSIGN_COMP( \
00869 matrix, \
00870 cos_theta, 0, -sin_theta, \
00871 0, 1, 0, \
00872 sin_theta, 0, cos_theta \
00873 ); \
00874 MATVEC_END
00875
00896 #define MATVEC_MAT3X3_ROT_Z(matrix,theta) \
00897 MATVEC_BEGIN \
00898 double cos_theta = cos(theta); \
00899 double sin_theta = sin(theta); \
00900 MATVEC_MAT3X3_ASSIGN_COMP( \
00901 matrix, \
00902 cos_theta, sin_theta, 0, \
00903 -sin_theta, cos_theta, 0, \
00904 0, 0, 1 \
00905 ); \
00906 MATVEC_END
00907
00908
00909
00910
01001 #define MATVEC_MAT3X3_ROT_YMXZ( \
01002 matrix,azimuth,elevation,clock \
01003 ) \
01004 MATVEC_BEGIN \
01005 double cos_az = cos(azimuth); \
01006 double sin_az = sin(azimuth); \
01007 double cos_el = cos(elevation); \
01008 double sin_el = sin(elevation); \
01009 double cos_z = cos(clock); \
01010 double sin_z = sin(clock); \
01011 MATVEC_MAT3X3_ASSIGN_COMP( \
01012 matrix, \
01013 cos_z * cos_az - sin_z * sin_el * sin_az, \
01014 sin_z * cos_el, \
01015 -cos_z * sin_az - sin_z * sin_el * cos_az, \
01016 -sin_z * cos_az - cos_z * sin_el * sin_az, \
01017 cos_z * cos_el, \
01018 sin_z * sin_az - cos_z * sin_el * cos_az, \
01019 cos_el * sin_az, \
01020 sin_el, \
01021 cos_el * cos_az \
01022 ); \
01023 MATVEC_END
01024
01025
01026
01051
01052
01053 #define MATVEC_MAT3X3_ROT_ZXZ( \
01054 matrix,phi,theta,psi \
01055 ) \
01056 MATVEC_BEGIN \
01057 double cos_phi = cos( phi ); \
01058 double sin_phi = sin( phi ); \
01059 double cos_theta = cos( theta ); \
01060 double sin_theta = sin( theta ); \
01061 double cos_psi = cos( psi ); \
01062 double sin_psi = sin( psi ); \
01063 MATVEC_MAT3X3_ASSIGN_COMP( \
01064 matrix, \
01065 cos_psi * cos_phi - cos_theta * sin_phi * sin_psi, \
01066 cos_psi * sin_phi + cos_theta * cos_phi * sin_psi, \
01067 sin_psi * sin_theta, \
01068 -sin_psi * cos_phi - cos_theta * sin_phi * cos_psi, \
01069 -sin_psi * sin_phi + cos_theta * cos_phi * cos_psi, \
01070 cos_psi * sin_theta, \
01071 sin_theta * sin_phi, \
01072 -sin_theta * cos_phi, \
01073 cos_theta \
01074 ); \
01075 MATVEC_END
01076
01080
01081
01106 #define MATVEC_VEC_ASSIGN_COMP( \
01107 d0,d1,d2, \
01108 s0,s1,s2 \
01109 ) \
01110 MATVEC_BEGIN \
01111 (d0) = (s0); \
01112 (d1) = (s1); \
01113 (d2) = (s2); \
01114 MATVEC_END
01115
01128 #define MATVEC_VEC_ASSIGN(dst,src) \
01129 MATVEC_VEC_ASSIGN_COMP(dst,src)
01130
01131
01132
01133
01151 #define MATVEC_VEC_SCALE_COMP( \
01152 d0,d1,d2, \
01153 s0,s1,s2, \
01154 fac \
01155 ) \
01156 MATVEC_VEC_ASSIGN_COMP( \
01157 (d0), (d1), (d2), \
01158 (fac) * (s0), \
01159 (fac) * (s1), \
01160 (fac) * (s2) )
01161
01162
01178 #define MATVEC_VEC_SCALE(res,vin,fac) \
01179 MATVEC_VEC_SCALE_COMP(res,vin,fac)
01180
01181
01182
01204 #define MATVEC_VEC_ADD_COMP( \
01205 d0,d1,d2, \
01206 a0,a1,a2, \
01207 b0,b1,b2 \
01208 ) \
01209 MATVEC_BEGIN \
01210 d0 = (a0) + (b0); \
01211 d1 = (a1) + (b1); \
01212 d2 = (a2) + (b2); \
01213 MATVEC_END
01214
01229 #define MATVEC_VEC_ADD(sum,veca,vecb) \
01230 MATVEC_VEC_ADD_COMP(sum,veca,vecb)
01231
01252 #define MATVEC_VEC_SUB_COMP( \
01253 d0,d1,d2, \
01254 a0,a1,a2, \
01255 b0,b1,b2 \
01256 ) \
01257 MATVEC_BEGIN \
01258 d0 = (a0) - (b0); \
01259 d1 = (a1) - (b1); \
01260 d2 = (a2) - (b2); \
01261 MATVEC_END
01262
01277 #define MATVEC_VEC_SUB(res,veca,vecb) \
01278 MATVEC_VEC_SUB_COMP(res,veca,vecb)
01279
01280
01302 #define MATVEC_VEC_PROJECT_COMP( \
01303 res0,res1,res2, \
01304 pos0,pos1,pos2, \
01305 dir0,dir1,dir2, \
01306 s \
01307 ) \
01308 MATVEC_BEGIN \
01309 res0 = pos0 + (s) * (dir0); \
01310 res1 = pos1 + (s) * (dir1); \
01311 res2 = pos2 + (s) * (dir2); \
01312 MATVEC_END
01313
01330 #define MATVEC_VEC_PROJECT(res,pos,dir,s) \
01331 MATVEC_VEC_PROJECT_COMP(res,pos,dir,s)
01332
01347 #define MATVEC_VEC_DOTPROD_COMP(v0,v1,v2,w0,w1,w2) \
01348 (v0 * w0 + v1 * w1 + v2 * w2) \
01349
01350
01360 #define MATVEC_VEC_DOTPROD(v,w) \
01361 MATVEC_VEC_DOTPROD_COMP(v,w)
01362
01380 #define MATVEC_VEC_CROSSPROD_COMP( \
01381 r0,r1,r2, \
01382 a0,a1,a2, \
01383 b0,b1,b2 \
01384 ) \
01385 MATVEC_BEGIN \
01386 (r0) = (a1) * (b2) - (b1) * (a2); \
01387 (r1) = (a2) * (b0) - (b2) * (a0); \
01388 (r2) = (a0) * (b1) - (b0) * (a1); \
01389 MATVEC_END
01390
01402 #define MATVEC_VEC_CROSSPROD(res,v,w) \
01403 MATVEC_VEC_CROSSPROD_COMP(res,v,w)
01404
01405
01417 #define MATVEC_VEC_NORM_COMP(v0,v1,v2) \
01418 MATVEC_BEGIN \
01419 double _norm = sqrt(MATVEC_VEC_DOTPROD_COMP(v0,v1,v2,v0,v1,v2)); \
01420 v0 /= _norm; \
01421 v1 /= _norm; \
01422 v2 /= _norm; \
01423 MATVEC_END
01424
01435 #define MATVEC_VEC_NORM(vector) \
01436 MATVEC_VEC_NORM_COMP(vector)
01437
01440
01441
01491 #define MATVEC_VEC_ROTATE_TRANSLATE_COMP( \
01492 r0, r1, r2, \
01493 vin0, vin1, vin2, \
01494 m00, m01, m02, \
01495 m10, m11, m12, \
01496 m20, m21, m22, \
01497 vtr0, vtr1, vtr2 \
01498 ) \
01499 MATVEC_BEGIN \
01500 r0 \
01501 = (m00) * (vin0) \
01502 + (m01) * (vin1) \
01503 + (m02) * (vin2) + (vtr0); \
01504 r1 \
01505 = (m10) * (vin0) \
01506 + (m11) * (vin1) \
01507 + (m12) * (vin2) + (vtr1); \
01508 r2 \
01509 = (m20) * (vin0) \
01510 + (m21) * (vin1) \
01511 + (m22) * (vin2) + (vtr2); \
01512 MATVEC_END
01513
01514
01573 #define MATVEC_VEC_ROTATE_TRANSLATE(res,vin,matrix,vtrans) \
01574 MATVEC_VEC_ROTATE_TRANSLATE_COMP(res,vin,matrix,vtrans)
01575
01576
01577
01613 #define MATVEC_VEC_TRANSLATE_ROTATE_COMP( \
01614 ctype, \
01615 res0, res1, res2, \
01616 vin0, vin1, vin2, \
01617 vtr0, vtr1, vtr2, \
01618 m00, m01, m02, \
01619 m10, m11, m12, \
01620 m20, m21, m22 \
01621 ) \
01622 MATVEC_BEGIN \
01623 ctype _v0 = (vin0) + (vtr0); \
01624 ctype _v1 = (vin1) + (vtr1); \
01625 ctype _v2 = (vin2) + (vtr2); \
01626 (res0) \
01627 = (m00) * _v0 \
01628 + (m01) * _v1 \
01629 + (m02) * _v2; \
01630 (res1) \
01631 = (m10) * _v0 \
01632 + (m11) * _v1 \
01633 + (m12) * _v2; \
01634 (res2) \
01635 = (m20) * _v0 \
01636 + (m21) * _v1 \
01637 + (m22) * _v2; \
01638 MATVEC_END
01639
01640
01699 #define MATVEC_VEC_TRANSLATE_ROTATE(type,vout,vin,vtrans,matrix) \
01700 MATVEC_VEC_TRANSLATE_ROTATE_COMP(type,vout,vin,vtrans,matrix)
01701
01705 #endif