tinymath.h

#if !defined( TINYMATH_H )

/*
	------------------------------------------------------------------------------
		Licensing information can be found at the end of the file.
	------------------------------------------------------------------------------

	tinymath - v1.1

	Revision history:
	1.0  (12/21/2016) initial release
	1.1  (10/05/2017) vfloat data type added, removed out-dated comments,
	                  added compute_mouse_ray, added more m3 ops, added lookAt


	SUMMARY:
		A professional level implementation of SIMD intrinsics, suitable for creating high
		performance output.

		The __vectorcall convention needs to be used to build on MSVC. Set the flag /Gv to
		setup this convention by default (recommended). This flag will not affect class
		methods, so annotate methods with TM_VCALL as appropriate. Note that when compiling
		and linking to dynamic libs, older libs are probably not aware of __vectorcall and
		will need __cdecl (this is what TM_CDECL is for). Statically linking will bring
		no problems, as __vectorcall will be applied to the statically linked lib.

		This header is not particularly customized for general graphics programming since
		there are no functions implemented here for 4x4 matrices. Personally I never use
		4x4 matrices and instead prefer to represent affine transormations in block form:
		Ax + b, where A is a 3x3 rotation matrix (and possibly scale), and b performs the
		affine translation. A 4x4 matrix would store an additional row of { 0, 0, 0, 1 },
		so in most cases this bottom row is wasted anyways. This is all my own preference
		so feel free to adjust the header and add in 4x4 matrix routines as desired.
*/

#include <stdint.h>
#include <xmmintrin.h>

#if 1
	#include <assert.h>
	#define TM_ASSERT assert
#else
	#define TM_ASSERT( ... )
#endif

#define TM_SHUFFLE( a, b, x, y, z ) _mm_shuffle_ps( a, b, _MM_SHUFFLE( 3, z, y, x ) )
#define TM_CDECL __cdecl
#define TM_VCALL __vectorcall

#ifdef _WIN32

	#define TM_INLINE __forceinline
	#define TM_SELECTANY extern const __declspec( selectany )

#else

	#define TM_INLINE __attribute__((always_inline))
	#define TM_SELECTANY extern const __attribute__((selectany))

#endif

struct v3
{
	TM_INLINE v3( ) { }
	TM_INLINE explicit v3( float x, float y, float z ) { m = _mm_set_ps( 0, z, y, x ); }
	TM_INLINE explicit v3( float a ) { m = _mm_set_ps( 0, a, a, a ); }
	TM_INLINE explicit v3( float *a ) { m = _mm_set_ps( 0, a[ 2 ], a[ 1 ], a[ 0 ] ); }
	TM_INLINE explicit v3( __m128 v ) { m = v; }
	TM_INLINE operator __m128( ) { return m; }
	TM_INLINE operator __m128( ) const { return m; }

	__m128 m;
};

struct vfloat
{
	TM_INLINE vfloat( ) { }
	TM_INLINE explicit vfloat( float a ) { m = _mm_set_ps( 0, a, a, a ); }
	TM_INLINE explicit vfloat( v3& a ) { m = _mm_shuffle_ps( a, a, _MM_SHUFFLE( 0, 0, 0, 0 ) ); }
	TM_INLINE explicit vfloat( __m128 v ) { m = v; }
	TM_INLINE operator __m128( ) { return m; }
	TM_INLINE operator __m128( ) const { return m; }

	float to_float( ) { return _mm_cvtss_f32( m ); }
	float to_float( ) const { return _mm_cvtss_f32( m ); }

	operator float( ) { return to_float( ); }
	operator float( ) const { return to_float( ); }

	__m128 m;
};

TM_INLINE vfloat getx( v3 a ) { return vfloat( TM_SHUFFLE( a, a, 0, 0, 0 ) ); }
TM_INLINE vfloat gety( v3 a ) { return vfloat( TM_SHUFFLE( a, a, 1, 1, 1 ) ); }
TM_INLINE vfloat getz( v3 a ) { return vfloat( TM_SHUFFLE( a, a, 2, 2, 2 ) ); }

TM_INLINE v3 splatx( v3 a ) { return v3( TM_SHUFFLE( a, a, 0, 0, 0 ) ); }
TM_INLINE v3 splaty( v3 a ) { return v3( TM_SHUFFLE( a, a, 1, 1, 1 ) ); }
TM_INLINE v3 splatz( v3 a ) { return v3( TM_SHUFFLE( a, a, 2, 2, 2 ) ); }

struct m3
{
	TM_INLINE v3 operator[]( int i )
	{
		switch ( i )
		{
			case 0: return x;
			case 1: return y;
			case 2: return z;
			default: TM_ASSERT( 0 ); return x;
		}
	}

