dontcry coder 1337 pt hd youtuber

	void math::set_matrix_position( vec3_t pos, matrix3x4& matrix ) {
		for( size_t i{ }; i < 3; ++i ) {
			matrix[ i ][ 3 ] = pos[ i ];
		}
	}

	vec3_t math::get_matrix_position( const matrix3x4& src ) {
		return vec3_t( src[ 0 ][ 3 ], src[ 1 ][ 3 ], src[ 2 ][ 3 ] );
	}

	void angle_matrix( vec3_t angles, matrix3x4& matrix ) {
		float sr, sp, sy, cr, cp, cy;

		sp = sinf( angles.x * M_PIRAD );
		cp = cosf( angles.x * M_PIRAD );
		sy = sinf( angles.y * M_PIRAD );
		cy = cosf( angles.y * M_PIRAD );
		sr = sinf( angles.z * M_PIRAD );
		cr = cosf( angles.z * M_PIRAD );

		matrix[ 0 ][ 0 ] = cp * cy;
		matrix[ 1 ][ 0 ] = cp * sy;
		matrix[ 2 ][ 0 ] = -sp;

		float crcy = cr * cy;
		float crsy = cr * sy;
		float srcy = sr * cy;
		float srsy = sr * sy;
		matrix[ 0 ][ 1 ] = sp * srcy - crsy;
		matrix[ 1 ][ 1 ] = sp * srsy + crcy;
		matrix[ 2 ][ 1 ] = sr * cp;

		matrix[ 0 ][ 2 ] = ( sp*crcy + srsy );
		matrix[ 1 ][ 2 ] = ( sp*crsy - srcy );
		matrix[ 2 ][ 2 ] = cr * cp;

		matrix[ 0 ][ 3 ] = 0.f;
		matrix[ 1 ][ 3 ] = 0.f;
		matrix[ 2 ][ 3 ] = 0.f;
	}

	void angle_imatrix( vec3_t angles, matrix3x4& matrix ) {
		float sr, sp, sy, cr, cp, cy;

		sp = sinf( angles.x * M_PIRAD );
		cp = cosf( angles.x * M_PIRAD );
		sy = sinf( angles.y * M_PIRAD );
		cy = cosf( angles.y * M_PIRAD );
		sr = sinf( angles.z * M_PIRAD );
		cr = cosf( angles.z * M_PIRAD );

		matrix[ 0 ][ 0 ] = cp * cy;
		matrix[ 0 ][ 1 ] = cp * sy;
		matrix[ 0 ][ 2 ] = -sp;
		matrix[ 1 ][ 0 ] = sr * sp*cy + cr * -sy;
		matrix[ 1 ][ 1 ] = sr * sp*sy + cr * cy;
		matrix[ 1 ][ 2 ] = sr * cp;
		matrix[ 2 ][ 0 ] = ( cr*sp*cy + -sr * -sy );
		matrix[ 2 ][ 1 ] = ( cr*sp*sy + -sr * cy );
		matrix[ 2 ][ 2 ] = cr * cp;
		matrix[ 0 ][ 3 ] = 0.f;
		matrix[ 1 ][ 3 ] = 0.f;
		matrix[ 2 ][ 3 ] = 0.f;
	}

	void angle_matrix( vec3_t angles, matrix3x4& matrix, vec3_t origin ) {
		angle_matrix( angles, matrix );
		set_matrix_position( origin, matrix );
	}

	vec3_t matrix_angles( const matrix3x4& matrix ) {
		//thx strackoverflow
		vec3_t angles;
		float forward[ 3 ];
		float left[ 3 ];
		float up[ 3 ];

		forward[ 0 ] = matrix[ 0 ][ 0 ];
		forward[ 1 ] = matrix[ 1 ][ 0 ];
		forward[ 2 ] = matrix[ 2 ][ 0 ];
		left[ 0 ] = matrix[ 0 ][ 1 ];
		left[ 1 ] = matrix[ 1 ][ 1 ];
		left[ 2 ] = matrix[ 2 ][ 1 ];
		up[ 2 ] = matrix[ 2 ][ 2 ];

		float xy_dist = sqrtf( forward[ 0 ] * forward[ 0 ] + forward[ 1 ] * forward[ 1 ] );

		if( xy_dist > 0.001f ) {
			angles.y = RAD2DEG( atan2f( forward[ 1 ], forward[ 0 ] ) );
			angles.x = RAD2DEG( atan2f( -forward[ 2 ], xy_dist ) );
			angles.z = RAD2DEG( atan2f( left[ 2 ], up[ 2 ] ) );
		}
		else {
			angles.y = RAD2DEG( atan2f( -left[ 0 ], left[ 1 ] ) );
			angles.x = RAD2DEG( atan2f( -forward[ 2 ], xy_dist ) );

			angles.z = 0;
		}

		return angles;
	}

	void rotate_matrix( vec3_t angles, vec3_t origin, float degrees, matrix3x4& matrix ) {
		angles.y += degrees;
		angles.clamp( );

		vec3_t rotated( 0, degrees, 0 );
		matrix3x4 rotated_matrix;
		angle_matrix( rotated, rotated_matrix );

		vec3_t delta = get_matrix_position( matrix ) - origin;
		vec3_t out = vector_transform( delta, rotated_matrix );

		matrix3x4 bone_rotation, matrix_out;
		memcpy( &bone_rotation, &matrix, sizeof( matrix3x4 ) );

		set_matrix_position( vec3_t( ), bone_rotation );
		concat_transforms( rotated_matrix, bone_rotation, matrix_out );
		auto angles_out = matrix_angles( matrix_out );
		angle_matrix( angles_out, matrix, out );
	}