mirror of
https://github.com/thug1src/thug.git
synced 2025-01-22 05:43:47 +00:00
415 lines
7.5 KiB
C++
415 lines
7.5 KiB
C++
|
|
|
|
//--------------------------------------------------------------
|
|
// V E C T O R A N D M A T R I X R O U T I N E S
|
|
//--------------------------------------------------------------
|
|
|
|
|
|
#include <core/math.h>
|
|
#include <core/defines.h>
|
|
#include "mikemath.h"
|
|
|
|
namespace NxPs2
|
|
{
|
|
|
|
|
|
// copy a vector
|
|
// vd = vs
|
|
void VecCopy(Vec vd, Vec vs)
|
|
{
|
|
vd[0]=vs[0], vd[1]=vs[1], vd[2]=vs[2], vd[3]=vs[3];
|
|
}
|
|
|
|
|
|
// set a vector
|
|
// v = (x,y,z,w)
|
|
void VecSet(Vec v, float x, float y, float z, float w)
|
|
{
|
|
v[0]=x, v[1]=y, v[2]=z, v[3]=w;
|
|
}
|
|
|
|
|
|
// add two vectors to make another
|
|
// vd = v0+v1
|
|
void VecSum(Vec vd, Vec v0, Vec v1)
|
|
{
|
|
int i;
|
|
for (i=0; i<4; i++)
|
|
vd[i] = v0[i]+v1[i];
|
|
}
|
|
|
|
|
|
// add one vector onto another
|
|
// vd += vs
|
|
void VecAdd(Vec vd, Vec vs)
|
|
{
|
|
int i;
|
|
for (i=0; i<4; i++)
|
|
vd[i] += vs[i];
|
|
}
|
|
|
|
|
|
// subtract two vectors to make another
|
|
// vd = v0-v1
|
|
void VecDifference(Vec vd, Vec v0, Vec v1)
|
|
{
|
|
int i;
|
|
for (i=0; i<4; i++)
|
|
vd[i] = v0[i]-v1[i];
|
|
}
|
|
|
|
|
|
// subtract one vector from another
|
|
// vd -= vs
|
|
void VecSubtract(Vec vd, Vec vs)
|
|
{
|
|
int i;
|
|
for (i=0; i<4; i++)
|
|
vd[i] -= vs[i];
|
|
}
|
|
|
|
|
|
// negate a vector
|
|
// v = -v
|
|
void VecNegate(Vec v)
|
|
{
|
|
int i;
|
|
for (i=0; i<4; i++)
|
|
v[i] = -v[i];
|
|
}
|
|
|
|
|
|
// multiply a vector by a matrix
|
|
// v = v * M
|
|
void VecTransform(Vec v, Mat M)
|
|
{
|
|
int i,j;
|
|
float sum;
|
|
Vec TempVec;
|
|
for (i=0; i<4; i++)
|
|
{
|
|
sum = 0.0;
|
|
for (j=0; j<4; j++)
|
|
sum += v[j]*M[j][i];
|
|
TempVec[i] = sum;
|
|
}
|
|
VecCopy(v,TempVec);
|
|
}
|
|
|
|
|
|
// product of a vector by a matrix
|
|
// vd = vs * M
|
|
void VecTransformed(Vec vd, Vec vs, Mat M)
|
|
{
|
|
int i,j;
|
|
float sum;
|
|
for (i=0; i<4; i++)
|
|
{
|
|
sum = 0.0;
|
|
for (j=0; j<4; j++)
|
|
sum += vs[j]*M[j][i];
|
|
vd[i] = sum;
|
|
}
|
|
}
|
|
|
|
|
|
// rotate a vector through an angle about an axis
|
|
void VecRotate(Vec v, float Angle, char Axis)
|
|
{
|
|
Mat M;
|
|
MatRotation(M, Angle, Axis);
|
|
VecTransform(v, M);
|
|
}
|
|
|
|
|
|
// normalise a vector
|
|
// v = v / |v|
|
|
void VecNormalise(Vec v)
|
|
{
|
|
int i;
|
|
float RecipLength = 1.0 / sqrtf(v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3]);
|
|
for (i=0; i<4; i++)
|
|
v[i] *= RecipLength;
|
|
}
|
|
|
|
|
|
// scale a vector
|
|
// v *= k
|
|
void VecScale(Vec v, float k)
|
|
{
|
|
int i;
|
|
for (i=0; i<4; i++)
|
|
v[i] *= k;
|
|
}
|
|
|
|
|
|
// scaled vector
|
|
// vd = k*vs
|
|
void VecScaled(Vec vd, float k, Vec vs)
|
|
{
|
|
int i;
|
|
for (i=0; i<4; i++)
|
|
vd[i] = k*vs[i];
|
|
}
|
|
|
|
|
|
// add scaled vector
|
|
// vd += k*vs
|
|
void VecAddScaled(Vec vd, float k, Vec vs)
|
|
{
|
|
int i;
|
|
for (i=0; i<4; i++)
|
|
vd[i] += k*vs[i];
|
|
}
|
|
|
|
|
|
// weighted mean of 2 vectors
|
|
// v = (k0*v0 + k1*v1)/(k0+k1)
|
|
void VecWeightedMean2(Vec v, float k0, Vec v0, float k1, Vec v1)
|
|
{
|
|
Vec Temp;
|
|
VecScaled(v,k0,v0);
|
|
VecScaled(Temp,k1,v1);
|
|
VecAdd(v,Temp);
|
|
VecScale(v,1.0f/(k0+k1));
|
|
}
|
|
|
|
|
|
// weighted mean of 3 vectors
|
|
// v = (k0*v0 + k1*v1 + k2*v2)/(k0+k1+k2)
|
|
void VecWeightedMean3(Vec v, float k0, Vec v0, float k1, Vec v1, float k2, Vec v2)
|
|
{
|
|
Vec Temp;
|
|
VecScaled(v,k0,v0);
|
|
VecScaled(Temp,k1,v1);
|
|
VecAdd(v,Temp);
|
|
VecScaled(Temp,k2,v2);
|
|
VecAdd(v,Temp);
|
|
VecScale(v,1.0f/(k0+k1+k2));
|
|
}
|
|
|
|
|
|
// cross product
|
|
void CrossProduct(Vec vd, Vec v0, Vec v1)
|
|
{
|
|
vd[0] = v0[1]*v1[2] - v0[2]*v1[1];
|
|
vd[1] = v0[2]*v1[0] - v0[0]*v1[2];
|
|
vd[2] = v0[0]*v1[1] - v0[1]*v1[0];
|
|
}
|
|
|
|
|
|
// dot product (3 components)
|
|
float DotProduct3(Vec v0, Vec v1)
|
|
{
|
|
return v0[0]*v1[0] + v0[1]*v1[1] + v0[2]*v1[2];
|
|
}
|
|
|
|
|
|
// dot product (4 components)
|
|
float DotProduct4(Vec v0, Vec v1)
|
|
{
|
|
return v0[0]*v1[0] + v0[1]*v1[1] + v0[2]*v1[2] + v0[3]*v1[3];
|
|
}
|
|
|
|
|
|
// length of a vector (3 components)
|
|
float VecLength3(Vec v)
|
|
{
|
|
return sqrtf(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
|
|
}
|
|
|
|
|
|
// copy a matrix
|
|
// Md = Ms
|
|
//void MatCopy(Mat Md, Mat Ms)
|
|
//{
|
|
// int i,j;
|
|
// for (i=0; i<4; i++)
|
|
// for (j=0; j<4; j++)
|
|
// Md[i][j] = Ms[i][j];
|
|
// *(Mth::Matrix*)Md = *(Mth::Matrix*)Ms;
|
|
//}
|
|
|
|
|
|
// set a matrix
|
|
// M = ((m00,m01,m02,m03), (m10,m11,m12,m13), (m20,m21,m22,m23), (m30,m31,m32,m33))
|
|
void MatSet(Mat M, float m00, float m01, float m02, float m03,
|
|
float m10, float m11, float m12, float m13,
|
|
float m20, float m21, float m22, float m23,
|
|
float m30, float m31, float m32, float m33)
|
|
{
|
|
M[0][0]=m00, M[0][1]=m01, M[0][2]=m02, M[0][3]=m03;
|
|
M[1][0]=m10, M[1][1]=m11, M[1][2]=m12, M[1][3]=m13;
|
|
M[2][0]=m20, M[2][1]=m21, M[2][2]=m22, M[2][3]=m23;
|
|
M[3][0]=m30, M[3][1]=m31, M[3][2]=m32, M[3][3]=m33;
|
|
}
|
|
|
|
|
|
|
|
|
|
// zero a matrix
|
|
// set M to 4x4 zero matrix
|
|
void MatZero(Mat M)
|
|
{
|
|
int i,j;
|
|
for (i=0; i<4; i++)
|
|
for (j=0; j<4; j++)
|
|
M[i][j] = 0.0;
|
|
}
|
|
|
|
|
|
// set identity matrix
|
|
// set M to 4x4 identity
|
|
void MatIdentity(Mat M)
|
|
{
|
|
int i,j;
|
|
for (i=0; i<4; i++)
|
|
for (j=0; j<4; j++)
|
|
M[i][j] = (i==j) ? 1.0 : 0.0;
|
|
}
|
|
|
|
|
|
// transpose a matrix
|
|
// M = MT
|
|
void MatTranspose(Mat M)
|
|
{
|
|
float temp;
|
|
int i,j;
|
|
for (i=0; i<4; i++)
|
|
for (j=i+1; j<4; j++)
|
|
temp=M[i][j], M[i][j]=M[j][i], M[j][i]=temp;
|
|
}
|
|
|
|
|
|
// transpose a matrix
|
|
// Md = MsT
|
|
void MatTransposed(Mat Md, Mat Ms)
|
|
{
|
|
int i,j;
|
|
for (i=0; i<4; i++)
|
|
for (j=0; j<4; j++)
|
|
Md[i][j]=Ms[j][i];
|
|
}
|
|
|
|
|
|
// make a diagonal matrix from the elements of v
|
|
void MatDiagonal(Mat M, Vec v)
|
|
{
|
|
int i;
|
|
MatZero(M);
|
|
for (i=0; i<4; i++)
|
|
M[i][i] = v[i];
|
|
}
|
|
|
|
|
|
// scale all elements of a matrix
|
|
void MatScale(Mat M, float k)
|
|
{
|
|
int i,j;
|
|
for (i=0; i<4; i++)
|
|
for (j=0; j<4; j++)
|
|
M[i][j] *= k;
|
|
}
|
|
|
|
|
|
// set M to matrix for rotation through Angle (radians) about Axis ('x','y' or 'z')
|
|
void MatRotation(Mat M, float Angle, char Axis)
|
|
{
|
|
float s,c;
|
|
int i0=(int)((Axis+1-'x'))%3, i1=(int)((Axis+2-'x'))%3;
|
|
s=sinf(Angle), c=cosf(Angle);
|
|
MatIdentity(M);
|
|
M[i0][i0] = M[i1][i1] = c;
|
|
M[i1][i0] = -(M[i0][i1] = s);
|
|
}
|
|
|
|
|
|
// set M to matrix for translation by v
|
|
void MatTranslation(Mat M, Vec v)
|
|
{
|
|
MatIdentity(M);
|
|
VecCopy(M[3],v);
|
|
}
|
|
|
|
|
|
// multiply two matrices to make another
|
|
// set AB = A * B
|
|
void MatProduct(Mat AB, Mat A, Mat B)
|
|
{
|
|
|
|
#if 1
|
|
sceVu0MulMatrix((sceVu0FVECTOR*)AB,(sceVu0FVECTOR*)B,(sceVu0FVECTOR*)A);
|
|
return;
|
|
#else
|
|
|
|
float sum;
|
|
int i,j,k;
|
|
for (i=0; i<4; i++)
|
|
for (j=0; j<4; j++)
|
|
{
|
|
sum=0.0;
|
|
for (k=0; k<4; k++)
|
|
sum += A[i][k]*B[k][j];
|
|
AB[i][j] = sum;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
|
|
// premulitplication of one matrix by another
|
|
// A = B * A
|
|
void MatPremultiply(Mat A, Mat B)
|
|
{
|
|
Mat TempMat;
|
|
MatProduct(TempMat,B,A);
|
|
MatCopy(A,TempMat);
|
|
}
|
|
|
|
|
|
// postmultiplication of one matrix by another
|
|
// A = A * B
|
|
void MatPostmultiply(Mat A, Mat B)
|
|
{
|
|
Mat TempMat;
|
|
MatProduct(TempMat,A,B);
|
|
MatCopy(A,TempMat);
|
|
}
|
|
|
|
|
|
// invert a 4x4 matrix
|
|
// Md = inv(Ms)
|
|
// Note: this is not a general matrix invert: it only works for matrices which incorporate rotation and translation.
|
|
void MatInverse(Mat Md, Mat Ms)
|
|
{
|
|
int i,j;
|
|
Mat InvT, InvR;
|
|
// set up negative of original translation
|
|
MatIdentity(InvT);
|
|
InvT[3][0] = -Ms[3][0];
|
|
InvT[3][1] = -Ms[3][1];
|
|
InvT[3][2] = -Ms[3][2];
|
|
// set up transpose of original rotation
|
|
MatIdentity(InvR);
|
|
for (i=0; i<3; i++)
|
|
for (j=0; j<3; j++)
|
|
InvR[i][j] = Ms[j][i];
|
|
// take product
|
|
MatProduct(Md,InvT,InvR);
|
|
}
|
|
|
|
|
|
// build a matrix from 3 Euler angles
|
|
// M = Rz(Angles[2]) * Rx(Angles[0]) * Ry(Angles[1])
|
|
void MatEuler(Mat M, Vec Angles)
|
|
{
|
|
Mat Temp;
|
|
MatRotation(M, Angles[2], 'z');
|
|
MatRotation(Temp, Angles[0], 'x');
|
|
MatPostmultiply(M, Temp);
|
|
MatRotation(Temp, Angles[1], 'y');
|
|
MatPostmultiply(M, Temp);
|
|
}
|
|
|
|
} // namespace NxPs2
|
|
|