Skip to content

Commit 54ad07c

Browse files
author
based
committed
some quaternion functions
1 parent a116123 commit 54ad07c

File tree

3 files changed

+127
-2
lines changed

3 files changed

+127
-2
lines changed

‎src/gum/Makefile.am‎

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -17,11 +17,13 @@ lib_LIBRARIES = libpspgum.a libpspgum_vfpu.a
1717

1818
noinst_HEADERS = gumInternal.h
1919

20-
PSPGUM_SHARED_OBJS = gumLoadMatrix.o gumOrtho.o gumPerspective.o \
20+
PSPGUM_SHARED_OBJS = gumLoadMatrix.o gumOrtho.o gumPerspective.o gumLoadQuaternion.o \
2121
gumLookAt.o gumRotateXYZ.o gumRotateZYX.o gumFullInverse.o \
2222
gumCrossProduct.o gumDotProduct.o gumNormalize.o \
23+
gumLoadAxisAngle.o gumMultQuaternion.o gumNormalizeQuaternion.o \
24+
gumRotateVector.o gumRotateMatrix.o \
2325
sceGumDrawArray.o sceGumDrawArrayN.o sceGumDrawBezier.o sceGumDrawSpline.o \
24-
sceGumRotateXYZ.o sceGumRotateZYX.o
26+
sceGumRotateXYZ.o sceGumRotateZYX.o sceGumRotate.o
2527

2628
PSPGUM_FPU_OBJS = sceGumLoadIdentity.o sceGumLoadMatrix.o sceGumMatrixMode.o \
2729
sceGumMultMatrix.o sceGumOrtho.o sceGumPerspective.o sceGumPopMatrix.o \

‎src/gum/pspgum.c‎

Lines changed: 108 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -131,6 +131,44 @@ void gumLoadMatrix(ScePspFMatrix4* r, const ScePspFMatrix4* a)
131131
}
132132
#endif
133133

