shithub: tinygl

ref: 9c620f78c79a668824dfd967598bcfc58a249e4e
dir: /Raw_Demos/include/3dMath.h/

View raw version
/* Public Domain / CC0 C99 Vector Math Library

*/

#ifndef CHAD_MATH_H
#define CHAD_MATH_H
//#define CHAD_MATH_NO_ALIGN
#ifndef CHAD_MATH_NO_ALIGN
#include <stdalign.h>
#define CHAD_ALIGN alignas(16)
#else
#define CHAD_ALIGN /*a comment*/
#endif
#include <math.h>
#include <string.h>
typedef float f_;
typedef unsigned int uint;
#define MAX(x,y) (x>y?x:y)
#define MIN(x,y) (x<y?x:y)
typedef struct {CHAD_ALIGN f_ d[3];} vec3;
typedef struct {CHAD_ALIGN int d[3];} ivec3;
typedef struct {CHAD_ALIGN f_ d[4];} vec4;
typedef struct {CHAD_ALIGN f_ d[16];} mat4;

//Collision detection
//These Algorithms return the penetration vector into
//the shape in the first argument
//With depth of penetration in element 4
//if depth of penetration is zero or lower then there is no penetration.
typedef struct{
	vec4 c;
	vec3 e;
}aabb;
typedef aabb colshape; //c.d[3] determines if it's a sphere or box. 0 or less = box, greater than 0 = sphere




static inline vec4 getrow( mat4 a,  uint index){
	return (vec4){
		.d[0]=a.d[0*4+index],
		.d[1]=a.d[1*4+index],
		.d[2]=a.d[2*4+index],
		.d[3]=a.d[3*4+index]
	};
}
static inline mat4 swapRowColumnMajor( mat4 in){
	mat4 result;
	vec4 t;
	int i = 0;
	t = getrow(in,i);
	memcpy(result.d+i*4, t.d, 4*4);i++;
	t = getrow(in,i);
	memcpy(result.d+i*4, t.d, 4*4);i++;
	t = getrow(in,i);
	memcpy(result.d+i*4, t.d, 4*4);i++;
	t = getrow(in,i);
	memcpy(result.d+i*4, t.d, 4*4);
	return result;
}

static inline vec4 getcol( mat4 a,  uint index){
	return (vec4){
		.d[0]=a.d[index*4+0],
		.d[1]=a.d[index*4+1],
		.d[2]=a.d[index*4+2],
		.d[3]=a.d[index*4+3]
	};
}
static inline mat4 scalemat4( vec4 s){
	mat4 ret;
	for(int i = 1; i < 16; i++)
		ret.d[i]= 0.0;
	ret.d[0*4 + 0] = s.d[0]; //x scale
	ret.d[1*4 + 1] = s.d[1]; //y scale
	ret.d[2*4 + 2] = s.d[2]; //z scale
	ret.d[3*4 + 3] = s.d[3]; //w scale
	return ret;
}