	v3 x;
	v3 y;
	v3 z;
};

TM_INLINE m3 rows( v3 x, v3 y, v3 z )
{
	m3 m;
	m.x = x;
	m.y = y;
	m.z = z;
	return m;
}

// helpers for static data
struct v3_consti
{
	union { uint32_t i[ 4 ]; __m128 m; };
	TM_INLINE operator v3( ) const { return v3( m ); }
	TM_INLINE operator __m128( ) const { return m; }
};

struct v3_constf
{
	union { float f[ 4 ]; __m128 m; };
	TM_INLINE operator v3( ) const { return v3( m ); }
	TM_INLINE operator __m128( ) const { m; }
};

TM_SELECTANY v3_consti tmMaskSign = { 0x80000000, 0x80000000, 0x80000000, 0x80000000 };
TM_SELECTANY v3_consti tmMaskAllBits = { 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x00000000 };
TM_SELECTANY v3_constf tmMaskBasis = { 0.57735027f, 0.57735027f, 0.57735027f };

// the binary ops
TM_INLINE v3 operator+( v3 a, v3 b ) { return v3( _mm_add_ps( a, b ) ); }
TM_INLINE v3 operator-( v3 a, v3 b ) { return v3( _mm_sub_ps( a, b ) ); }
TM_INLINE v3& operator+=( v3 &a, v3 b ) { a = a + b; return a; }
TM_INLINE v3& operator-=( v3 &a, v3 b ) { a = a - b; return a; }

TM_INLINE vfloat operator+( vfloat a, vfloat b ) { return vfloat( _mm_add_ps( a, b ) ); }
TM_INLINE vfloat operator-( vfloat a, vfloat b ) { return vfloat( _mm_sub_ps( a, b ) ); }
TM_INLINE vfloat operator*( vfloat a, vfloat b ) { return vfloat( _mm_mul_ps( a, b ) ); }
TM_INLINE vfloat operator/( vfloat a, vfloat b ) { return vfloat( _mm_div_ps( a, b ) ); }
TM_INLINE vfloat& operator+=( vfloat &a, vfloat b ) { a = a + b; return a; }
TM_INLINE vfloat& operator-=( vfloat &a, vfloat b ) { a = a - b; return a; }
TM_INLINE vfloat& operator*=( vfloat &a, vfloat b ) { a = a * b; return a; }
TM_INLINE vfloat& operator/=( vfloat &a, vfloat b ) { a = a / b; return a; }

TM_INLINE vfloat operator+( vfloat a, float b ) { return vfloat( _mm_add_ps( a, vfloat( b ) ) ); }
TM_INLINE vfloat operator-( vfloat a, float b ) { return vfloat( _mm_sub_ps( a, vfloat( b ) ) ); }
TM_INLINE vfloat operator*( vfloat a, float b ) { return vfloat( _mm_mul_ps( a, vfloat( b ) ) ); }
TM_INLINE vfloat operator/( vfloat a, float b ) { return vfloat( _mm_div_ps( a, vfloat( b ) ) ); }
TM_INLINE vfloat& operator+=( vfloat &a, float b ) { a = a + b; return a; }
TM_INLINE vfloat& operator-=( vfloat &a, float b ) { a = a - b; return a; }
TM_INLINE vfloat& operator*=( vfloat &a, float b ) { a = a * b; return a; }
TM_INLINE vfloat& operator/=( vfloat &a, float b ) { a = a / b; return a; }

TM_INLINE vfloat operator+( float a, vfloat b ) { return vfloat( _mm_add_ps( vfloat( a ), b ) ); }
TM_INLINE vfloat operator-( float a, vfloat b ) { return vfloat( _mm_sub_ps( vfloat( a ), b ) ); }
TM_INLINE vfloat operator*( float a, vfloat b ) { return vfloat( _mm_mul_ps( vfloat( a ), b ) ); }
TM_INLINE vfloat operator/( float a, vfloat b ) { return vfloat( _mm_div_ps( vfloat( a ), b ) ); }
TM_INLINE float& operator+=( float &a, vfloat b ) { a = a + b; return a; }
TM_INLINE float& operator-=( float &a, vfloat b ) { a = a - b; return a; }
TM_INLINE float& operator*=( float &a, vfloat b ) { a = a * b; return a; }
TM_INLINE float& operator/=( float &a, vfloat b ) { a = a / b; return a; }

