Archive/Portal64
1
0
Fork 0
Code Issues Pull requests Actions Packages Projects Releases Wiki Activity

Refactor physics

Browse source
This commit is contained in:
James Lambert 2022-03-12 22:04:07 -07:00
parent d14cc482e5
commit 5788dcc232
30 changed files with 915 additions and 100 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

8
src/math/mathf.c
View file

@ -110,4 +110,12 @@ char floatTos8norm(float input) {
} else {
return (char)result;
}
}
float safeInvert(float input) {
if (input == 0.0f) {
return 0.0f;
}
return 1.0f / input;
}

2
src/math/mathf.h
View file

@ -28,6 +28,8 @@ float maxf(float a, float b);
char floatTos8norm(float input);
float safeInvert(float input);
#define isnan(a) ((a) != (a))
#endif

6
src/math/quaternion.c
View file

@ -101,9 +101,9 @@ void quatMultiply(struct Quaternion* a, struct Quaternion* b, struct Quaternion*
void quatAdd(struct Quaternion* a, struct Quaternion* b, struct Quaternion* out) {
out->x = a->x + b->x;
out->x = a->y + b->y;
out->x = a->z + b->z;
out->x = a->w + b->w;
out->y = a->y + b->y;
out->z = a->z + b->z;
out->w = a->w + b->w;
}
void quatToMatrix(struct Quaternion* q, float out[4][4]) {

12
src/math/transform.c
View file

@ -8,22 +8,22 @@ void transformInitIdentity(struct Transform* in) {
in->scale = gOneVec;
}
void transformToMatrix(struct Transform* in, float mtx[4][4]) {
void transformToMatrix(struct Transform* in, float mtx[4][4], float sceneScale) {
quatToMatrix(&in->rotation, mtx);
mtx[0][0] *= in->scale.x; mtx[0][1] *= in->scale.x; mtx[0][2] *= in->scale.x;
mtx[1][0] *= in->scale.y; mtx[1][1] *= in->scale.y; mtx[1][2] *= in->scale.y;
mtx[2][0] *= in->scale.z; mtx[2][1] *= in->scale.z; mtx[2][2] *= in->scale.z;
mtx[3][0] = in->position.x;
mtx[3][1] = in->position.y;
mtx[3][2] = in->position.z;
mtx[3][0] = in->position.x * sceneScale;
mtx[3][1] = in->position.y * sceneScale;
mtx[3][2] = in->position.z * sceneScale;
}
void transformToMatrixL(struct Transform* in, Mtx* mtx) {
void transformToMatrixL(struct Transform* in, Mtx* mtx, float sceneScale) {
float mtxf[4][4];
transformToMatrix(in, mtxf);
transformToMatrix(in, mtxf, sceneScale);
guMtxF2L(mtxf, mtx);
}

4
src/math/transform.h
View file

@ -12,8 +12,8 @@ struct Transform {
};
void transformInitIdentity(struct Transform* in);
void transformToMatrix(struct Transform* in, float mtx[4][4]);
void transformToMatrixL(struct Transform* in, Mtx* mtx);
void transformToMatrix(struct Transform* in, float mtx[4][4], float sceneScale);
void transformToMatrixL(struct Transform* in, Mtx* mtx, float sceneScale);
void transformInvert(struct Transform* in, struct Transform* out);
void transformPoint(struct Transform* transform, struct Vector3* in, struct Vector3* out);
void transformPointInverse(struct Transform* transform, struct Vector3* in, struct Vector3* out);

222
src/models/cube.inc.h Normal file
View file

@ -0,0 +1,222 @@
Lights1 cube_cube_lights = gdSPDefLights1(
0x7F, 0x7F, 0x7F,
0xFE, 0xFE, 0xFE, 0x28, 0x28, 0x28);
u64 cube_CubeTexture_rgba16[] = {
0x94a394a394a394a3, 0x94a394a394a394a3, 0x94a394a394a394a3, 0x94a394a38ca38ca3, 0x8c63846184218421, 0x7c215ad74a535295, 0x8c638c638ca38ca3, 0x8ca394a394a394a3,
0x94a394a394a394a3, 0x94a394a394a394a3, 0x94a394a394a394a3, 0x94a394a38ca38c63, 0x8c6384217c217c1f, 0x7c1f635b4a954253, 0x8c638c638ca38ca3, 0x8ca394a394a394a3,
0x94a394a394a394a3, 0x94a394a394a394a3, 0x94a394a394a394a3, 0x94a394a38ca38ca3, 0x8c6384617c217c1f, 0x73df63194a954253, 0x8c638c638ca38ca3, 0x8ca394a394a394a3,
0x94a394a394a394a3, 0x94a394a394a394a3, 0x94a394a394a394a3, 0x94a394a38ca38c63, 0x8ca384617c1f7c1f, 0x73df63194a954253, 0x8c638c638ca394a3, 0x94a394a394a394a3,
0x94a394a394a394a3, 0x94a394a394a394a3, 0x94a394a394a394a3, 0x94a394a38ca38ca3, 0x8ca384617c217bdf, 0x73df63194a954213, 0x8c638c638ca38ca3, 0x8ca38ca394a394a3,
0x94a394a394a394a3, 0x8c6394a394a394a3, 0x94a394a394a394a3, 0x94a394a38ca38ca3, 0x8ca384637c2173df, 0x73dd63194a954253, 0x8c638c638c638ca3, 0x8ca38ca38ca38ca3,
0x94a394a394a394a3, 0x94a394a394a394a3, 0x94a394a394a394a3, 0x94a38ca38c638c63, 0x8ca384637c1f73df, 0x73dd63594ad74253, 0x7c218c638ca38ca3, 0x8c638ca38ca38c63,
0x94a38ca38ca394a3, 0x94a394a394a394a3, 0x94a394a394a394a3, 0x94a38ca38ca38c63, 0x846184617c2173df, 0x73dd635b52d74a97, 0x5b1784618c638461, 0x842184217c217c21,
0x94a38ca394a394a3, 0x94a394a394a394a3, 0x94a394a394a394a3, 0x8ca38ca38ca38461, 0x84617c217c2173df, 0x73df6b5b52d75319, 0x4a956b5b7c1f7c1f, 0x7bdf7bdf7bdf7bdf,
0x8ca38ca394a394a3, 0x94a394a394a394a3, 0x94a394a38ca38ca3, 0x8ca38c6384618421, 0x7c217c217c217c1f, 0x73df6b5b53195b1b, 0x5b1952d773dd6b9b, 0x635b63595b193a55,
0x8ca38ca394a394a3, 0x94a394a394a394a3, 0x94a394a394a38ca3, 0x8c63846184617c21, 0x7c217c217c1f741f, 0x7c1f6b9b5b19635b, 0x5b195b195b195b19, 0x5b19631963193ad9,
0x94a38ca394a394a3, 0x94a394a394a394a3, 0x94a38ca38ca38c63, 0x846184617c217c21, 0x7c217c217c1f73df, 0x73df6b9d5b196319, 0x63195b195b195b19, 0x63196319635b3ad9,
0x8ca38ca38ca38ca3, 0x94a394a394a394a3, 0x8ca38ca38c638461, 0x84617c217c217c21, 0x7c1f7c1f73df73dd, 0x635b5b195b195b19, 0x5b195b195b196319, 0x63196319635b3ad9,
0x8ca38ca38ca38ca3, 0x8ca38ca394a38ca3, 0x8ca38c6384618461, 0x84217c217c217c1f, 0x7c1f7bdf6b9d5b19, 0x5b195b195b195b19, 0x5b195b195b195b19, 0x63195b19635b3ad9,
0x8c638ca38ca38ca3, 0x8ca38ca38ca38ca3, 0x8ca3846184618421, 0x7c217c217c1f7c1f, 0x73df635b5b195b19, 0x5b195b195b195b19, 0x5b195b195b195b19, 0x5b195b1963193ad9,
0x8c638ca38ca38ca3, 0x8ca38ca38ca38ca3, 0x8463846184617c21, 0x7c217c1f7c1f73df, 0x635b5b195b195b19, 0x5b195b195b195b19, 0x5b195b195b195b19, 0x5b195b195b193ad7,
0x8c638ca38ca38ca3, 0x8ca38ca38ca38c63, 0x846184617c217c21, 0x7c1f7c1f73df635b, 0x5b195b195b195b19, 0x5b195b195b195b19, 0x5b195b195ad75ad7, 0x5ad752d752d73255,
0x8c638c638c638c63, 0x8c6384638c638461, 0x84617c217c217c1f, 0x7c1f73df6b5b5b19, 0x5b195b195b195b19, 0x5b195b195b195b19, 0x5b1952d752d75295, 0x4a954a5342112211,
0x8461842184218421, 0x7c217c217c217c21, 0x7c217c217c217c1f, 0x73df6b9d5b195b19, 0x5b195b195b195b19, 0x5b195b195b1952d7, 0x52d74a9542534211, 0x4a5352d56319635b,
0x7c217c217c217c21, 0x7c1f7c1f73df73df, 0x741f7c217c1f741f, 0x73df63195b195b19, 0x5b195b195b195b19, 0x5b195ad752d75295, 0x425342115ad773dd, 0x846184638c638c63,
0x7c217c1f7c1f73df, 0x73df73df73df73df, 0x73df73df7c1f73df, 0x6b5b5b195b195b19, 0x5b195b195b195b19, 0x5b1752d74a954211, 0x52957bdf8c638c63, 0x8c638c638c638ca3,
0x5b17739d6b9b6b9b, 0x635b6b5b6b9b6b9b, 0x6b9b6b9d6b9d73dd, 0x6319631963196319, 0x5b195b195b195ad7, 0x52d74a9542116319, 0x7c218c638c638c63, 0x8c638c638ca38ca3,
0x4a934a954a954a95, 0x4a954a954a954a97, 0x52d752d752d75b19, 0x631963195b195b19, 0x5b195b195ad752d7, 0x4a953a1163598461, 0x8c638c638c638c63, 0x8ca38ca38ca38ca3,
0x4a934a5342534253, 0x3a53425342534ad7, 0x5b195b195b196319, 0x63195b195b195b19, 0x5b195b1952d74a95, 0x42115b1984618c63, 0x8c638c638c638c63, 0x8ca38ca38ca38ca3,
0x7c217c1f7c1f7c1f, 0x7bdf7c1f73df52d7, 0x4a955b195b1b6319, 0x6319631963196319, 0x5b195b1952d74253, 0x4a937c218c638c63, 0x8c638c638ca38c63, 0x8c638c6184218421,
0x8c638c638c638c63, 0x8c638c638c638421, 0x635b52d75b195b19, 0x6319631963195b19, 0x5b195ad74a953a11, 0x739d8c638c638c63, 0x8ca38ca38c638c61, 0x8c618c638c638421,
0x8c638ca38c638c63, 0x8ca38c638c638c63, 0x7c217bdf5b195b19, 0x63195b195b195b19, 0x5b1952d74a535295, 0x84618c638c638ca3, 0x8ca38c6384618421, 0x842184218c618c63,
0x8c638c638ca38ca3, 0x8ca38ca38c638c63, 0x7c2173dd5b196319, 0x63195b195b195b19, 0x5b1752d73a116b9b, 0x8c638c638c638ca3, 0x8ca38c6184218421, 0x842184218c618c63,
0x8c638ca394a394a3, 0x94a38ca38ca38c63, 0x7c1f6b9d5b195b19, 0x631963195b195b19, 0x5ad74a953a117c1f, 0x8c638c638ca38ca3, 0x8c638c618c618c63, 0x8c6394a394a394a3,
0x8c6394a394a394a3, 0x94a38c638ca38461, 0x7c1f6b9b63196319, 0x5b1963195b195b19, 0x52d74a954a538461, 0x8ca38c638ca38ca3, 0x8c638c618c618c61, 0x8c6394a394a394a3,
0x8c6394a394a394a3, 0x94a394a38ca38461, 0x7c1f6b5b6319635b, 0x6319635b63195b19, 0x5ad74a5352d78c63, 0x8ca38c638ca38ca3, 0x8461842184218c63, 0x94a394a394a394a3,
0x8ca394a394a394a3, 0x94a38ca38ca38421, 0x7c1f531942d74ad9, 0x4ad94ad942d942d7, 0x3a95321352d78c63, 0x8c638c638ca38ca3, 0x842184218c6194a3, 0x94a394a394a394a3,
};
Vtx cube_CubeSimpleBevel_mesh_vtx_0[98] = {
{{{-73, -81, 73},0, {27, 1982},{0xB7, 0xB7, 0x49, 0xFF}}},
{{{-73, -73, 81},0, {27, 2084},{0xB7, 0xB7, 0x49, 0xFF}}},
{{{-81, -73, 73},0, {-75, 1982},{0xB7, 0xB7, 0x49, 0xFF}}},
{{{-73, 73, 81},0, {27, 2084},{0xB7, 0x49, 0x49, 0xFF}}},
{{{-73, 81, 73},0, {27, 1982},{0xB7, 0x49, 0x49, 0xFF}}},
{{{-81, 73, 73},0, {-75, 1982},{0xB7, 0x49, 0x49, 0xFF}}},
{{{-73, -81, -73},0, {27, 31},{0xB7, 0xB7, 0xB7, 0xFF}}},
{{{-81, -73, -73},0, {-75, 31},{0xB7, 0xB7, 0xB7, 0xFF}}},
{{{-73, -73, -81},0, {27, -71},{0xB7, 0xB7, 0xB7, 0xFF}}},
{{{-73, 81, -73},0, {27, 31},{0xB7, 0x49, 0xB7, 0xFF}}},
{{{-73, 73, -81},0, {27, -71},{0xB7, 0x49, 0xB7, 0xFF}}},
{{{-81, 73, -73},0, {-75, 31},{0xB7, 0x49, 0xB7, 0xFF}}},
{{{73, -81, 73},0, {1977, 1982},{0x49, 0xB7, 0x49, 0xFF}}},
{{{81, -73, 73},0, {2079, 1982},{0x49, 0xB7, 0x49, 0xFF}}},
{{{73, -73, 81},0, {1977, 2084},{0x49, 0xB7, 0x49, 0xFF}}},
{{{73, 81, 73},0, {1977, 1982},{0x49, 0x49, 0x49, 0xFF}}},
{{{73, 73, 81},0, {1977, 2084},{0x49, 0x49, 0x49, 0xFF}}},
{{{81, 73, 73},0, {2079, 1982},{0x49, 0x49, 0x49, 0xFF}}},
{{{73, -81, -73},0, {1977, 31},{0x49, 0xB7, 0xB7, 0xFF}}},
{{{73, -73, -81},0, {1977, -71},{0x49, 0xB7, 0xB7, 0xFF}}},
{{{81, -73, -73},0, {2079, 31},{0x49, 0xB7, 0xB7, 0xFF}}},
{{{73, 81, -73},0, {1977, 31},{0x49, 0x49, 0xB7, 0xFF}}},
{{{81, 73, -73},0, {2079, 31},{0x49, 0x49, 0xB7, 0xFF}}},
{{{73, 73, -81},0, {1977, -71},{0x49, 0x49, 0xB7, 0xFF}}},
{{{-73, 81, 73},0, {27, 1982},{0x0, 0x5A, 0x5A, 0xFF}}},
{{{-73, 73, 81},0, {27, 2084},{0x0, 0x5A, 0x5A, 0xFF}}},
{{{73, 73, 81},0, {1977, 2084},{0x0, 0x5A, 0x5A, 0xFF}}},
{{{73, 81, 73},0, {1977, 1982},{0x0, 0x5A, 0x5A, 0xFF}}},
{{{73, 73, 81},0, {1979, 33},{0x0, 0x0, 0x7F, 0xFF}}},
{{{-73, 73, 81},0, {27, 33},{0x0, 0x0, 0x7F, 0xFF}}},
{{{-73, -73, 81},0, {27, 1984},{0x0, 0x0, 0x7F, 0xFF}}},
{{{73, 73, 81},0, {1979, 33},{0x0, 0x0, 0x7F, 0xFF}}},
{{{-73, -73, 81},0, {27, 1984},{0x0, 0x0, 0x7F, 0xFF}}},
{{{73, -73, 81},0, {1979, 1984},{0x0, 0x0, 0x7F, 0xFF}}},
{{{81, 73, -73},0, {1979, 32},{0x7F, 0x0, 0x0, 0xFF}}},
{{{81, 73, 73},0, {29, 32},{0x7F, 0x0, 0x0, 0xFF}}},
{{{81, -73, 73},0, {29, 1982},{0x7F, 0x0, 0x0, 0xFF}}},
{{{81, -73, -73},0, {1979, 1982},{0x7F, 0x0, 0x0, 0xFF}}},
{{{-73, 81, -73},0, {27, 31},{0x0, 0x7F, 0x0, 0xFF}}},
{{{-73, 81, 73},0, {27, 1982},{0x0, 0x7F, 0x0, 0xFF}}},
{{{73, 81, 73},0, {1977, 1982},{0x0, 0x7F, 0x0, 0xFF}}},
{{{73, 81, -73},0, {1977, 31},{0x0, 0x7F, 0x0, 0xFF}}},
{{{-81, 73, 73},0, {29, 32},{0x81, 0x0, 0x0, 0xFF}}},
{{{-81, 73, -73},0, {1979, 32},{0x81, 0x0, 0x0, 0xFF}}},
{{{-81, -73, -73},0, {1979, 1982},{0x81, 0x0, 0x0, 0xFF}}},
{{{-81, -73, 73},0, {29, 1982},{0x81, 0x0, 0x0, 0xFF}}},
{{{73, -73, 81},0, {1979, 1984},{0x5A, 0x0, 0x5A, 0xFF}}},
{{{81, -73, 73},0, {2081, 1984},{0x5A, 0x0, 0x5A, 0xFF}}},
{{{81, 73, 73},0, {2081, 33},{0x5A, 0x0, 0x5A, 0xFF}}},
{{{73, 73, 81},0, {1979, 33},{0x5A, 0x0, 0x5A, 0xFF}}},
{{{-73, -81, 73},0, {27, 1982},{0xA6, 0xA6, 0x0, 0xFF}}},
{{{-81, -73, 73},0, {-75, 1982},{0xA6, 0xA6, 0x0, 0xFF}}},
{{{-81, -73, -73},0, {-75, 31},{0xA6, 0xA6, 0x0, 0xFF}}},
{{{-73, -81, -73},0, {27, 31},{0xA6, 0xA6, 0x0, 0xFF}}},
{{{81, -73, -73},0, {2081, 1984},{0x5A, 0x0, 0xA6, 0xFF}}},
{{{73, -73, -81},0, {1979, 1984},{0x5A, 0x0, 0xA6, 0xFF}}},
{{{73, 73, -81},0, {1979, 33},{0x5A, 0x0, 0xA6, 0xFF}}},
{{{81, 73, -73},0, {2081, 33},{0x5A, 0x0, 0xA6, 0xFF}}},
{{{73, 81, 73},0, {1977, 1982},{0x5A, 0x5A, 0x0, 0xFF}}},
{{{81, 73, 73},0, {2079, 1982},{0x5A, 0x5A, 0x0, 0xFF}}},
{{{81, 73, -73},0, {2079, 31},{0x5A, 0x5A, 0x0, 0xFF}}},
{{{73, 81, -73},0, {1977, 31},{0x5A, 0x5A, 0x0, 0xFF}}},
{{{-73, -73, -81},0, {27, 1984},{0xA6, 0x0, 0xA6, 0xFF}}},
{{{-81, -73, -73},0, {-75, 1984},{0xA6, 0x0, 0xA6, 0xFF}}},
{{{-81, 73, -73},0, {-75, 33},{0xA6, 0x0, 0xA6, 0xFF}}},
{{{-73, 73, -81},0, {27, 33},{0xA6, 0x0, 0xA6, 0xFF}}},
{{{73, -81, -73},0, {1977, 31},{0x5A, 0xA6, 0x0, 0xFF}}},
{{{81, -73, -73},0, {2079, 31},{0x5A, 0xA6, 0x0, 0xFF}}},
{{{81, -73, 73},0, {2079, 1982},{0x5A, 0xA6, 0x0, 0xFF}}},
{{{73, -81, 73},0, {1977, 1982},{0x5A, 0xA6, 0x0, 0xFF}}},
{{{-73, 73, 81},0, {27, 33},{0xA6, 0x0, 0x5A, 0xFF}}},
{{{-81, 73, 73},0, {-75, 33},{0xA6, 0x0, 0x5A, 0xFF}}},
{{{-81, -73, 73},0, {-75, 1984},{0xA6, 0x0, 0x5A, 0xFF}}},
{{{-73, -73, 81},0, {27, 1984},{0xA6, 0x0, 0x5A, 0xFF}}},
{{{73, 81, -73},0, {1977, 31},{0x0, 0x5A, 0xA6, 0xFF}}},
{{{73, 73, -81},0, {1977, -71},{0x0, 0x5A, 0xA6, 0xFF}}},
{{{-73, 73, -81},0, {27, -71},{0x0, 0x5A, 0xA6, 0xFF}}},
{{{-73, 81, -73},0, {27, 31},{0x0, 0x5A, 0xA6, 0xFF}}},
{{{-73, -81, -73},0, {27, 31},{0x0, 0xA6, 0xA6, 0xFF}}},
{{{-73, -73, -81},0, {27, -71},{0x0, 0xA6, 0xA6, 0xFF}}},
{{{73, -73, -81},0, {1977, -71},{0x0, 0xA6, 0xA6, 0xFF}}},
{{{73, -81, -73},0, {1977, 31},{0x0, 0xA6, 0xA6, 0xFF}}},
{{{-73, 81, -73},0, {27, 31},{0xA6, 0x5A, 0x0, 0xFF}}},
{{{-81, 73, -73},0, {-75, 31},{0xA6, 0x5A, 0x0, 0xFF}}},
{{{-81, 73, 73},0, {-75, 1982},{0xA6, 0x5A, 0x0, 0xFF}}},
{{{-73, 81, 73},0, {27, 1982},{0xA6, 0x5A, 0x0, 0xFF}}},
{{{73, -81, 73},0, {1977, 1982},{0x0, 0xA6, 0x5A, 0xFF}}},
{{{73, -73, 81},0, {1977, 2084},{0x0, 0xA6, 0x5A, 0xFF}}},
{{{-73, -73, 81},0, {27, 2084},{0x0, 0xA6, 0x5A, 0xFF}}},
{{{-73, -81, 73},0, {27, 1982},{0x0, 0xA6, 0x5A, 0xFF}}},
{{{-73, 73, -81},0, {27, 33},{0x0, 0x0, 0x81, 0xFF}}},
{{{73, 73, -81},0, {1979, 33},{0x0, 0x0, 0x81, 0xFF}}},
{{{73, -73, -81},0, {1979, 1984},{0x0, 0x0, 0x81, 0xFF}}},
{{{-73, -73, -81},0, {27, 1984},{0x0, 0x0, 0x81, 0xFF}}},
{{{73, -81, -73},0, {1977, 31},{0x0, 0x81, 0x0, 0xFF}}},
{{{73, -81, 73},0, {1977, 1982},{0x0, 0x81, 0x0, 0xFF}}},
{{{-73, -81, 73},0, {27, 1982},{0x0, 0x81, 0x0, 0xFF}}},
{{{-73, -81, -73},0, {27, 31},{0x0, 0x81, 0x0, 0xFF}}},
};
Gfx cube_CubeSimpleBevel_mesh_tri_0[] = {
gsSPVertex(cube_CubeSimpleBevel_mesh_vtx_0 + 0, 15, 0),
gsSP1Triangle(0, 1, 2, 0),
gsSP1Triangle(3, 4, 5, 0),
gsSP1Triangle(6, 7, 8, 0),
gsSP1Triangle(9, 10, 11, 0),
gsSP1Triangle(12, 13, 14, 0),
gsSPVertex(cube_CubeSimpleBevel_mesh_vtx_0 + 15, 16, 0),
gsSP1Triangle(0, 1, 2, 0),
gsSP1Triangle(3, 4, 5, 0),
gsSP1Triangle(6, 7, 8, 0),
gsSP1Triangle(9, 10, 11, 0),
gsSP1Triangle(9, 11, 12, 0),
gsSP1Triangle(13, 14, 15, 0),
gsSPVertex(cube_CubeSimpleBevel_mesh_vtx_0 + 31, 15, 0),
gsSP1Triangle(0, 1, 2, 0),
gsSP1Triangle(3, 4, 5, 0),
gsSP1Triangle(3, 5, 6, 0),
gsSP1Triangle(7, 8, 9, 0),
gsSP1Triangle(7, 9, 10, 0),
gsSP1Triangle(11, 12, 13, 0),
gsSP1Triangle(11, 13, 14, 0),
gsSPVertex(cube_CubeSimpleBevel_mesh_vtx_0 + 46, 16, 0),
gsSP1Triangle(0, 1, 2, 0),
gsSP1Triangle(0, 2, 3, 0),
gsSP1Triangle(4, 5, 6, 0),
gsSP1Triangle(4, 6, 7, 0),
gsSP1Triangle(8, 9, 10, 0),
gsSP1Triangle(8, 10, 11, 0),
gsSP1Triangle(12, 13, 14, 0),
gsSP1Triangle(12, 14, 15, 0),
gsSPVertex(cube_CubeSimpleBevel_mesh_vtx_0 + 62, 16, 0),
gsSP1Triangle(0, 1, 2, 0),
gsSP1Triangle(0, 2, 3, 0),
gsSP1Triangle(4, 5, 6, 0),
gsSP1Triangle(4, 6, 7, 0),
gsSP1Triangle(8, 9, 10, 0),
gsSP1Triangle(8, 10, 11, 0),
gsSP1Triangle(12, 13, 14, 0),
gsSP1Triangle(12, 14, 15, 0),
gsSPVertex(cube_CubeSimpleBevel_mesh_vtx_0 + 78, 16, 0),
gsSP1Triangle(0, 1, 2, 0),
gsSP1Triangle(0, 2, 3, 0),
gsSP1Triangle(4, 5, 6, 0),
gsSP1Triangle(4, 6, 7, 0),
gsSP1Triangle(8, 9, 10, 0),
gsSP1Triangle(8, 10, 11, 0),
gsSP1Triangle(12, 13, 14, 0),
gsSP1Triangle(12, 14, 15, 0),
gsSPVertex(cube_CubeSimpleBevel_mesh_vtx_0 + 94, 4, 0),
gsSP1Triangle(0, 1, 2, 0),
gsSP1Triangle(0, 2, 3, 0),
gsSPEndDisplayList(),
};
Gfx mat_cube_cube[] = {
gsDPPipeSync(),
gsDPSetCombineLERP(TEXEL0, 0, SHADE, 0, 0, 0, 0, ENVIRONMENT, TEXEL0, 0, SHADE, 0, 0, 0, 0, ENVIRONMENT),
gsSPTexture(65535, 65535, 0, 0, 1),
gsDPTileSync(),
gsDPSetTextureImage(G_IM_FMT_RGBA, G_IM_SIZ_16b_LOAD_BLOCK, 1, cube_CubeTexture_rgba16),
gsDPSetTile(G_IM_FMT_RGBA, G_IM_SIZ_16b_LOAD_BLOCK, 0, 0, 7, 0, G_TX_WRAP | G_TX_MIRROR, 5, 0, G_TX_WRAP | G_TX_MIRROR, 5, 0),
gsDPLoadSync(),
gsDPLoadBlock(7, 0, 0, 1023, 256),
gsDPPipeSync(),
gsDPSetTile(G_IM_FMT_RGBA, G_IM_SIZ_16b, 8, 0, 0, 0, G_TX_WRAP | G_TX_MIRROR, 5, 0, G_TX_WRAP | G_TX_MIRROR, 5, 0),
gsDPSetTileSize(0, 0, 0, 124, 124),
gsSPSetLights1(cube_cube_lights),
gsSPEndDisplayList(),
};
Gfx cube_CubeSimpleBevel_mesh[] = {
gsSPDisplayList(mat_cube_cube),
gsSPDisplayList(cube_CubeSimpleBevel_mesh_tri_0),
gsDPPipeSync(),
gsSPSetGeometryMode(G_LIGHTING),
gsSPClearGeometryMode(G_TEXTURE_GEN),
gsDPSetCombineLERP(0, 0, 0, SHADE, 0, 0, 0, ENVIRONMENT, 0, 0, 0, SHADE, 0, 0, 0, ENVIRONMENT),
gsSPTexture(65535, 65535, 0, 0, 0),
gsSPEndDisplayList(),
};

3
src/models/models.c
View file

@ -7,4 +7,5 @@
#include "subject_geo.inc.h"
#include "sphere_geo.inc.h"
#include "portal_mask.inc.h"
#include "portal_outline.inc.h"
#include "portal_outline.inc.h"
#include "cube.inc.h"

3
src/models/models.h
View file

@ -5,4 +5,5 @@
#include "sphere.h"
extern Gfx portal_mask_Circle_mesh[];
extern Gfx portal_outline_portal_outline_mesh[];
extern Gfx portal_outline_portal_outline_mesh[];
extern Gfx cube_CubeSimpleBevel_mesh[];

14
src/physics/collision.h
View file

@ -2,6 +2,9 @@
#define __COLLISION_H__
#include "../math/vector3.h"
#include "../math/transform.h"
#include "../math/plane.h"
#include "contact_solver.h"
enum CollisionShapeType {
CollisionShapeTypeBox,
@ -17,10 +20,19 @@ struct ColliderTypeData;
typedef float (*MomentOfInertiaCalculator)(struct ColliderTypeData* typeData, float mass);
typedef int (*CollideWithPlane)(void* data, struct Transform* transform, struct Plane* plane, struct ContactConstraintState* contact);
struct ColliderCallbacks {
CollideWithPlane collideWithPlane;
MomentOfInertiaCalculator mofICalculator;
};
struct ColliderTypeData {
enum CollisionShapeType type;
void* data;
MomentOfInertiaCalculator mofICalculator;
float bounce;
float friction;
struct ColliderCallbacks* callbacks;
};
typedef void (*ContactCallback)(void* data, struct ContactPoint* contact);

92
src/physics/collision_box.c
View file

@ -4,23 +4,40 @@
#define NORMAL_ZERO_BIAS 0.001f
int _collsionBuildPlaneContact(struct Transform* boxTransform, struct Plane* plane, struct Vector3* point, struct ContactPoint* contact) {
transformPoint(boxTransform, point, &contact->point);
struct ColliderCallbacks gCollisionBoxCallbacks = {
collisionBoxCollidePlane,
collisionBoxSolidMofI,
};
contact->intersectionDepth = -planePointDistance(plane, &contact->point);
int _collsionBuildPlaneContact(struct Transform* boxTransform, struct Plane* plane, struct Vector3* point, struct ContactState* contact) {
quatMultVector(&boxTransform->rotation, point, &contact->rb);
if (contact->intersectionDepth <= 0.0f) {
struct Vector3 worldPoint;
vector3Add(&contact->rb, &boxTransform->position, &worldPoint);
contact->ra = gZeroVec;
contact->penetration = planePointDistance(plane, &worldPoint);
if (contact->penetration >= 0.0f) {
return 0;
}
contact->normal = plane->normal;
contact->normalImpulse = 0.0f;
contact->tangentImpulse[0] = 0.0f;
contact->tangentImpulse[1] = 0.0f;
contact->bias = 0;
contact->normalMass = 0;
contact->tangentMass[0] = 0.0f;
contact->tangentMass[1] = 0.0f;
vector3AddScaled(&contact->point, &plane->normal, -contact->intersectionDepth, &contact->point);
return 1;
}
int collisionBoxCollidePlane(struct CollisionBox* box, struct Transform* boxTransform, struct Plane* plane, struct ContactPoint* output) {
int collisionBoxCollidePlane(void* data, struct Transform* boxTransform, struct Plane* plane, struct ContactConstraintState* output) {
struct CollisionBox* box = (struct CollisionBox*)data;
float boxDistance = planePointDistance(plane, &boxTransform->position);
float maxBoxReach = vector3MagSqrd(&box->sideLength);
@ -53,55 +70,63 @@ int collisionBoxCollidePlane(struct CollisionBox* box, struct Transform* boxTran
}
}
int contactIndex = 0;
output->contactCount = 0;
output->normal = plane->normal;
// TODO actually calculate tangent
output->tangentVectors[0] = gRight;
output->tangentVectors[1] = gForward;
output->restitution = 0.0f;
output->friction = 1.0f;
if (splitAxisCount == 0) {
if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output[contactIndex])) {
return contactIndex;
if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output->contacts[output->contactCount])) {
return output->contactCount;
}
++contactIndex;
++output->contactCount;
} else if (splitAxisCount == 1) {
VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[0]] = VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[0]];
if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output[contactIndex])) {
return contactIndex;
if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output->contacts[output->contactCount])) {
return output->contactCount;
}
++contactIndex;
++output->contactCount;
VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[0]] = -VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[0]];
if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output[contactIndex])) {
return contactIndex;
if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output->contacts[output->contactCount])) {
return output->contactCount;
}
++contactIndex;
++output->contactCount;
} else if (splitAxisCount == 2) {
VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[0]] = VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[0]];
VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[1]] = VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[1]];
if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output[contactIndex])) {
return contactIndex;
if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output->contacts[output->contactCount])) {
return output->contactCount;
}
++contactIndex;
++output->contactCount;
VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[0]] = -VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[0]];
if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output[contactIndex])) {
return contactIndex;
if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output->contacts[output->contactCount])) {
return output->contactCount;
}
++contactIndex;
++output->contactCount;
VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[0]] = VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[0]];
VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[1]] = -VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[1]];
if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output[contactIndex])) {
return contactIndex;
if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output->contacts[output->contactCount])) {
return output->contactCount;
}
++contactIndex;
++output->contactCount;
VECTOR3_AS_ARRAY(&deepestCorner)[splitAxis[0]] = -VECTOR3_AS_ARRAY(&box->sideLength)[splitAxis[0]];
if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output[contactIndex])) {
return contactIndex;
if (!_collsionBuildPlaneContact(boxTransform, plane, &deepestCorner, &output->contacts[output->contactCount])) {
return output->contactCount;
}
++contactIndex;
++output->contactCount;
} else {
// should not happen
return contactIndex;
return output->contactCount;
}
return contactIndex;
return output->contactCount;
}
void collisionBoxCollideQuad(struct CollisionBox* box, struct Transform* boxTransform, struct CollisionQuad* quad) {
@ -113,4 +138,9 @@ void collisionBoxCollideQuad(struct CollisionBox* box, struct Transform* boxTran
quatMultVector(&inverseBoxRotation, &quad->normal, &normalInBoxSpace);
}
float collisionBoxSolidMofI(struct ColliderTypeData* typeData, float mass) {
float singleSide = sqrtf(vector3MagSqrd(&((struct CollisionBox*)typeData->data)->sideLength));
return mass * singleSide * singleSide * (1.0f / 6.0f);
}

7
src/physics/collision_box.h
View file

@ -6,12 +6,17 @@
#include "../math/vector3.h"
#include "../math/transform.h"
#include "../math/plane.h"
#include "contact_solver.h"
struct CollisionBox {
struct Vector3 sideLength;
};
int collisionBoxCollidePlane(struct CollisionBox* box, struct Transform* boxTransform, struct Plane* plane, struct ContactPoint* output);
extern struct ColliderCallbacks gCollisionBoxCallbacks;
int collisionBoxCollidePlane(void* data, struct Transform* boxTransform, struct Plane* plane, struct ContactConstraintState* contact);
void collisionBoxCollideQuad(struct CollisionBox* box, struct Transform* boxTransform, struct CollisionQuad* quad);
float collisionBoxSolidMofI(struct ColliderTypeData* typeData, float mass);
#endif

6
src/physics/config.h Normal file
View file

@ -0,0 +1,6 @@
#ifndef __PHYSICS_CONFIG_H__
#define __PHYSICS_CONFIG_H__
#define GRAVITY_CONSTANT -9.8f
#endif

268
src/physics/contact_solver.c Normal file
View file

@ -0,0 +1,268 @@
#include "contact_solver.h"
#include "../util/time.h"
#include "../math/mathf.h"
#include "rigid_body.h"
void contactSolverPreSolve(struct ContactSolver* contactSolver) {
for ( int i = 0; i < contactSolver->contactCount; ++i )
{
struct ContactConstraintState *cs = contactSolver->contacts + i;
struct Vector3* vA;
struct Vector3* wA;
struct Vector3* vB;
struct Vector3* wB;
if (cs->bodyA) {
vA = &cs->bodyA->velocity;
wA = &cs->bodyA->angularVelocity;
} else {
vA = NULL;
wA = NULL;
}
if (cs->bodyB) {
vB = &cs->bodyB->velocity;
wB = &cs->bodyB->angularVelocity;
} else {
vB = NULL;
wB = NULL;
}
for ( int j = 0; j < cs->contactCount; ++j )
{
struct ContactState *c = cs->contacts + j;
// Precalculate JM^-1JT for contact and friction constraints
struct Vector3 raCn;
vector3Cross(&c->ra, &cs->normal, &raCn);
struct Vector3 rbCn;
vector3Cross(&c->rb, &cs->normal, &rbCn);
float nm = 0;
if (cs->bodyA) {
nm += cs->bodyA->massInv;
}
if (cs->bodyB) {
nm += cs->bodyB->massInv;
}
float tm[ 2 ];
tm[ 0 ] = nm;
tm[ 1 ] = nm;
if (cs->bodyA) {
nm += cs->bodyA->momentOfInertiaInv * vector3MagSqrd(&raCn);
}
if (cs->bodyB) {
nm += cs->bodyB->momentOfInertiaInv * vector3MagSqrd(&rbCn);
}
c->normalMass = safeInvert( nm );
for (int i = 0; i < 2; ++i )
{
struct Vector3 raCt;
vector3Cross(&cs->tangentVectors[ i ], &c->ra, &raCt);
struct Vector3 rbCt;
vector3Cross(&cs->tangentVectors[ i ], &c->rb, &rbCt);
if (cs->bodyA) {
tm[ i ] += cs->bodyA->momentOfInertiaInv * vector3MagSqrd(&raCt);
}
if (cs->bodyB) {
tm[ i ] += cs->bodyB->momentOfInertiaInv * vector3MagSqrd(&rbCt);
}
c->tangentMass[ i ] = safeInvert( tm[ i ] );
}
// Precalculate bias factor
c->bias = -Q3_BAUMGARTE * (1.0f / FIXED_DELTA_TIME) * minf(0.0f, c->penetration + Q3_PENETRATION_SLOP);
// Warm start contact
struct Vector3 P;
vector3Scale(&cs->normal, &P, c->normalImpulse);
if ( ENABLE_FRICTION )
{
vector3AddScaled(&P, &cs->tangentVectors[0], c->tangentImpulse[0], &P);
vector3AddScaled(&P, &cs->tangentVectors[1], c->tangentImpulse[1], &P);
}
struct Vector3 w;
if (cs->bodyA) {
vector3AddScaled(vA, &P, -cs->bodyA->massInv, vA);
vector3Cross(&c->ra, &P, &w);
vector3AddScaled(wA, &w, -cs->bodyA->momentOfInertiaInv, wA);
}
if (cs->bodyB) {
vector3AddScaled(vB, &P, cs->bodyB->massInv, vB);
vector3Cross(&c->rb, &P, &w);
vector3AddScaled(wB, &w, cs->bodyB->momentOfInertiaInv, wB);
}
struct Vector3 velocity;
struct Vector3 angularVelocity;
if (vB) {
vector3Cross(wB, &c->rb, &angularVelocity);
vector3Add(&angularVelocity, vB, &velocity);
} else {
velocity = gZeroVec;
}
if (vA) {
vector3Cross(wA, &c->ra, &angularVelocity);
vector3Sub(&velocity, &angularVelocity, &velocity);
vector3Sub(&velocity, vA, &velocity);
}
float dv = vector3Dot(&velocity, &cs->normal);
if ( dv < -1.0f )
c->bias += -(cs->restitution) * dv;
}
}
}
void contactSolverIterate(struct ContactSolver* contactSolver) {
for ( int i = 0; i < contactSolver->contactCount; ++i )
{
struct ContactConstraintState *cs = contactSolver->contacts + i;
struct Vector3* vA;
struct Vector3* wA;
struct Vector3* vB;
struct Vector3* wB;
if (cs->bodyA) {
vA = &cs->bodyA->velocity;
wA = &cs->bodyA->angularVelocity;
} else {
vA = NULL;
wA = NULL;
}
if (cs->bodyB) {
vB = &cs->bodyB->velocity;
wB = &cs->bodyB->angularVelocity;
} else {
vB = NULL;
wB = NULL;
}
for ( int j = 0; j < cs->contactCount; ++j )
{
struct ContactState *c = cs->contacts + j;
// relative velocity at contact
struct Vector3 dv;
struct Vector3 angularVelocity;
if (wB) {
vector3Cross(wB, &c->rb, &angularVelocity);
vector3Add(&angularVelocity, vB, &dv);
} else {
dv = gZeroVec;
}
if (wA) {
vector3Cross(wA, &c->ra, &angularVelocity);
vector3Sub(&dv, &angularVelocity, &dv);
vector3Sub(&dv, vA, &dv);
}
// Friction
if ( ENABLE_FRICTION )
{
for ( int i = 0; i < 2; ++i )
{
float lambda = -vector3Dot( &dv, &cs->tangentVectors[ i ] ) * c->tangentMass[ i ];
// Calculate frictional impulse
float maxLambda = cs->friction * c->normalImpulse;
// Clamp frictional impulse
float oldPT = c->tangentImpulse[ i ];
c->tangentImpulse[ i ] = clampf( oldPT + lambda , -maxLambda, maxLambda);
lambda = c->tangentImpulse[ i ] - oldPT;
// Apply friction impulse
struct Vector3 impulse;
vector3Scale(&cs->tangentVectors[i], &impulse, lambda);
struct Vector3 w;
if (vA) {
vector3AddScaled(vA, &impulse, -cs->bodyA->massInv, vA);
vector3Cross(&c->ra, &impulse, &w);
vector3AddScaled(wA, &w, -cs->bodyA->momentOfInertiaInv, wA);
}
if (vB) {
vector3AddScaled(vB, &impulse, cs->bodyB->massInv, vB);
vector3Cross(&c->rb, &impulse, &w);
vector3AddScaled(wB, &w, cs->bodyB->momentOfInertiaInv, wB);
}
}
}
// Normal
{
if (wB) {
vector3Cross(wB, &c->rb, &angularVelocity);
vector3Add(&angularVelocity, vB, &dv);
} else {
dv = gZeroVec;
}
if (wA) {
vector3Cross(wA, &c->ra, &angularVelocity);
vector3Sub(&dv, &angularVelocity, &dv);
vector3Sub(&dv, vA, &dv);
}
// Normal impulse
float vn = vector3Dot( &dv, &cs->normal );
// Factor in positional bias to calculate impulse scalar j
float lambda = c->normalMass * (-vn + c->bias);
// Clamp impulse
float tempPN = c->normalImpulse;
c->normalImpulse = maxf( tempPN + lambda, 0.0f );
lambda = c->normalImpulse - tempPN;
// Apply impulse
struct Vector3 impulse;
vector3Scale(&cs->normal, &impulse, lambda);
struct Vector3 w;
if (vA) {
vector3AddScaled(vA, &impulse, -cs->bodyA->massInv, vA);
vector3Cross(&c->ra, &impulse, &w);
vector3AddScaled(wA, &w, -cs->bodyA->momentOfInertiaInv, wA);
}
if (vB) {
vector3AddScaled(vB, &impulse, cs->bodyB->massInv, vB);
vector3Cross(&c->rb, &impulse, &w);
vector3AddScaled(wB, &w, cs->bodyB->momentOfInertiaInv, wB);
}
}
}
}
}
void contactSolverSolve(struct ContactSolver* solver) {
contactSolverPreSolve(solver);
contactSolverIterate(solver);
contactSolverIterate(solver);
// contactSolverIterate(solver);
}

54
src/physics/contact_solver.h Normal file
View file

@ -0,0 +1,54 @@
#ifndef __CONTACT_SOLVER_H__
#define __CONTACT_SOLVER_H__
#include "../math/vector3.h"
struct RigidBody;
#define Q3_BAUMGARTE 0.2f
#define Q3_PENETRATION_SLOP 0.05f
#define ENABLE_FRICTION 1
struct VelocityState
{
struct Vector3 w;
struct Vector3 v;
};
struct ContactState
{
struct Vector3 ra; // Vector from C.O.M to contact position
struct Vector3 rb; // Vector from C.O.M to contact position
float penetration; // Depth of penetration from collision
float normalImpulse; // Accumulated normal impulse
float tangentImpulse[ 2 ]; // Accumulated friction impulse
float bias; // Restitution + baumgarte
float normalMass; // Normal constraint mass
float tangentMass[ 2 ]; // Tangent constraint mass
};
struct ContactConstraintState
{
struct ContactState contacts[ 8 ];
int contactCount;
struct Vector3 tangentVectors[ 2 ]; // Tangent vectors
struct Vector3 normal; // From A to B
float restitution;
float friction;
struct RigidBody* bodyA;
struct RigidBody* bodyB;
};
#define MAX_CONTACT_COUNT 8
struct ContactSolver {
struct ContactConstraintState contacts[MAX_CONTACT_COUNT];
int contactCount;
int contactCapacity;
};
void contactSolverSolve(struct ContactSolver* solver);
#endif

152
src/physics/rigid_body.c
View file

@ -1,19 +1,21 @@
#include "rigid_body.h"
#include "../util/time.h"
#include "../physics/config.h"
#include "contact_solver.h"
#include "defs.h"
#include <math.h>
void rigidBodyInit(struct RigidBody* rigidBody, struct ColliderTypeData* collider, float mass) {
rigidBody->collisionType = collider->type;
rigidBody->collisionShape = collider->data;
rigidBody->collider = collider;
transformInitIdentity(&rigidBody->transform);
rigidBody->velocity = gZeroVec;
rigidBody->angularVelocity = gZeroVec;
rigidBody->mass = mass;
rigidBody->mass = 1.0f / mass;
rigidBody->massInv = 1.0f / mass;
rigidBody->momentOfInertia = collider->mofICalculator(collider, mass);
rigidBody->momentOfInertia = collider->callbacks->mofICalculator(collider, mass);
rigidBody->momentOfInertiaInv = 1.0f / rigidBody->momentOfInertia;
}
@ -28,7 +30,147 @@ void rigidBodyAppyImpulse(struct RigidBody* rigidBody, struct Vector3* worldPoin
vector3AddScaled(&rigidBody->velocity, impulse, rigidBody->massInv, &rigidBody->velocity);
}
#define ENERGY_SCALE_PER_STEP 0.99f
void rigidBodyUpdate(struct RigidBody* rigidBody) {
rigidBody->velocity.y += GRAVITY_CONSTANT * FIXED_DELTA_TIME;
vector3AddScaled(&rigidBody->transform.position, &rigidBody->velocity, FIXED_DELTA_TIME, &rigidBody->transform.position);
quatApplyAngularVelocity(&rigidBody->transform.rotation, &rigidBody->angularVelocity, FIXED_DELTA_TIME, &rigidBody->transform.rotation);
vector3Scale(&rigidBody->velocity, &rigidBody->velocity, ENERGY_SCALE_PER_STEP);
vector3Scale(&rigidBody->angularVelocity, &rigidBody->angularVelocity, ENERGY_SCALE_PER_STEP);
}
void rigidBodyVelocityAtLocalPoint(struct RigidBody* rigidBody, struct Vector3* localPoint, struct Vector3* worldVelocity) {
vector3Cross(&rigidBody->angularVelocity, localPoint, worldVelocity);
vector3Add(worldVelocity, &rigidBody->velocity, worldVelocity);
}
void rigidBodyVelocityAtWorldPoint(struct RigidBody* rigidBody, struct Vector3* worldPoint, struct Vector3* worldVelocity) {
struct Vector3 relativePos;
vector3Sub(worldPoint, &rigidBody->transform.position, &relativePos);
vector3Cross(&rigidBody->angularVelocity, &relativePos, worldVelocity);
vector3Add(worldVelocity, &rigidBody->velocity, worldVelocity);
}
float rigidBodyMassInverseAtLocalPoint(struct RigidBody* rigidBody, struct Vector3* localPoint, struct Vector3* normal) {
struct Vector3 crossPoint;
vector3Cross(localPoint, normal, &crossPoint);
return rigidBody->massInv + rigidBody->momentOfInertiaInv * vector3MagSqrd(&crossPoint);
}
void rigidBodyResolveContact(struct RigidBody* bodyA, struct RigidBody* bodyB, struct ContactPoint* contactPoint) {
struct Vector3 aVelocity;
struct Vector3 bVelocity;
struct Vector3 localPositionA;
struct Vector3 localPositionB;
if (bodyA) {
vector3Sub(&contactPoint->point, &bodyA->transform.position, &localPositionA);
rigidBodyVelocityAtLocalPoint(bodyA, &localPositionA, &aVelocity);
} else {
aVelocity = gZeroVec;
}
if (bodyB) {
vector3Sub(&contactPoint->point, &bodyB->transform.position, &localPositionB);
rigidBodyVelocityAtLocalPoint(bodyB, &localPositionB, &bVelocity);
} else {
bVelocity = gZeroVec;
}
struct Vector3 relativeVelocity;
vector3Sub(&aVelocity, &bVelocity, &relativeVelocity);
float massInverse = 0.0f;
float frictionMassInverse = 0.0f;
float bounce = 1.0f;
float friction = 0.0f;
float normalVelocity = vector3Dot(&relativeVelocity, &contactPoint->normal);
struct Vector3 tangentVelocity;
vector3AddScaled(&relativeVelocity, &contactPoint->normal, -normalVelocity, &tangentVelocity);
struct Vector3 tangentDirection;
vector3Normalize(&tangentVelocity, &tangentDirection);
if (bodyA) {
massInverse += rigidBodyMassInverseAtLocalPoint(bodyA, &localPositionA, &contactPoint->normal);
frictionMassInverse += rigidBodyMassInverseAtLocalPoint(bodyA, &localPositionA, &tangentDirection);
bounce = bodyA->collider->bounce;
friction = bodyA->collider->friction;
}
if (bodyB) {
massInverse += rigidBodyMassInverseAtLocalPoint(bodyB, &localPositionA, &contactPoint->normal);
frictionMassInverse += rigidBodyMassInverseAtLocalPoint(bodyB, &localPositionB, &tangentDirection);
bounce = MIN(bounce, bodyB->collider->bounce);
friction = MAX(friction, bodyB->collider->friction);
}
if (massInverse == 0.0f) {
// two immovable objects
return;
}
struct Vector3 tangentImpulse;
float tangentSpeed = sqrt(vector3MagSqrd(&tangentVelocity));
massInverse = 1.0f / massInverse;
frictionMassInverse = 1.0f / frictionMassInverse;
float impulse = -(1.0f + bounce) * normalVelocity * massInverse;
impulse += contactPoint->intersectionDepth * (0.5f * FIXED_DELTA_TIME);
float frictionImpulse = impulse * friction;
float tangentImpulseMag = tangentSpeed * frictionMassInverse;
if (tangentImpulseMag > frictionImpulse) {
tangentImpulseMag = frictionImpulse;
}
if (tangentImpulseMag > 0.00001f) {
vector3Scale(&tangentDirection, &tangentImpulse, -tangentImpulseMag);
}
struct Vector3 impulseVector;
vector3AddScaled(&tangentImpulse, &contactPoint->normal, impulse, &impulseVector);
if (bodyA) {
rigidBodyAppyImpulse(bodyA, &contactPoint->point, &impulseVector);
}
if (bodyB) {
vector3Scale(&impulseVector, &impulseVector, -impulse);
rigidBodyAppyImpulse(bodyB, &contactPoint->point, &impulseVector);
}
}
void rigidBodyCollideWithPlane(struct RigidBody* rigidBody, struct Plane* plane, struct ContactSolver* contactSolver) {
CollideWithPlane planeCollider = rigidBody->collider->callbacks->collideWithPlane;
if (!planeCollider) {
return;
}
struct ContactConstraintState* contact = &contactSolver->contacts[contactSolver->contactCount];
// int prevCount = contact->contactCount;
int collisionCount = planeCollider(rigidBody->collider->data, &rigidBody->transform, plane, contact);
if (collisionCount) {
contact->bodyB = rigidBody;
// if (contact->bodyB != rigidBody || contact->contactCount != prevCount) {
// for (int i = 0; i < contact->contactCount; ++i) {
// }
// }
++contactSolver->contactCount;
} else {
contact->bodyB = NULL;
}
}

14
src/physics/rigid_body.h
View file

@ -4,12 +4,8 @@
#include "../math/transform.h"
#include "collision.h"
#define MAX_CONTACT_POINT_COUNT 8
struct RigidBody {
enum CollisionShapeType collisionType;
void* collisionShape;
struct ContactPoint gContactList[MAX_CONTACT_POINT_COUNT];
struct ColliderTypeData* collider;
struct Transform transform;
struct Vector3 velocity;
@ -27,7 +23,13 @@ struct RigidBody {
};
void rigidBodyInit(struct RigidBody* rigidBody, struct ColliderTypeData* collider, float mass);
void rigidBodyAppyImpulse(struct RigidBody* rigidBody, struct Vector3* worldPoint, struct Vector3* impulse);
void rigidBodyUpdate(struct RigidBody* rigidBody);
void rigidBodyVelocityAtLocalPoint(struct RigidBody* rigidBody, struct Vector3* localPoint, struct Vector3* worldVelocity);
void rigidBodyVelocityAtWorldPoint(struct RigidBody* rigidBody, struct Vector3* worldPoint, struct Vector3* worldVelocity);
void rigidBodyResolveContact(struct RigidBody* bodyA, struct RigidBody* bodyB, struct ContactPoint* contactPoint);
void rigidBodyCollideWithPlane(struct RigidBody* rigidBody, struct Plane* plane, struct ContactSolver* contactSolver);
#endif

10
src/player/player.c
View file

@ -8,7 +8,7 @@ void playerInit(struct Player* player) {
transformInitIdentity(&player->transform);
}
#define PLAYER_SPEED (SCENE_SCALE * 2.0f)
#define PLAYER_SPEED (2.0f)
#define ROTATE_RATE (M_PI * 0.25f)
@ -27,15 +27,15 @@ void playerUpdate(struct Player* player, struct Transform* cameraTransform) {
OSContPad* controllerInput = controllersGetControllerData(0);
vector3AddScaled(&player->transform.position, &forward, -controllerInput->stick_y * gTimeDelta * PLAYER_SPEED / 80.0f, &player->transform.position);
vector3AddScaled(&player->transform.position, &right, controllerInput->stick_x * gTimeDelta * PLAYER_SPEED / 80.0f, &player->transform.position);
vector3AddScaled(&player->transform.position, &forward, -controllerInput->stick_y * FIXED_DELTA_TIME * PLAYER_SPEED / 80.0f, &player->transform.position);
vector3AddScaled(&player->transform.position, &right, controllerInput->stick_x * FIXED_DELTA_TIME * PLAYER_SPEED / 80.0f, &player->transform.position);
float rotate = 0.0f;
if (controllerGetButton(0, L_CBUTTONS)) {
rotate += ROTATE_RATE * gTimeDelta;
rotate += ROTATE_RATE * FIXED_DELTA_TIME;
} else if (controllerGetButton(0, R_CBUTTONS)) {
rotate -= ROTATE_RATE * gTimeDelta;
rotate -= ROTATE_RATE * FIXED_DELTA_TIME;
}
if (rotate) {

4
src/scene/camera.c
View file

@ -1,6 +1,7 @@
#include "camera.h"
#include "math/transform.h"
#include "defs.h"
void cameraInit(struct Camera* camera, float fov, float near, float far) {
transformInitIdentity(&camera->transform);
@ -11,9 +12,10 @@ void cameraInit(struct Camera* camera, float fov, float near, float far) {
void cameraBuildViewMatrix(struct Camera* camera, float matrix[4][4]) {
struct Transform cameraTransCopy = camera->transform;
vector3Scale(&cameraTransCopy.position, &cameraTransCopy.position, SCENE_SCALE);
struct Transform inverse;
transformInvert(&cameraTransCopy, &inverse);
transformToMatrix(&inverse, matrix);
transformToMatrix(&inverse, matrix, 1.0f);
}
void cameraBuildProjectionMatrix(struct Camera* camera, float matrix[4][4], u16* perspectiveNormalize, float aspectRatio) {

43
src/scene/cube.c Normal file
View file

@ -0,0 +1,43 @@
#include "cube.h"
#include "../physics/collision_box.h"
#include "../models/models.h"
#include "defs.h"
struct CollisionBox gCubeCollisionBox = {
{0.3165f, 0.3165f, 0.3165f}
};
struct Plane gFloor = {{0.0f, 1.0f, 0.0f}, 0.0f};
struct ContactSolver gContactSolver;
struct ColliderTypeData gCubeCollider = {
CollisionShapeTypeBox,
&gCubeCollisionBox,
0.5f,
0.5f,
&gCollisionBoxCallbacks,
};
void cubeInit(struct Cube* cube) {
rigidBodyInit(&cube->rigidBody, &gCubeCollider, 1.0f);
}
void cubeUpdate(struct Cube* cube) {
rigidBodyUpdate(&cube->rigidBody);
gContactSolver.contactCount = 0;
rigidBodyCollideWithPlane(&cube->rigidBody, &gFloor, &gContactSolver);
contactSolverSolve(&gContactSolver);
}
void cubeRender(struct Cube* cube, struct RenderState* renderState) {
Mtx* matrix = renderStateRequestMatrices(renderState, 1);
transformToMatrixL(&cube->rigidBody.transform, matrix, SCENE_SCALE);
gSPMatrix(renderState->dl++, matrix, G_MTX_MODELVIEW | G_MTX_PUSH | G_MTX_MUL);
gSPDisplayList(renderState->dl++, cube_CubeSimpleBevel_mesh);
gSPPopMatrix(renderState->dl++, G_MTX_MODELVIEW);
}

15
src/scene/cube.h Normal file
View file

@ -0,0 +1,15 @@
#ifndef __SCENE_CUBE_H__
#define __SCENE_CUBE_H__
#include "../physics/rigid_body.h"
#include "../graphics/renderstate.h"
struct Cube {
struct RigidBody rigidBody;
};
void cubeInit(struct Cube* cube);
void cubeUpdate(struct Cube* cube);
void cubeRender(struct Cube* cube, struct RenderState* renderState);
#endif

6
src/scene/point_light.c
View file

@ -39,7 +39,7 @@ void pointLightSetColor(struct PointLight* pointLight, struct Coloru8* color) {
}
void pointLightAttenuate(struct PointLight* pointLight, float distnaceSq, struct Coloru8* output) {
float factor = pointLight->intensity * (SCENE_SCALE * SCENE_SCALE) / distnaceSq;
float factor = pointLight->intensity / distnaceSq;
factor = MIN(factor, pointLight->maxFactor);
colorU8Lerp(&gColorBlack, &pointLight->color, factor, output);
@ -152,8 +152,8 @@ void pointLightableMeshInit(struct PointLightableMesh* mesh, Vtx* inputVertices,
}
}
#define RENDERED_LIGHT_HEIGHT (0.5f * SCENE_SCALE)
#define RENDERED_LIGHT_TEX_SIZE (4.0f * SCENE_SCALE)
#define RENDERED_LIGHT_HEIGHT (0.5f)
#define RENDERED_LIGHT_TEX_SIZE (4.0f)
#define RENDERED_LIGHT_TEX_UV_SIZE 4096
void pointLightableCalc(struct PointLightableMesh* mesh, struct Transform* meshTransform, struct PointLight* pointLight) {

4
src/scene/portal.c
View file

@ -63,7 +63,7 @@ void portalRender(struct Portal* portal, struct Portal* otherPortal, struct Rend
if (props->currentDepth == 0) {
Mtx* matrix = renderStateRequestMatrices(renderState, 1);
transformToMatrixL(&portal->transform, matrix);
transformToMatrixL(&portal->transform, matrix, SCENE_SCALE);
gSPMatrix(renderState->dl++, matrix, G_MTX_MODELVIEW | G_MTX_PUSH | G_MTX_MUL);
gDPSetPrimColor(renderState->dl++, 0, 0, 255, 128, 0, 255);
gSPDisplayList(renderState->dl++, portal_outline_portal_outline_mesh);
@ -82,7 +82,7 @@ void portalRender(struct Portal* portal, struct Portal* otherPortal, struct Rend
quatMultiply(&portal->transform.rotation, &gVerticalFlip, &finalTransform.rotation);
}
transformToMatrix(&finalTransform, portalTransform);
transformToMatrix(&finalTransform, portalTransform, SCENE_SCALE);
screenClipperInitWithCamera(&clipper, &props->camera, (float)SCREEN_WD / (float)SCREEN_HT, portalTransform);

31
src/scene/scene.c
View file

@ -15,28 +15,38 @@
#include "../levels/test_chamber_00_0/header.h"
struct Vector3 gStartPosition = {5.0f * SCENE_SCALE, 0.0f, -5.0f * SCENE_SCALE};
struct Vector3 gStartPosition = {5.0f, 0.0f, -5.0f};
struct Vector3 gCameraOffset = {0.0f, 1.2f * SCENE_SCALE, 0.0f};
struct Vector3 gCameraOffset = {0.0f, 1.2f, 0.0f};
void sceneInit(struct Scene* scene) {
cameraInit(&scene->camera, 45.0f, SCENE_SCALE * 0.25f, SCENE_SCALE * 80.0f);
cameraInit(&scene->camera, 45.0f, 0.25f * SCENE_SCALE, 80.0f * SCENE_SCALE);
playerInit(&scene->player);
scene->player.transform.position = gStartPosition;
quatAxisAngle(&gUp, M_PI, &scene->player.transform.rotation);
portalInit(&scene->portals[0], 0);
portalInit(&scene->portals[1], PortalFlagsOddParity);
scene->portals[0].transform.position.x = 5.0f * SCENE_SCALE;
scene->portals[0].transform.position.y = 1.0f * SCENE_SCALE;
scene->portals[0].transform.position.z = -0.1f * SCENE_SCALE;
scene->portals[0].transform.position.x = 5.0f;
scene->portals[0].transform.position.y = 1.0f;
scene->portals[0].transform.position.z = -0.1f;
scene->portals[1].transform.position.x = 0.1f * SCENE_SCALE;
scene->portals[1].transform.position.y = 1.0f * SCENE_SCALE;
scene->portals[1].transform.position.z = -6.0f * SCENE_SCALE;
scene->portals[1].transform.position.x = 0.1f;
scene->portals[1].transform.position.y = 1.0f;
scene->portals[1].transform.position.z = -6.0f;
quatAxisAngle(&gUp, M_PI * 0.5f, &scene->portals[1].transform.rotation);
cubeInit(&scene->cube);
scene->cube.rigidBody.transform.position.x = 5.0f;
scene->cube.rigidBody.transform.position.y = 1.0f;
scene->cube.rigidBody.transform.position.z = -1.0f;
quatAxisAngle(&gRight, M_PI * 0.125f, &scene->cube.rigidBody.transform.rotation);
// scene->cube.rigidBody.angularVelocity = gOneVec;
}
void sceneRenderWithProperties(void* data, struct RenderProps* properties, struct RenderState* renderState) {
@ -47,6 +57,8 @@ void sceneRenderWithProperties(void* data, struct RenderProps* properties, struc
gDPSetRenderMode(renderState->dl++, G_RM_ZB_OPA_SURF, G_RM_ZB_OPA_SURF2);
gSPDisplayList(renderState->dl++, test_chamber_00_0_test_chamber_00_0_mesh);
cubeRender(&scene->cube, renderState);
}
void sceneRender(struct Scene* scene, struct RenderState* renderState, struct GraphicsTask* task) {
@ -67,4 +79,5 @@ void sceneUpdate(struct Scene* scene) {
scene->camera.transform.rotation = scene->player.transform.rotation;
playerUpdate(&scene->player, &scene->camera.transform);
cubeUpdate(&scene->cube);
}

2
src/scene/scene.h
View file

@ -8,11 +8,13 @@
#include "point_light.h"
#include "portal.h"
#include "../player/player.h"
#include "cube.h"
struct Scene {
struct Camera camera;
struct Player player;
struct Portal portals[2];
struct Cube cube;
};
struct GraphicsTask;

5
src/scene/shadow_map.c
View file

@ -4,6 +4,7 @@
#include "math/matrix.h"
#include "graphics/graphics.h"
#include "math/plane.h"
#include "defs.h"
#define SHADOW_MAP_WIDTH 64
#define SHADOW_MAP_HEIGHT 64
@ -148,12 +149,12 @@ void shadowMapRender(struct ShadowMap* shadowMap, struct RenderState* renderStat
struct Transform povInverse;
transformInvert(&lightPovTransform, &povInverse);
float cameraView[4][4];
transformToMatrix(&povInverse, cameraView);
transformToMatrix(&povInverse, cameraView, SCENE_SCALE);
float viewProj[4][4];
guMtxCatF(cameraView, projMatrix, viewProj);
float subjectMatrix[4][4];
transformToMatrix(subjectTransform, subjectMatrix);
transformToMatrix(subjectTransform, subjectMatrix, SCENE_SCALE);
guMtxCatF(subjectMatrix, viewProj, projMatrix);
Mtx* lightMtx = renderStateRequestMatrices(renderState, 1);

8
src/scene/shadow_renderer.c
View file

@ -157,7 +157,7 @@ void shadowRendererRender(
for (unsigned i = 0; i < recieverCount; ++i) {
struct ShadowReceiver* reciever = &recievers[i];
transformToMatrixL(&reciever->transform, &recieverMatrices[i]);
transformToMatrixL(&reciever->transform, &recieverMatrices[i], SCENE_SCALE);
if (reciever->flags & ShadowReceiverFlagsUseLight) {
++lightCount;
@ -189,7 +189,7 @@ void shadowRendererRender(
// calculate position of top and bottom of shadow
Mtx* shadowMatrices = renderStateRequestMatrices(renderState, 2);
transformToMatrixL(&shadowRenderer->casterTransform, &shadowMatrices[TOP_MATRIX_INDEX]);
transformToMatrixL(&shadowRenderer->casterTransform, &shadowMatrices[TOP_MATRIX_INDEX], SCENE_SCALE);
struct Vector3 lightOffset;
vector3Sub(&shadowRenderer->casterTransform.position, &fromLight->position, &lightOffset);
@ -209,7 +209,7 @@ void shadowRendererRender(
&shadowEnd.position
);
vector3Scale(&gOneVec, &shadowEnd.scale, (lightDistance + shadowRenderer->shadowLength) / lightDistance);
transformToMatrixL(&shadowEnd, &shadowMatrices[BOTTOM_MATRIX_INDEX]);
transformToMatrixL(&shadowEnd, &shadowMatrices[BOTTOM_MATRIX_INDEX], SCENE_SCALE);
// render back of shadows
gDPPipeSync(renderState->dl++);
@ -302,7 +302,7 @@ void shadowRendererRenderProjection(
Mtx* matrix = renderStateRequestMatrices(renderState, 1);
transformToMatrixL(&finalTransform, matrix);
transformToMatrixL(&finalTransform, matrix, SCENE_SCALE);
gSPMatrix(renderState->dl++, matrix, G_MTX_PUSH | G_MTX_MUL | G_MTX_MODELVIEW);
gSPDisplayList(renderState->dl++, shadowRenderer->shadowProfile);

2
src/sk64/skelatool_animator.c
View file

@ -432,7 +432,7 @@ void skAnimatorUpdate(struct SKAnimator* animator, struct Transform* transforms,
animator->currTick = animator->nextTick;
float currTick = animator->currentTime * animator->currentAnimation->ticksPerSecond;
animator->currentTime += gTimeDelta * timeScale;
animator->currentTime += FIXED_DELTA_TIME * timeScale;
animator->nextTick = (u16)(animator->currentTime * animator->currentAnimation->ticksPerSecond);
if (animator->currTick <= animator->currentAnimation->maxTicks && transforms) {

2
src/sk64/skelatool_armature.c
View file

@ -36,7 +36,7 @@ void skRenderObject(struct SKArmature* object, struct RenderState* intoState) {
void skCalculateTransforms(struct SKArmature* object, Mtx* into) {
for (int i = 0; i < object->numberOfBones; ++i) {
transformToMatrixL(&object->boneTransforms[i], &into[i]);
transformToMatrixL(&object->boneTransforms[i], &into[i], 1.0f);
}
}

13
src/util/time.c
View file

@ -1,23 +1,10 @@
#include "time.h"
#define MAX_DELTA_TIME 0.1f
float gTimePassed = 0.0f;
float gTimeDelta;
OSTime gLastTime;
void timeUpdateDelta() {
OSTime currTime = osGetTime();
OSTime timeDelta = currTime - gLastTime;
gTimePassed = (float)OS_CYCLES_TO_USEC(currTime) / 1000000.0f;
gTimeDelta = (float)OS_CYCLES_TO_USEC(timeDelta) / 1000000.0f;
if (gTimeDelta > MAX_DELTA_TIME) {
gTimeDelta = MAX_DELTA_TIME;
}
gLastTime = currTime;
}

3
src/util/time.h
View file

@ -3,11 +3,10 @@
#include <ultra64.h>
extern float gTimeDelta;
extern float gTimePassed;
extern OSTime gLastTime;
#define FIXED_DELTA_TIME (1.0f / 30.0f)
#define FIXED_DELTA_TIME (1.0f / 60.0f)
void timeUpdateDelta();