static inline int invmat4( mat4 m, mat4* invOut) //returns 1 if successful
{
    mat4 inv;
    f_ det;
    int i;

    inv.d[0] = m.d[5]  * m.d[10] * m.d[15] - 
             m.d[5]  * m.d[11] * m.d[14] - 
             m.d[9]  * m.d[6]  * m.d[15] + 
             m.d[9]  * m.d[7]  * m.d[14] +
             m.d[13] * m.d[6]  * m.d[11] - 
             m.d[13] * m.d[7]  * m.d[10];

    inv.d[4] = -m.d[4]  * m.d[10] * m.d[15] + 
              m.d[4]  * m.d[11] * m.d[14] + 
              m.d[8]  * m.d[6]  * m.d[15] - 
              m.d[8]  * m.d[7]  * m.d[14] - 
              m.d[12] * m.d[6]  * m.d[11] + 
              m.d[12] * m.d[7]  * m.d[10];

    inv.d[8] = m.d[4]  * m.d[9] * m.d[15] - 
             m.d[4]  * m.d[11] * m.d[13] - 
             m.d[8]  * m.d[5] * m.d[15] + 
             m.d[8]  * m.d[7] * m.d[13] + 
             m.d[12] * m.d[5] * m.d[11] - 
             m.d[12] * m.d[7] * m.d[9];

    inv.d[12] = -m.d[4]  * m.d[9] * m.d[14] + 
               m.d[4]  * m.d[10] * m.d[13] +
               m.d[8]  * m.d[5] * m.d[14] - 
               m.d[8]  * m.d[6] * m.d[13] - 
               m.d[12] * m.d[5] * m.d[10] + 
               m.d[12] * m.d[6] * m.d[9];

    inv.d[1] = -m.d[1]  * m.d[10] * m.d[15] + 
              m.d[1]  * m.d[11] * m.d[14] + 
              m.d[9]  * m.d[2] * m.d[15] - 
              m.d[9]  * m.d[3] * m.d[14] - 
              m.d[13] * m.d[2] * m.d[11] + 
              m.d[13] * m.d[3] * m.d[10];

    inv.d[5] = m.d[0]  * m.d[10] * m.d[15] - 
             m.d[0]  * m.d[11] * m.d[14] - 
             m.d[8]  * m.d[2] * m.d[15] + 
             m.d[8]  * m.d[3] * m.d[14] + 
             m.d[12] * m.d[2] * m.d[11] - 
             m.d[12] * m.d[3] * m.d[10];

    inv.d[9] = -m.d[0]  * m.d[9] * m.d[15] + 
              m.d[0]  * m.d[11] * m.d[13] + 
              m.d[8]  * m.d[1] * m.d[15] - 
              m.d[8]  * m.d[3] * m.d[13] - 
              m.d[12] * m.d[1] * m.d[11] + 
              m.d[12] * m.d[3] * m.d[9];

    inv.d[13] = m.d[0]  * m.d[9] * m.d[14] - 
              m.d[0]  * m.d[10] * m.d[13] - 
              m.d[8]  * m.d[1] * m.d[14] + 
              m.d[8]  * m.d[2] * m.d[13] + 
              m.d[12] * m.d[1] * m.d[10] - 
              m.d[12] * m.d[2] * m.d[9];

    inv.d[2] = m.d[1]  * m.d[6] * m.d[15] - 
             m.d[1]  * m.d[7] * m.d[14] - 
             m.d[5]  * m.d[2] * m.d[15] + 
             m.d[5]  * m.d[3] * m.d[14] + 
             m.d[13] * m.d[2] * m.d[7] - 
             m.d[13] * m.d[3] * m.d[6];

    inv.d[6] = -m.d[0]  * m.d[6] * m.d[15] + 
              m.d[0]  * m.d[7] * m.d[14] + 
              m.d[4]  * m.d[2] * m.d[15] - 
              m.d[4]  * m.d[3] * m.d[14] - 
              m.d[12] * m.d[2] * m.d[7] + 
              m.d[12] * m.d[3] * m.d[6];

    inv.d[10] = m.d[0]  * m.d[5] * m.d[15] - 
              m.d[0]  * m.d[7] * m.d[13] - 
              m.d[4]  * m.d[1] * m.d[15] + 
              m.d[4]  * m.d[3] * m.d[13] + 
              m.d[12] * m.d[1] * m.d[7] - 
              m.d[12] * m.d[3] * m.d[5];

    inv.d[14] = -m.d[0]  * m.d[5] * m.d[14] + 
               m.d[0]  * m.d[6] * m.d[13] + 
               m.d[4]  * m.d[1] * m.d[14] - 
               m.d[4]  * m.d[2] * m.d[13] - 
               m.d[12] * m.d[1] * m.d[6] + 
               m.d[12] * m.d[2] * m.d[5];

    inv.d[3] = -m.d[1] * m.d[6] * m.d[11] + 
              m.d[1] * m.d[7] * m.d[10] + 
              m.d[5] * m.d[2] * m.d[11] - 
              m.d[5] * m.d[3] * m.d[10] - 
              m.d[9] * m.d[2] * m.d[7] + 
              m.d[9] * m.d[3] * m.d[6];

    inv.d[7] = m.d[0] * m.d[6] * m.d[11] - 
             m.d[0] * m.d[7] * m.d[10] - 
             m.d[4] * m.d[2] * m.d[11] + 
             m.d[4] * m.d[3] * m.d[10] + 
             m.d[8] * m.d[2] * m.d[7] - 
             m.d[8] * m.d[3] * m.d[6];

    inv.d[11] = -m.d[0] * m.d[5] * m.d[11] + 
               m.d[0] * m.d[7] * m.d[9] + 
               m.d[4] * m.d[1] * m.d[11] - 
               m.d[4] * m.d[3] * m.d[9] - 
               m.d[8] * m.d[1] * m.d[7] + 
               m.d[8] * m.d[3] * m.d[5];

    inv.d[15] = m.d[0] * m.d[5] * m.d[10] - 
              m.d[0] * m.d[6] * m.d[9] - 
              m.d[4] * m.d[1] * m.d[10] + 
              m.d[4] * m.d[2] * m.d[9] + 
              m.d[8] * m.d[1] * m.d[6] - 
              m.d[8] * m.d[2] * m.d[5];

    det = m.d[0] * inv.d[0] + m.d[1] * inv.d[4] + m.d[2] * inv.d[8] + m.d[3] * inv.d[12];
    if (det == 0)
        return 0;
    det = 1.0 / det;
    for (i = 0; i < 16; i++)
        invOut->d[i] = inv.d[i] * det;
    return 1;
}
static inline mat4 perspective( f_ fov,  f_ aspect,  f_ near,  f_ far){
	mat4 ret;
	f_ D2R = 3.14159265358979323 / 180.0;
	f_ yScale = 1.0/tanf(D2R * fov/2);
	f_ xScale = yScale/aspect;
	f_ nearmfar = near-far;
	
	ret.d[0*4+0] = xScale; 	ret.d[0*4+1]=0; 	ret.d[0*4+2]=0;					ret.d[0*4+3]=0;
	ret.d[1*4+0]=0; 		ret.d[1*4+1]=yScale;ret.d[1*4+2]=0;					ret.d[1*4+3]=0;
	ret.d[2*4+0]=0; 		ret.d[2*4+1]=0;		ret.d[2*4+2]=(far+near)/nearmfar;ret.d[2*4+3]=-1;
	ret.d[3*4+0]=0; 		ret.d[3*4+1]=0;		ret.d[3*4+2]=2*far*near/nearmfar;ret.d[3*4+3]=0;
	
	/*
	ret.d[0*4+0] = xScale; 	ret.d[0*4+1]=0; 	ret.d[0*4+2]=0;						ret.d[0*4+3]=0;
	ret.d[1*4+0]=0; 		ret.d[1*4+1]=yScale;ret.d[1*4+2]=0;						ret.d[1*4+3]=0;
	ret.d[2*4+0]=0; 		ret.d[2*4+1]=0;		ret.d[2*4+2]=(far+near)/nearmfar;	ret.d[2*4+3]=2*far*near/nearmfar;
	ret.d[3*4+0]=0; 		ret.d[3*4+1]=0;		ret.d[3*4+2]=-1;					ret.d[3*4+3]=0;
	*/
	return ret;
}
static inline vec3 viewport( uint xdim,  uint ydim,  vec3 input){
	input.d[0] += 1;
	input.d[1] += 1;
	input.d[0] *= (f_)xdim / 2.0;
	input.d[1] *= (f_)ydim / 2.0;
	input.d[2] = (input.d[2])/2.0;
	return input;
}
static inline mat4 rotate( vec3 rotation){
	f_ a = rotation.d[0];
	f_ b = rotation.d[1];
	f_ c = rotation.d[2];
	mat4 rm;
	rm.d[0*4 + 0] = cosf(a)*cosf(b);
	rm.d[1*4 + 0] = sinf(a)*cosf(b);
	rm.d[2*4 + 0] = -sinf(b);
	rm.d[0*4 + 1] = cosf(a)*sinf(b)*sinf(c)-sinf(a)*cosf(c);
	rm.d[1*4 + 1] = sinf(a)*sinf(b)*sinf(c)+cosf(a)*cosf(c);
	rm.d[2*4 + 1] = cosf(b)*sinf(c);
	rm.d[0*4 + 2] = cosf(a)*sinf(b)*cosf(c)+sinf(a)*sinf(c);
	rm.d[1*4 + 2] = sinf(a)*sinf(b)*cosf(c)-cosf(a)*sinf(c);
	rm.d[2*4 + 2] = cosf(b)*cosf(c);
	//the other parts
	rm.d[0*4 + 3] = 0;
	rm.d[1*4 + 3] = 0;
	rm.d[2*4 + 3] = 0;
	rm.d[3*4 + 3] = 1; //the bottom right corner of the matrix.
	rm.d[3*4 + 0] = 0;
	rm.d[3*4 + 1] = 0;
	rm.d[3*4 + 2] = 0;
	return rm;
}