// generally comparisons are followed up with a mask(v3) call (or any(v3))
TM_INLINE v3 operator==( v3 a, v3 b ) { return v3( _mm_cmpeq_ps( a, b ) ); }
TM_INLINE v3 operator!=( v3 a, v3 b ) { return v3( _mm_cmpneq_ps( a, b ) ); }
TM_INLINE v3 operator<( v3 a, v3 b ) { return v3( _mm_cmplt_ps( a, b ) ); }
TM_INLINE v3 operator>( v3 a, v3 b ) { return v3( _mm_cmpgt_ps( a, b ) ); }
TM_INLINE v3 operator<=( v3 a, v3 b ) { return v3( _mm_cmple_ps( a, b ) ); }
TM_INLINE v3 operator>=( v3 a, v3 b ) { return v3( _mm_cmpge_ps( a, b ) ); }
TM_INLINE v3 operator-( v3 a ) { return v3( _mm_setzero_ps( ) ) - a; }

TM_INLINE vfloat operator==( vfloat a, vfloat b ) { return vfloat( _mm_cmpeq_ps( a, b ) ); }
TM_INLINE vfloat operator!=( vfloat a, vfloat b ) { return vfloat( _mm_cmpneq_ps( a, b ) ); }
TM_INLINE vfloat operator<( vfloat a, vfloat b ) { return vfloat( _mm_cmplt_ps( a, b ) ); }
TM_INLINE vfloat operator>( vfloat a, vfloat b ) { return vfloat( _mm_cmpgt_ps( a, b ) ); }
TM_INLINE vfloat operator<=( vfloat a, vfloat b ) { return vfloat( _mm_cmple_ps( a, b ) ); }
TM_INLINE vfloat operator>=( vfloat a, vfloat b ) { return vfloat( _mm_cmpge_ps( a, b ) ); }
TM_INLINE vfloat operator-( vfloat a ) { return vfloat( _mm_setzero_ps( ) ) - a; }

TM_INLINE unsigned mask( vfloat a ) { return _mm_movemask_ps( a ) & 7; }
TM_INLINE int any( vfloat a ) { return mask( a ) != 0; }
TM_INLINE int all( vfloat a ) { return mask( a ) == 7; }

TM_INLINE unsigned mask( v3 a ) { return _mm_movemask_ps( a ) & 7; }
TM_INLINE int any( v3 a ) { return mask( a ) != 0; }
TM_INLINE int all( v3 a ) { return mask( a ) == 7; }

TM_INLINE v3 setx( v3 a, float x )
{
	v3 t0 = v3( _mm_set_ss( x ) );
	return v3( _mm_move_ss( a, t0 ) );
}

TM_INLINE v3 sety( v3 a, float y )
{
	v3 t0 = v3( TM_SHUFFLE( a, a, 1, 0, 2 ) );
	v3 t1 = v3( _mm_set_ss( y ) );
	v3 t2 = v3( _mm_move_ss( t0, t1 ) );
	return v3( TM_SHUFFLE( t2, t2, 1, 0, 2 ) );
}

TM_INLINE v3 setz( v3 a, float z )
{
	v3 t0 = v3( TM_SHUFFLE( a, a, 2, 1, 0 ) );
	v3 t1 = v3( _mm_set_ss( z ) );
	v3 t2 = v3( _mm_move_ss( t0, t1 ) );
	return v3( TM_SHUFFLE( t2, t2, 2, 1, 0 ) );
}

TM_INLINE v3 operator*( v3 a, v3 b ) { return v3( _mm_mul_ps( a, b ) ); }
TM_INLINE v3 operator/( v3 a, v3 b ) { return v3( _mm_div_ps( a, b ) ); }
TM_INLINE v3& operator*=( v3& a, v3 b ) { a = a * b; return a; }
TM_INLINE v3& operator/=( v3& a, v3 b ) { a = a / b; return a; }
TM_INLINE v3 operator*( v3 a, vfloat b ) { return v3( _mm_mul_ps( a, b ) ); }
TM_INLINE v3 operator/( v3 a, vfloat b ) { return v3( _mm_div_ps( a, b ) ); }
TM_INLINE v3& operator*=( v3& a, vfloat b ) { a = a * b; return a; }
TM_INLINE v3& operator/=( v3& a, vfloat b ) { a = a / b; return a; }
TM_INLINE v3 operator*( v3 a, float b ) { return v3( _mm_mul_ps( a, vfloat( b ) ) ); }
TM_INLINE v3 operator/( v3 a, float b ) { return v3( _mm_div_ps( a, vfloat( b ) ) ); }
TM_INLINE v3& operator*=( v3& a, float b ) { a = a * b; return a; }
TM_INLINE v3& operator/=( v3& a, float b ) { a = a / b; return a; }

// f must be 16 byte aligned
TM_INLINE v3 load( float* f ) { return v3( _mm_load_ps( f ) ); }
TM_INLINE void store( v3 v, float* f ) { _mm_store_ps( f, v ); }

