|
|
请使用QQ关联注册PLM之家,学习更多关于内容,更多精彩原创视频供你学习!
您需要 登录 才可以下载或查看,没有账号?注册
x
" p% d, k3 s+ m% E
$ n( s4 e* a5 z: A! r
6 B$ _- g: y* p' j/ h
#include <math.h>
6 b+ A1 D/ G" v% V. K' Q//矩阵复制
9 L5 ^/ Y% d& W3 ]' ivoid Project_Matrix_Copy(double from_mtx[4][4], double to_mtx[4][4]): d+ U0 [* Q% w) P; p+ w
{
( o1 B: L& O1 A% H7 J" f int i,j;$ h- o; c G0 q; S! Q$ j4 V" I
" j6 o, y1 G8 K! D) c% C for(i=0;i<4;i++)
6 P% S( u* y1 B% S for(j=0;j<4;j++)
' f1 A6 A- v( ^$ e( R to_mtx[i][j]=from_mtx[i][j];
9 C" U0 _6 N T( z( ?/ s8 @) m}" D* t# H/ p" \: N! z& F1 z8 E
//矩阵初始化/ q" ~+ k( X/ f. k; I i/ @
void Project_Matrix_Identity(double mtx[4][4])
5 o9 ]7 c q- ?{, S1 j5 M1 t9 A6 A; v) j" b8 N
int i,j;$ e7 B! r' f! H' }6 |) b
4 l H6 `( M, |* B4 ?$ v% z
for(i=0;i<4;i++)' P' U3 Z2 [ `
for(j=0;j<4;j++)6 T0 A5 A) ^# }; X
if(i==j)9 e' w8 \2 O# _" R; N! a4 @2 A$ @( G
mtx[i][j]=1.0;
- R" q& j+ z6 d) z( c else1 n; o+ N3 w0 b u
mtx[i][j]=0.0;
$ f5 \. i5 T! ]}
6 ^8 Y9 q8 _9 p( p$ v: t" z% ~//矩阵相乘
. O8 d; z( n# r5 v, J! g/ y) f( Lvoid Project_Matrix_Product(double left_mtx[4][4],double right_mtx[4][4],double get_mtx[4][4])
{0 {% [* ^( }9 A# y, G{( O, {1 ~* C7 M4 _9 Z
4 E0 O o8 E( K+ U int i,j,k;
+ k9 Z& z* Z* x, w' Z' k double left[4][4],right[4][4];
( t5 b5 o" G' M P; c
4 p0 D" |. a% U2 j Project_Matrix_Copy(left_mtx,left); q* m+ B$ R$ r, }) V; D
Project_Matrix_Copy(right_mtx,right);4 @9 ^0 I0 _" i* L, M/ f' `# Y
for(i=0;i<4;i++)
6 L' |, h4 q- j8 }" D; K6 \ for(j=0;j<4;j++)
+ T- [6 B0 T5 a9 P5 s {7 o& p% E8 n+ {( K ~7 x) D+ `
get_mtx[i][j]=0.0;. `- E2 f0 t, \- e. R; @
for(k=0;k<4;k++)
3 m1 ^) ~3 n; I get_mtx[i][j]+=left[i][k]*right[k][j];& _' J' k v3 n @. Q8 I6 F6 s
}. W; ^1 F2 V% u/ \7 O1 Z i
}
; r2 I3 d! m1 V9 |//转置矩阵! Z5 N9 X. L" @' r" v! c/ f2 t
void Project_MatrixGet_Transpose(double mtx[4][4],double transpose_mtx[4][4])
3 D0 D8 ?0 P( g' S{, y' S8 W& w( F- X% ]
int i,j;. g8 A$ K( V6 }* B7 e
for(i=0;i<4;i++)
$ i W8 |2 m7 h, ~) g" n6 m H {; H4 v- x" P/ b
for(j=0;j<4;j++)
) v, l! g" K; i2 O {
! Q8 S' w! l3 U; [5 p+ f; [4 y transpose_mtx[i][j]=mtx[j][i];
/ ?% D m& L% i9 T* M. j& d1 e }
# ?1 [, v9 |* F# H( ^ }
% c& Q9 L# F+ ~( [3 C7 }}
2 J0 M, W( ^$ |* k+ Z- B//从11元组获取变换矩阵9 i! z- h4 d- e& j, }
void Project_Matrix_Get_From_Move(double translation[3],double point[3],double rotation[3],double angle,double pos[4][4])9 k0 w0 Z' y8 x& l; f7 Z: ?+ H C
{
, e. L' N/ A3 l; E6 }
! F8 p) O: }1 M, a3 p8 V //Project_Matrix_Get_Rotation(rotation,point,angle,pos);
: t2 l0 y2 x) _+ Y9 ~ int i,j;
& Q' w3 }; n3 k2 y1 f; a; X for(i=0;i<4;i++)
" N! `9 ^% w9 l3 ]; `9 F4 K for(j=0;j<4;j++)- l( m) R: j/ n0 t4 c) |/ _/ o% _
if(i==j)5 c" e3 K. P ^ ]' ?" z6 S# Q
pos[i][j]=1.0;
& k9 N4 L$ U; Z1 k9 E1 u. }% j$ w else
. q Y/ x- |/ t% P, |- R! t pos[i][j]=0.0;, {4 B# L8 \6 \1 m0 r3 @
pos[3][0]+=translation[0];
3 h# ?$ E9 u2 e+ m \ pos[3][1]+=translation[1];) r" ?* J0 x9 X2 y
pos[3][2]+=translation[2];6 ?; L+ X: M: t) o
}3 f9 X ?: a: t5 }9 {
//向量缩放2 h" _; ]: y* g4 y9 h3 p6 D/ w
void Project_Vector_Scale(double a[3],double scale,double b[3])' l& m5 t& E1 o h' E+ K; }, B
{
: j# |% p4 t8 O; D6 I b[0]=a[0]*scale;2 H" J/ X0 G% [
b[1]=a[1]*scale;4 W2 {& F4 _3 U7 f! h( _
b[2]=a[2]*scale;9 K2 m Y& |7 o1 y9 l
}
+ h7 v$ W3 M% W9 P' T4 [+ t0 {! c1 q4 ?0 A) e* G5 U. Z6 S$ p
|
|