static inline f_ clampf( f_ a,  f_ min,  f_ max){
	if(a<min) return min;
	if(a>max) return max;
	return a;
}
static inline f_ lengthv3( vec3 a){
	return sqrtf(a.d[0] * a.d[0] + a.d[1] * a.d[1] + a.d[2] * a.d[2]);
}
static inline f_ lengthv4( vec4 a){
	return sqrtf(a.d[0] * a.d[0] + a.d[1] * a.d[1] + a.d[2] * a.d[2] + a.d[3] * a.d[3]);
}
static inline vec3 multvec3( vec3 a,  vec3 b){
	return (vec3){
		.d[0]=a.d[0]*b.d[0],
		.d[1]=a.d[1]*b.d[1],
		.d[2]=a.d[2]*b.d[2]
	};
}
static inline vec4 multvec4( vec4 a,  vec4 b){
	return (vec4){
		.d[0]=a.d[0]*b.d[0],
		.d[1]=a.d[1]*b.d[1],
		.d[2]=a.d[2]*b.d[2],
		.d[3]=a.d[3]*b.d[3]
	};
}
static inline vec3 clampvec3( vec3 a,  vec3 min,  vec3 max){
	vec3 ret;
	ret.d[0] = clampf(a.d[0],min.d[0],max.d[0]);
	ret.d[1] = clampf(a.d[1],min.d[1],max.d[1]);
	ret.d[2] = clampf(a.d[2],min.d[2],max.d[2]);
	return ret;
}
static inline vec4 clampvec4( vec4 a,  vec4 min,  vec4 max){
	vec4 ret;
	ret.d[0] = clampf(a.d[0],min.d[0],max.d[0]);
	ret.d[1] = clampf(a.d[1],min.d[1],max.d[1]);
	ret.d[2] = clampf(a.d[2],min.d[2],max.d[2]);
	ret.d[3] = clampf(a.d[3],min.d[3],max.d[3]);
	return ret;
}
static inline f_ dotv3( vec3 a,  vec3 b){
	return a.d[0] * b.d[0] + a.d[1] * b.d[1] + a.d[2] * b.d[2]; 
}
static inline f_ dotv4( vec4 a,  vec4 b){
	return a.d[0] * b.d[0] + a.d[1] * b.d[1] + a.d[2] * b.d[2] + a.d[3] * b.d[3]; 
}
static inline mat4 multm4( mat4 a,  mat4 b){
	mat4 ret;
	for(int i = 0; i < 4; i++)
	for(int j = 0; j < 4; j++)
		ret.d[i*4 + j] = dotv4(
			getrow(a, j),
			getcol(b, i)
		);
	return ret;
}
static inline vec4 mat4xvec4( mat4 t,  vec4 v){
	uint i = 0;
	vec4 vr;
	vr.d[0] = 	t.d[0*4+i] * v.d[0] + 
				t.d[1*4+i] * v.d[1] +
				t.d[2*4+i] * v.d[2] +
				t.d[3*4+i] * v.d[3];
	i++;
	vr.d[1] = 	t.d[0*4+i] * v.d[0] +
				t.d[1*4+i] * v.d[1] + 
				t.d[2*4+i] * v.d[2] + 
				t.d[3*4+i] * v.d[3];
	i++;
	vr.d[2] = 	t.d[0*4+i] * v.d[0] + 
				t.d[1*4+i] * v.d[1] + 
				t.d[2*4+i] * v.d[2] + 
				t.d[3*4+i] * v.d[3];
	i++;
	vr.d[3] = 	t.d[0*4+i] * v.d[0] + 
				t.d[1*4+i] * v.d[1] + 
				t.d[2*4+i] * v.d[2] + 
				t.d[3*4+i] * v.d[3];
	return vr;
}
static inline vec3 crossv3( vec3 a,  vec3 b){
	vec3 retval;
	retval.d[0] = a.d[1] * b.d[2] - a.d[2] * b.d[1];
	retval.d[1] = a.d[2] * b.d[0] - a.d[0] * b.d[2];
	retval.d[2] = a.d[0] * b.d[1] - a.d[1] * b.d[0];
	return retval;
}
static inline vec3 scalev3( f_ s,  vec3 i){i.d[0] *= s; i.d[1] *= s; i.d[2] *= s; return i;}

