Initial commit
This commit is contained in:
167
Mat3f.c
Normal file
167
Mat3f.c
Normal file
@@ -0,0 +1,167 @@
|
||||
#include "CgeMath.h"
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#define EPSILON 0.00001f
|
||||
#define PI 3.14159265358979323846f
|
||||
#define SET_ROW(row, val) row[0] = row[1] = row[2] = val
|
||||
|
||||
void CgeMat3fIdentity(float out[9]) {
|
||||
static const float ident[9] = {
|
||||
1.0f, 0.0f, 0.0f,
|
||||
0.0f, 1.0f, 0.0f,
|
||||
0.0f, 0.0f, 1.0f
|
||||
};
|
||||
|
||||
memcpy(out, ident, sizeof(ident));
|
||||
}
|
||||
|
||||
void CgeMat3fAdd(const float a[9], const float b[9], float out[9]) {
|
||||
CgeVec3fAdd(&a[0], &b[0], &out[0]);
|
||||
CgeVec3fAdd(&a[3], &b[3], &out[3]);
|
||||
CgeVec3fAdd(&a[6], &b[6], &out[6]);
|
||||
}
|
||||
|
||||
void CgeMat3fSub(const float a[9], const float b[9], float out[9]) {
|
||||
CgeVec3fSub(&a[0], &b[0], &out[0]);
|
||||
CgeVec3fSub(&a[3], &b[3], &out[3]);
|
||||
CgeVec3fSub(&a[6], &b[6], &out[6]);
|
||||
}
|
||||
|
||||
void CgeMat3fMul(const float a[9], const float b[9], float out[9]) {
|
||||
float tmp[9], row[3];
|
||||
|
||||
SET_ROW(row, b[0]); CgeVec3fMul(&a[0], row, &tmp[0]);
|
||||
SET_ROW(row, b[1]); CgeVec3fMulAdd(&a[3], row, &tmp[0], &tmp[0]);
|
||||
SET_ROW(row, b[2]); CgeVec3fMulAdd(&a[6], row, &tmp[0], &tmp[0]);
|
||||
|
||||
SET_ROW(row, b[3]); CgeVec3fMul(&a[0], row, &tmp[3]);
|
||||
SET_ROW(row, b[4]); CgeVec3fMulAdd(&a[3], row, &tmp[3], &tmp[3]);
|
||||
SET_ROW(row, b[5]); CgeVec3fMulAdd(&a[6], row, &tmp[3], &tmp[3]);
|
||||
|
||||
SET_ROW(row, b[6]); CgeVec3fMul(&a[0], row, &tmp[6]);
|
||||
SET_ROW(row, b[7]); CgeVec3fMulAdd(&a[3], row, &tmp[6], &tmp[6]);
|
||||
SET_ROW(row, b[8]); CgeVec3fMulAdd(&a[6], row, &tmp[6], &tmp[6]);
|
||||
|
||||
memcpy(out, tmp, sizeof(tmp));
|
||||
}
|
||||
|
||||
void CgeMat3fScale(const float a[9], float b, float out[9]) {
|
||||
CgeVec3fScale(&a[0], b, &out[0]);
|
||||
CgeVec3fScale(&a[3], b, &out[3]);
|
||||
CgeVec3fScale(&a[6], b, &out[6]);
|
||||
}
|
||||
|
||||
void CgeMat3fTranspose(const float in[9], float out[9]) {
|
||||
float tmp[9];
|
||||
|
||||
tmp[0] = in[0]; tmp[3] = in[1]; tmp[6] = in[2];
|
||||
tmp[1] = in[3]; tmp[4] = in[4]; tmp[7] = in[5];
|
||||
tmp[2] = in[6]; tmp[5] = in[7]; tmp[8] = in[8];
|
||||
|
||||
memcpy(out, tmp, sizeof(tmp));
|
||||
}
|
||||
|
||||
float CgeMat3fTrace(const float in[9]) {
|
||||
return in[0] + in[4] + in[8];
|
||||
}
|
||||
|
||||
float CgeMat3fDet(const float in[9]) {
|
||||
float a, b, c, result;
|
||||
|
||||
a = in[4] * in[8] - in[7] * in[5];
|
||||
b = in[1] * in[8] - in[7] * in[2];
|
||||
c = in[1] * in[5] - in[4] * in[2];
|
||||
|
||||
result = 0.0f;
|
||||
result += in[0] * a;
|
||||
result -= in[3] * b;
|
||||
result += in[6] * c;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
int CgeMat3fInverse(const float in[9], float out[9]) {
|
||||
float a, b, c, det;
|
||||
float tmp[16];
|
||||
|
||||
a = in[4] * in[8] - in[7] * in[5];
|
||||
b = in[1] * in[8] - in[7] * in[2];
|
||||
c = in[1] * in[5] - in[4] * in[2];
|
||||
|
||||
tmp[0] = a;
|
||||
tmp[1] = -b;
|
||||
tmp[2] = c;
|
||||
|
||||
det = 0.0f;
|
||||
det += in[0] * tmp[0];
|
||||
det += in[3] * tmp[1];
|
||||
det += in[6] * tmp[2];
|
||||
|
||||
if (fabsf(det) < EPSILON)
|
||||
return 0;
|
||||
|
||||
a = in[3] * in[8] - in[6] * in[5];
|
||||
b = in[0] * in[8] - in[6] * in[2];
|
||||
c = in[0] * in[5] - in[3] * in[2];
|
||||
|
||||
tmp[3] = -a;
|
||||
tmp[4] = b;
|
||||
tmp[5] = -c;
|
||||
|
||||
a = in[3] * in[7] - in[6] * in[4];
|
||||
b = in[0] * in[7] - in[6] * in[1];
|
||||
c = in[0] * in[4] - in[3] * in[1];
|
||||
|
||||
tmp[6] = a;
|
||||
tmp[7] = -b;
|
||||
tmp[8] = c;
|
||||
|
||||
CgeMat3fScale(tmp, 1.0f / det, out);
|
||||
return 1;
|
||||
}
|
||||
|
||||
void CgeMat3fFromScale(float x, float y, float out[9]) {
|
||||
CgeMat3fIdentity(out);
|
||||
out[0] = x;
|
||||
out[4] = y;
|
||||
}
|
||||
|
||||
void CgeMat3fFromTranslation(float x, float y, float out[9]) {
|
||||
CgeMat3fIdentity(out);
|
||||
out[6] = x;
|
||||
out[7] = y;
|
||||
}
|
||||
|
||||
void CgeMat3fFromRotation(float angle, float out[9]) {
|
||||
float c, s;
|
||||
|
||||
c = cosf(angle);
|
||||
s = sinf(angle);
|
||||
|
||||
CgeMat3fIdentity(out);
|
||||
out[0] = c;
|
||||
out[1] = s;
|
||||
out[3] = -s;
|
||||
out[4] = c;
|
||||
}
|
||||
|
||||
void CgeMat3fApplyVec3f(float a[9], float b[3], float out[3]) {
|
||||
float tmp[3], row[3];
|
||||
|
||||
SET_ROW(row, b[0]); CgeVec3fMul(&a[0], row, tmp);
|
||||
SET_ROW(row, b[1]); CgeVec3fMulAdd(&a[3], row, tmp, tmp);
|
||||
SET_ROW(row, b[2]); CgeVec3fMulAdd(&a[6], row, tmp, tmp);
|
||||
|
||||
memcpy(out, tmp, sizeof(tmp));
|
||||
}
|
||||
|
||||
void CgeMat3fApplyVec2f(float a[9], float b[2], float out[2]) {
|
||||
float tmp[3], row[3];
|
||||
|
||||
SET_ROW(row, b[0]); CgeVec3fMul(&a[0], row, tmp);
|
||||
SET_ROW(row, b[1]); CgeVec3fMulAdd(&a[3], row, tmp, tmp);
|
||||
SET_ROW(row, 1.0f); CgeVec3fMulAdd(&a[6], row, tmp, tmp);
|
||||
|
||||
memcpy(out, tmp, sizeof(float) * 2);
|
||||
}
|
||||
Reference in New Issue
Block a user