|
|
请使用QQ关联注册PLM之家,学习更多关于内容,更多精彩原创视频供你学习!
您需要 登录 才可以下载或查看,没有账号?注册
x
4 ^8 P* o' o! r( z/ i- F* ?, p0 N# {1 {! w+ A" f8 R
& e" L- ~- T' }- k* V0 f
#include <math.h> 1 R7 j8 T1 f) K' N( m: o5 @$ Z% @
//矩阵复制$ T! e6 p; n: ]8 p D0 o, h
void Project_Matrix_Copy(double from_mtx[4][4], double to_mtx[4][4])
: e: i' a( X# B U! Y2 y! L/ j{# E# x7 N) L% y3 c6 ?
int i,j;
8 x8 k o" r2 `9 Y- U: N
* v$ z$ ?( @ f9 \0 b% x/ g% B for(i=0;i<4;i++)- X1 f+ x' Y; s$ p; E
for(j=0;j<4;j++)/ _4 e$ o: G4 I5 \- U+ C: q$ M
to_mtx[i][j]=from_mtx[i][j];6 I3 C" X7 {: ~! g, I
}
/ a, ^7 h2 E- z7 _ y) M, f//矩阵初始化
& b# [6 ~; O/ I( L5 [4 O1 ^( |void Project_Matrix_Identity(double mtx[4][4])
- ^% m- P8 W' u! L. {{1 m- s* K+ a. Q8 e
int i,j;- U; z9 h- S$ ^
$ n" o: x2 T" Z% Z' x7 E for(i=0;i<4;i++)
/ D3 H3 q2 R0 L% ~ D* P! x' c for(j=0;j<4;j++)# p: z- Z+ ]. u4 ~# ?
if(i==j)
, l) E: O- f" o0 `9 D mtx[i][j]=1.0;
' z1 }+ x x) ]: M6 [ else
) e# P# p, r4 i+ M4 Q* `! r4 y3 K* }' k mtx[i][j]=0.0;( \0 F7 l9 P' m5 ?! W) {& W, `
}- Z" Z1 C0 \ E z" Z. p; A
//矩阵相乘. n9 w& v3 w2 o) ` d
void Project_Matrix_Product(double left_mtx[4][4],double right_mtx[4][4],double get_mtx[4][4])
/ M7 F) M: r c. L8 |) `{. t& ]' Z2 T3 Q& u
5 ~. [) c% u- L, w8 O. m' I0 g& f2 S7 _
int i,j,k;' ?! `+ Y2 |4 j3 X
double left[4][4],right[4][4];
& y6 T% S# W% A3 S; G3 v; L) M! O, U( A
Project_Matrix_Copy(left_mtx,left);
$ t. _8 l1 c6 {$ k& W4 j5 z Project_Matrix_Copy(right_mtx,right);- j: j& K" \9 @6 j1 R* S( f+ Z
for(i=0;i<4;i++). S0 o0 C2 j8 `/ ?
for(j=0;j<4;j++)% X! z1 c$ {' S! ]
{ x+ |! B1 m5 U% L a
get_mtx[i][j]=0.0;
/ [& v$ f' H: |2 E6 `& p1 g$ g0 F for(k=0;k<4;k++)' q k x' l/ r" E* b7 r3 k4 u
get_mtx[i][j]+=left[i][k]*right[k][j];
$ K0 }4 {8 r# Q) `. e }) z ~) [; ?' a s5 `' ?
}
. N; i9 t& @6 W4 L( B8 [, x//转置矩阵3 h2 B R" v7 W! T1 z1 d
void Project_MatrixGet_Transpose(double mtx[4][4],double transpose_mtx[4][4])
, b' b: _5 u' C5 i3 y# Q{
/ v: N" h" \" m C9 p int i,j;
; o% J5 K! V9 ?1 v* a2 C' U for(i=0;i<4;i++)7 x: B8 x# V$ r
{
, @% V9 }# q! Q+ r7 ^3 }: `' u for(j=0;j<4;j++) o& L0 i' J T, K! s/ T
{0 d' P1 r4 [7 d& Q% b/ H3 i [& u& _/ y
transpose_mtx[i][j]=mtx[j][i];
8 O+ y" j3 [' x$ w }( v; d+ x& F6 ?/ K, \1 q3 _0 R
}, ^6 c& L; e, u3 |/ v: l. N
}
& G& i! H* E1 `//从11元组获取变换矩阵
, |4 T4 z7 }+ Y& Yvoid Project_Matrix_Get_From_Move(double translation[3],double point[3],double rotation[3],double angle,double pos[4][4])4 G+ |' W" z+ R* Q) s) ]
{% ~$ ]. n' O/ L- d4 U
9 l5 @ n! ~- N- w //Project_Matrix_Get_Rotation(rotation,point,angle,pos);
8 N4 t. S* w/ g) o5 d% e/ l; d! B int i,j;
) r# k& m& j7 j# I4 H& u for(i=0;i<4;i++)8 l. n$ O+ m9 W6 a( ?4 _
for(j=0;j<4;j++)
% ]6 `! c6 O7 j if(i==j)0 J# A# \9 d, `% B. h: k
pos[i][j]=1.0;" C9 w, U; O5 K$ ?0 A
else2 M$ } d h: i3 @+ F ?/ O
pos[i][j]=0.0;
% ` j/ E. F2 G4 `. E pos[3][0]+=translation[0];( v& q) ^0 g# x" T$ y5 t& U: E
pos[3][1]+=translation[1];8 m# o: u3 H# C* p
pos[3][2]+=translation[2];
( c, ]' \: D- @/ _8 M, d, |. H% z}& ~0 y- ~$ Q" O4 ~
//向量缩放
. x6 y1 t9 E. { t+ b A! K s" Nvoid Project_Vector_Scale(double a[3],double scale,double b[3])
3 \' T2 T. n; M0 Y- [{
8 l9 \* H# _# Q: T. n b[0]=a[0]*scale;( P2 l8 N6 s1 c$ z t
b[1]=a[1]*scale;
$ H" p: F5 {; {3 p b[2]=a[2]*scale;
' ^# f% e& v! {+ K- v3 l0 H}2 e& D; _, F% Y |& k# N
, M3 k: q; T) a/ y' q
|
|