|
|
请使用QQ关联注册PLM之家,学习更多关于内容,更多精彩原创视频供你学习!
您需要 登录 才可以下载或查看,没有账号?注册
x
3 E% ?4 w2 z' A# ?+ ^
9 t5 t0 c! Q9 Y8 K
8 Q, [$ l% g) s9 E' q2 o2 l#include <math.h> 0 I9 t" V1 n3 O' E: k' V& a. b
//矩阵复制, B2 k2 f2 W) C
void Project_Matrix_Copy(double from_mtx[4][4], double to_mtx[4][4])* n( R) T) x& @8 M, Y
{ m+ _ }9 N+ D% R2 B
int i,j;8 g1 y% O3 p' d5 _3 A9 ]' C8 [
" t3 I# s: C- W; l: J* Y w! m6 C for(i=0;i<4;i++)
& M- ?! p: W0 Y) [6 c for(j=0;j<4;j++)
1 {( }) d+ _" L* r$ s4 u to_mtx[i][j]=from_mtx[i][j];
' R" A% g1 q F0 s6 Q}
( ]- q$ `/ E2 y, { g//矩阵初始化
# D- C8 {) l$ l+ ?+ c5 fvoid Project_Matrix_Identity(double mtx[4][4])+ S2 A+ q6 J7 i
{
( g$ D( h) t$ X! [ int i,j;
2 Y# R* g5 E: K0 `+ d9 B , _6 u$ u6 d$ R( f7 L: j
for(i=0;i<4;i++)+ A, ^! u& W. K3 f0 Q4 j& R
for(j=0;j<4;j++)" k- W3 D5 | I
if(i==j)- }( |. {' b- @6 Q, D; ]7 I) F
mtx[i][j]=1.0;
7 V" p# ^ H5 x0 n3 f else9 k0 X. c. u# M2 S6 [+ J
mtx[i][j]=0.0;( s0 h; O9 g6 x2 x: c' y
}
- I" k, T5 d* \. E g//矩阵相乘+ V) ~8 O$ W1 n* Z' B7 }" m1 M
void Project_Matrix_Product(double left_mtx[4][4],double right_mtx[4][4],double get_mtx[4][4])
2 y; D4 k. z* K9 _4 \9 m" q{
$ `6 N. g6 F% X0 v" V
' V0 \ D) B/ D8 d/ R; d: o int i,j,k;
# z7 P* `6 c; L8 x1 i double left[4][4],right[4][4];# `! P! T$ |0 u/ h# f W4 ^
+ v- g1 L7 j' Y# j) e, D- a6 _2 J
Project_Matrix_Copy(left_mtx,left);/ ^; @; Z2 K, n/ \* z: L
Project_Matrix_Copy(right_mtx,right);
1 Y$ l! B1 l+ y; a6 P3 j, U for(i=0;i<4;i++)
8 K. W- U' D) @ for(j=0;j<4;j++)# `2 B- o0 {7 M2 M W5 S2 `) a' Y
{
^! g4 T8 j/ k; P. n; s6 N$ ^ get_mtx[i][j]=0.0;$ J2 D% u9 H- u. Y- u) y" |
for(k=0;k<4;k++)
" ^3 H% f5 |6 f* J" s3 T) w get_mtx[i][j]+=left[i][k]*right[k][j];4 D4 `# z$ }/ _. u0 w8 o1 g
}
2 k' I, G/ M: }}
5 t3 ?' X$ n2 j: d6 e- w//转置矩阵! s; y6 ?( A' q8 L% S
void Project_MatrixGet_Transpose(double mtx[4][4],double transpose_mtx[4][4])( J7 F9 w, Z+ _: `' X/ W
{
% e2 z+ K, |3 s% l( ] int i,j;
J0 Y! W& ~* O5 l! L9 D for(i=0;i<4;i++)% j. ~' s8 F h8 a! ^
{/ }. m; a2 @; @8 u. S) A
for(j=0;j<4;j++)" o9 u8 F! O& d: ~& l
{; M, K3 T* c/ E
transpose_mtx[i][j]=mtx[j][i];# v7 k+ f& A3 P5 X
}9 w# b( y2 e+ f1 s$ d8 P! M
}
6 ^0 z- v2 E1 k" P9 E1 W! Y7 q}; U$ y0 e( z$ Y# o' C* @
//从11元组获取变换矩阵
$ j5 D" W8 W }' rvoid Project_Matrix_Get_From_Move(double translation[3],double point[3],double rotation[3],double angle,double pos[4][4])5 b3 }" m$ e2 I# y. p% d
{0 q) Y. D+ G1 D- Z0 F
) @9 J. `6 {/ y7 V0 f6 p0 @ //Project_Matrix_Get_Rotation(rotation,point,angle,pos);& N- C- O( n0 \; z0 Y) a
int i,j;
3 _/ [3 ]# r- R, J for(i=0;i<4;i++): k9 h. ^1 D9 S1 u, b0 ~6 t' I
for(j=0;j<4;j++)3 o/ S3 M# u D# I+ T! |# U6 c0 {
if(i==j): D9 k+ q( [" t* t) Y7 c2 r; w
pos[i][j]=1.0; X' o, {2 N. D1 O% h+ a' m: \- I: h
else
( Z c# o/ D" r1 l pos[i][j]=0.0;
- I+ q$ K" N4 g1 m$ w# [! |: H pos[3][0]+=translation[0];
( D7 ~- |0 s. A$ W/ m0 ^7 o pos[3][1]+=translation[1];) S I9 _ p! D7 O) z! d
pos[3][2]+=translation[2];
/ `9 u4 S% }) Q7 Y7 ?}
" U1 D, r; b: Y8 ]: c//向量缩放6 F- ~# p8 s1 F2 B+ i
void Project_Vector_Scale(double a[3],double scale,double b[3])
+ q( P2 h5 S3 j{/ |1 {1 G- m+ V6 C
b[0]=a[0]*scale;$ h9 }$ ?% T: I5 A% K8 I, }
b[1]=a[1]*scale;( E1 W' ~ y0 {! J: J
b[2]=a[2]*scale;& I+ L! O6 e7 h4 o
}
$ L0 z/ V. h- c9 _/ |1 q5 R$ u- o1 C" U% P
|
|