TM_INLINE vfloat dot( v3 a, v3 b )
{
	v3 t0 = v3( _mm_mul_ps( a, b ) );
	v3 t1 = v3( TM_SHUFFLE( t0, t0, 1, 0, 0 ) );
	v3 t2 = v3( _mm_add_ss( t0, t1 ) );
	v3 t3 = v3( TM_SHUFFLE( t2, t2, 2, 0, 0 ) );
	v3 t4 = v3( _mm_add_ss( t2, t3 ) );
	return vfloat( t4 );
}

TM_INLINE v3 cross( v3 a, v3 b )
{
	v3 t0 = v3( TM_SHUFFLE( a, a, 1, 2, 0 ) );
	v3 t1 = v3( TM_SHUFFLE( b, b, 2, 0, 1 ) );
	v3 t2 = v3( _mm_mul_ps( t0, t1 ) );

	t0 = v3( TM_SHUFFLE( t0, t0, 1, 2, 0 ) );
	t1 = v3( TM_SHUFFLE( t1, t1, 2, 0, 1 ) );
	t0 = v3( _mm_mul_ps( t0, t1 ) );

	return v3( _mm_sub_ps( t2, t0 ) );
}

TM_INLINE vfloat lengthSq( v3 a ) { return dot( a, a ); }
TM_INLINE vfloat sqrt( vfloat a ) {	return vfloat( _mm_sqrt_ps( a ) ); }
TM_INLINE vfloat length( v3 a ) { return sqrt( dot( a, a ) ); }
TM_INLINE v3 abs( v3 a ) { return v3( _mm_andnot_ps( tmMaskSign, a ) ); }
TM_INLINE v3 min( v3 a, v3 b ) { return v3( _mm_min_ps( a, b ) ); }
TM_INLINE v3 max( v3 a, v3 b ) { return v3( _mm_max_ps( a, b ) ); }
TM_INLINE vfloat abs( vfloat a ) { return vfloat( _mm_andnot_ps( tmMaskSign, a ) ); }
TM_INLINE vfloat min( vfloat a, vfloat b ) { return vfloat( _mm_min_ps( a, b ) ); }
TM_INLINE vfloat max( vfloat a, vfloat b ) { return vfloat( _mm_max_ps( a, b ) ); }
TM_INLINE v3 select( v3 a, v3 b, v3 mask ) { return v3( _mm_xor_ps( a, _mm_and_ps( mask, _mm_xor_ps( b, a ) ) ) ); }
TM_INLINE v3 lerp( v3 a, v3 b, vfloat t ) { return a + (b - a) * t; }

// for posterity
// TM_INLINE v3 lerp( v3 a, v3 b, float t ) { return a * (1.0f - t) + b * t; }
// TM_INLINE v3 lerp( v3 a, v3 b, v3 t ) { return a * (1.0f - t) + b * t; }

TM_INLINE vfloat hmin( v3 a )
{
	v3 t0 = v3( TM_SHUFFLE( a, a, 1, 0, 2 ) );
	a = min( a, t0 );
	v3 t1 = v3( TM_SHUFFLE( a, a, 2, 0, 1 ) );
	return vfloat( min( a, t1 ) );
}

TM_INLINE vfloat hmax( v3 a )
{
	v3 t0 = v3( TM_SHUFFLE( a, a, 1, 0, 2 ) );
	a = max( a, t0 );
	v3 t1 = v3( TM_SHUFFLE( a, a, 2, 0, 1 ) );
	return vfloat( max( a, t1 ) );
}

TM_INLINE v3 norm( v3 a )
{ 
	vfloat t0 = dot( a, a );
	vfloat t1 = sqrt( t0 );
	v3 t2 = v3( _mm_div_ps( a, t1 ) );
	return v3( _mm_and_ps( t2, tmMaskAllBits ) );
}

TM_INLINE v3 clamp( v3 a, v3 vmin, v3 vmax )
{
	v3 t0 = v3( _mm_max_ps( vmin, a ) );
	return v3( _mm_min_ps( t0, vmax ) );
}

// sets up a mask of { x ? ~0 : 0, y ? ~0 : 0, z ? ~0 : 0 }
// x, y and z should be 0 or 1
TM_INLINE v3 mask( int x, int y, int z )
{
	v3_consti c;
	unsigned elements[] = { 0x00000000, 0xFFFFFFFF };

	TM_ASSERT( x < 2 && x >= 0 );
	TM_ASSERT( y < 2 && y >= 0 );
	TM_ASSERT( z < 2 && z >= 0 );

	c.i[ 0 ] = elements[ x ];
	c.i[ 1 ] = elements[ y ];
	c.i[ 2 ] = elements[ z ];
	c.i[ 3 ] = elements[ 0 ];

	return c;
}

