From 54ad07c2c220106ef225dfdca091d2109aaa13e8 Mon Sep 17 00:00:00 2001 From: based Date: Mon, 9 Jun 2025 15:56:39 -0400 Subject: [PATCH] some quaternion functions --- src/gum/Makefile.am | 6 ++- src/gum/pspgum.c | 108 ++++++++++++++++++++++++++++++++++++++++++++ src/gum/pspgum.h | 15 ++++++ 3 files changed, 127 insertions(+), 2 deletions(-) diff --git a/src/gum/Makefile.am b/src/gum/Makefile.am index 541c31a8..31ee0e53 100644 --- a/src/gum/Makefile.am +++ b/src/gum/Makefile.am @@ -17,11 +17,13 @@ lib_LIBRARIES = libpspgum.a libpspgum_vfpu.a noinst_HEADERS = gumInternal.h -PSPGUM_SHARED_OBJS = gumLoadMatrix.o gumOrtho.o gumPerspective.o \ +PSPGUM_SHARED_OBJS = gumLoadMatrix.o gumOrtho.o gumPerspective.o gumLoadQuaternion.o \ gumLookAt.o gumRotateXYZ.o gumRotateZYX.o gumFullInverse.o \ gumCrossProduct.o gumDotProduct.o gumNormalize.o \ + gumLoadAxisAngle.o gumMultQuaternion.o gumNormalizeQuaternion.o \ + gumRotateVector.o gumRotateMatrix.o \ sceGumDrawArray.o sceGumDrawArrayN.o sceGumDrawBezier.o sceGumDrawSpline.o \ - sceGumRotateXYZ.o sceGumRotateZYX.o + sceGumRotateXYZ.o sceGumRotateZYX.o sceGumRotate.o PSPGUM_FPU_OBJS = sceGumLoadIdentity.o sceGumLoadMatrix.o sceGumMatrixMode.o \ sceGumMultMatrix.o sceGumOrtho.o sceGumPerspective.o sceGumPopMatrix.o \ diff --git a/src/gum/pspgum.c b/src/gum/pspgum.c index 52ed0306..db145f24 100644 --- a/src/gum/pspgum.c +++ b/src/gum/pspgum.c @@ -131,6 +131,44 @@ void gumLoadMatrix(ScePspFMatrix4* r, const ScePspFMatrix4* a) } #endif +#ifdef F_gumLoadQuaternion +void gumLoadQuaternion(ScePspFMatrix4 *m, const ScePspFQuaternion* q) +{ + m->x.x = 1 - 2 * (q->y * q->y + q->z * q->z); + m->y.x = 2 * (q->x * q->y - q->w * q->z); + m->z.x = 2 * (q->x * q->z + q->w * q->y); + m->w.x = 0; + + m->x.y = 2 * (q->x * q->y + q->w * q->z); + m->y.y = 1 - 2 * (q->x * q->x + q->z * q->z); + m->z.y = 2 * (q->y * q->z - q->w * q->x); + m->w.y = 0; + + m->x.z = 2 * (q->x * q->z - q->w * q->y); + m->y.z = 2 * (q->y * q->z + q->w * q->x); + m->z.z = 1 - 2 * (q->x * q->x + q->y * q->y); + m->w.z = 0; + + m->x.w = 0; + m->y.w = 0; + m->z.w = 0; + m->w.w = 1; +} +#endif + +#ifdef F_gumLoadAxisAngle +void gumLoadAxisAngle(ScePspFQuaternion* q, ScePspFVector3* axis, float theta) +{ + gumNormalize(axis); + float sin_t = sinf(theta / 2); + float cos_t = cosf(theta / 2); + q->x = axis->x * sin_t; + q->y = axis->y * sin_t; + q->z = axis->z * sin_t; + q->w = cos_t; +} +#endif + #ifdef F_gumLookAt void gumLookAt(ScePspFMatrix4* m, ScePspFVector3* eye, ScePspFVector3* center, ScePspFVector3* up) { @@ -191,6 +229,36 @@ void gumMultMatrix(ScePspFMatrix4* result, const ScePspFMatrix4* a, const ScePsp } #endif +#ifdef F_gumMultQuaternion +void gumMultQuaternion(ScePspFQuaternion* result, const ScePspFQuaternion* a, const ScePspFQuaternion* b) +{ + ScePspFVector3 cp; + ScePspFQuaternion res; + gumCrossProduct(&cp, (ScePspFVector3 *)a, (ScePspFVector3 *)b); + res.w = (a->w * b->w) - gumDotProduct((ScePspFVector3 *)a, (ScePspFVector3 *)b); + res.x = (a->w * b->x) + (b->w * a->x) + cp.x; + res.y = (a->w * b->y) + (b->w * a->y) + cp.y; + res.z = (a->w * b->z) + (b->w * a->z) + cp.z; + gumNormalizeQuaternion(&res); + *result = res; +} +#endif + +#ifdef F_gumNormalizeQuaternion +void gumNormalizeQuaternion(ScePspFQuaternion* q) +{ + float l = sqrtf((q->w * q->w) + (q->x * q->x) + (q->y * q->y) + (q->z * q->z)); + if (l > GUM_EPSILON) + { + float il = 1 / l; + q->w *= il; + q->x *= il; + q->y *= il; + q->z *= il; + } +} +#endif + #ifdef F_gumNormalize void gumNormalize(ScePspFVector3* v) { @@ -318,6 +386,38 @@ void gumRotateZYX(ScePspFMatrix4* m, const ScePspFVector3* v) } #endif +#ifdef F_gumRotateMatrix +void gumRotateMatrix(ScePspFMatrix4* m, const ScePspFQuaternion* q) +{ + ScePspFMatrix4 rot_m; + gumLoadQuaternion(&rot_m, q); + gumMultMatrix(m, m, &rot_m); +} +#endif + +#ifdef F_gumRotateVector +void gumRotateVector(ScePspFVector3* r, const ScePspFQuaternion* q, const ScePspFVector3* v) +{ + ScePspFQuaternion conjugate = { + -q->x, + -q->y, + -q->z, + q->w, + }; + ScePspFQuaternion vector = { + v->x, + v->y, + v->z, + 0, + }; + gumMultQuaternion(&vector, &vector, &conjugate); + gumMultQuaternion(&vector, q, &vector); + r->x = vector.x; + r->y = vector.y; + r->z = vector.z; +} +#endif + #ifdef F_gumScale void gumScale(ScePspFMatrix4* m, const ScePspFVector3* v) { @@ -534,6 +634,14 @@ void sceGumRotateZYX(const ScePspFVector3* v) } #endif +#ifdef F_sceGumRotate +void sceGumRotate(const ScePspFQuaternion* q) +{ + gumRotateMatrix(gum_current_matrix, q); + gum_current_matrix_update = 1; +} +#endif + #ifdef F_sceGumScale void sceGumScale(const ScePspFVector3* v) { diff --git a/src/gum/pspgum.h b/src/gum/pspgum.h index 0baf46af..418e7898 100644 --- a/src/gum/pspgum.h +++ b/src/gum/pspgum.h @@ -120,6 +120,13 @@ void sceGumRotateXYZ(const ScePspFVector3* v); **/ void sceGumRotateZYX(const ScePspFVector3* v); +/** + * Apply rotation represented by quaternion q + * + * @param q - Pointer to quaternion +**/ +void sceGumRotate(const ScePspFQuaternion* q); + /** * Scale matrix * @@ -198,6 +205,7 @@ void gumInit(void); **/ void gumLoadIdentity(ScePspFMatrix4* m); +void gumLoadQuaternion(ScePspFMatrix4* r, const ScePspFQuaternion* q); void gumLoadMatrix(ScePspFMatrix4* r, const ScePspFMatrix4* a); void gumLookAt(ScePspFMatrix4* m, ScePspFVector3* eye, ScePspFVector3* center, ScePspFVector3* up); void gumMultMatrix(ScePspFMatrix4* result, const ScePspFMatrix4* a, const ScePspFMatrix4* b); @@ -208,6 +216,7 @@ void gumRotateXYZ(ScePspFMatrix4* m, const ScePspFVector3* v); void gumRotateY(ScePspFMatrix4* m, float angle); void gumRotateZ(ScePspFMatrix4* m, float angle); void gumRotateZYX(ScePspFMatrix4* m, const ScePspFVector3* v); +void gumRotateMatrix(ScePspFMatrix4* m, const ScePspFQuaternion* q); void gumScale(ScePspFMatrix4* m, const ScePspFVector3* v); void gumTranslate(ScePspFMatrix4* m, const ScePspFVector3* v); void gumFullInverse(ScePspFMatrix4* r, const ScePspFMatrix4* a); @@ -228,6 +237,12 @@ void gumFastInverse(ScePspFMatrix4* r, const ScePspFMatrix4* a); void gumCrossProduct(ScePspFVector3* r, const ScePspFVector3* a, const ScePspFVector3* b); float gumDotProduct(const ScePspFVector3* a, const ScePspFVector3* b); void gumNormalize(ScePspFVector3* v); +void gumRotateVector(ScePspFVector3* r, const ScePspFQuaternion* q, const ScePspFVector3* v); + +// quaternion functions +void gumNormalizeQuaternion(ScePspFQuaternion* q); +void gumLoadAxisAngle(ScePspFQuaternion* r, ScePspFVector3* axis, float t); +void gumMultQuaternion(ScePspFQuaternion* result, const ScePspFQuaternion* a, const ScePspFQuaternion* b); #if defined(__cplusplus) }