mat4 cube_map_projectino_rh( float znear, float zfar )
{
SLMATH_VEC_ASSERT( znear > 1e-5f && znear < 1e6f );
SLMATH_VEC_ASSERT( zfar > 1e-5f && zfar < 1e6f );
SLMATH_VEC_ASSERT( zfar > znear );
mat4 m = perspective_fov_rh( radians(90.f), 1.f, znear, zfar );
m[1][1] = -m[1][1];
return m;
}
mat4 cubeMapProjectionRH( float znear, float zfar )
{
SLMATH_VEC_ASSERT( znear > 1e-5f && znear < 1e6f );
SLMATH_VEC_ASSERT( zfar > 1e-5f && zfar < 1e6f );
SLMATH_VEC_ASSERT( zfar > znear );
mat4 m = perspectiveFovRH( radians(90.f), 1.f, znear, zfar );
m[1][1] = -m[1][1];
return m;
}
bool mat4::operator==( const mat4& o ) const
{
SLMATH_VEC_ASSERT( check(*this) );
SLMATH_VEC_ASSERT( check(o) );
register const vec4* const m = v4();
for ( size_t i = 0 ; i < 4 ; ++i )
if ( m[i] != o[i] )
return false;
return true;
}
mat4 cube_map_view_rh( size_t face, const vec3& worldpos )
{
SLMATH_VEC_ASSERT( face < 6 );
SLMATH_VEC_ASSERT( check(worldpos) );
vec3 x,y;
switch ( face )
{
case 0: // X+
x = vec3(0,0,-1);
y = vec3(0,-1,0);
break;
case 1: // X-
x = vec3(0,0,1);
y = vec3(0,-1,0);
break;
case 2: // Y+
x = vec3(1,0,0);
y = vec3(0,0,1);
break;
case 3: // Y-
x = vec3(1,0,0);
y = vec3(0,0,-1);
break;
case 4: // Z+
x = vec3(1,0,0);
y = vec3(0,-1,0);
break;
case 5: // Z-
x = vec3(-1,0,0);
y = vec3(0,-1,0);
break;
default:
x = y = vec3(0.f,0.f,0.f);
SLMATH_VEC_ASSERT( false );
break;
}
mat4 tm;
tm[0] = vec4(x,0);
tm[1] = vec4(y,0);
tm[2] = vec4( normalize( cross(x,y) ), 0.f );
tm[3] = vec4( worldpos, 1.f );
return inverse(tm);
}
mat4 target_at_rh( const vec3& eye, const vec3& at, const vec3& up )
{
SLMATH_VEC_ASSERT( distance(eye,at) > FLT_MIN );
SLMATH_VEC_ASSERT( length(up) > FLT_MIN );
vec3 zaxis = normalize(eye - at);
vec3 xaxis = normalize(cross(up, zaxis));
vec3 yaxis = cross(zaxis, xaxis);
mat4 m;
m[0] = vec4(xaxis,0);
m[1] = vec4(yaxis,0);
m[2] = vec4(zaxis,0);
m[3] = vec4(eye,1);
return m;
}
mat4::mat4( float a, const vec3& v )
{
SLMATH_VEC_ASSERT( length(v) > FLT_MIN );
float s, c;
sincos( a, &s, &c );
const float t = 1.f - c;
const vec3 n = normalize( v );
const float x = n.x;
const float y = n.y;
const float z = n.z;
register vec4* const m = v4();
m[0][0] = t*x*x + c;
m[0][1] = t*x*y + z*s;
m[0][2] = t*x*z - y*s;
m[0][3] = 0.f;
m[1][0] = t*x*y - z*s;
m[1][1] = t*y*y + c;
m[1][2] = t*y*z + x*s;
m[1][3] = 0.f;
m[2][0] = t*x*z + y*s;
m[2][1] = t*y*z - x*s;
m[2][2] = t*z*z + c;
m[2][3] = 0.f;
m[3][0] = 0.f;
m[3][1] = 0.f;
m[3][2] = 0.f;
m[3][3] = 1.f;
}
/**
* Orthonormal projection setup implementation for LH/RH.
* @param handedness 1.f for RH, -1.f for LH
*/
static inline mat4 ortho( float w, float h, float znear, float zfar, float sign )
{
SLMATH_VEC_ASSERT( w > FLT_MIN );
SLMATH_VEC_ASSERT( h > FLT_MIN );
SLMATH_VEC_ASSERT( znear > FLT_MIN );
SLMATH_VEC_ASSERT( fabsf(znear-zfar) > FLT_MIN );
const float dz = (znear - zfar) * sign;
const float dzi = 1.f/dz;
mat4 m;
m[0] = vec4( 2.f/w, 0, 0, 0 );
m[1] = vec4( 0, 2.f/h, 0, 0 );
m[2] = vec4( 0, 0, dzi, 0 );
m[3] = vec4( 0, 0, znear*dzi * sign, 1.f );
return m;
}
mat4 scaling( float s )
{
SLMATH_VEC_ASSERT( check(s) );
mat4 tm;
tm[0] = vec4( s, 0, 0, 0 );
tm[1] = vec4( 0, s, 0, 0 );
tm[2] = vec4( 0, 0, s, 0 );
tm[3] = vec4( 0, 0, 0, 1.f );
return tm;
}
mat4 frame_from_normal( const vec3& z )
{
SLMATH_VEC_ASSERT( fabsf(length(z)-1.f) < 1e-6f );
vec3 y = vec3( -z.z, z.x, -z.y );
y = normalize( y - z*dot(z,y) );
vec3 x = cross( z, y );
mat4 m;
m[0].xyz() = x; m[0].w = 0.f;
m[1].xyz() = y; m[1].w = 0.f;
m[2].xyz() = z; m[2].w = 0.f;
m[3] = vec4(0,0,0,1.f);
SLMATH_VEC_ASSERT( dot(x,y) < 1e-6f );
SLMATH_VEC_ASSERT( dot(y,z) < 1e-6f );
SLMATH_VEC_ASSERT( dot(x,z) < 1e-6f );
return m;
}
/**
* Perspective projection matrix.
* @param handedness 1.f for RH, -1.f for LH
*/
static mat4 perspectiveFov( float fovy, float aspect, float znear, float zfar, float handedness )
{
SLMATH_VEC_ASSERT( aspect > 0.001f );
SLMATH_VEC_ASSERT( fabsf(zfar-znear) > 1e-6f );
SLMATH_VEC_ASSERT( fovy >= radians(0.1f) );
SLMATH_VEC_ASSERT( fovy <= radians(179.f) );
const float y = cot( fovy * .5f );
const float x = y / aspect;
const float zdist = (znear-zfar)*handedness;
const float zfar_per_zdist = zfar / zdist;
mat4 m;
m[0] = vec4(x, 0, 0, 0 );
m[1] = vec4(0, y, 0, 0 );
m[2] = vec4(0, 0, zfar_per_zdist, -1.f*handedness );
m[3] = vec4(0, 0, znear*zfar_per_zdist*handedness, 0 );
return m;
}
mat4 scaling( const vec3& v )
{
SLMATH_VEC_ASSERT( check(v) );
mat4 tm;
tm[0] = vec4( v.x, 0, 0, 0 );
tm[1] = vec4( 0, v.y, 0, 0 );
tm[2] = vec4( 0, 0, v.z, 0 );
tm[3] = vec4( 0, 0, 0, 1.f );
return tm;
}
mat4 translation( const vec3& t )
{
SLMATH_VEC_ASSERT( check(t) );
mat4 tm;
tm[0] = vec4( 1, 0, 0, 0 );
tm[1] = vec4( 0, 1, 0, 0 );
tm[2] = vec4( 0, 0, 1, 0 );
tm[3] = vec4( t.x, t.y, t.z, 1 );
return tm;
}
mat4 rotation_z( float a )
{
float s, c;
sincos( a, &s, &c );
mat4 m;
m[0] = vec4( c, s, 0, 0 );
m[1] = vec4(-s, c, 0, 0 );
m[2] = vec4( 0, 0, 1, 0 );
m[3] = vec4( 0, 0, 0, 1 );
SLMATH_VEC_ASSERT( det(m) != 0.f );
return m;
}
mat4 mat4::operator*( const mat4& o ) const
{
mat4 res;
#ifdef SLMATH_SIMD
#define VTMP(i,j) SLMATH_MUL_PS( m_m128[i], SLMATH_LOAD_PS1(&o[j][i]) )
m128_t* const o128 = res.m128();
o128[0] = SLMATH_ADD_PS( SLMATH_ADD_PS(VTMP(0,0),VTMP(1,0)), SLMATH_ADD_PS(VTMP(2,0),VTMP(3,0)) );
o128[1] = SLMATH_ADD_PS( SLMATH_ADD_PS(VTMP(0,1),VTMP(1,1)), SLMATH_ADD_PS(VTMP(2,1),VTMP(3,1)) );
o128[2] = SLMATH_ADD_PS( SLMATH_ADD_PS(VTMP(0,2),VTMP(1,2)), SLMATH_ADD_PS(VTMP(2,2),VTMP(3,2)) );
o128[3] = SLMATH_ADD_PS( SLMATH_ADD_PS(VTMP(0,3),VTMP(1,3)), SLMATH_ADD_PS(VTMP(2,3),VTMP(3,3)) );
#undef VTMP
#else
// note: above SIMD-macro version works also on non-SIMD platforms, this is much faster if there is no SIMD support
const vec4* const mp = &get(0);
const vec4* const op = &o.get(0);
vec4* const resp = res.v4();
MAT4_MUL_MAT4( resp, mp, op );
#endif
SLMATH_VEC_ASSERT( check(res) );
return res;
}
mat4::mat4( const quat& q )
{
const float sqw = q.w*q.w;
const float sqx = q.x*q.x;
const float sqy = q.y*q.y;
const float sqz = q.z*q.z;
const float qlen = sqx + sqy + sqz + sqw;
SLMATH_VEC_ASSERT( qlen > FLT_MIN );
const float invs = 1.f / qlen; // only needed if not normalized
register vec4* const m = v4();
m[0][0] = ( sqx - sqy - sqz + sqw)*invs;
m[1][1] = (-sqx + sqy - sqz + sqw)*invs;
m[2][2] = (-sqx - sqy + sqz + sqw)*invs;
register float tmp1 = q.x*q.y;
register float tmp2 = q.z*q.w;
m[0][1] = 2.0f * (tmp1 + tmp2)*invs;
m[1][0] = 2.0f * (tmp1 - tmp2)*invs;
tmp1 = q.x*q.z;
tmp2 = q.y*q.w;
m[0][2] = 2.0f * (tmp1 - tmp2)*invs;
m[2][0] = 2.0f * (tmp1 + tmp2)*invs;
tmp1 = q.y*q.z;
tmp2 = q.x*q.w;
m[1][2] = 2.0f * (tmp1 + tmp2)*invs;
m[2][1] = 2.0f * (tmp1 - tmp2)*invs;
m[0][3] = 0.f;
m[1][3] = 0.f;
m[2][3] = 0.f;
m[3] = vec4(0,0,0,1.f);
}
void vec2::normalize()
{
float len = length( *this );
SLMATH_VEC_ASSERT( len >= FLT_MIN );
*this *= 1.f / len;
}
mat4 inverse( const mat4& m0 )
{
SLMATH_VEC_ASSERT( check(m0) );
const float* const mp = &m0[0][0];
const float a = mp[0]; const float b = mp[1]; const float c = mp[2]; const float d = mp[3];
const float e = mp[4]; const float f = mp[5]; const float g = mp[6]; const float h = mp[7];
const float i = mp[8]; const float j = mp[9]; const float k = mp[10]; const float l = mp[11];
const float m = mp[12]; const float n = mp[13]; const float o = mp[14]; const float p = mp[15];
const float min_a = DET3(f,g,h,j,k,l,n,o,p);
const float min_b = DET3(e,g,h,i,k,l,m,o,p);
const float min_c = DET3(e,f,h,i,j,l,m,n,p);
const float min_d = DET3(e,f,g,i,j,k,m,n,o);
const float det_m = a*min_a - b*min_b + c*min_c - d*min_d;
SLMATH_VEC_ASSERT( det_m > FLT_MIN || det_m < -FLT_MIN ); // invertible?
mat4 res;
res[0][0] = min_a;
res[1][0] = -min_b;
res[2][0] = min_c;
res[3][0] = -min_d;
res[0][1] = -DET3(b,c,d,j,k,l,n,o,p);
res[1][1] = DET3(a,c,d,i,k,l,m,o,p);
res[2][1] = -DET3(a,b,d,i,j,l,m,n,p);
res[3][1] = DET3(a,b,c,i,j,k,m,n,o);
res[0][2] = DET3(b,c,d,f,g,h,n,o,p);
res[1][2] = -DET3(a,c,d,e,g,h,m,o,p);
res[2][2] = DET3(a,b,d,e,f,h,m,n,p);
res[3][2] = -DET3(a,b,c,e,f,g,m,n,o);
res[0][3] = -DET3(b,c,d,f,g,h,j,k,l);
res[1][3] = DET3(a,c,d,e,g,h,i,k,l);
res[2][3] = -DET3(a,b,d,e,f,h,i,j,l);
res[3][3] = DET3(a,b,c,e,f,g,i,j,k);
return res * (1.f/det_m);
/*
res *= (1.f/det_m);
mat4 res2;
res2[0][0] = m0[1][2]*m0[2][3]*m0[3][1] - m0[1][3]*m0[2][2]*m0[3][1] + m0[1][3]*m0[2][1]*m0[3][2] - m0[1][1]*m0[2][3]*m0[3][2] - m0[1][2]*m0[2][1]*m0[3][3] + m0[1][1]*m0[2][2]*m0[3][3];
res2[0][1] = m0[0][3]*m0[2][2]*m0[3][1] - m0[0][2]*m0[2][3]*m0[3][1] - m0[0][3]*m0[2][1]*m0[3][2] + m0[0][1]*m0[2][3]*m0[3][2] + m0[0][2]*m0[2][1]*m0[3][3] - m0[0][1]*m0[2][2]*m0[3][3];
res2[0][2] = m0[0][2]*m0[1][3]*m0[3][1] - m0[0][3]*m0[1][2]*m0[3][1] + m0[0][3]*m0[1][1]*m0[3][2] - m0[0][1]*m0[1][3]*m0[3][2] - m0[0][2]*m0[1][1]*m0[3][3] + m0[0][1]*m0[1][2]*m0[3][3];
res2[0][3] = m0[0][3]*m0[1][2]*m0[2][1] - m0[0][2]*m0[1][3]*m0[2][1] - m0[0][3]*m0[1][1]*m0[2][2] + m0[0][1]*m0[1][3]*m0[2][2] + m0[0][2]*m0[1][1]*m0[2][3] - m0[0][1]*m0[1][2]*m0[2][3];
res2[1][0] = m0[1][3]*m0[2][2]*m0[3][0] - m0[1][2]*m0[2][3]*m0[3][0] - m0[1][3]*m0[2][0]*m0[3][2] + m0[1][0]*m0[2][3]*m0[3][2] + m0[1][2]*m0[2][0]*m0[3][3] - m0[1][0]*m0[2][2]*m0[3][3];
res2[1][1] = m0[0][2]*m0[2][3]*m0[3][0] - m0[0][3]*m0[2][2]*m0[3][0] + m0[0][3]*m0[2][0]*m0[3][2] - m0[0][0]*m0[2][3]*m0[3][2] - m0[0][2]*m0[2][0]*m0[3][3] + m0[0][0]*m0[2][2]*m0[3][3];
res2[1][2] = m0[0][3]*m0[1][2]*m0[3][0] - m0[0][2]*m0[1][3]*m0[3][0] - m0[0][3]*m0[1][0]*m0[3][2] + m0[0][0]*m0[1][3]*m0[3][2] + m0[0][2]*m0[1][0]*m0[3][3] - m0[0][0]*m0[1][2]*m0[3][3];
res2[1][3] = m0[0][2]*m0[1][3]*m0[2][0] - m0[0][3]*m0[1][2]*m0[2][0] + m0[0][3]*m0[1][0]*m0[2][2] - m0[0][0]*m0[1][3]*m0[2][2] - m0[0][2]*m0[1][0]*m0[2][3] + m0[0][0]*m0[1][2]*m0[2][3];
res2[2][0] = m0[1][1]*m0[2][3]*m0[3][0] - m0[1][3]*m0[2][1]*m0[3][0] + m0[1][3]*m0[2][0]*m0[3][1] - m0[1][0]*m0[2][3]*m0[3][1] - m0[1][1]*m0[2][0]*m0[3][3] + m0[1][0]*m0[2][1]*m0[3][3];
res2[2][1] = m0[0][3]*m0[2][1]*m0[3][0] - m0[0][1]*m0[2][3]*m0[3][0] - m0[0][3]*m0[2][0]*m0[3][1] + m0[0][0]*m0[2][3]*m0[3][1] + m0[0][1]*m0[2][0]*m0[3][3] - m0[0][0]*m0[2][1]*m0[3][3];
res2[2][2] = m0[0][1]*m0[1][3]*m0[3][0] - m0[0][3]*m0[1][1]*m0[3][0] + m0[0][3]*m0[1][0]*m0[3][1] - m0[0][0]*m0[1][3]*m0[3][1] - m0[0][1]*m0[1][0]*m0[3][3] + m0[0][0]*m0[1][1]*m0[3][3];
res2[2][3] = m0[0][3]*m0[1][1]*m0[2][0] - m0[0][1]*m0[1][3]*m0[2][0] - m0[0][3]*m0[1][0]*m0[2][1] + m0[0][0]*m0[1][3]*m0[2][1] + m0[0][1]*m0[1][0]*m0[2][3] - m0[0][0]*m0[1][1]*m0[2][3];
res2[3][0] = m0[1][2]*m0[2][1]*m0[3][0] - m0[1][1]*m0[2][2]*m0[3][0] - m0[1][2]*m0[2][0]*m0[3][1] + m0[1][0]*m0[2][2]*m0[3][1] + m0[1][1]*m0[2][0]*m0[3][2] - m0[1][0]*m0[2][1]*m0[3][2];
res2[3][1] = m0[0][1]*m0[2][2]*m0[3][0] - m0[0][2]*m0[2][1]*m0[3][0] + m0[0][2]*m0[2][0]*m0[3][1] - m0[0][0]*m0[2][2]*m0[3][1] - m0[0][1]*m0[2][0]*m0[3][2] + m0[0][0]*m0[2][1]*m0[3][2];
res2[3][2] = m0[0][2]*m0[1][1]*m0[3][0] - m0[0][1]*m0[1][2]*m0[3][0] - m0[0][2]*m0[1][0]*m0[3][1] + m0[0][0]*m0[1][2]*m0[3][1] + m0[0][1]*m0[1][0]*m0[3][2] - m0[0][0]*m0[1][1]*m0[3][2];
res2[3][3] = m0[0][1]*m0[1][2]*m0[2][0] - m0[0][2]*m0[1][1]*m0[2][0] + m0[0][2]*m0[1][0]*m0[2][1] - m0[0][0]*m0[1][2]*m0[2][1] - m0[0][1]*m0[1][0]*m0[2][2] + m0[0][0]*m0[1][1]*m0[2][2];
res2 *= 1.f/det_m;
mat4 dres = res2-res;
float diff = length(dres[0])+length(dres[1])+length(dres[2])+length(dres[3]);
SLMATH_VEC_ASSERT( diff < 1e-6f );
return res;
*/
}
float det( const mat4& m )
{
const float res = DET4( m[0][0],m[0][1],m[0][2],m[0][3], m[1][0],m[1][1],m[1][2],m[1][3], m[2][0],m[2][1],m[2][2],m[2][3], m[3][0],m[3][1],m[3][2],m[3][3] );
SLMATH_VEC_ASSERT( check(res) );
return res;
}