TM_INLINE m3 m3_from_quat( vfloat x, vfloat y, vfloat z, vfloat w )
{
	vfloat x2 = x + x;
	vfloat y2 = y + y;
	vfloat z2 = z + z;

	vfloat xx = x * x2;
	vfloat xy = x * y2;
	vfloat xz = x * z2;
	vfloat xw = w * x2;
	vfloat yy = y * y2;
	vfloat yz = y * z2;
	vfloat yw = w * y2;
	vfloat zz = z * z2;
	vfloat zw = w * z2;

	vfloat one = vfloat( 1.0f );

	return rows(
		v3( one - yy - zz, xy + zw, xz - yw ),
		v3( xy - zw, one - xx - zz, yz + xw ),
		v3( xz + yw, yz - xw, one - xx - yy )
	);
}

TM_INLINE m3 m3_axis_angle( v3 axis, vfloat angle )
{
	vfloat s = vfloat( sinf( angle * 0.5f ) );
	vfloat c = vfloat( cosf( angle * 0.5f ) );

	vfloat x = getx( axis ) * s;
	vfloat y = gety( axis ) * s;
	vfloat z = getz( axis ) * s;
	vfloat w = c;

	return m3_from_quat( x, y, z, w );
}

TM_INLINE m3 m3_axis_angle( v3 axis, float angle )
{
	return m3_axis_angle( axis, vfloat( angle ) );
}

// Does not preserve 0.0f in w to get rid of extra shuffles.
// Oh well. Anyone have a better idea?
TM_INLINE m3 transpose( m3 a )
{
	v3 t0 = v3( _mm_shuffle_ps( a.x, a.y, _MM_SHUFFLE( 1, 0, 1, 0 ) ) );
	v3 t1 = v3( _mm_shuffle_ps( a.x, a.y, _MM_SHUFFLE( 2, 2, 2, 2 ) ) );
	v3 x =  v3( _mm_shuffle_ps( t0, a.z, _MM_SHUFFLE( 0, 0, 2, 0 ) ) );
	v3 y =  v3( _mm_shuffle_ps( t0, a.z, _MM_SHUFFLE( 0, 1, 3, 1 ) ) );
	v3 z =  v3( _mm_shuffle_ps( t1, a.z, _MM_SHUFFLE( 0, 2, 2, 0 ) ) );

	a.x = x;
	a.y = y;
	a.z = z;

	return a;
}

//  a * b
TM_INLINE v3 mul( m3 a, v3 b )
{
	v3 x = splatx( b );
	v3 y = splaty( b );
	v3 z = splatz( b );
	x = v3( _mm_mul_ps( x, a.x ) );
	y = v3( _mm_mul_ps( y, a.y ) );
	z = v3( _mm_mul_ps( z, a.z ) );
	v3 t0 = v3( _mm_add_ps( x, y ) );
	return v3( _mm_add_ps( t0, z ) );
}

// a^T * b
TM_INLINE v3 mulT( m3 a, v3 b ) { mul( transpose( a ), b ); }

// a * b
TM_INLINE m3 mul( m3 a, m3 b )
{
	v3 x = mul( a, b.x );
	v3 y = mul( a, b.y );
	v3 z = mul( a, b.z );
	return rows( x, y, z );
}

// a^T * b
TM_INLINE m3 mulT( m3 a, m3 b ) { return mul( transpose( a ), b ); }

// http://box2d.org/2014/02/computing-a-basis/
TM_INLINE m3 basis( v3 a )
{
	// Suppose vector a has all equal components and is a unit vector: a = (s, s, s)
	// Then 3*s*s = 1, s = sqrt(1/3) = 0.57735027. This means that at least one component
	// of a unit vector must be greater or equal to 0.57735027.

	v3 negA = -a;
	v3 t0 = v3( TM_SHUFFLE( a, negA, 1, 1, 0 ) );
	v3 b0 = v3( TM_SHUFFLE( t0, t0, 0, 2, 3 ) );
	t0 = v3( TM_SHUFFLE( a, negA, 2, 2, 1 ) );
	v3 b1 = v3( TM_SHUFFLE( t0, t0, 3, 1, 2 ) );

	v3 mask = v3( _mm_cmpge_ps( tmMaskBasis, abs( a ) ) );
	mask = splatx( mask );
	v3 b = select( b0, b1, mask );
	b = v3( norm( b ).m );
	v3 c = cross( a, b );

	return rows( a, b, c );
}

