jcrussell/libsst
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
71230f918f95b75d8569cd2ccd6395d0919f873d
libsst/libsst-math/SST_Transform.c
James Russell eca1e8c458 Initial commit
2026-04-03 00:22:39 -05:00

545 lines
13 KiB
C
Raw Blame History

#include <math.h> /* tanf() */
#include <string.h> /* memset() */
#include <SST/SST_Math.h>
/* Will form the 4x4 identity matrix.
* Remember that you can save operations by
* simply zeroing what you're not going to fill in yourself */
void SST_Math_Mat33fCreateIdentity(SST_Mat33f* Mat)
{
memset(Mat, 0, sizeof(SST_Mat33f));
Mat->v[0] = 1.f;
Mat->v[4] = 1.f;
Mat->v[8] = 1.f;
}
void SST_Math_Mat33fFromVec3f(SST_Mat33f* Mat, const SST_Vec3f* v, const int column)
{
Mat->v[0+3*column] = v->v[0];
Mat->v[1+3*column] = v->v[1];
Mat->v[2+3*column] = v->v[2];
}
void SST_Math_Mat44fFromVec4f(SST_Mat44f* Mat, const SST_Vec4f* v, const int column)
{
Mat->v[0+4*column] = v->v[0];
Mat->v[1+4*column] = v->v[1];
Mat->v[2+4*column] = v->v[2];
Mat->v[3+4*column] = v->v[3];
}
void SST_Math_Mat33dFromVec3d(SST_Mat33d* Mat, const SST_Vec3d* v, const int column)
{
Mat->v[0+3*column] = v->v[0];
Mat->v[1+3*column] = v->v[1];
Mat->v[2+3*column] = v->v[2];
}
void SST_Math_Mat44dFromVec4d(SST_Mat44d* Mat, const SST_Vec4d* v, const int column)
{
Mat->v[0+4*column] = v->v[0];
Mat->v[1+4*column] = v->v[1];
Mat->v[2+4*column] = v->v[2];
Mat->v[3+4*column] = v->v[3];
}
/* Will form the 4x4 identity matrix.
* Remember that you can save operations by
* simply zeroing what you're not going to fill in yourself */
void SST_Math_Mat44fCreateIdentity(SST_Mat44f* Mat)
{
memset(Mat, 0, sizeof(SST_Mat44f));
Mat->v[0] = 1.f;
Mat->v[5] = 1.f;
Mat->v[10] = 1.f;
Mat->v[15] = 1.f;
}
void SST_Math_Mat33fApplyTransforms(SST_Vec3f** Vecs, SST_Mat33f* Transforms, const int num_Vecs)
{
int i;
for(i=0; i < num_Vecs; i++)
SST_Math_Mat33fMultiplyVectorLocal(Transforms, Vecs[i]);
}
void SST_Math_Mat33fCreateTranslation(SST_Mat33f* Mat, const float x, const float y)
{
memset(Mat, 0, sizeof(SST_Mat33f));
Mat->v[0] = 1.f;
Mat->v[4] = 1.f;
Mat->v[6] = x;
Mat->v[7] = y;
Mat->v[8] = 1.f;
}
void SST_Math_Mat44fCreateTranslation(SST_Mat44f* Mat, const float x, const float y, const float z)
{
memset(Mat, 0, sizeof(SST_Mat44f));
Mat->v[0] = 1.f;
Mat->v[5] = 1.f;
Mat->v[10] = 1.f;
Mat->v[12] = x;
Mat->v[13] = y;
Mat->v[14] = z;
Mat->v[15] = 1.f;
}
/* Forms the matrix to rotate around the X-axis some angle (RADIANS)*/
void SST_Math_Mat44fCreateEulerX(SST_Mat44f* Mat, const float angle)
{
Mat->v[0] = 1.f;
Mat->v[1] = 0.f;
Mat->v[2] = 0.f;
Mat->v[3] = 0.f;
Mat->v[4] = 0.f;
Mat->v[5] = cosf(angle);
Mat->v[6] = sinf(angle);
Mat->v[7] = 0.f;
Mat->v[8] = 0.f;
Mat->v[9] = -sinf(angle);
Mat->v[10] = cosf(angle);
Mat->v[11] = 0.f;
Mat->v[12] = 0.f;
Mat->v[13] = 0.f;
Mat->v[14] = 0.f;
Mat->v[15] = 1.f;
}
/* Forms the matrix to rotate around the X-axis some angle(RADIANS)
* and applies it to the given Mat */
void SST_Math_Mat44fCreateEulerY(SST_Mat44f* Mat, const float angle)
{
Mat->v[0] = cosf(angle);
Mat->v[1] = 0.f;
Mat->v[2] = -sinf(angle);
Mat->v[3] = 0.f;
Mat->v[4] = 0.f;
Mat->v[5] = 1.f;
Mat->v[6] = 0.f;
Mat->v[7] = 0.f;
Mat->v[8] = sinf(angle);
Mat->v[9] = 0.f;
Mat->v[10] = cosf(angle);
Mat->v[11] = 0.f;
Mat->v[12] = 0.f;
Mat->v[13] = 0.f;
Mat->v[14] = 0.f;
Mat->v[15] = 1.f;
}
void SST_Math_Mat33fRAT(SST_Mat33f* _mat,const SST_Vec2f* _about, const SST_Vec2f* _translation, const float angle)
{
const float cosa = cosf(angle);
const float sina = sinf(angle);
_mat->v[0] = cosa;
_mat->v[1] = sina;
_mat->v[2] = 0.f;
_mat->v[3] = -sina;
_mat->v[4] = cosa;
_mat->v[5] = 0.f;
_mat->v[6] = -cosa * _about->v[0] + sina * _about->v[1] + _about->v[0] + _translation->v[0];
_mat->v[6] = -sina * _about->v[0] + -cosa * _about->v[1] + _about->v[1] + _translation->v[1];
_mat->v[8] = 1.f;
}
void SST_Math_Mat33fCreateEulerZ(SST_Mat33f* Mat, const float angle)
{
const float cosa = cosf(angle);
const float sina = sinf(angle);
Mat->v[0] = cosa;
Mat->v[1] = sina;
Mat->v[2] = 0.f;
Mat->v[3] = -sina;
Mat->v[4] = cosa;
Mat->v[5] = 0.f;
Mat->v[6] = 0.f;
Mat->v[7] = 0.f;
Mat->v[8] = 1.f;
}
void SST_Math_Mat44fCreateEulerZ(SST_Mat44f* Mat, const float angle)
{
const float cosa = cosf(angle);
const float sina = sinf(angle);
Mat->v[0] = cosa;
Mat->v[1] = sina;
Mat->v[2] = 0.f;
Mat->v[3] = 0.f;
Mat->v[4] = -sina;
Mat->v[5] = cosa;
Mat->v[6] = 0.f;
Mat->v[7] = 0.f;
Mat->v[8] = 0.f;
Mat->v[9] = 0.f;
Mat->v[10] = 1.f;
Mat->v[11] = 0.f;
Mat->v[12] = 0.f;
Mat->v[13] = 0.f;
Mat->v[14] = 0.f;
Mat->v[15] = 1.f;
}
/* The chained forms are designed so that they can be chained if desired, e.g.:
float Original[16];
Original = SST_Math_Mat33fCreateEulerZ(-30, SST_Math_Mat33fCreateEulerX(-45, SST_Math_Mat33fCreateEulerY(30, Original)));
--Charles
*/
/* Forms the matrix to rotate around the X-axis some angle (RADIANS)*/
/* Much faster to do this than Create and then multiply */
void SST_Math_Mat44fCreateEulerXC(SST_Mat44f* Mat, const float angle)
{
#define Mat(i,j) Mat->v[i+4*j]
float cosv = cosf(angle);
float sinv = sinf(angle);
float nsinv = -sinv;
float t[8];
int i;
t[0] = nsinv*Mat(2,0);
t[1] = nsinv*Mat(2,1);
t[2] = nsinv*Mat(2,2);
t[3] = nsinv*Mat(2,3);
t[4] = sinv*Mat(1,0);
t[5] = sinv*Mat(1,1);
t[6] = sinv*Mat(1,2);
t[7] = sinv*Mat(1,3);
//If this fails, it's probably the next two lines,and the offset of t[];
for(i=0;i<4;i++)
Mat(1,i)= cosv*Mat(1,i)+t[i];
for(i=0;i<4;i++)
Mat(2,i)= cosv*Mat(2,i)+t[i+4];
#undef Mat
}
/* Forms the matrix to rotate around the X-axis some angle(RADIANS)
* and applies it to the given Mat */
void SST_Math_Mat44fCreateEulerYC(SST_Mat44f* Mat, const float angle)
{
#define Mat(i,j) Mat->v[i+4*j]
float cosv = cosf(angle);
float sinv = sinf(angle);
float t[8];
int i;
t[0] = sinv*Mat(2,0);
t[1] = sinv*Mat(2,1);
t[2] = sinv*Mat(2,2);
t[3] = sinv*Mat(2,3);
t[4] = -sinv*Mat(0,0);
t[5] = -sinv*Mat(0,1);
t[6] = -sinv*Mat(0,2);
t[7] = -sinv*Mat(0,3);
for(i=0;i<4;i++)
Mat(0,i) = cosv*Mat(0,i)+t[i];
for(i=0;i<4;i++)
Mat(2,i) = cosv*Mat(2,i)+t[i+4];
#undef Mat
}
void SST_Math_Mat44fCreateEulerZC(SST_Mat44f* Mat, const float angle)
{
#define Mat(i,j) Mat->v[i+4*j]
float cosv = cosf(angle);
float sinv = sinf(angle);
float t[8];
int i;
t[0] = -sinv*Mat(1,0);
t[1] = -sinv*Mat(1,1);
t[2] = -sinv*Mat(1,2);
t[3] = -sinv*Mat(1,3);
t[4] = sinv*Mat(0,0);
t[5] = sinv*Mat(0,1);
t[6] = sinv*Mat(0,2);
t[7] = sinv*Mat(0,3);
for(i=0;i<4;i++)
Mat(0,i) = cosv*Mat(0,i)+t[i];
for(i=0;i<4;i++)
Mat(1,i) = cosv*Mat(1,i)+t[i+4];
#undef Mat
}
/* Forms the Shear Mat described as:
* X' = M * X
* where M = I + ShearX + ShearY + ShearZ
* shearing[0:1] -- x in y, x in z
* shearing[2:3] -- y in x, y in z
* shearing[4:5] -- z in x, z in y
*/
void SST_Math_Mat44fCreateShear(SST_Mat44f* Mat, const float x_in_y, const float x_in_z, const float y_in_x, const float y_in_z, const float z_in_x, const float z_in_y)
{
#define Mat(i,j) Mat->v[i+4*j]
Mat(0,0) = 1.0;
Mat(1,0) = y_in_x;
Mat(2,0) = z_in_x;
Mat(3,0) = 0.0;
Mat(0,1) = x_in_y;
Mat(1,1) = 1.0;
Mat(2,1) = z_in_y;
Mat(3,1) = 0.0;
Mat(0,2) = x_in_z;
Mat(1,2) = y_in_z;
Mat(2,2) = 1.0;
Mat(3,2) = 0.0;
Mat(0,3) = 0.0;
Mat(1,3) = 0.0;
Mat(2,3) = 0.0;
Mat(3,3) = 1.0;
#undef Mat
}
/* Forms the Free Transform Mat described as:
* X' = M * X
* where M = ScaleM + Shear_forX + Shear_forY + Shear_forZ
* scale[0:2] -- scale x,y,z
* shearing[0:1] -- x in y, x in z
* shearing[2:3] -- y in x, y in z
* shearing[4:5] -- z in x, z in y
*/
void SST_Math_Mat44fCreateFreeTransform(SST_Mat44f* Mat,const float scale_x, const float scale_y, const float scale_z, const float x_in_y, const float x_in_z, const float y_in_x, const float y_in_z, const float z_in_x, const float z_in_y)
{
#define Mat(i,j) Mat->v[i+4*j]
Mat(0,0) = scale_x;
Mat(1,0) = y_in_x;
Mat(2,0) = z_in_x;
Mat(3,0) = 0.0;
Mat(0,1) = x_in_y;
Mat(1,1) = scale_y;
Mat(2,1) = z_in_y;
Mat(3,1) = 0.0;
Mat(0,2) = x_in_z;
Mat(1,2) = y_in_z;
Mat(2,2) = scale_z;
Mat(3,2) = 0.0;
Mat(0,3) = 0.0;
Mat(1,3) = 0.0;
Mat(2,3) = 0.0;
Mat(3,3) = 1.0;
#undef Mat
}
void SST_Math_Mat44fConvertQuaternion(SST_Mat44f* Mat,const SST_Vec4f* q)
{
/* From Wikipedia - 8/30/2012 */
#define Mat(i,j) Mat->v[i+4*j]
Mat(0,0) = q->x;
Mat(1,0) = -q->y;
Mat(2,0) = -q->z;
Mat(3,0) = -q->w;
Mat(0,1) = q->y;
Mat(1,1) = q->x;
Mat(2,1) = q->w;
Mat(3,1) = -q->z;
Mat(0,2) = q->z;
Mat(1,2) = -q->w;
Mat(2,2) = q->x;
Mat(3,2) = q->y;
Mat(0,3) = q->w;
Mat(1,3) = q->z;
Mat(2,3) = -q->y;
Mat(3,3) = q->x;
#undef Mat
}
void SST_Math_Mat44fCreateRotationFromQuaternion(SST_Mat44f* Mat,const SST_Vec4f* q)
{
/* From Wikipedia - 8/30/2012 */
#define Mat(i,j) Mat->v[i+4*j]
const float norm = sqrtf(q->x * q->x + q->y * q->y + q->z * q->z + q->w * q->w);
const float a = q->x / norm;
const float b = q->y / norm;
const float c = q->z / norm;
const float d = q->w / norm;
Mat(0,0) = a*a + b*b - c*c - d*d;
Mat(1,0) = 2.f * (b*c+a*d);
Mat(2,0) = 2.f * (b*d-a*c);
Mat(3,0) = 0.f;
Mat(0,1) = 2.f * (b*c-a*d);
Mat(1,1) = a*a - b*b + c*c - d*d;
Mat(2,1) = 2.f * (c*d-a*b);
Mat(3,1) = 0.f;
Mat(0,2) = 2.f * (b*d + a*c);
Mat(1,2) = 2.f * (c*d - a*b);
Mat(2,2) = a*a-b*b-c*c+d*d;
Mat(3,2) = 0.f;
Mat(0,3) = 0.f;
Mat(1,3) = 0.f;
Mat(2,3) = 0.f;
Mat(3,3) = 1.f;
#undef Mat
}
void SST_Math_Mat44fCreateScale(SST_Mat44f* Mat, const float scale_x, const float scale_y, const float scale_z)
{
memset(Mat, 0, sizeof(SST_Mat44f));
Mat->v[0] = scale_x;
Mat->v[5] = scale_y;
Mat->v[10] = scale_z;
Mat->v[15] = 1;
}
void SST_Math_Mat44fCreateLookAt(SST_Mat44f* Mat, const SST_Vec3f* eye,const SST_Vec3f* at, const SST_Vec3f* up)
{
#define Mat(i,j) Mat->v[i+4*j]
int i;
SST_Vec3f F;
SST_Vec3f UP;
SST_Vec3f s;
SST_Vec3f u;
SST_Mat44f tmp;
SST_Math_Vec3fNormalize(up,&UP);
SST_Math_Vec3fSubtract(at,eye,&F);
SST_Math_Vec3fNormalizeLocal(&F);
SST_Math_Vec3fCross(&F,&UP,&s);
SST_Math_Vec3fCross(&s,&F,&u);
for(i=0;i<3;i++)
{
Mat(0,i) = s.v[i];
Mat(1,i) = u.v[i];
Mat(2,i) = -F.v[i];
Mat(3,i) = 0.0f;
}
Mat(0,3) = 0;
Mat(1,3) = 0;
Mat(2,3) = 0;
Mat(3,3) = 1.f;
/* Multiply by translation matrix */
SST_Math_Mat44fCreateTranslation(&tmp, -eye->x, -eye->y, -eye->z);
SST_Math_Mat44fMultiplyMatrixLocal(Mat, &tmp);
#undef Mat
}
void SST_Math_Mat44fCreateLookDir(SST_Mat44f* Mat, const SST_Vec3f* eye,const SST_Vec3f* dir, const SST_Vec3f* up)
{
#define Mat(i,j) Mat->v[i+4*j]
int i;
SST_Vec3f F;
SST_Vec3f UP;
SST_Vec3f s;
SST_Vec3f u;
SST_Vec3f at;
SST_Mat44f tmp;
SST_Math_Vec3fAdd(eye,dir,&at);
SST_Math_Vec3fNormalize(up,&UP);
SST_Math_Vec3fSubtract(&at,eye,&F);
SST_Math_Vec3fNormalizeLocal(&F);
SST_Math_Vec3fCross(&F,&UP,&s);
SST_Math_Vec3fCross(&s,&F,&u);
for(i=0;i<3;i++)
{
Mat(0,i) = s.v[i];
Mat(1,i) = u.v[i];
Mat(2,i) = -F.v[i];
Mat(3,i) = 0.0f;
}
Mat(0,3) = 0;
Mat(1,3) = 0;
Mat(2,3) = 0;
Mat(3,3) = 1.f;
/* Multiply by translation matrix */
SST_Math_Mat44fCreateTranslation(&tmp, -eye->x, -eye->y, -eye->z);
SST_Math_Mat44fMultiplyMatrixLocal(Mat, &tmp);
#undef Mat
}
/* Extracts the 3x3 matrix most used from Geo routines from a 4x4 matrix */
void SST_Math_Mat33fCreateFromMat44f(const SST_Mat44f* Mat44, SST_Mat33f* Mat33)
{
#define Mat33(i,j) Mat33->v[i+3*j]
#define Mat44(i,j) Mat44->v[i+4*j]
int i;
for(i=0;i<3;i++)
{
Mat33(i,0) = Mat44(i,0);
Mat33(i,1) = Mat44(i,1);
Mat33(i,2) = Mat44(i,2);
}
#undef Mat33
#undef Mat44
}
void SST_Math_Mat44fCreatePerspective(SST_Mat44f* Mat, const float FOV_in_y, const float aspect_in_x, const float zNear, const float zFar)
{
#define Mat(i,j) Mat->v[i + 4*j]
const float f = tanf(((float)M_PI_2) - FOV_in_y * ((float)M_PI) / 360.f);
memset(Mat, 0, sizeof(SST_Mat44f));
Mat(0,0) = f/aspect_in_x;
Mat(1,1) = f;
Mat(2,2) = (zFar+zNear)/(zNear-zFar);
Mat(3,2) = -1.f;
Mat(2,3) = (2.f * zFar * zNear)/(zNear-zFar);
#undef Mat
}
void SST_Math_Mat44fCreateOrtho(SST_Mat44f* Mat, const float left, const float right, const float bottom, const float top, const float nearVal, const float farVal)
{
#define Mat(i,j) Mat->v[i + 4*j]
const float rl = right - left;
const float tb = top - bottom;
const float fn = farVal - nearVal;
memset(Mat, 0, sizeof(SST_Mat44f));
Mat(0,0) = 2.f/rl;
Mat(1,1) = 2.f/tb;
Mat(2,2) = -2.f/fn;
Mat(0,3) = -(right+left)/rl;
Mat(1,3) = -(top+bottom)/tb;
Mat(2,3) = -(farVal+nearVal)/fn;
Mat(3,3) = 1.f;
#undef Mat
}
void SST_Math_Mat33fHomogenousMultiplyAssumed(const SST_Mat33f* Mat,const SST_Vec2f* VecIn, SST_Vec2f* VecOut)
{
#define Mat(i,j) Mat->v[i + 3*j]
#define VI(i) VecIn->v[i]
float recipz;
recipz = 1.f / (Mat(2,0)*VI(0) + Mat(2,1) * VI(1) + Mat(2,2));
VecOut->v[0] = recipz*(Mat(0,0)*VI(0) + Mat(0,1) * VI(1) + Mat(0,2));
VecOut->v[1] = recipz*(Mat(1,0)*VI(0) + Mat(1,1) * VI(1) + Mat(1,2));
#undef Mat
#undef VI
}
void SST_Math_Mat33fHomogenousMultiplyAssumedLocal(const SST_Mat33f* Mat, SST_Vec2f* Vec)
{
#define Mat(i,j) Mat->v[i + 3*j]
#define VI(i) Vec->v[i]
float tmp,tmp2,recipz;
tmp = Mat(0,0)*VI(0) + Mat(0,1) * VI(1) + Mat(0,2);
tmp2 = Mat(1,0)*VI(0) + Mat(1,1) * VI(1) + Mat(1,2);
recipz = 1.f / (Mat(2,0)*VI(0) + Mat(2,1) * VI(1) + Mat(2,2));
VI(0) = tmp * recipz;
VI(1) = tmp2 * recipz;
#undef Mat
#undef VI
}
Reference in New Issue View Git Blame Copy Permalink