Cluster/Cluster
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
ebc372aa44ed59b97f9ce0f1b18130a815018f68
Cluster/insDavi2.0/src/lib/InsMatrix.c

308 lines
7.7 KiB
C
Raw Normal View History

feat():initial version
2026-04-17 18:22:45 +08:00
#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) //<2F>Ƚ<EFBFBD><C8BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>ȣ<EFBFBD><C8A3><EFBFBD><EFBFBD><EFBFBD>Ϊ1<CEAA><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ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;
}
/*
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>axis_nor<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>תangle<EFBFBD><EFBFBD>
*/
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;
}
/*
* <EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>axis_nor<EFBFBD><EFBFBD>ʱ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>תangle<EFBFBD><EFBFBD>
*/
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;
}
Reference in New Issue Copy Permalink