TM_INLINE m3 operator-( m3 a, m3 b )
{
	m3 c;
	c.x = a.x - b.x;
	c.y = a.y - b.y;
	c.z = a.z - b.z;
	return c;
}

TM_INLINE m3 operator+( m3 a, m3 b )
{
	m3 c;
	c.x = a.x + b.x;
	c.y = a.y + b.y;
	c.z = a.z + b.z;
	return c;
}

TM_INLINE m3& operator+=( m3& a, m3 b ) { a = a + b; return a; }
TM_INLINE m3& operator-=( m3& a, m3 b ) { a = a - b; return a; }

TM_INLINE m3 operator*( vfloat a, m3 b )
{
	m3 c;
	c.x = b.x * a;
	c.y = b.y * a;
	c.z = b.z * a;
	return c;
}

TM_INLINE m3 operator*( float a, m3 b )
{
	return vfloat( a ) * b;
}

struct transform
{
	v3 p; // position
	m3 r; // rotation
};

TM_INLINE v3 mul( transform tx, v3 a ) { return mul( tx.r, a ) + tx.p; }
TM_INLINE v3 mulT( transform tx, v3 a ) { return mul( tx.r, a - tx.p ); }

TM_INLINE transform mul( transform a, transform b )
{
	transform c;
	c.p = mul( a.r, b.p ) + a.p;
	c.r = mul( a.r, b.r );
	return c;
}

TM_INLINE transform mulT( transform a, transform b )
{
	transform c;
	c.p = mulT( a.r, b.p - a.p );
	c.r = mulT( a.r, b.r );
	return c;
}

struct halfspace
{
	v3 n;
	vfloat d;
};

TM_INLINE v3 origin( halfspace h ) { return h.n * h.d; }
TM_INLINE vfloat distance( halfspace h, v3 p ) { return dot( h.n, p ) - h.d; }
TM_INLINE v3 projected( halfspace h, v3 p ) { return p - h.n * distance( h, p ); }

TM_INLINE halfspace mul( transform a, halfspace b )
{
	v3 o = origin( b );
	o = mul( a, o );
	v3 normal = mul( a.r, b.n );
	halfspace c;
	c.n = normal;
	c.d = dot( o, normal );
	return c;
}

TM_INLINE halfspace mulT( transform a, halfspace b )
{
	v3 o = origin( b );
	o = mulT( a, o );
	v3 normal = mulT( a.r, b.n );
	halfspace c;
	c.n = normal;
	c.d = dot( o, normal );
	return c;
}

// da and db should be distances to plane, i.e. halfspace::distance
TM_INLINE v3 intersect( v3 a, v3 b, vfloat da, vfloat db )
{
	return a + (b - a) * (da / (da - db));
}

// carefully choose kTol, see: http://www.randygaul.net/2014/11/07/robust-parallel-vector-test/
TM_INLINE int parallel( v3 a, v3 b, float kTol )
{
	vfloat k = length( a ) / length( b );
	v3 bk = b * k;
	if ( all( abs( a - bk ) < v3( kTol ) ) ) return 1;
	return 0;
}

TM_INLINE m3 outer( v3 u, v3 v )
{
	v3 a = v * getx( u );
	v3 b = v * gety( u );
	v3 c = v * getz( u );
	return rows( a, b, c );
}

void lookAt( float* world_to_cam, v3 eye, v3 target, v3 up, float* cam_to_world = 0 )
{
	v3 front = norm( target - eye );
	v3 side = norm( cross( front, up ) );
	v3 top = norm( cross( side, front ) );

	world_to_cam[ 0 ] = getx( side );
	world_to_cam[ 1 ] = getx( top );
	world_to_cam[ 2 ] = -getx( front );
	world_to_cam[ 3 ] = 0;

	world_to_cam[ 4 ] = gety( side );
	world_to_cam[ 5 ] = gety( top );
	world_to_cam[ 6 ] = -gety( front );
	world_to_cam[ 7 ] = 0;

	world_to_cam[ 8 ] = getz( side );
	world_to_cam[ 9 ] = getz( top );
	world_to_cam[ 10 ] = -getz( front );
	world_to_cam[ 11 ] = 0;

	v3 x = v3( world_to_cam[ 0 ], world_to_cam[ 4 ], world_to_cam[ 8 ] );
	v3 y = v3( world_to_cam[ 1 ], world_to_cam[ 5 ], world_to_cam[ 9 ] );
	v3 z = v3( world_to_cam[ 2 ], world_to_cam[ 6 ], world_to_cam[ 10 ] );

	world_to_cam[ 12 ] = -dot( x, eye );
	world_to_cam[ 13 ] = -dot( y, eye );
	world_to_cam[ 14 ] = -dot( z, eye );
	world_to_cam[ 15 ] = 1.0f;

	if ( cam_to_world )
	{
		cam_to_world[ 0 ] = getx( side );
		cam_to_world[ 1 ] = gety( side );
		cam_to_world[ 2 ] = getz( side );
		cam_to_world[ 3 ] = 0;

		cam_to_world[ 4 ] = getx( top );
		cam_to_world[ 5 ] = gety( top );
		cam_to_world[ 6 ] = getz( top );
		cam_to_world[ 7 ] = 0;

		cam_to_world[ 8 ] = -getx( front );
		cam_to_world[ 9 ] = -gety( front );
		cam_to_world[ 10 ] = -getz( front );
		cam_to_world[ 11 ] = 0;

		cam_to_world[ 12 ] = getx( eye );
		cam_to_world[ 13 ] = gety( eye );
		cam_to_world[ 14 ] = getz( eye );
		cam_to_world[ 15 ] = 1.0f;
	}
}

