|
|
请使用QQ关联注册PLM之家,学习更多关于内容,更多精彩原创视频供你学习!
您需要 登录 才可以下载或查看,没有账号?注册
x
- J+ r5 l2 [4 b7 E$ M3 Q& r5 @- u. }7 f7 u5 a& j
\* y8 ^% |& s% H/ H- }6 W#include <math.h>
4 d6 N6 V* E& `/ s6 Y//矩阵复制
% C, x- Z8 h; S7 Rvoid Project_Matrix_Copy(double from_mtx[4][4], double to_mtx[4][4]): Q- P- x- Q9 X6 h2 o
{8 O' C$ [% u. L3 \9 s+ ~
int i,j;2 \: G4 V7 @- b$ G" _: v' B
6 e8 V- ^) ?- V+ F
for(i=0;i<4;i++)4 L8 a/ l4 \! P: R6 V' C( N
for(j=0;j<4;j++)* @: S( ?* X+ s! U. V+ \
to_mtx[i][j]=from_mtx[i][j];
: J8 M& g' [9 T1 P}
4 Z" [" J9 ?' M5 e* H) t# x, h: q//矩阵初始化2 m6 O; m- `# F
void Project_Matrix_Identity(double mtx[4][4])
. l; B1 u( _7 B5 \" z{/ N3 b2 ?8 Y( @/ r
int i,j;
5 @7 q" c! M+ r3 f9 B! _4 S " r) b9 V/ c+ Z' X0 ^
for(i=0;i<4;i++) {* j# e0 Y: i: K, s( a3 P
for(j=0;j<4;j++)/ d8 ]/ m$ `5 w" n/ }
if(i==j)* t6 \; k* z# x' R$ N2 K
mtx[i][j]=1.0;: X* Z) K |, J/ R+ s, Q, z1 \% j
else3 ^; m3 Q( S3 i* i
mtx[i][j]=0.0;1 b( c c' A4 G' S- N8 z2 T+ E' c
}! G8 D. a! f% C% c5 u2 P4 H! L& E! R
//矩阵相乘0 n q1 P9 Y1 P5 f# H9 j9 I) _: ?7 Z
void Project_Matrix_Product(double left_mtx[4][4],double right_mtx[4][4],double get_mtx[4][4])
* f3 u; n5 M0 C{
/ v. [1 Q [: Q8 X+ c8 o3 l; t6 F7 M& N7 |& j
int i,j,k;& O7 ]; E, M$ E- j
double left[4][4],right[4][4];
1 D+ @( I/ L6 B3 w
/ ]6 J( |7 M/ e# k6 S7 c Project_Matrix_Copy(left_mtx,left);% n4 `. ]$ X* f
Project_Matrix_Copy(right_mtx,right);
. ~/ y! g2 m2 u9 F1 f for(i=0;i<4;i++)+ ?' x2 y% O/ P6 ?$ p
for(j=0;j<4;j++)) j, j4 H1 a* P y: ~0 J
{6 ~% q( K6 P3 a; H
get_mtx[i][j]=0.0;
! s$ g! P$ b2 ` for(k=0;k<4;k++)7 a& w8 N V1 `+ I$ K" B& m
get_mtx[i][j]+=left[i][k]*right[k][j];: K% H' w+ B \) i
}# }/ f% C+ k1 z$ @& x
}8 h4 L% P; I5 z* H; _
//转置矩阵
% S: G+ R( R* z& b( _# _0 p" Qvoid Project_MatrixGet_Transpose(double mtx[4][4],double transpose_mtx[4][4])- P" ~! j6 r0 [
{
, p4 _" w! F4 y int i,j;
# B) F: Y/ l* B- W; Q t for(i=0;i<4;i++)
$ ] L' C9 y2 f; [( y {2 `: R& I( C, _: M
for(j=0;j<4;j++)/ Y4 C0 D+ v! J/ k0 y# v
{6 ?- C4 N, |. S/ a3 F
transpose_mtx[i][j]=mtx[j][i];) I- \$ _; Z, x/ W: b6 n; y# D
}6 E8 X. K9 |& _" y% w' `6 n/ m; r ~
}
1 @8 O" a) P( X2 W6 X) P* N* G; M}
! u% y d7 s. @$ o) x& ]//从11元组获取变换矩阵
0 [( J0 _! H$ J F/ Y5 [1 }void Project_Matrix_Get_From_Move(double translation[3],double point[3],double rotation[3],double angle,double pos[4][4])
7 R: j& u1 z+ E! i7 x, i: o2 ?{
5 ]1 e5 s) w! N( l. [6 R3 k) C4 {% W% t
//Project_Matrix_Get_Rotation(rotation,point,angle,pos);0 i/ u X' m4 a5 M) F* D
int i,j;
4 {( d! H/ D3 ? for(i=0;i<4;i++)
6 m( V# y+ C% D' `1 x- c0 t for(j=0;j<4;j++)/ B6 J6 j2 ]* }( t9 v
if(i==j)
: a( G& s( {/ o- \ pos[i][j]=1.0;
% G( ]" @* p Y2 e- Y' W else
, o$ c2 d) W5 w( q- c& c) k pos[i][j]=0.0;
: \" r2 S' A, a pos[3][0]+=translation[0];+ m/ h, J* F9 L9 _
pos[3][1]+=translation[1];; l1 g& H9 t6 l! u8 V
pos[3][2]+=translation[2];
, D! j0 F1 D0 M- z- C3 s}/ l. w$ E4 c# F' j4 ?) \& `
//向量缩放4 @; P% j# r) a5 M* G8 F( M8 Z
void Project_Vector_Scale(double a[3],double scale,double b[3])# D3 y& M+ ?: r5 T, s# U' k& X& O
{2 y) \: H" I4 Y; H% I
b[0]=a[0]*scale;
! ]0 o) C7 B( S b[1]=a[1]*scale;1 \% f6 o' k1 M" M3 _) Y4 K4 E( ?
b[2]=a[2]*scale;
/ K) b6 _( J" P* K {}' |8 P, {% t, C+ k, @) x1 r* J' V
, H$ R; Y: i5 h1 Q, |/ I
|
|