计算机图形学实现机器人

float Txyz[4][4],R[4][4],Sxyz[4][4];
matTxyz(Txyz,tx,ty,tz);
matR(R,theta,rx,ry,rz);
matSxyz(Sxyz,sx,sy,sz);
MatMul(newMatrix,newMatrix,Txyz);
MatMul(newMatrix,newMatrix,R);
MatMul(newMatrix,newMatrix,Sxyz);
MatMul(newMatrix,newMatrix,localMatrix);

 

#include <math.h>
#include <FLOAT.H>
#include <stdio.h>
#include "transformation.h"
// 平移(tx,ty,tz)的矩阵Txyz 
void matTxyz(float Txyz[4][4],float tx,float ty,float tz)
{
SetIdentity(Txyz);
Txyz[0][3]=tx;
Txyz[1][3]=ty;
Txyz[2][3]=tz;
}

// 绕x轴旋转theta角的矩阵Rx 
void matRx(float Rx[4][4],float theta)
{
SetIdentity(Rx);       //theta余弦:cos(theta),正弦:sin(theta)
Rx[1][1]=cos(theta);
Rx[1][2]=-sin(theta);