#if !defined( TINYGL_H )
void tgMulv( float* a, float* b )
{
	float result[ 4 ];

	result[ 0 ] = a[ 0 ] * b[ 0 ] + a[4] * b[ 1 ] + a[ 8 ] * b[ 2 ] + a[ 12 ] * b[ 3 ];
	result[ 1 ] = a[ 1 ] * b[ 0 ] + a[5] * b[ 1 ] + a[ 9 ] * b[ 2 ] + a[ 13 ] * b[ 3 ];
	result[ 2 ] = a[ 2 ] * b[ 0 ] + a[6] * b[ 1 ] + a[ 10 ] * b[ 2 ] + a[ 14 ] * b[ 3 ];
	result[ 3 ] = a[ 3 ] * b[ 0 ] + a[7] * b[ 1 ] + a[ 11 ] * b[ 2 ] + a[ 15 ] * b[ 3 ];

	b[ 0 ] = result[ 0 ];
	b[ 1 ] = result[ 1 ];
	b[ 2 ] = result[ 2 ];
	b[ 3 ] = result[ 3 ];
}
#endif

#include <cmath>

void compute_mouse_ray( float mouse_x, float mouse_y, float fov, float viewport_w, float viewport_h, float* cam_inv, float near_plane_dist, v3* mouse_pos, v3* mouse_dir )
{
	float aspect = (float)viewport_w / (float)viewport_h;
	float px = 2.0f * aspect * mouse_x / viewport_w - aspect;
	float py = -2.0f * mouse_y / viewport_h + 1.0f;
	float pz = -1.0f / tanf( fov / 2.0f );
	v3 point_in_view_space( px, py, pz );

	v3 cam_pos( cam_inv[ 12 ], cam_inv[ 13 ], cam_inv[ 14 ] );
	float pf[ 4 ] = { getx( point_in_view_space ), gety( point_in_view_space ), getz( point_in_view_space ), 1.0f };
	tgMulv( cam_inv, pf );
	v3 point_on_clipping_plane( pf[ 0 ] , pf[ 1 ], pf[ 2 ] );
	v3 dir_in_world_space = point_on_clipping_plane - cam_pos;

	v3 dir = norm( dir_in_world_space );
	v3 cam_forward( cam_inv[ 8 ], cam_inv[ 9 ], cam_inv[ 10 ] );

	*mouse_dir = dir;
	*mouse_pos = cam_pos + dir * dot( dir, cam_forward ) * vfloat( near_plane_dist );
}

struct q4
{
	q4( ) { }
	TM_INLINE explicit q4( v3& vector_part, vfloat& scalar_part ) { m = _mm_set_ps( scalar_part, getz( vector_part ), gety( vector_part ), getx( vector_part ) ); }
	TM_INLINE explicit q4( float x, float y, float z, float w ) { m = _mm_set_ps( w, z, y, x ); }

	TM_INLINE operator __m128( ) { return m; }
	TM_INLINE operator __m128( ) const { return m; }

	__m128 m;
};

TM_INLINE q4 q3_axis_angle( v3 axis_normalized, vfloat angle )
{
	vfloat s = vfloat( sinf( angle * 0.5f ) );
	vfloat c = vfloat( cosf( angle * 0.5f ) );
	return q4( axis_normalized * s, c );
}

TM_INLINE vfloat getx( q4 a ) { return vfloat( TM_SHUFFLE( a, a, 0, 0, 0 ) ); }
TM_INLINE vfloat gety( q4 a ) { return vfloat( TM_SHUFFLE( a, a, 1, 1, 1 ) ); }
TM_INLINE vfloat getz( q4 a ) { return vfloat( TM_SHUFFLE( a, a, 2, 2, 2 ) ); }
TM_INLINE vfloat getw( q4 a ) { return vfloat( TM_SHUFFLE( a, a, 3, 3, 3 ) ); }