134+
#ifdef F_gumLoadQuaternion
135+
void gumLoadQuaternion(ScePspFMatrix4 *m, const ScePspFQuaternion* q)
136+
{
137+
m->x.x = 1 - 2 * (q->y * q->y + q->z * q->z);
138+
m->y.x = 2 * (q->x * q->y - q->w * q->z);
139+
m->z.x = 2 * (q->x * q->z + q->w * q->y);
140+
m->w.x = 0;
141+
142+
m->x.y = 2 * (q->x * q->y + q->w * q->z);
143+
m->y.y = 1 - 2 * (q->x * q->x + q->z * q->z);
144+
m->z.y = 2 * (q->y * q->z - q->w * q->x);
145+
m->w.y = 0;
146+
147+
m->x.z = 2 * (q->x * q->z - q->w * q->y);
148+
m->y.z = 2 * (q->y * q->z + q->w * q->x);
149+
m->z.z = 1 - 2 * (q->x * q->x + q->y * q->y);
150+
m->w.z = 0;
151+
152+
m->x.w = 0;
153+
m->y.w = 0;
154+
m->z.w = 0;
155+
m->w.w = 1;
156+
}
157+
#endif
158+
159+
#ifdef F_gumLoadAxisAngle
160+
void gumLoadAxisAngle(ScePspFQuaternion* q, ScePspFVector3* axis, float theta)
161+
{
162+
gumNormalize(axis);
163+
float sin_t = sinf(theta / 2);
164+
float cos_t = cosf(theta / 2);
165+
q->x = axis->x * sin_t;
166+
q->y = axis->y * sin_t;
167+
q->z = axis->z * sin_t;
168+
q->w = cos_t;
169+
}
170+
#endif
171+
134172
#ifdef F_gumLookAt
135173
void gumLookAt(ScePspFMatrix4* m, ScePspFVector3* eye, ScePspFVector3* center, ScePspFVector3* up)
136174
{
@@ -191,6 +229,36 @@ void gumMultMatrix(ScePspFMatrix4* result, const ScePspFMatrix4* a, const ScePsp
191229
}
192230
#endif
193231

232+
#ifdef F_gumMultQuaternion
233+
void gumMultQuaternion(ScePspFQuaternion* result, const ScePspFQuaternion* a, const ScePspFQuaternion* b)
234+
{
235+
ScePspFVector3 cp;
236+
ScePspFQuaternion res;
237+
gumCrossProduct(&cp, (ScePspFVector3 *)a, (ScePspFVector3 *)b);
238+
res.w = (a->w * b->w) - gumDotProduct((ScePspFVector3 *)a, (ScePspFVector3 *)b);
239+
res.x = (a->w * b->x) + (b->w * a->x) + cp.x;
240+
res.y = (a->w * b->y) + (b->w * a->y) + cp.y;
241+
res.z = (a->w * b->z) + (b->w * a->z) + cp.z;
242+
gumNormalizeQuaternion(&res);
243+
*result = res;
244+
}
245+
#endif
246+
247+
#ifdef F_gumNormalizeQuaternion
248+
void gumNormalizeQuaternion(ScePspFQuaternion* q)
249+
{
250+
float l = sqrtf((q->w * q->w) + (q->x * q->x) + (q->y * q->y) + (q->z * q->z));
251+
if (l > GUM_EPSILON)
252+
{
253+
float il = 1 / l;
254+
q->w *= il;
255+
q->x *= il;
256+
q->y *= il;
257+
q->z *= il;
258+
}
259+
}
260+
#endif
261+
194262
#ifdef F_gumNormalize
195263
void gumNormalize(ScePspFVector3* v)
196264
{
@@ -318,6 +386,38 @@ void gumRotateZYX(ScePspFMatrix4* m, const ScePspFVector3* v)
318386
}
319387
#endif
320388

389+
#ifdef F_gumRotateMatrix
390+
void gumRotateMatrix(ScePspFMatrix4* m, const ScePspFQuaternion* q)
391+
{
392+
ScePspFMatrix4 rot_m;
393+
gumLoadQuaternion(&rot_m, q);
394+
gumMultMatrix(m, m, &rot_m);
395+
}
396+
#endif
397+
398+
#ifdef F_gumRotateVector
399+
void gumRotateVector(ScePspFVector3* r, const ScePspFQuaternion* q, const ScePspFVector3* v)
400+
{
401+
ScePspFQuaternion conjugate = {
402+
-q->x,
403+
-q->y,
404+
-q->z,
405+
q->w,
406+
};
407+
ScePspFQuaternion vector = {
408+
v->x,
409+
v->y,
410+
v->z,
411+
0,
412+
};
413+
gumMultQuaternion(&vector, &vector, &conjugate);
414+
gumMultQuaternion(&vector, q, &vector);
415+
r->x = vector.x;
416+
r->y = vector.y;
417+
r->z = vector.z;
418+
}
419+
#endif
420+
321421
#ifdef F_gumScale
322422
void gumScale(ScePspFMatrix4* m, const ScePspFVector3* v)
323423
{
@@ -534,6 +634,14 @@ void sceGumRotateZYX(const ScePspFVector3* v)
534634
}
535635
#endif
536636

637+
#ifdef F_sceGumRotate
638+
void sceGumRotate(const ScePspFQuaternion* q)
639+
{
640+
gumRotateMatrix(gum_current_matrix, q);
641+
gum_current_matrix_update = 1;
642+
}
643+
#endif
644+
537645
#ifdef F_sceGumScale
538646
void sceGumScale(const ScePspFVector3* v)
539647
{

‎src/gum/pspgum.h‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -120,6 +120,13 @@ void sceGumRotateXYZ(const ScePspFVector3* v);
120120
**/
121121
void sceGumRotateZYX(const ScePspFVector3* v);
122122

123+
/**
124+
* Apply rotation represented by quaternion q
125+
*
126+
* @param q - Pointer to quaternion
127+
**/
128+
void sceGumRotate(const ScePspFQuaternion* q);
129+
123130
/**
124131
* Scale matrix
125132
*
@@ -198,6 +205,7 @@ void gumInit(void);
198205
**/
199206
void gumLoadIdentity(ScePspFMatrix4* m);
200207

208+
void gumLoadQuaternion(ScePspFMatrix4* r, const ScePspFQuaternion* q);
201209
void gumLoadMatrix(ScePspFMatrix4* r, const ScePspFMatrix4* a);
202210
void gumLookAt(ScePspFMatrix4* m, ScePspFVector3* eye, ScePspFVector3* center, ScePspFVector3* up);
203211
void gumMultMatrix(ScePspFMatrix4* result, const ScePspFMatrix4* a, const ScePspFMatrix4* b);
@@ -208,6 +216,7 @@ void gumRotateXYZ(ScePspFMatrix4* m, const ScePspFVector3* v);
208216
void gumRotateY(ScePspFMatrix4* m, float angle);
209217
void gumRotateZ(ScePspFMatrix4* m, float angle);
210218
void gumRotateZYX(ScePspFMatrix4* m, const ScePspFVector3* v);
219+
void gumRotateMatrix(ScePspFMatrix4* m, const ScePspFQuaternion* q);
211220
void gumScale(ScePspFMatrix4* m, const ScePspFVector3* v);
212221
void gumTranslate(ScePspFMatrix4* m, const ScePspFVector3* v);
213222
void gumFullInverse(ScePspFMatrix4* r, const ScePspFMatrix4* a);
@@ -228,6 +237,12 @@ void gumFastInverse(ScePspFMatrix4* r, const ScePspFMatrix4* a);
228237
void gumCrossProduct(ScePspFVector3* r, const ScePspFVector3* a, const ScePspFVector3* b);
229238
float gumDotProduct(const ScePspFVector3* a, const ScePspFVector3* b);
230239
void gumNormalize(ScePspFVector3* v);
240+
void gumRotateVector(ScePspFVector3* r, const ScePspFQuaternion* q, const ScePspFVector3* v);
241+
242+
// quaternion functions
243+
void gumNormalizeQuaternion(ScePspFQuaternion* q);
244+
void gumLoadAxisAngle(ScePspFQuaternion* r, ScePspFVector3* axis, float t);
245+
void gumMultQuaternion(ScePspFQuaternion* result, const ScePspFQuaternion* a, const ScePspFQuaternion* b);
231246

232247
#if defined(__cplusplus)
233248
}

0 commit comments

Comments
 (0)