308 lines
7.7 KiB
C
308 lines
7.7 KiB
C
#include <lib/InsMatrix.h>
|
||
|
||
#include <math.h>
|
||
|
||
#define PI_OVER_180 0.01745329251994329444f
|
||
#define EPSILON_E3 (float)(1E-3)
|
||
#define CompareFloat(a, b) ((fabs(a-b)<EPSILON_E3)?1:0) //比较两个浮点数是否相等，相等为1，不等为0
|
||
|
||
GLfloat *MultMatrix4x4(GLfloat *matC, GLfloat *matA, GLfloat *matB)
|
||
{
|
||
GLfloat matT[4*4];
|
||
matT[ 0] = matA[ 0] * matB[ 0] + matA[ 1] * matB[ 4] + matA[ 2] * matB[ 8] + matA[ 3] * matB[12];
|
||
matT[ 1] = matA[ 0] * matB[ 1] + matA[ 1] * matB[ 5] + matA[ 2] * matB[ 9] + matA[ 3] * matB[13];
|
||
matT[ 2] = matA[ 0] * matB[ 2] + matA[ 1] * matB[ 6] + matA[ 2] * matB[10] + matA[ 3] * matB[14];
|
||
matT[ 3] = matA[ 0] * matB[ 3] + matA[ 1] * matB[ 7] + matA[ 2] * matB[11] + matA[ 3] * matB[15];
|
||
matT[ 4] = matA[ 4] * matB[ 0] + matA[ 5] * matB[ 4] + matA[ 6] * matB[ 8] + matA[ 7] * matB[12];
|
||
matT[ 5] = matA[ 4] * matB[ 1] + matA[ 5] * matB[ 5] + matA[ 6] * matB[ 9] + matA[ 7] * matB[13];
|
||
matT[ 6] = matA[ 4] * matB[ 2] + matA[ 5] * matB[ 6] + matA[ 6] * matB[10] + matA[ 7] * matB[14];
|
||
matT[ 7] = matA[ 4] * matB[ 3] + matA[ 5] * matB[ 7] + matA[ 6] * matB[11] + matA[ 7] * matB[15];
|
||
matT[ 8] = matA[ 8] * matB[ 0] + matA[ 9] * matB[ 4] + matA[10] * matB[ 8] + matA[11] * matB[12];
|
||
matT[ 9] = matA[ 8] * matB[ 1] + matA[ 9] * matB[ 5] + matA[10] * matB[ 9] + matA[11] * matB[13];
|
||
matT[10] = matA[ 8] * matB[ 2] + matA[ 9] * matB[ 6] + matA[10] * matB[10] + matA[11] * matB[14];
|
||
matT[11] = matA[ 8] * matB[ 3] + matA[ 9] * matB[ 7] + matA[10] * matB[11] + matA[11] * matB[15];
|
||
matT[12] = matA[12] * matB[ 0] + matA[13] * matB[ 4] + matA[14] * matB[ 8] + matA[15] * matB[12];
|
||
matT[13] = matA[12] * matB[ 1] + matA[13] * matB[ 5] + matA[14] * matB[ 9] + matA[15] * matB[13];
|
||
matT[14] = matA[12] * matB[ 2] + matA[13] * matB[ 6] + matA[14] * matB[10] + matA[15] * matB[14];
|
||
matT[15] = matA[12] * matB[ 3] + matA[13] * matB[ 7] + matA[14] * matB[11] + matA[15] * matB[15];
|
||
|
||
matC[ 0] = matT[ 0]; matC[ 1] = matT[ 1]; matC[ 2] = matT[ 2]; matC[ 3] = matT[ 3];
|
||
matC[ 4] = matT[ 4]; matC[ 5] = matT[ 5]; matC[ 6] = matT[ 6]; matC[ 7] = matT[ 7];
|
||
matC[ 8] = matT[ 8]; matC[ 9] = matT[ 9]; matC[10] = matT[10]; matC[11] = matT[11];
|
||
matC[12] = matT[12]; matC[13] = matT[13]; matC[14] = matT[14]; matC[15] = matT[15];
|
||
|
||
return matC;
|
||
}
|
||
|
||
GLfloat *MultMatrix4x4Vec4x1(GLfloat *vecB, GLfloat *matA, GLfloat *vecA)
|
||
{
|
||
GLfloat vecTmp[4];
|
||
|
||
vecTmp[0] = vecA[0]*matA[0*4+0] + vecA[1]*matA[1*4+0] + vecA[2]*matA[2*4+0] + vecA[3]*matA[3*4+0];
|
||
vecTmp[1] = vecA[0]*matA[0*4+1] + vecA[1]*matA[1*4+1] + vecA[2]*matA[2*4+1] + vecA[3]*matA[3*4+1];
|
||
vecTmp[2] = vecA[0]*matA[0*4+2] + vecA[1]*matA[1*4+2] + vecA[2]*matA[2*4+2] + vecA[3]*matA[3*4+2];
|
||
vecTmp[3] = vecA[0]*matA[0*4+3] + vecA[1]*matA[1*4+3] + vecA[2]*matA[2*4+3] + vecA[3]*matA[3*4+3];
|
||
|
||
vecB[0] = vecTmp[0];
|
||
vecB[1] = vecTmp[1];
|
||
vecB[2] = vecTmp[2];
|
||
vecB[3] = vecTmp[3];
|
||
|
||
return vecB;
|
||
}
|
||
|
||
GLfloat *CopyMatrix4x4(GLfloat *matDes, GLfloat *matSrc)
|
||
{
|
||
matDes[0] = matSrc[0]; matDes[1] = matSrc[1];
|
||
matDes[2] = matSrc[2]; matDes[3] = matSrc[3];
|
||
matDes[4] = matSrc[4]; matDes[5] = matSrc[5];
|
||
matDes[6] = matSrc[6]; matDes[7] = matSrc[7];
|
||
matDes[8] = matSrc[8]; matDes[9] = matSrc[9];
|
||
matDes[10] = matSrc[10]; matDes[11] = matSrc[11];
|
||
matDes[12] = matSrc[12]; matDes[13] = matSrc[13];
|
||
matDes[14] = matSrc[14]; matDes[15] = matSrc[15];
|
||
|
||
return matDes;
|
||
}
|
||
|
||
Bool InvertMatrix4x4(GLfloat *inverse, GLfloat *src)
|
||
{
|
||
int i, j, k;
|
||
double t;
|
||
double temp[4][4];
|
||
|
||
for (i=0; i<4; i++) {
|
||
for (j=0; j<4; j++) {
|
||
temp[i][j] = src[i*4+j];
|
||
}
|
||
}
|
||
|
||
LoadIdentityMatrix(inverse);
|
||
|
||
for (i = 0; i < 4; i++) {
|
||
/*
|
||
** Look for largest element in column
|
||
*/
|
||
int swap = i;
|
||
for (j = i + 1; j < 4; j++) {
|
||
if (fabs(temp[j][i]) > fabs(temp[i][i])) {
|
||
swap = j;
|
||
}
|
||
}
|
||
|
||
if (swap != i) {
|
||
/*
|
||
** Swap rows.
|
||
*/
|
||
for (k = 0; k < 4; k++) {
|
||
t = temp[i][k];
|
||
temp[i][k] = temp[swap][k];
|
||
temp[swap][k] = t;
|
||
|
||
t = inverse[i*4+k];
|
||
inverse[i*4+k] = inverse[swap*4+k];
|
||
inverse[swap*4+k] = t;
|
||
}
|
||
}
|
||
|
||
if (temp[i][i] == 0) {
|
||
/*
|
||
** No non-zero pivot. The matrix is singular, which shouldn't
|
||
** happen. This means the user gave us a bad matrix.
|
||
*/
|
||
return INS_FALSE;
|
||
}
|
||
|
||
t = temp[i][i];
|
||
for (k = 0; k < 4; k++) {
|
||
temp[i][k] /= t;
|
||
inverse[i*4+k] /= t;
|
||
}
|
||
for (j = 0; j < 4; j++) {
|
||
if (j != i) {
|
||
t = temp[j][i];
|
||
for (k = 0; k < 4; k++) {
|
||
temp[j][k] -= temp[i][k]*t;
|
||
inverse[j*4+k] -= inverse[i*4+k]*t;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
return INS_TRUE;
|
||
}
|
||
|
||
GLfloat *SetRotateMatrix4x4_X_Axis(GLfloat *m, GLfloat angle)
|
||
{
|
||
float radians = PI_OVER_180*angle;
|
||
|
||
LoadIdentityMatrix(m);
|
||
|
||
m[5] = cosf(radians);
|
||
m[6] = sinf(radians);
|
||
m[9] = -m[6];//-sinf(radians);
|
||
m[10] = m[5];//cosf(radians);
|
||
return m;
|
||
}
|
||
|
||
GLfloat *RotateMatrix4x4_X_Axis(GLfloat *m, GLfloat angle)
|
||
{
|
||
GLfloat tm[4*4];
|
||
|
||
SetRotateMatrix4x4_X_Axis(tm, angle);
|
||
MultMatrix4x4(m, tm, m);
|
||
|
||
return m;
|
||
}
|
||
|
||
GLfloat *SetRotateMatrix4x4_Y_Axis(GLfloat *m, GLfloat angle)
|
||
{
|
||
float radians = PI_OVER_180*angle;
|
||
|
||
LoadIdentityMatrix(m);
|
||
|
||
m[0] = cosf(radians);
|
||
m[2] = -sinf(radians);
|
||
m[8] = -m[2];//sinf(radians);
|
||
m[10] = m[0];//cosf(radians);
|
||
return m;
|
||
}
|
||
|
||
GLfloat *RotateMatrix4x4_Y_Axis(GLfloat *m, GLfloat angle)
|
||
{
|
||
GLfloat tm[4*4];
|
||
|
||
SetRotateMatrix4x4_Y_Axis(tm, angle);
|
||
MultMatrix4x4(m, tm, m);
|
||
|
||
return m;
|
||
}
|
||
|
||
GLfloat *SetRotateMatrix4x4_Z_Axis(GLfloat *m, GLfloat angle)
|
||
{
|
||
float radians = PI_OVER_180*angle;
|
||
|
||
LoadIdentityMatrix(m);
|
||
|
||
m[0] = cosf(radians);
|
||
m[1] = sinf(radians);
|
||
m[4] = -m[1];//-sinf(radians);
|
||
m[5] = m[0];//cosf(radians);
|
||
return m;
|
||
}
|
||
|
||
GLfloat *RotateMatrix4x4_Z_Axis(GLfloat *m, GLfloat angle)
|
||
{
|
||
GLfloat tm[4*4];
|
||
|
||
SetRotateMatrix4x4_Z_Axis(tm, angle);
|
||
MultMatrix4x4(m, tm, m);
|
||
|
||
return m;
|
||
}
|
||
|
||
/*
|
||
* 绕任意轴向量axis_nor逆时针旋转angle度
|
||
*/
|
||
GLfloat *SetRotateMat4x4(GLfloat *mat4x4, Vec3f *axis_nor, GLfloat angle)
|
||
{
|
||
float c, s, one_c;
|
||
|
||
float xx, yy, zz;
|
||
float xs, ys, zs;
|
||
float xy, xz, yz;
|
||
|
||
c = cosf(angle*PI_OVER_180);
|
||
s = sinf(angle*PI_OVER_180);
|
||
one_c = 1.0 - c;
|
||
|
||
xx = axis_nor->x * axis_nor->x;
|
||
yy = axis_nor->y * axis_nor->y;
|
||
zz = axis_nor->z * axis_nor->z;
|
||
|
||
xs = axis_nor->x * s;
|
||
ys = axis_nor->y * s;
|
||
zs = axis_nor->z * s;
|
||
|
||
xy = axis_nor->x * axis_nor->y;
|
||
xz = axis_nor->x * axis_nor->z;
|
||
yz = axis_nor->y * axis_nor->z;
|
||
|
||
mat4x4[ 0] = (one_c * xx) + c;
|
||
mat4x4[ 1] = (one_c * xy) + zs;
|
||
mat4x4[ 2] = (one_c * xz) - ys;
|
||
mat4x4[ 3] = 0.0;
|
||
|
||
mat4x4[ 4] = (one_c * xy) - zs;
|
||
mat4x4[ 5] = (one_c * yy) + c;
|
||
mat4x4[ 6] = (one_c * yz) + xs;
|
||
mat4x4[ 7] = 0.0;
|
||
|
||
mat4x4[ 8] = (one_c * xz) + ys;
|
||
mat4x4[ 9] = (one_c * yz) - xs;
|
||
mat4x4[10] = (one_c * zz) + c;
|
||
mat4x4[11] = 0.0;
|
||
|
||
mat4x4[12] = 0.0;
|
||
mat4x4[13] = 0.0;
|
||
mat4x4[14] = 0.0;
|
||
mat4x4[15] = 1.0;
|
||
|
||
return mat4x4;
|
||
}
|
||
|
||
/*
|
||
* 绕任意轴向量axis_nor逆时针旋转angle度
|
||
*/
|
||
GLfloat *RotateMat4x4(GLfloat *mat4x4, Vec3f *axis_nor, GLfloat angle)
|
||
{
|
||
GLfloat tm[4*4];
|
||
|
||
SetRotateMat4x4(tm, axis_nor, angle);
|
||
MultMatrix4x4(mat4x4, tm, mat4x4);
|
||
|
||
return mat4x4;
|
||
}
|
||
|
||
GLfloat *SetTranslateMatrix4x4 (GLfloat *m, GLfloat trX, GLfloat trY, GLfloat trZ)
|
||
{
|
||
LoadIdentityMatrix(m);
|
||
|
||
m[12] = trX;
|
||
m[13] = trY;
|
||
m[14] = trZ;
|
||
|
||
return m;
|
||
}
|
||
|
||
GLfloat *TranslateMatrix4x4 (GLfloat *m, GLfloat trX, GLfloat trY, GLfloat trZ)
|
||
{
|
||
GLfloat tm[4*4];
|
||
|
||
SetTranslateMatrix4x4(tm, trX, trY, trZ);
|
||
MultMatrix4x4(m, tm, m);
|
||
|
||
return m;
|
||
}
|
||
|
||
GLfloat *SetScaleMatrix4x4 (GLfloat *m, GLfloat scaleX, GLfloat scaleY, GLfloat scaleZ)
|
||
{
|
||
LoadIdentityMatrix(m);
|
||
|
||
m[ 0] = scaleX;
|
||
m[ 5] = scaleY;
|
||
m[10] = scaleZ;
|
||
|
||
return m;
|
||
}
|
||
|
||
GLfloat *ScaleMatrix4x4 (GLfloat *m, GLfloat scaleX, GLfloat scaleY, GLfloat scaleZ)
|
||
{
|
||
GLfloat tm[4*4];
|
||
|
||
SetScaleMatrix4x4(tm, scaleX, scaleY, scaleZ);
|
||
MultMatrix4x4(m, tm, m);
|
||
|
||
return m;
|
||
}
|
||
|