|
请使用QQ关联注册PLM之家,学习更多关于内容,更多精彩原创视频供你学习!
您需要 登录 才可以下载或查看,没有账号?注册
x
$ I" z0 [# D/ z" c' a6 a
9 A& ]/ {7 e* Q7 W$ \. p
W8 E) k+ u# \$ C* A; m$ ]7 d& f7 o#include <math.h> : x& G/ ?4 V! O; A, V" N. v5 d% |
//矩阵复制
0 ^) N! T2 `3 a1 U2 f3 U, Pvoid Project_Matrix_Copy(double from_mtx[4][4], double to_mtx[4][4])8 @5 B; A: R/ B2 f" N
{' o. m F9 y- e( z5 v
int i,j;
+ w0 Z. Y( E3 @, |+ E' t$ C$ ]
for(i=0;i<4;i++)0 q1 s; t# l$ O4 D/ q' F, d7 ~$ ]
for(j=0;j<4;j++)
r1 e+ S, {9 U8 w/ W9 o to_mtx[i][j]=from_mtx[i][j];
& W- J% D h( b/ J7 ]7 C1 ]5 {}/ a; }0 p3 Q- ^$ J0 j3 i* r
//矩阵初始化0 N8 `; K- M* J# u' z% N1 S' |
void Project_Matrix_Identity(double mtx[4][4])
9 M6 Q! U. _( V/ ?' Y2 m. G2 o{# M6 V- w, |3 `& k) T4 d4 r7 y
int i,j;
4 e6 y8 Z$ y" G 7 T$ i- a3 l" I
for(i=0;i<4;i++)
" y% P; Y: f( P/ u. j for(j=0;j<4;j++)7 \6 S; O5 ~: A9 E
if(i==j)% S {; p1 y( e0 a2 ]& F9 ?" ^
mtx[i][j]=1.0;
, o, a* }7 i/ H E8 {& ? else0 `1 F# ?: {! b; ]1 F6 z& \: C
mtx[i][j]=0.0;
; T% O3 {' A, U/ B2 K6 \" m. l}: U* F; s" d D ^0 x; K0 k
//矩阵相乘0 W, @- ?2 D. X3 P" D' T
void Project_Matrix_Product(double left_mtx[4][4],double right_mtx[4][4],double get_mtx[4][4])/ Y4 Q+ T. k9 i; m
{4 U0 y, {. W2 h; ?
. H: j* _- D6 V5 ] int i,j,k;
/ A/ ~ Y5 N5 S9 K- Q# c; T double left[4][4],right[4][4];
+ ]0 D! O8 [6 I9 A5 H$ V6 C |# R' A/ p0 H6 J* i
Project_Matrix_Copy(left_mtx,left);
+ l& G. G% Y& I$ F+ p. Z Project_Matrix_Copy(right_mtx,right);
h6 ?( v7 G! b. g. p for(i=0;i<4;i++)
k% {8 r9 E9 D. y b% J' q8 [ for(j=0;j<4;j++)
. E2 f/ |, z! y! b3 `! l9 N {) G7 Y3 V8 ?1 {3 T6 B
get_mtx[i][j]=0.0;2 ]1 a7 G8 D$ L' k. o6 ~( Y
for(k=0;k<4;k++)
7 F7 B8 U$ e( s) V1 w1 V2 v' F1 U get_mtx[i][j]+=left[i][k]*right[k][j];( _# y/ }5 G+ X) A$ N
}
; K {& E, X6 X8 U, @- x! N}
/ ?! j$ a) n1 J- r* C" t//转置矩阵
9 D* {9 ^, j- p0 c) @void Project_MatrixGet_Transpose(double mtx[4][4],double transpose_mtx[4][4])" a0 k% a9 U; Z6 l
{
/ M! o5 {$ P# g0 N int i,j;
* I4 u4 B2 Y: s1 H2 x& `+ F. ] for(i=0;i<4;i++)$ f, u* G3 a# n. e$ U- Z
{
/ f" G( u; q# B$ Y for(j=0;j<4;j++)
$ n R: V- P8 A, i2 w3 H3 Q0 J {
/ D4 g1 T' y4 o8 |0 k transpose_mtx[i][j]=mtx[j][i];
2 s$ N( W6 Z! K. j, G }/ A# g% O( j: L1 a" {+ U2 W' ^
}) r7 U4 `$ ~9 l7 V" X9 w6 g$ U
}
& p8 d' j" |$ ]1 A8 O, w6 ~, O//从11元组获取变换矩阵
. [/ M8 }4 B$ X6 W# t+ Fvoid Project_Matrix_Get_From_Move(double translation[3],double point[3],double rotation[3],double angle,double pos[4][4])( ? ?% ]8 X- O; S
{ I" q, I' T8 g( Z8 y
7 A- A' `5 C% z2 ? o! q: Q //Project_Matrix_Get_Rotation(rotation,point,angle,pos);
: Z3 W! e# D& b int i,j;8 \ C+ t' ?" O; j( g; E
for(i=0;i<4;i++)1 p1 _7 P% O2 ]% b
for(j=0;j<4;j++)9 X: u" H7 \7 j$ C
if(i==j)7 V* C( K X8 P M, f- K. V8 L& `
pos[i][j]=1.0;
" p8 q/ S9 ]3 @4 o* \ else; i d. {# V& A& H7 A
pos[i][j]=0.0;# p' G3 ]5 O2 @& }- N9 k4 W, }
pos[3][0]+=translation[0];1 ^+ z" @9 N/ ~3 u! H" P
pos[3][1]+=translation[1];. x# \, \) I4 d$ ]4 R" Y/ x
pos[3][2]+=translation[2];
% T c. ]- e. w& B}
5 z: ~. E7 n- i! r, {1 X//向量缩放5 p( v1 s) i" b- T. G, @4 v8 M' R
void Project_Vector_Scale(double a[3],double scale,double b[3])
* e% v4 n* r: Q* l3 P{% [( T+ ]7 y3 s) y0 N' Q& B
b[0]=a[0]*scale;
+ g* a% u3 Z: V& { b[1]=a[1]*scale;
4 x* o' a0 i; o2 R* m b[2]=a[2]*scale;
: C3 C6 B% ?* L' E( k}; S& S) [1 ]9 _4 T# t6 ^/ m& P+ s
* J2 f5 _( O: ]5 z% E1 s- w- [0 Y |
|