static inline vec4 scalev4( f_ s,  vec4 i){i.d[0] *= s; i.d[1] *= s; i.d[2] *= s;i.d[3] *= s; return i;}

static inline vec3 normalizev3( vec3 a){
  	if(lengthv3(a)==0) return (vec3){.d[0]=0.0,.d[1]=0.0,.d[2]=1.0};
	return scalev3(1.0/lengthv3(a), a);
}
static inline vec4 normalizev4( vec4 a){
  	if(lengthv4(a)==0) return (vec4){.d[0]=0.0,.d[1]=0.0,.d[2]=1.0,.d[3]=0.0};
	return scalev4(1.0/lengthv4(a), a);
}
static inline vec3 addv3( vec3 aa,  vec3 b){
	vec3 a = aa;
	a.d[0] += b.d[0]; a.d[1] += b.d[1]; a.d[2] += b.d[2]; return a;
}
static inline vec3 rotatev3( vec3 in,  vec3 axis,  f_ ang){
	vec3 t1 = scalev3(cosf(ang),in);
	vec3 t2 = scalev3(sinf(ang),crossv3(axis,in));
	vec3 t3 = scalev3((1-cosf(ang))*dotv3(axis,in),axis);
	return addv3(t1,addv3(t2,t3));
}
static inline vec4 addv4( vec4 aa,  vec4 b){
	vec4 a = aa;
	a.d[0] += b.d[0]; a.d[1] += b.d[1]; a.d[2] += b.d[2]; a.d[3] += b.d[3]; return a;
}
static inline vec3 subv3( vec3 a,  vec3 b){
	return addv3(a,scalev3(-1,b));
}
static inline mat4 identitymat4(){
	return scalemat4(
		(vec4){.d[0]=1.0,.d[1]=1.0,.d[2]=1.0,.d[3]=1.0}
	);
}
static inline mat4 translate( vec3 t){
	mat4 tm = identitymat4();
	tm.d[3*4+0] = t.d[0];
	tm.d[3*4+1] = t.d[1];
	tm.d[3*4+2] = t.d[2];
	return tm;
}
static inline vec4 subv4( vec4 a,  vec4 b){
	return addv4(a,scalev4(-1,b));
}
static inline vec3 reflect( vec3 in,  vec3 norm){
	return 
	addv3(in, //I +
		scalev3(-2.0*dotv3(norm, in), //-2.0 * dotv3(norm,in) * 
			norm //N
		)
	);
}
static inline vec4 upv3( vec3 in,  f_ w){
	return (vec4){
		.d[0]=in.d[0],
		.d[1]=in.d[1],
		.d[2]=in.d[2],
		.d[3]=w
	};
}
static inline vec3 downv4( vec4 in){
	return (vec3){
		.d[0]=in.d[0],
		.d[1]=in.d[1],
		.d[2]=in.d[2]
	};
}
static inline mat4 lookAt( vec3 eye,  vec3 at,  vec3 up){
	mat4 cw = identitymat4();
	vec3 zaxis = normalizev3(subv3(at,eye));
	vec3 xaxis = normalizev3(crossv3(zaxis,up));
	vec3 yaxis = crossv3(xaxis, zaxis);
	zaxis = scalev3(-1,zaxis);
	cw.d[0*4+0] = xaxis.d[0];
	cw.d[1*4+0] = xaxis.d[1];
	cw.d[2*4+0] = xaxis.d[2];
	cw.d[3*4+0] = -dotv3(xaxis,eye);

	cw.d[0*4+1] = yaxis.d[0];
	cw.d[1*4+1] = yaxis.d[1];
	cw.d[2*4+1] = yaxis.d[2];
	cw.d[3*4+1] = -dotv3(yaxis,eye);

	cw.d[0*4+2] = zaxis.d[0];
	cw.d[1*4+2] = zaxis.d[1];
	cw.d[2*4+2] = zaxis.d[2];
	cw.d[3*4+2] = -dotv3(zaxis,eye);
	cw.d[0*4+3] = 0;
	cw.d[1*4+3] = 0;
	cw.d[2*4+3] = 0;
	cw.d[3*4+3] = 1;
	return cw;
}

