Mirrors/SBSPSS
1
0
Fork 0
Code Releases Activity
94be656b68
SBSPSS/source/utils/utils.h

651 lines
14 KiB
C
Raw Normal View History

2000-08-29 21:54:22 +02:00
/******************/
/*** Misc Utils ***/
/******************/
#ifndef __UTILS_HEADER__
#define __UTILS_HEADER__
#include "utils\mathmip.h"
#ifndef __GLOBAL_STRUCTS_HEADER__
#include "gstruct.h"
#endif
#ifndef __MATHTABLE_HEADER__
#include "utils\mathtab.h"
#endif
/*****************************************************************************/
#define NO_SIN 64
#define SINMASK (NO_SIN - 1)
#define COSPOS (NO_SIN / 4)
extern const s16 c_circle[ NO_SIN ];
extern const MATRIX IdentityMtx;
/*****************************************************************************/
u8 *MakePtr(void *BasePtr,int Offset);
/*****************************************************************************/
/*** Loads of inlines ********************************************************/
/*****************************************************************************/
inline void SetIdent(MATRIX *Mtx)
{
Mtx->m[0][0]=ONE; Mtx->m[0][1]=0; Mtx->m[0][2]=0;
Mtx->m[1][0]=0; Mtx->m[1][1]=ONE; Mtx->m[1][2]=0;
Mtx->m[2][0]=0; Mtx->m[2][1]=0; Mtx->m[2][2]=ONE;
Mtx->t[0]=0; Mtx->t[1]=0; Mtx->t[2]=0;
}
/*****************************************************************************/
inline void SetIdentNoTrans(MATRIX *Mtx)
{
Mtx->m[0][0]=ONE; Mtx->m[0][1]=0; Mtx->m[0][2]=0;
Mtx->m[1][0]=0; Mtx->m[1][1]=ONE; Mtx->m[1][2]=0;
Mtx->m[2][0]=0; Mtx->m[2][1]=0; Mtx->m[2][2]=ONE;
// Mtx->t[0]=0; Mtx->t[1]=0; Mtx->t[2]=0;
}
/*****************************************************************************/
inline void SetIdentScale(MATRIX *Mtx,u32 Scale)
{
Mtx->m[0][0]=Scale; Mtx->m[0][1]=0; Mtx->m[0][2]=0;
Mtx->m[1][0]=0; Mtx->m[1][1]=Scale; Mtx->m[1][2]=0;
Mtx->m[2][0]=0; Mtx->m[2][1]=0; Mtx->m[2][2]=Scale;
Mtx->t[0]=0; Mtx->t[1]=0; Mtx->t[2]=0;
}
/*****************************************************************************/
inline void SetIdentScaleNoTrans(MATRIX *Mtx,u32 Scale)
{
Mtx->m[0][0]=Scale; Mtx->m[0][1]=0; Mtx->m[0][2]=0;
Mtx->m[1][0]=0; Mtx->m[1][1]=Scale; Mtx->m[1][2]=0;
Mtx->m[2][0]=0; Mtx->m[2][1]=0; Mtx->m[2][2]=Scale;
// Mtx->t[0]=0; Mtx->t[1]=0; Mtx->t[2]=0;
}
/*****************************************************************************/
inline void InverseMatrix(MATRIX *m, MATRIX &im) //assumes no scale, just transformation and rotation
{
TransposeMatrix(m, &im);
ApplyMatrixLV(&im, (VECTOR*)&m->t[0], (VECTOR*)&im.t[0]);
im.t[0] = -im.t[0];
im.t[1] = -im.t[1];
im.t[2] = -im.t[2];
}
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
extern long s_randomSeed;
inline long getRndSeed()
{
const u32 INCREMENT = 1;
const u32 MULTIPLIER = 0x015a4e35L;
s_randomSeed = MULTIPLIER * s_randomSeed + INCREMENT;
return abs(s_randomSeed);
}
/*****************************************************************************/
inline void setRndSeed( long seed )
{
s_randomSeed = seed;
}
/*****************************************************************************/
inline long getRnd()
{
return getRndSeed();
}
/*****************************************************************************/
inline long getRndRange( long v )
{
if (v <= 0) return 0;
// high order bits are more "random" than low order bits
if (v < 0x0ffff) return (getRndSeed() >> 16) % v;
return getRndSeed() % v;
}
/*****************************************************************************/
/*****************************************************************************/
/*****************************************************************************/
// -- VECTOR Operator Arithmetic operation --
inline VECTOR operator - (const VECTOR& a)
{
VECTOR ret;
ret.vx = -a.vx;
ret.vy = -a.vy;
ret.vz = -a.vz;
return ret;
}
inline VECTOR operator + (const VECTOR& a, const VECTOR& b)
{
VECTOR ret;
ret.vx = a.vx + b.vx;
ret.vy = a.vy + b.vy;
ret.vz = a.vz + b.vz;
return ret;
}
inline VECTOR operator - (const VECTOR& a, const VECTOR& b)
{
VECTOR ret;
ret.vx = a.vx - b.vx;
ret.vy = a.vy - b.vy;
ret.vz = a.vz - b.vz;
return ret;
}
inline VECTOR operator *= (const VECTOR& a, const int & b)
{
VECTOR ret;
ret.vx = a.vx * b;
ret.vy = a.vy * b;
ret.vz = a.vz * b;
return ret;
}
inline VECTOR operator >>= (const VECTOR& a, const int & b)
{
VECTOR ret;
ret.vx = a.vx >> b;
ret.vy = a.vy >> b;
ret.vz = a.vz >> b;
return ret;
}
inline VECTOR operator += (const VECTOR& a, const VECTOR& b)
{
VECTOR ret;
ret.vx = a.vx + b.vx;
ret.vy = a.vy + b.vy;
ret.vz = a.vz + b.vz;
return ret;
}
inline int operator == (const VECTOR& a, const VECTOR& b)
{
return (a.vx == b.vx) && (a.vy == b.vy) && (a.vz == b.vz);
}
inline int operator != (const VECTOR& a, const VECTOR& b)
{
return !(a == b);
}
// -- VECTOR to sVtx Operator Arithmetic operation --
inline VECTOR operator - (const sShortXYZ& a, const sVtx& b)
{
VECTOR ret;
ret.vx = a.vx - b.vx;
ret.vy = a.vy - b.vy;
ret.vz = a.vz - b.vz;
return ret;
}
inline VECTOR operator - (const sVtx& a, const sVtx& b)
{
VECTOR ret;
ret.vx = a.vx - b.vx;
ret.vy = a.vy - b.vy;
ret.vz = a.vz - b.vz;
return ret;
}
inline VECTOR operator *= (const sVtx& a, const int & b)
{
VECTOR ret;
ret.vx = a.vx * b;
ret.vy = a.vy * b;
ret.vz = a.vz * b;
return ret;
}
inline VECTOR operator >>= (const sVtx& a, const int & b)
{
VECTOR ret;
ret.vx = a.vx >> b;
ret.vy = a.vy >> b;
ret.vz = a.vz >> b;
return ret;
}
// -- sShortXYZ Operator Arithmetic operation --
inline sShortXYZ operator - (const sShortXYZ& a)
{
sShortXYZ ret;
ret.vx = -a.vx;
ret.vy = -a.vy;
ret.vz = -a.vz;
return ret;
}
inline sShortXYZ operator + (const sShortXYZ& a, const sShortXYZ& b)
{
sShortXYZ ret;
ret.vx = a.vx + b.vx;
ret.vy = a.vy + b.vy;
ret.vz = a.vz + b.vz;
return ret;
}
inline sShortXYZ operator >> (const VECTOR& a, const int& b)
{
sShortXYZ ret;
ret.vx = a.vx >> b;
ret.vy = a.vy >> b;
ret.vz = a.vz >> b;
return ret;
}
inline int operator * (const sShortXYZ& a, const sShortXYZ& b) // DOT PRODUCT
{
return (a.vx * b.vx) + (a.vy * b.vy) + (a.vz * b.vz);
}
inline int operator == (const sShortXYZ& a, const sShortXYZ& b)
{
return (a.vx == b.vx) && (a.vy == b.vy) && (a.vz == b.vz);
}
inline int operator != (const sShortXYZ& a, const sShortXYZ& b)
{
return !(a == b);
}
// -- sShortXYZ to VECTOR Operator Arithmetic operation --
inline VECTOR operator + (const VECTOR& a, const sShortXYZ& b)
{
VECTOR ret;
ret.vx = a.vx + b.vx;
ret.vy = a.vy + b.vy;
ret.vz = a.vz + b.vz;
return ret;
}
inline VECTOR operator - (const VECTOR& a, const sShortXYZ& b)
{
VECTOR ret;
ret.vx = a.vx - b.vx;
ret.vy = a.vy - b.vy;
ret.vz = a.vz - b.vz;
return ret;
}
inline VECTOR operator - (const sShortXYZ& a, const sShortXYZ& b)
{
VECTOR ret;
ret.vx = a.vx - b.vx;
ret.vy = a.vy - b.vy;
ret.vz = a.vz - b.vz;
return ret;
}
inline int operator * (const VECTOR& a, const sShortXYZ& b) // DOT PRODUCT
{
return (a.vx * b.vx) + (a.vy * b.vy) + (a.vz * b.vz);
}
inline int operator * (const sShortXYZ& a, const VECTOR& b) // DOT PRODUCT
{
return (a.vx * b.vx) + (a.vy * b.vy) + (a.vz * b.vz);
}
inline VECTOR operator ^ (const VECTOR& a, const sShortXYZ& b) // CROSS PRODUCT
{
VECTOR ret;
ret.vx = (a.vy * b.vz) - (a.vz * b.vy);
ret.vy = (a.vz * b.vx) - (a.vx * b.vz);
ret.vz = (a.vx * b.vy) - (a.vy * b.vx);
return ret;
}
inline VECTOR operator ^ (const sShortXYZ& a, const sShortXYZ& b) // CROSS PRODUCT
{
VECTOR ret;
ret.vx = (a.vy * b.vz) - (a.vz * b.vy);
ret.vy = (a.vz * b.vx) - (a.vx * b.vz);
ret.vz = (a.vx * b.vy) - (a.vy * b.vx);
return ret;
}
/*****************************************************************************/
/*** Inlines *****************************************************************/
/*****************************************************************************/
inline u32 isqrt2(u32 v)
{
register u32 root = 0;
register u32 remhi = 0;
register u32 remlo = v;
register u32 count = 15;
register u32 testdiv;
do
{
remhi = (remhi << 2) | (remlo >> 30);
remlo <<= 2;
root <<= 1;
testdiv = (root << 1) + 1;
if (remhi >= testdiv)
{
remhi -= testdiv;
root += 1;
}
} while (count -- != 0);
return(root);
}
/*****************************************************************************/
inline u32 CalcLength(const SVECTOR *s)
{
int Dt;
gte_ldsv(s);
gte_sqr0();
CMX_StVecXYZMag(&Dt);
return(Dt);
}
/*****************************************************************************/
inline u32 CalcLengthSquared(const SVECTOR *s)
{
u32 Dt;
gte_ldsv(s);
gte_sqr0();
CMX_StVecXYZMag(&Dt);
return(Dt);
}
/*****************************************************************************/
inline u32 CalcLengthV(const VECTOR *s)
{
u32 Dt;
gte_ldlvl(s);
gte_sqr0();
CMX_StVecXYZMag(&Dt);
return(isqrt2(Dt));
}
/*****************************************************************************/
inline u32 CalcLengthSquaredV(const VECTOR *s)
{
u32 Dt;
gte_ldlvl(s);
gte_sqr0();
CMX_StVecXYZMag(&Dt);
return(Dt);
}
/*****************************************************************************/
inline u32 CalcDist(const SVECTOR *s, const SVECTOR *e)
{
s32 Dx = s->vx - e->vx;
s32 Dy = s->vy - e->vy;
s32 Dz = s->vz - e->vz;
u32 Dt;
CMX_ldXYZ(Dx,Dy,Dz);
gte_sqr0();
CMX_StVecXYZMag(&Dt);
return(isqrt2(Dt));
}
/*****************************************************************************/
inline u32 CalcDistXZ(const SVECTOR *s, const SVECTOR *e)
{
s32 Dx = s->vx - e->vx;
s32 Dz = s->vz - e->vz;
u32 Dt;
CMX_ldXZ(Dx,Dz);
gte_sqr0();
CMX_StVecXZMag(&Dt);
return(isqrt2(Dt));
}
/*****************************************************************************/
inline u32 CalcDistV(const VECTOR *s, const VECTOR *e)
{
s32 Dx = s->vx - e->vx;
s32 Dy = s->vy - e->vy;
s32 Dz = s->vz - e->vz;
u32 Dt;
CMX_ldXYZ(Dx,Dy,Dz);
gte_sqr0();
CMX_StVecXYZMag(&Dt);
return(isqrt2(Dt));
}
/*****************************************************************************/
inline u32 CalcDistV(const VECTOR *s, const VECTOR *e, u16 shift)
{
s32 Dx = (s->vx - e->vx) >> shift;
s32 Dz = (s->vz - e->vz) >> shift;
u32 Dt;
CMX_ldXZ(Dx,Dz);
gte_sqr0();
CMX_StVecXZMag(&Dt);
return(isqrt2(Dt) << shift);
}
/*****************************************************************************/
inline u32 CalcDistXZV(const VECTOR *s, const VECTOR *e)
{
s32 Dx = s->vx - e->vx;
s32 Dz = s->vz - e->vz;
u32 Dt;
CMX_ldXZ(Dx,Dz);
gte_sqr0();
CMX_StVecXZMag(&Dt);
return(isqrt2(Dt));
}
/*****************************************************************************/
inline u32 CalcDistXZVSquared(const VECTOR *s, const VECTOR *e)
{
s32 Dx = s->vx - e->vx;
s32 Dz = s->vz - e->vz;
u32 Dt;
CMX_ldXZ(Dx,Dz);
gte_sqr0();
CMX_StVecXZMag(&Dt);
return abs(Dt);
}
/*****************************************************************************/
inline u32 CalcDistXZV(const VECTOR *s, const VECTOR *e, u16 shift)
{
s32 Dx = (s->vx - e->vx) >> shift;
s32 Dz = (s->vz - e->vz) >> shift;
u32 Dt;
CMX_ldXZ(Dx,Dz);
gte_sqr0();
CMX_StVecXZMag(&Dt);
return(isqrt2(Dt) << shift);
}
/*****************************************************************************/
inline long CalcDistXZVAccurate(const VECTOR *s, const VECTOR *e)
{
long Dx = s->vx - e->vx;
long Dz = s->vz - e->vz;
long Dt = (Dx*Dx) + (Dz*Dz);
long len = SquareRoot0(Dt);
return(len);
}
/*****************************************************************************/
inline u32 CalcDist(const sShortXYZ *s, const sShortXYZ *e)
{
s32 Dx = s->vx - e->vx;
s32 Dy = s->vy - e->vy;
s32 Dz = s->vz - e->vz;
u32 Dt;
CMX_ldXYZ(Dx,Dy,Dz);
gte_sqr0();
CMX_StVecXYZMag(&Dt);
return(isqrt2(Dt));
}
/*****************************************************************************/
inline u32 CalcDistXZ(const sShortXYZ *s, const sShortXYZ *e)
{
s32 Dx = s->vx - e->vx;
s32 Dz = s->vz - e->vz;
u32 Dt;
CMX_ldXZ(Dx,Dz);
gte_sqr0();
CMX_StVecXZMag(&Dt);
return(isqrt2(Dt));
}
/*****************************************************************************/
inline u32 CalcDistXZSquared(const sShortXYZ *s, const sShortXYZ *e)
{
s32 Dx = s->vx - e->vx;
s32 Dz = s->vz - e->vz;
u32 Dt;
CMX_ldXZ(Dx,Dz);
gte_sqr0();
CMX_StVecXZMag(&Dt);
return(Dt);
}
/*****************************************************************************/
inline s32 DotProduct(const SVECTOR *V0, const SVECTOR *V1)
{
s32 Dpx,Dpy,Dpz;
Dpx=V0->vx*V1->vx;
Dpy=V0->vy*V1->vy;
Dpz=V0->vz*V1->vz;
return(Dpx+Dpy+Dpz);
}
/*****************************************************************************/
inline s32 DotProductV( const VECTOR *V0, const VECTOR *V1 )
{
s32 ax, ay, az;
s32 bx, by, bz;
s32 Dpx, Dpy, Dpz;
ax = V0->vx;
ay = V0->vy;
az = V0->vz;
bx = V1->vx;
by = V1->vy;
bz = V1->vz;
Dpx = ax * bx;
Dpy = ay * by;
Dpz = az * bz;
return (Dpx + Dpy + Dpz);
}
/*****************************************************************************/
inline s32 DotProductSV( const SVECTOR *V0, const VECTOR *V1 )
{
s32 ax, ay, az;
s32 bx, by, bz;
s32 Dpx, Dpy, Dpz;
ax = V0->vx;
ay = V0->vy;
az = V0->vz;
bx = V1->vx;
by = V1->vy;
bz = V1->vz;
Dpx = ax * bx;
Dpy = ay * by;
Dpz = az * bz;
return (Dpx + Dpy + Dpz);
}
/*****************************************************************************/
inline void QuatSlerp(sQuat *s, sQuat *d, int t, sQuat *o)
{
s32 xx, yy, zz, ww;
s32 cs;
s32 c0, c1;
sQuat to;
xx = (s->vx * d->vx);
yy = (s->vy * d->vy);
zz = (s->vz * d->vz);
ww = (s->vw * d->vw);
cs = xx + yy + zz + ww;
if (cs<0)
{
cs = -cs;
to.vx = -d->vx;
to.vy = -d->vy;
to.vz = -d->vz;
to.vw = -d->vw;
}
else
{
to.vx = d->vx;
to.vy = d->vy;
to.vz = d->vz;
to.vw = d->vw;
}
cs >>= 12;
s32 omega;
if (cs < 3500)
{ // standard case (slerp)
omega = macos(cs);
s32 sinom = monesin(omega);
c0 = (msin((((4096 - t) * omega) >> 12) & 4095) * sinom)>>12;
c1 = (msin(((t * omega) >> 12) & 4095) * sinom)>>12;
}
else
{
c0 = 4096 - t;
c1 = t;
}
gte_LoadAverageShort12(s, &to, c0, c1, o);
o->vw = ((c0 * s->vw) + (c1 * to.vw)) >> 12;
}
/*****************************************************************************/
static const s32 DeltaErr = 1000;
inline void NormalizeQuaternion(sQuat *o)
{
s32 t = (u32)((s32)o->vx*o->vx+(s32)o->vy*o->vy+(s32)o->vz*o->vz+(s32)o->vw*o->vw);
s32 diff = abs((1<<24) - t);
if (diff>DeltaErr)
{
s32 isqrs = isqrt2(t);
if (isqrs)
{
o->vx = (s16)(((s32)o->vx << 12) / (s32)isqrs);
o->vy = (s16)(((s32)o->vy << 12) / (s32)isqrs);
o->vz = (s16)(((s32)o->vz << 12) / (s32)isqrs);
o->vw = (s16)(((s32)o->vw << 12) / (s32)isqrs);
}
}
}
#endif
Copy Permalink