dolphin/Source/Plugins/Plugin_WiimoteNew/Src/WiimoteEmu/MatrixMath.h

85 lines
2.2 KiB
C

#ifndef MATRIXMATH_H
#define MATRIXMATH_H
#include <math.h>
typedef double Matrix[4][4];
typedef struct
{
double x,y,z;
} Vertex;
inline void MatrixIdentity(Matrix & m)
{
m[0][0]=1; m[0][1]=0; m[0][2]=0; m[0][3]=0;
m[1][0]=0; m[1][1]=1; m[1][2]=0; m[1][3]=0;
m[2][0]=0; m[2][1]=0; m[2][2]=1; m[2][3]=0;
m[3][0]=0; m[3][1]=0; m[3][2]=0; m[3][3]=1;
}
inline void MatrixFrustum(Matrix &m, double l, double r, double b, double t, double n, double f)
{
m[0][0]=2*n/(r-l); m[0][1]=0; m[0][2]=0; m[0][3]=0;
m[1][0]=0; m[1][1]=2*n/(t-b); m[1][2]=0; m[1][3]=0;
m[2][0]=(r+l)/(r-l); m[2][1]=(t+b)/(t-b); m[2][2]=(f+n)/(f-n); m[2][3]=-1;
m[3][0]=0; m[3][1]=0; m[3][2]=2*f*n/(f-n); m[3][3]=0;
}
inline void MatrixPerspective(Matrix & m, double fovy, double aspect, double nplane, double fplane)
{
double xmin,xmax,ymin,ymax;
ymax = nplane * tan(fovy * M_PI / 360.0);
ymin = -ymax;
xmin = ymin * aspect;
xmax = ymax * aspect;
MatrixFrustum(m,xmin,xmax,ymin,ymax,nplane,fplane);
}
inline void MatrixRotationByZ(Matrix &m, double sin, double cos)
{
m[0][0]=cos; m[0][1]=-sin; m[0][2]=0; m[0][3]=0;
m[1][0]=sin; m[1][1]=cos; m[1][2]=0; m[1][3]=0;
m[2][0]=0; m[2][1]=0; m[2][2]=1; m[2][3]=0;
m[3][0]=0; m[3][1]=0; m[3][2]=0; m[3][3]=1;
}
inline void MatrixScale(Matrix &m, double xfact, double yfact, double zfact)
{
m[0][0]=xfact; m[0][1]=0; m[0][2]=0; m[0][3]=0;
m[1][0]=0; m[1][1]=yfact; m[1][2]=0; m[1][3]=0;
m[2][0]=0; m[2][1]=0; m[2][2]=zfact; m[2][3]=0;
m[3][0]=0; m[3][1]=0; m[3][2]=0; m[3][3]=1;
}
inline void MatrixMultiply(Matrix &r, const Matrix &a, const Matrix &b)
{
for (int i=0; i<16; i++)
r[i>>2][i&3]=0.0f;
for (int i=0; i<4; i++)
for (int j=0; j<4; j++)
for (int k=0; k<4; k++)
r[i][j]+=a[i][k]*b[k][j];
}
inline void MatrixTransformVertex(Matrix const & m, Vertex & v)
{
Vertex ov;
double w;
ov.x=v.x;
ov.y=v.y;
ov.z=v.z;
v.x = m[0][0] * ov.x + m[0][1] * ov.y + m[0][2] * ov.z + m[0][3];
v.y = m[1][0] * ov.x + m[1][1] * ov.y + m[1][2] * ov.z + m[1][3];
v.z = m[2][0] * ov.x + m[2][1] * ov.y + m[2][2] * ov.z + m[2][3];
w = m[3][0] * ov.x + m[3][1] * ov.y + m[3][2] * ov.z + m[3][3];
if (w!=0)
{
v.x/=w;
v.y/=w;
v.z/=w;
}
}
#endif