//Collision detection
//These Algorithms return the penetration vector into
//the shape in the first argument
//With depth of penetration in element 4
//if depth of penetration is zero or lower then there is no penetration.
static inline vec4 spherevsphere( vec4 s1,  vec4 s2){ //x,y,z,radius
	vec4 ret;
	vec3 diff = subv3(
				downv4(s2),
				downv4(s1)
			);
	float lv3 = lengthv3(diff);
	float l = (s1.d[3] + s2.d[3]-lv3);
	
	if(l < 0 || lv3 == 0) {
		ret.d[3] = 0;return ret;
	}
	ret = upv3(
		scalev3(
			l/lv3,diff
		)
		,l
	);
	return ret;
}
static inline vec4 boxvbox( aabb b1,  aabb b2){ //Just points along the minimum separating axis, Nothing fancy.
	vec4 ret = (vec4){
		.d[0]=0,
		.d[1]=0,
		.d[2]=0,
		.d[3]=0
	};
	vec3 sumextents = addv3(b1.e,b2.e);
	vec3 b1c = downv4(b1.c);
	vec3 b2c = downv4(b2.c);

	vec3 b1min = subv3(b1c,b1.e);
	vec3 b2min = subv3(b2c,b2.e);

	vec3 b1max = addv3(b1c,b1.e);
	vec3 b2max = addv3(b2c,b2.e);
	
	if(
		!(
			(fabs(b1c.d[0] - b2c.d[0]) <= sumextents.d[0]) &&
			(fabs(b1c.d[1] - b2c.d[1]) <= sumextents.d[1]) &&
			(fabs(b1c.d[2] - b2c.d[2]) <= sumextents.d[2])
		)
	){
		return ret;
	}
	vec3 axispen[2];
	axispen[0] = subv3(b1max,b2min);
	axispen[1] = subv3(b1min,b2max);
	ret.d[3] = axispen[0].d[0];
	ret.d[0] = axispen[0].d[0];
	for(int i = 1; i < 6; i++){
		if(fabs(axispen[i/3].d[i%3]) < fabs(ret.d[3])){
			ret = (vec4){
						.d[0]=0,
						.d[1]=0,
						.d[2]=0,
						.d[3]=(axispen[i/3].d[i%3])
					};
			ret.d[i%3] = ret.d[3];
			ret.d[3] = fabs(ret.d[3]);
		}
	}
	return ret;
}
static inline vec3 closestpointAABB( aabb b,  vec3 p){
	vec3 b1min = subv3(downv4(b.c),b.e);
	vec3 b1max = addv3(downv4(b.c),b.e);
	return clampvec3(p,b1min,b1max);
}
static inline vec4 spherevaabb( vec4 sph,  aabb box){
	vec4 ret;
	vec3 p = closestpointAABB(box,downv4(sph));
	vec3 v = subv3(p,downv4(sph));
	f_ d2 = dotv3(v,v);
	
	if(d2 <= sph.d[3] * sph.d[3]){
		f_ len = lengthv3(v);
		f_ diff = (sph.d[3] - len);
		if(len > 0){
			f_ factor = diff/len;
			vec3 bruh = scalev3(factor, v);
			ret = upv3(bruh, diff);
			return ret;
		} else {
			aabb virt;
			virt.c = sph;
			virt.e.d[0] = sph.d[3];
			virt.e.d[1] = sph.d[3];
			virt.e.d[2] = sph.d[3];
			return boxvbox(virt,box);
		}
	}
	else
		return (vec4){
			.d[0]=0,
			.d[1]=0,
			.d[2]=0,
			.d[3]=0
		};
		
}
//end of chad math impl

//END Math_Library.h~~~~~~~~~~~~~~~~~~~~

#endif