|
|
请使用QQ关联注册PLM之家,学习更多关于内容,更多精彩原创视频供你学习!
您需要 登录 才可以下载或查看,没有账号?注册
x
+ f2 e! p7 R* P6 b( c w, d
+ ]1 }6 ?* K' S. s& L1 C$ M6 ], c! `0 o+ t3 k8 _
#include <math.h> 8 S8 i9 s& U4 f. h' J8 k- ~
//矩阵复制
2 S# d, l% }) j8 ?* bvoid Project_Matrix_Copy(double from_mtx[4][4], double to_mtx[4][4])$ P' h4 w, N/ o( c, i
{
: }! s' w2 Y% {, X6 C int i,j;: t3 C" @4 ^. Y0 i& \2 t7 i& U
+ R$ @ d% S- x( b) n6 ~# ]- h
for(i=0;i<4;i++)
9 {0 `4 R; }, T$ C* S7 `) V2 i for(j=0;j<4;j++)8 a0 ]4 a- Q0 f1 |* H
to_mtx[i][j]=from_mtx[i][j];
! s% K( ]( s* q" m# R& p2 n. u}( v' x+ K. H- ^- P' J: D' F: X# d
//矩阵初始化. L" E1 H1 @ b5 J
void Project_Matrix_Identity(double mtx[4][4])
0 o( q$ }6 R: p. E{
7 ]) v I' b8 T% }7 \5 V! u5 d& W int i,j;( Z9 e6 J4 A. _5 P1 C
( Q! r& L5 ^+ G* O$ A4 p6 X2 @+ u
for(i=0;i<4;i++)7 }$ a0 `. D( e% F* D% o
for(j=0;j<4;j++)$ G: M4 b& N: _# x' T3 ?
if(i==j)7 c( w x3 D- o# f, x7 V6 Y: N+ ~
mtx[i][j]=1.0;
. e5 v: @& P# |, s% I else
# _$ d* I* h; |2 s- W mtx[i][j]=0.0;
+ f8 n! V% m0 I9 n}$ C; l+ X% \, u
//矩阵相乘! R6 n7 B! M, i5 i
void Project_Matrix_Product(double left_mtx[4][4],double right_mtx[4][4],double get_mtx[4][4])0 Q8 R# R5 k# D- T. j' {
{( n4 r, H- O. @ D+ N
* O; W* F2 ^) }* e# S- S- P
int i,j,k;
! p7 g' s* L) h8 s! ?# o- n double left[4][4],right[4][4];- Y X! Y2 d2 k4 L
0 S4 H" A% {$ ?0 ~1 a7 t
Project_Matrix_Copy(left_mtx,left);" l% o: {5 k+ Q2 f
Project_Matrix_Copy(right_mtx,right);! ]5 C: A" e! e
for(i=0;i<4;i++)
3 G# [* C% x: Z, D+ K- y for(j=0;j<4;j++)
2 i1 W, r( n) a4 W {
9 F7 _, A" ]( V" o% e7 S8 Q3 [ get_mtx[i][j]=0.0;
( _1 U* @ P8 S for(k=0;k<4;k++)% O; J9 l9 j6 h8 k. _* a
get_mtx[i][j]+=left[i][k]*right[k][j];. q# a% E; G4 ?; a# r! o
}
5 d9 W$ Z, z2 O* B9 j}
% n9 z: x; ~, x) S: ]//转置矩阵- y6 }$ ^$ z8 O* ]4 r- D2 [( t
void Project_MatrixGet_Transpose(double mtx[4][4],double transpose_mtx[4][4])6 s/ g( O. j, c; ^7 B5 Z
{
+ h# `2 ]% h4 r# G$ x0 H/ p int i,j;
# h" E6 B7 ?- V for(i=0;i<4;i++)5 _! y! c. F L- ^ o5 H
{7 A* G; o/ l! e- c
for(j=0;j<4;j++). J1 R1 u. M1 f4 w7 `/ ]
{ ]: m0 s9 F% `" Z
transpose_mtx[i][j]=mtx[j][i];/ W2 g: ^: E( s) {9 w
}' m' t- k; z y* n# [: [
}
+ \7 w0 z( O5 V0 g8 M}4 c8 g0 k% o# C" ^ G, I
//从11元组获取变换矩阵 V9 w5 N# I, t0 |3 A: L
void Project_Matrix_Get_From_Move(double translation[3],double point[3],double rotation[3],double angle,double pos[4][4])) Q& n3 t t6 [' y
{: t7 u$ f9 ~% s+ \7 W O8 F6 B
; v& d6 v5 I2 o! @# U+ |2 f //Project_Matrix_Get_Rotation(rotation,point,angle,pos);
' z1 z7 W) i6 e int i,j;
; e& A8 g9 o) }8 e for(i=0;i<4;i++)
) E, r3 ]9 P; i9 R$ K1 o for(j=0;j<4;j++)
) B8 Q% B0 v4 [& m6 K& [6 } if(i==j)
9 d* V0 J+ p" p7 S3 M; u' i pos[i][j]=1.0;( t) R: O- \: e# r! T2 V4 E% i
else
3 D2 w& f2 P+ ~$ j pos[i][j]=0.0;/ [9 y6 g7 l/ B" m% N
pos[3][0]+=translation[0];/ Q/ {2 N7 F( o
pos[3][1]+=translation[1];
; {+ {' v5 ^3 l$ ]0 u( o0 C4 S$ M pos[3][2]+=translation[2]; T& p0 N P0 E
}
% b+ t% u& z5 r; q//向量缩放
; h. @: P3 G+ C* tvoid Project_Vector_Scale(double a[3],double scale,double b[3])
6 x& M- Z; K/ ^% R) h Q0 M{7 i7 a9 l2 N) `) v
b[0]=a[0]*scale;7 D- D3 w( K" U+ X
b[1]=a[1]*scale;* R1 J& P' W2 a4 M
b[2]=a[2]*scale;$ b! j, E% }0 X* F
}4 Z, Y1 P* w- J v/ A$ |7 l5 N# l" G
! |+ b/ v: X7 C9 S; b" z |
|