// un-optimized
TM_INLINE q4 norm( q4 q )
{
	vfloat x = getx( q );
	vfloat y = gety( q );
	vfloat z = getz( q );
	vfloat w = getw( q );

	vfloat d = w * w + x * x + y * y + z * z;

	if( d == 0 )
		w = vfloat( 1.0 );

	d = vfloat( 1.0 ) / sqrtf( d );

	if ( d > vfloat( 1.0e-8f ) )
	{
		x *= d;
		y *= d;
		z *= d;
		w *= d;
	}

	return q4( x, y, z, w );
}

// un-optimized
TM_INLINE q4 operator*( q4 a, q4 b )
{
	return q4(
		getw( a ) * getx( b ) + getx( a ) * getw( b ) + gety( a ) * getz( b ) - getz( a ) * gety( b ),
		getw( a ) * gety( b ) + gety( a ) * getw( b ) + getz( a ) * getx( b ) - getx( a ) * getz( b ),
		getw( a ) * getz( b ) + getz( a ) * getw( b ) + getx( a ) * gety( b ) - gety( a ) * getx( b ),
		getw( a ) * getw( b ) - getx( a ) * getx( b ) - gety( a ) * gety( b ) - getz( a ) * getz( b )
		);
}

// un-optimized
TM_INLINE q4 integrate( q4 q, v3 w, vfloat h )
{
	q4 wq( getx( w ) * h, gety( w ) * h, getz( w ) * h, 0.0f );

	wq = wq * q;

	q4 q0 = q4(
		getx( q ) + getx( wq ) * vfloat( 0.5 ),
		gety( q ) + gety( wq ) * vfloat( 0.5 ),
		getz( q ) + getz( wq ) * vfloat( 0.5 ),
		getw( q ) + getw( wq ) * vfloat( 0.5 )
	);

	return norm( q0 );
}

// un-optimized
TM_INLINE m3 m3_from_quat( q4 q )
{
	return m3_from_quat( getx( q ), gety( q ), getz( q ), getw( q ) );
}

// globals
TM_SELECTANY m3 identity_m3 = rows( v3( 1.0f, 0.0f, 0.0f ), v3( 0.0f, 1.0f, 0.0f ), v3( 0.0f, 0.0f, 1.0f ) );
TM_SELECTANY m3 zero_m3 = rows( v3( 0.0f, 0.0f, 0.0f ), v3( 0.0f, 0.0f, 0.0f ), v3( 0.0f, 0.0f, 0.0f ) );
TM_SELECTANY v3 zero_v3 = v3( 0.0f, 0.0f, 0.0f );
TM_SELECTANY vfloat zero_vf = vfloat( 0.0f );
TM_SELECTANY vfloat one_vf = vfloat( 1.0f );
TM_SELECTANY q4 identity_q4 = q4( 0.0f, 0.0f, 0.0f, 1.0f );

#define TINYMATH_H
#endif


/*
	------------------------------------------------------------------------------
	This software is available under 2 licenses - you may choose the one you like.
	------------------------------------------------------------------------------
	ALTERNATIVE A - zlib license
	Copyright (c) 2017 Randy Gaul http://www.randygaul.net
	This software is provided 'as-is', without any express or implied warranty.
	In no event will the authors be held liable for any damages arising from
	the use of this software.
	Permission is granted to anyone to use this software for any purpose,
	including commercial applications, and to alter it and redistribute it
	freely, subject to the following restrictions:
	  1. The origin of this software must not be misrepresented; you must not
	     claim that you wrote the original software. If you use this software
	     in a product, an acknowledgment in the product documentation would be
	     appreciated but is not required.
	  2. Altered source versions must be plainly marked as such, and must not
	     be misrepresented as being the original software.
	  3. This notice may not be removed or altered from any source distribution.
	------------------------------------------------------------------------------
	ALTERNATIVE B - Public Domain (www.unlicense.org)
	This is free and unencumbered software released into the public domain.
	Anyone is free to copy, modify, publish, use, compile, sell, or distribute this 
	software, either in source code form or as a compiled binary, for any purpose, 
	commercial or non-commercial, and by any means.
	In jurisdictions that recognize copyright laws, the author or authors of this 
	software dedicate any and all copyright interest in the software to the public 
	domain. We make this dedication for the benefit of the public at large and to 
	the detriment of our heirs and successors. We intend this dedication to be an 
	overt act of relinquishment in perpetuity of all present and future rights to 
	this software under copyright law.
	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
	AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN 
	ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 
	WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
	------------------------------------------------------------------------------
*/