17 files changed, 3675 insertions, 0 deletions
diff --git a/SixenseSDK/include/sixense.h b/SixenseSDK/include/sixense.h
new file mode 100755
index 0000000..b010a90
--- /dev/null
+++ b/SixenseSDK/include/sixense.h
@@ -0,0 +1,109 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+#ifndef _SIXENSE_H_
+#define _SIXENSE_H_
+
+#if defined(WIN32)
+  #ifdef SIXENSE_STATIC_LIB
+    #define SIXENSE_EXPORT 
+  #else
+    #ifdef SIXENSE_BUILDING_DLL
+      #define SIXENSE_EXPORT __declspec(dllexport)
+    #else
+      #define SIXENSE_EXPORT __declspec(dllimport)
+    #endif
+  #endif
+#else
+  #define SIXENSE_EXPORT 
+#endif
+
+#define SIXENSE_BUTTON_BUMPER   (0x01<<7)
+#define SIXENSE_BUTTON_JOYSTICK (0x01<<8)
+#define SIXENSE_BUTTON_1        (0x01<<5)
+#define SIXENSE_BUTTON_2        (0x01<<6)
+#define SIXENSE_BUTTON_3        (0x01<<3)
+#define SIXENSE_BUTTON_4        (0x01<<4)
+#define SIXENSE_BUTTON_START    (0x01<<0)
+
+#define SIXENSE_SUCCESS 0
+#define SIXENSE_FAILURE -1
+
+#define SIXENSE_MAX_CONTROLLERS 4
+
+typedef struct _sixenseControllerData {
+  float pos[3];
+  float rot_mat[3][3];
+  float joystick_x;
+  float joystick_y;
+  float trigger;
+  unsigned int buttons;
+  unsigned char sequence_number;
+  float rot_quat[4];
+  unsigned short firmware_revision;
+  unsigned short hardware_revision;
+  unsigned short packet_type;
+  unsigned short magnetic_frequency;
+  int enabled;
+  int controller_index;
+  unsigned char is_docked;
+  unsigned char which_hand;
+  unsigned char hemi_tracking_enabled;
+} sixenseControllerData;
+
+typedef struct _sixenseAllControllerData {
+  sixenseControllerData controllers[4];
+} sixenseAllControllerData;
+
+#if defined(__LANGUAGE_C_PLUS_PLUS)||defined(__cplusplus)||defined(c_plusplus)
+extern "C" {
+#endif
+
+SIXENSE_EXPORT int sixenseInit( void );
+SIXENSE_EXPORT int sixenseExit( void );
+
+SIXENSE_EXPORT int sixenseGetMaxBases();
+SIXENSE_EXPORT int sixenseSetActiveBase( int i );
+SIXENSE_EXPORT int sixenseIsBaseConnected( int i );
+
+SIXENSE_EXPORT int sixenseGetMaxControllers( void );
+SIXENSE_EXPORT int sixenseIsControllerEnabled( int which );
+SIXENSE_EXPORT int sixenseGetNumActiveControllers();
+
+SIXENSE_EXPORT int sixenseGetHistorySize();
+
+SIXENSE_EXPORT int sixenseGetData( int which, int index_back, sixenseControllerData * );
+SIXENSE_EXPORT int sixenseGetAllData( int index_back, sixenseAllControllerData * );
+SIXENSE_EXPORT int sixenseGetNewestData( int which, sixenseControllerData * );
+SIXENSE_EXPORT int sixenseGetAllNewestData( sixenseAllControllerData * );
+
+SIXENSE_EXPORT int sixenseSetHemisphereTrackingMode( int which_controller, int state );
+SIXENSE_EXPORT int sixenseGetHemisphereTrackingMode( int which_controller, int *state );
+
+SIXENSE_EXPORT int sixenseAutoEnableHemisphereTracking( int which_controller );
+
+SIXENSE_EXPORT int sixenseSetHighPriorityBindingEnabled( int on_or_off );
+SIXENSE_EXPORT int sixenseGetHighPriorityBindingEnabled( int *on_or_off );
+
+SIXENSE_EXPORT int sixenseTriggerVibration( int controller_id, int duration_100ms, int pattern_id );
+
+SIXENSE_EXPORT int sixenseSetFilterEnabled( int on_or_off );
+SIXENSE_EXPORT int sixenseGetFilterEnabled( int *on_or_off );
+
+SIXENSE_EXPORT int sixenseSetFilterParams( float near_range, float near_val, float far_range, float far_val );
+SIXENSE_EXPORT int sixenseGetFilterParams( float *near_range, float *near_val, float *far_range, float *far_val );
+
+SIXENSE_EXPORT int sixenseSetBaseColor( unsigned char red, unsigned char green, unsigned char blue );
+SIXENSE_EXPORT int sixenseGetBaseColor( unsigned char *red, unsigned char *green, unsigned char *blue );
+
+#if defined(__LANGUAGE_C_PLUS_PLUS)||defined(__cplusplus)||defined(c_plusplus)
+}
+#endif
+
+#endif /* _SIXENSE_H_ */
diff --git a/SixenseSDK/include/sixense_math.cpp b/SixenseSDK/include/sixense_math.cpp
new file mode 100755
index 0000000..1ba1b43
--- /dev/null
+++ b/SixenseSDK/include/sixense_math.cpp
@@ -0,0 +1,1398 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+
+// Vector2
+
+inline Vector2::Vector2() { 
+	_vec[0] = 0.0f; 
+	_vec[1] = 0.0f; 
+}
+
+inline Vector2::Vector2( float x, float y ) { 
+	_vec[0] = x; 
+	_vec[1] = y; 
+}
+
+inline float& Vector2::operator [](const int idx) {
+	return _vec[idx]; 
+}
+
+inline Vector2 Vector2::operator -(const Vector2 rhs) {
+	Vector2 rhs_const_copy(rhs);
+	return Vector2( _vec[0] - rhs_const_copy[0], _vec[1] - rhs_const_copy[1] );
+}
+
+inline Vector2 Vector2::operator +(const Vector2 rhs) {
+	Vector2 rhs_const_copy(rhs);
+	return Vector2( _vec[0] + rhs_const_copy[0], _vec[1] + rhs_const_copy[1] );
+}
+
+inline Vector2& Vector2::operator +=(const Vector2& rhs) {
+	Vector2 rhs_const_copy( rhs );
+	_vec[0] += rhs_const_copy[0];
+	_vec[1] += rhs_const_copy[1];
+	return *this;
+}
+
+inline Vector2& Vector2::operator -=(const Vector2& rhs) {
+	Vector2 rhs_const_copy( rhs );
+	_vec[0] -= rhs_const_copy[0];
+	_vec[1] -= rhs_const_copy[1];
+	return *this;
+}
+
+inline float Vector2::operator *(const Vector2 rhs) {
+	Vector2 rhs_const_copy( rhs );
+	return _vec[0]*rhs_const_copy[0] + _vec[1]*rhs_const_copy[1];
+}
+
+inline Vector2 Vector2::operator *(const float rhs) {
+	float rhs_const_copy( rhs );
+	return Vector2( _vec[0]*rhs_const_copy, _vec[1]*rhs_const_copy );
+}
+
+inline Vector2& Vector2::operator *=(const float& rhs) {
+	float rhs_const_copy( rhs );
+	_vec[0]*=rhs_const_copy;
+	_vec[1]*=rhs_const_copy;
+	return *this;
+}
+
+inline Vector2 Vector2::operator /(const float rhs) {
+	float rhs_const_copy( rhs );
+	return Vector2( _vec[0]/rhs_const_copy, _vec[1]/rhs_const_copy);
+}
+
+
+
+
+inline bool Vector2::operator ==(const Vector2& rhs) {
+	Vector2 rhs_const_copy( rhs );
+	if( _vec[0] == rhs_const_copy._vec[0] && _vec[1] == rhs_const_copy._vec[1] ) return true;
+	else return false;
+}
+
+
+
+inline void Vector2::normalize() {
+	float len = sqrtf( _vec[0]*_vec[0] + _vec[1]*_vec[1] );
+	_vec[0]/=len;
+	_vec[1]/=len;
+}
+
+inline float Vector2::length() {
+	return sqrtf( _vec[0]*_vec[0] + _vec[1]*_vec[1] );
+}
+
+inline void Vector2::print( std::string name ) {
+	if( name != "" ) {
+		std::cout << "Vector2 " << name << " = { ";
+	} else {
+		std::cout << "Vector2 = { ";
+	}
+
+	std::cout << _vec[0] << ", " << _vec[1] << " }" << std::endl << std::endl;
+
+}
+
+inline void Vector2::fill( float vec[2] ) {
+	vec[0] = _vec[0];
+	vec[1] = _vec[1];
+}
+
+
+// Vector3
+
+inline Vector3::Vector3() { 
+	_vec[0] = 0.0f; 
+	_vec[1] = 0.0f; 
+	_vec[2] = 0.0f; 
+
+}
+
+inline Vector3::Vector3( const Vector3& rhs ) {
+	_vec[0] = rhs._vec[0]; 
+	_vec[1] = rhs._vec[1]; 
+	_vec[2] = rhs._vec[2]; 
+
+}
+
+
+inline Vector3::Vector3( float x, float y, float z ) { 
+	_vec[0] = x; 
+	_vec[1] = y; 
+	_vec[2] = z; 
+
+}
+
+inline Vector3::Vector3( const float vec[3] ) {
+	_vec[0] = vec[0]; 
+	_vec[1] = vec[1]; 
+	_vec[2] = vec[2]; 
+
+}
+
+inline float& Vector3::operator [](const int idx) { 
+	return _vec[idx]; 
+}
+
+inline Vector3 Vector3::operator -(const Vector3 rhs) {
+	Vector3 rhs_const_copy( rhs );
+	return Vector3( _vec[0] - rhs_const_copy[0], _vec[1] - rhs_const_copy[1], _vec[2] - rhs_const_copy[2] );
+}
+
+inline Vector3 Vector3::operator +(const Vector3 rhs) {
+	Vector3 rhs_const_copy( rhs );
+	return Vector3( _vec[0] + rhs_const_copy[0], _vec[1] + rhs_const_copy[1], _vec[2] + rhs_const_copy[2] );
+}
+
+inline Vector3& Vector3::operator +=(const Vector3& rhs) {
+	Vector3 rhs_const_copy( rhs );
+	_vec[0] += rhs_const_copy[0];
+	_vec[1] += rhs_const_copy[1];
+	_vec[2] += rhs_const_copy[2];
+	return *this;
+}
+
+inline Vector3& Vector3::operator -=(const Vector3& rhs) {
+	Vector3 rhs_const_copy( rhs );
+	_vec[0] -= rhs_const_copy[0];
+	_vec[1] -= rhs_const_copy[1];
+	_vec[2] -= rhs_const_copy[2];
+	return *this;
+}
+
+inline Vector3 Vector3::operator ^(const Vector3 rhs) {
+	Vector3 rhs_const_copy( rhs );
+	return Vector3( _vec[1]*rhs_const_copy[2]-_vec[2]*rhs_const_copy[1], _vec[2]*rhs_const_copy[0]-_vec[0]*rhs_const_copy[2], _vec[0]*rhs_const_copy[1]-_vec[1]*rhs_const_copy[0] );
+}
+
+
+inline float Vector3::operator *(const Vector3 rhs) {
+	Vector3 rhs_const_copy( rhs );
+	return _vec[0]*rhs_const_copy[0] + _vec[1]*rhs_const_copy[1] + _vec[2]*rhs_const_copy[2];
+}
+
+inline Vector3 Vector3::operator *(const float rhs) {
+	float rhs_const_copy( rhs );
+	return Vector3( _vec[0]*rhs_const_copy, _vec[1]*rhs_const_copy, _vec[2]*rhs_const_copy);
+}
+
+inline Vector3& Vector3::operator *=(const float& rhs) {
+	float rhs_const_copy( rhs );
+	_vec[0]*=rhs_const_copy;
+	_vec[1]*=rhs_const_copy;
+	_vec[2]*=rhs_const_copy;
+	return *this;
+}
+
+inline Vector3 Vector3::operator /(const float rhs) {
+	float rhs_const_copy( rhs );
+	return Vector3( _vec[0]/rhs_const_copy, _vec[1]/rhs_const_copy, _vec[2]/rhs_const_copy);
+}
+
+
+inline bool Vector3::operator ==(const Vector3& rhs) {
+	Vector3 rhs_const_copy( rhs );
+	if( _vec[0] == rhs_const_copy._vec[0] && _vec[1] == rhs_const_copy._vec[1] && _vec[2] == rhs_const_copy._vec[2] ) return true;
+	else return false;
+}
+
+
+
+
+
+inline void Vector3::normalize() {
+	float len = length();
+	_vec[0]/=len;
+	_vec[1]/=len;
+	_vec[2]/=len;
+}
+
+
+inline float Vector3::length() {
+	return sqrt( _vec[0]*_vec[0] + _vec[1]*_vec[1] + _vec[2]*_vec[2] );
+}
+
+inline void Vector3::print( std::string name ) {
+	if( name != "" ) {
+		std::cout << "Vector3 " << name << " = { ";
+	} else {
+		std::cout << "Vector3 = { ";
+	}
+
+	std::cout << _vec[0] << ", " << _vec[1] << ", " << _vec[2] << " }" << std::endl << std::endl;
+
+}
+
+inline Vector3 Vector3::normalize( Vector3 m ) {
+	Vector3 norm(m);
+	norm.normalize();
+	return norm;
+}
+
+inline void Vector3::fill( float vec[3] ) {
+	vec[0] = _vec[0];
+	vec[1] = _vec[1];
+	vec[2] = _vec[2];
+}
+
+
+
+// Vector4
+
+inline Vector4::Vector4() { 
+	_vec[0] = 0.0f; 
+	_vec[1] = 0.0f; 
+	_vec[2] = 0.0f; 
+	_vec[3] = 1.0f;
+
+}
+
+inline Vector4::Vector4( const Vector4& rhs ) {
+	_vec[0] = rhs._vec[0]; 
+	_vec[1] = rhs._vec[1]; 
+	_vec[2] = rhs._vec[2]; 
+	_vec[3] = rhs._vec[3];
+
+}
+
+inline Vector4::Vector4( const Vector3& rhs, float w ) {
+	Vector3 rhs_c(rhs);
+	_vec[0] = rhs_c[0]; 
+	_vec[1] = rhs_c[1]; 
+	_vec[2] = rhs_c[2]; 
+	_vec[3] = w;
+}
+
+
+inline Vector4::Vector4( const float vec[4] ) {
+	_vec[0] = vec[0]; 
+	_vec[1] = vec[1]; 
+	_vec[2] = vec[2]; 
+	_vec[3] = vec[3]; 
+
+}
+
+inline Vector4::Vector4( const float a, const float b, const float c, const float d ) {
+	_vec[0] = a; 
+	_vec[1] = b; 
+	_vec[2] = c; 
+	_vec[3] = d;
+
+}
+
+inline float& Vector4::operator [](const int idx) { 
+	return _vec[idx]; 
+}
+
+inline Vector4 Vector4::operator +(const Vector4 rhs) const {
+	Vector4 rhs_const_copy( rhs );
+	return Vector4( _vec[0] + rhs_const_copy[0], _vec[1] + rhs_const_copy[1], _vec[2] + rhs_const_copy[2], _vec[3] + rhs_const_copy[3] );
+}
+
+inline Vector4 Vector4::operator -(const Vector4 rhs) const {
+	Vector4 rhs_const_copy( rhs );
+	return Vector4( _vec[0] - rhs_const_copy[0], _vec[1] - rhs_const_copy[1], _vec[2] - rhs_const_copy[2], _vec[3] - rhs_const_copy[3] );
+}
+
+inline Vector4 Vector4::operator *(const float rhs) const {
+	float rhs_const_copy( rhs );
+	return Vector4( _vec[0]*rhs_const_copy, _vec[1]*rhs_const_copy, _vec[2]*rhs_const_copy, _vec[3]*rhs_const_copy);
+}
+
+inline Vector4 Vector4::operator /(const float rhs) const {
+	float rhs_const_copy( rhs );
+	return Vector4( _vec[0]/rhs_const_copy, _vec[1]/rhs_const_copy, _vec[2]/rhs_const_copy, _vec[3]/rhs_const_copy);
+}
+
+
+inline Vector4 Vector4::operator -(const float rhs) const {
+	float rhs_const_copy( rhs );
+	return Vector4( _vec[0]-rhs_const_copy, _vec[1]-rhs_const_copy, _vec[2]-rhs_const_copy, _vec[3]-rhs_const_copy);
+}
+
+inline float Vector4::operator *(const Vector4 rhs) { // dot product
+	Vector4 rhs_const_copy( rhs );
+
+	float dot = _vec[0]*rhs_const_copy[0] + _vec[1]*rhs_const_copy[1] + _vec[2]*rhs_const_copy[2] + _vec[3]*rhs_const_copy[3];
+
+	if( dot > 1.0f ) dot = 1.0f;
+	if( dot < -1.0f ) dot = -1.0f;
+	return dot;
+}
+
+
+inline Vector4 Vector4::operator *(const class Matrix4 rhs_in) const {
+	Matrix4 rhs(rhs_in);
+	return Vector4( rhs[0][0]*_vec[0] + rhs[0][1]*_vec[1] + rhs[0][2]*_vec[2] + rhs[0][3]*_vec[3],
+		              rhs[1][0]*_vec[0] + rhs[1][1]*_vec[1] + rhs[1][2]*_vec[2] + rhs[1][3]*_vec[3], 
+		              rhs[2][0]*_vec[0] + rhs[2][1]*_vec[1] + rhs[2][2]*_vec[2] + rhs[2][3]*_vec[3],
+		              rhs[3][0]*_vec[0] + rhs[3][1]*_vec[1] + rhs[3][2]*_vec[2] + rhs[3][3]*_vec[3] );
+}
+
+inline bool Vector4::operator ==(const Vector4& rhs) {
+	Vector4 rhs_const_copy( rhs );
+	if( 
+		_vec[0] == rhs_const_copy._vec[0] && 
+		_vec[1] == rhs_const_copy._vec[1] && 
+		_vec[2] == rhs_const_copy._vec[2] && 
+		_vec[3] == rhs_const_copy._vec[3] ) return true;
+	else return false;
+}
+
+
+
+
+
+
+inline void Vector4::normalize() {
+	float len = length();
+	_vec[0]/=len;
+	_vec[1]/=len;
+	_vec[2]/=len;
+	_vec[3]/=len;
+}
+
+
+inline float Vector4::length() {
+	return sqrt( _vec[0]*_vec[0] + _vec[1]*_vec[1] + _vec[2]*_vec[2] + _vec[3]*_vec[3] );
+}
+
+inline void Vector4::print( std::string name ) {
+	if( name != "" ) {
+		std::cout << "Vector4 " << name << " = { ";
+	} else {
+		std::cout << "Vector4 = { ";
+	}
+
+	std::cout << _vec[0] << ", " << _vec[1] << ", " << _vec[2] << ", " << _vec[3] << " }" << std::endl << std::endl;
+
+}
+
+inline void Vector4::fill( float vec[4] ) {
+	vec[0] = _vec[0];
+	vec[1] = _vec[1];
+	vec[2] = _vec[2];
+	vec[3] = _vec[3];
+}
+
+
+
+
+// Quat
+
+inline Quat::Quat() { 
+	_vec[0] = 0.0f; 
+	_vec[1] = 0.0f; 
+	_vec[2] = 0.0f; 
+	_vec[3] = 1.0f;
+
+}
+
+inline Quat::Quat( const float a, const float b, const float c, const float d ) {
+	_vec[0] = a; 
+	_vec[1] = b; 
+	_vec[2] = c; 
+	_vec[3] = d;
+
+}
+
+inline Quat::Quat( const Vector4& rhs ) : Vector4( rhs ) {
+	
+}
+
+inline Quat::Quat( const Vector3 xyz, float w ) {
+	Vector3 deconst( xyz );
+	_vec[0] = deconst[0]; 
+	_vec[1] = deconst[1]; 
+	_vec[2] = deconst[2];
+	_vec[3] = w; 
+	
+}
+
+inline Quat::Quat( const Matrix3& mat_c ) {
+	
+	Matrix3 mat(mat_c);
+
+	float T = 1.0f + mat[0][0] + mat[1][1] + mat [2][2];
+
+	float S, X, Y, Z, W;
+
+#if 1
+	if( T > 0.000001f ) {
+		S = sqrt( T ) * 2.0f;
+		X = ( mat[1][2] - mat[2][1] ) / S;
+		Y = ( mat[2][0] - mat[0][2] ) / S;
+		Z = ( mat[0][1] - mat[1][0] ) / S;
+		W = 0.25f * S;
+	} else {
+		if ( mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2] )  {	// Column 0: 
+			S  = sqrt( 1.0f + mat[0][0] - mat[1][1] - mat[2][2] ) * 2.0f;
+			X = 0.25f * S;
+			Y = (mat[0][1] + mat[1][0] ) / S;
+			Z = (mat[2][0] + mat[0][2] ) / S;
+			W = (mat[1][2] - mat[2][1] ) / S;
+		} else if ( mat[1][1] > mat[2][2] ) {			// Column 1: 
+			S  = sqrt( 1.0f + mat[1][1] - mat[0][0] - mat[2][2] ) * 2.0f;
+			X = (mat[0][1] + mat[1][0] ) / S;
+			Y = 0.25f * S;
+			Z = (mat[1][2] + mat[2][1] ) / S;
+			W = (mat[2][0] - mat[0][2] ) / S;
+		} else {						// Column 2:
+			S  = sqrt( 1.0f + mat[2][2] - mat[0][0] - mat[1][1] ) * 2.0f;
+			X = (mat[2][0] + mat[0][2] ) / S;
+			Y = (mat[1][2] + mat[2][1] ) / S;
+			Z = 0.25f * S;
+			W = (mat[0][1] - mat[1][0] ) / S;
+		}
+	}
+#else
+	if( T > 0.000001f ) {
+		S = sqrt( T ) * 2;
+		X = ( mat[2][1] - mat[1][2] ) / S;
+		Y = ( mat[0][2] - mat[2][0] ) / S;
+		Z = ( mat[1][0] - mat[0][1] ) / S;
+		W = 0.25 * S;
+	} else {
+		if ( mat[0][0] > mat[1][1] && mat[0][0] > mat[2][2] )  {	// Column 0: 
+			S  = sqrt( 1.0 + mat[0][0] - mat[1][1] - mat[2][2] ) * 2;
+			X = 0.25 * S;
+			Y = (mat[1][0] + mat[0][1] ) / S;
+			Z = (mat[0][2] + mat[2][0] ) / S;
+			W = (mat[2][1] - mat[1][2] ) / S;
+		} else if ( mat[1][1] > mat[2][2] ) {			// Column 1: 
+			S  = sqrt( 1.0 + mat[1][1] - mat[0][0] - mat[2][2] ) * 2;
+			X = (mat[1][0] + mat[0][1] ) / S;
+			Y = 0.25 * S;
+			Z = (mat[2][1] + mat[1][2] ) / S;
+			W = (mat[0][2] - mat[2][0] ) / S;
+		} else {						// Column 2:
+			S  = sqrt( 1.0 + mat[2][2] - mat[0][0] - mat[1][1] ) * 2;
+			X = (mat[0][2] + mat[2][0] ) / S;
+			Y = (mat[2][1] + mat[1][2] ) / S;
+			Z = 0.25 * S;
+			W = (mat[1][0] - mat[0][1] ) / S;
+		}
+	}
+#endif
+
+	_vec[0] = X;
+	_vec[1] = Y;
+	_vec[2] = Z;
+	_vec[3] = W;
+
+}
+
+inline Vector3 Quat::operator *(const Vector3 rhs) const {
+	return Matrix3::rotation( *this ) * rhs;
+}
+
+inline float Quat::dot(const Quat rhs) const {
+	Vector4 t(*this), that(rhs);
+
+	float dot = t * that;
+
+	return dot;
+}
+
+inline Quat Quat::operator *(const Quat rhs) const {
+	Quat a(rhs), b(*this);
+
+	Vector3 vec_a(a[0],a[1],a[2]);
+	Vector3 vec_b(b[0],b[1],b[2]);
+	float scalar_a(a[3]), scalar_b(b[3]);
+
+	Quat retval( vec_b*scalar_a + vec_a*scalar_b + (vec_a^vec_b), scalar_a*scalar_b - vec_a*vec_b );
+	retval.normalize();
+	return retval;
+
+}
+
+
+inline Quat Quat::rotation( const float angle_in_rad, const Vector3 axis_in ) {
+
+	// from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+	Vector3 axis( axis_in );
+	axis.normalize();
+
+	float c = cos( angle_in_rad/2.0f );
+	float s = sin( angle_in_rad/2.0f );
+
+	Quat result( axis[0] * s, axis[1] * s, axis[2] * s, c );
+	result.normalize();
+
+	return result;
+}
+
+inline Quat Quat::rotation( const float heading_in_rad, const float pitch_in_rad, const float roll_in_rad ) {
+
+	Matrix3 mat = 
+		Matrix3::rotation( heading_in_rad, Vector3( 0, 1, 0 ) ) *
+		Matrix3::rotation( pitch_in_rad, Vector3( 1, 0, 0 ) ) *
+		Matrix3::rotation( roll_in_rad, Vector3( 0, 0, -1 ) );
+	return Quat(mat);
+	
+}
+
+inline Quat Quat::rotation( const Vector3 hpr_in_rad ) {
+	Vector3 deconst_hpr_in_rad( hpr_in_rad );
+	return Quat::rotation( deconst_hpr_in_rad[0], deconst_hpr_in_rad[1], deconst_hpr_in_rad[2] );
+
+}
+
+
+inline Quat Quat::rotation( const Vector3 from_vec_in, const Vector3 to_vec_in ) {
+
+	Vector3 to_vec(to_vec_in);
+	Vector3 from_vec(from_vec_in);
+
+	from_vec.normalize();
+	to_vec.normalize();
+
+	Vector3 axis = from_vec ^ to_vec;
+	float dot = from_vec * to_vec;
+
+	float angle = acosf( dot );
+
+	return rotation( angle, axis );
+}
+
+inline void Quat::print( const std::string name ) {
+	if( name != "" ) {
+		std::cout << "Quat " << name << " = { ";
+	} else {
+		std::cout << "Quat = { ";
+	}
+
+	std::cout << _vec[0] << ", " << _vec[1] << ", " << _vec[2] << ", " << _vec[3] << " }" << std::endl << std::endl;
+
+}
+
+inline void Quat::invert() { // invert in place
+	float norm = _vec[0]*_vec[0] + _vec[1]*_vec[1] + _vec[2]*_vec[2] + _vec[3]*_vec[3];
+	_vec[0] *= -1.0f/norm;
+	_vec[1] *= -1.0f/norm;
+	_vec[2] *= -1.0f/norm;
+	_vec[2] *= 1.0f/norm;
+}
+
+inline Quat Quat::inverse() { // leave this alone and return inverted copy
+	Quat copy( *this );
+
+	copy.invert();
+
+	return copy;
+}
+
+
+
+inline Quat Quat::slerp( const float t, const Quat a, const Quat b) {
+
+
+	// Cant just dot quats, compiler doesnt see Vector4::operator* for some reason...
+	Quat nca(a), ncb(b);
+
+	Vector4 va(nca[0], nca[1], nca[2], nca[3]);
+	Vector4 vb(ncb[0], ncb[1], ncb[2], ncb[3]);
+
+
+	// via google via Graphics Gems
+
+	double beta;
+	double theta;
+	double sin_t, cos_t;
+	double alpha = t;
+	bool bflip=false;
+
+	cos_t = va*vb;
+
+	if( cos_t < 0.0 ) {
+		cos_t = -cos_t;
+		bflip = true;
+	}
+
+	if( 1.0 - cos_t < 1e-7 ) {
+		beta = 1.0 - alpha;
+	} else {
+		theta = acos( cos_t );
+		sin_t = sin( theta );
+		beta = sin( theta * (1.0 - alpha)) / sin_t;
+		alpha = sin( theta * alpha ) / sin_t;
+	}
+
+	if( bflip ) {
+		alpha = -alpha;
+	}
+
+	Quat retval( va*(float)beta + vb*(float)alpha );
+
+	retval.normalize();
+
+	return retval;
+}
+
+
+inline Vector3 Quat::getEulerAngles() {
+	
+	const Quat const_this(*this);
+	Matrix3 mat = Matrix3::rotation( const_this );
+
+	return mat.getEulerAngles();
+}
+
+
+
+// Matrix3
+
+inline Matrix3::Matrix3() {
+	_cols[0] = Vector3( 1, 0 ,0 );
+	_cols[1] = Vector3( 0, 1 ,0 );
+	_cols[2] = Vector3( 0, 0 ,1 );
+}
+
+
+inline Matrix3::Matrix3( const float mat[3][3] ) {
+	_cols[0] = Vector3( mat[0][0], mat[0][1], mat[0][2] );
+	_cols[1] = Vector3( mat[1][0], mat[1][1], mat[1][2] );
+	_cols[2] = Vector3( mat[2][0], mat[2][1], mat[2][2] );
+
+}
+
+inline Matrix3::Matrix3( const Matrix3& rhs ) {
+	_cols[0] = rhs._cols[0];
+	_cols[1] = rhs._cols[1];
+	_cols[2] = rhs._cols[2];
+
+}
+
+inline Matrix3::Matrix3( const float m00, const float m10, const float m20, const float m01, const float m11, const float m21, const float m02, const float m12, const float m22 ) {
+	
+	_cols[0] = Vector3( m00, m01, m02 );
+	_cols[1] = Vector3( m10, m11, m12 );
+	_cols[2] = Vector3( m20, m21, m22 );
+
+}
+
+
+inline void Matrix3::fill( float mat[3][3] ) {
+	for( unsigned int i=0; i<3; i++ ) {
+		for( int j=0; j<3; j++ ) {
+			mat[i][j] = _cols[i][j];
+		}
+	}
+}
+
+
+inline Matrix3::Matrix3( const Vector3 col0, const Vector3 col1, const Vector3 col2 ) {
+	_cols[0] = col0;
+	_cols[1] = col1;
+	_cols[2] = col2;
+
+}
+
+
+inline Vector3& Matrix3::operator [](const int idx) {
+	return _cols[idx]; 
+}
+
+
+inline Matrix3 Matrix3::operator *(const Matrix3 rhs_in) {
+	Matrix3 rhs(rhs_in);
+	Matrix3 retmat( 
+		Vector3( rhs[0][0]*_cols[0][0] + rhs[1][0]*_cols[0][1] + rhs[2][0]*_cols[0][2],
+		rhs[0][1]*_cols[0][0] + rhs[1][1]*_cols[0][1] + rhs[2][1]*_cols[0][2], 
+		rhs[0][2]*_cols[0][0] + rhs[1][2]*_cols[0][1] + rhs[2][2]*_cols[0][2] ),
+
+		Vector3( rhs[0][0]*_cols[1][0] + rhs[1][0]*_cols[1][1] + rhs[2][0]*_cols[1][2],
+		rhs[0][1]*_cols[1][0] + rhs[1][1]*_cols[1][1] + rhs[2][1]*_cols[1][2], 
+		rhs[0][2]*_cols[1][0] + rhs[1][2]*_cols[1][1] + rhs[2][2]*_cols[1][2] ),
+
+		Vector3( rhs[0][0]*_cols[2][0] + rhs[1][0]*_cols[2][1] + rhs[2][0]*_cols[2][2],
+		rhs[0][1]*_cols[2][0] + rhs[1][1]*_cols[2][1] + rhs[2][1]*_cols[2][2], 
+		rhs[0][2]*_cols[2][0] + rhs[1][2]*_cols[2][1] + rhs[2][2]*_cols[2][2] ) );
+
+	return retmat;
+}
+
+inline Matrix3 Matrix3::operator *(const Quat rhs) {
+	return *this * Matrix3::rotation( rhs );
+}
+
+
+inline Vector3 Matrix3::operator *(const Vector3 rhs_in) {
+	Vector3 rhs(rhs_in);
+	return Vector3( _cols[0][0]*rhs[0] + _cols[1][0]*rhs[1] + _cols[2][0]*rhs[2],
+		_cols[0][1]*rhs[0] + _cols[1][1]*rhs[1] + _cols[2][1]*rhs[2], 
+		_cols[0][2]*rhs[0] + _cols[1][2]*rhs[1] + _cols[2][2]*rhs[2] );
+}
+
+inline Matrix3 Matrix3::operator *(const float rhs) {
+	return Matrix3( _cols[0] * rhs, _cols[1] * rhs, _cols[2] * rhs );
+}
+
+inline Matrix3 Matrix3::operator /(const float rhs) {
+	return Matrix3( _cols[0] / rhs, _cols[1] / rhs, _cols[2] / rhs );
+}
+
+inline Matrix3 Matrix3:: operator +(const Matrix3 rhs) {
+	return Matrix3( _cols[0] + rhs._cols[0], _cols[1] + rhs._cols[1], _cols[2] + rhs._cols[2] );
+
+}
+
+inline Matrix3 Matrix3:: operator -(const Matrix3 rhs) {
+	return Matrix3( _cols[0] - rhs._cols[0], _cols[1] - rhs._cols[1], _cols[2] - rhs._cols[2] );
+
+}
+
+
+inline void Matrix3::transpose() {
+	Matrix3 mat( 
+		Vector3( _cols[0][0], _cols[1][0], _cols[2][0] ),
+		Vector3( _cols[0][1], _cols[1][1], _cols[2][1] ),
+		Vector3( _cols[0][2], _cols[1][2], _cols[2][2] )
+		);
+
+	_cols[0] = mat._cols[0];
+	_cols[1] = mat._cols[1];
+	_cols[2] = mat._cols[2];
+}
+
+inline float Matrix3::trace() {
+	return( _cols[0][0] + _cols[1][1] + _cols[2][2] );
+}
+
+inline Vector3 Matrix3::col( const int i ) {
+	return _cols[i];
+}
+
+inline Vector3 Matrix3::row( const int i ) {
+	return Vector3( _cols[0][i], _cols[1][i], _cols[2][i] );
+}
+
+inline void Matrix3::set_col( const int which, const Vector3 col ) {
+	_cols[which] = col;
+}
+
+inline bool Matrix3::is_identity() {
+	if( _cols[0][0] == 1.0f &&
+		  _cols[0][1] == 0.0f &&
+			_cols[0][2] == 0.0f &&
+			_cols[1][0] == 0.0f &&
+			_cols[1][1] == 1.0f &&
+			_cols[1][2] == 0.0f &&
+			_cols[2][0] == 0.0f &&
+			_cols[2][1] == 0.0f &&
+			_cols[2][2] == 1.0f ) {
+		return true;
+	} else {
+		return false;
+	}
+}
+
+inline Matrix3 Matrix3::rotation( const Vector3 hpr_in_deg ) {
+	return Matrix3::rotation( Quat::rotation( hpr_in_deg ) );
+}
+
+inline Matrix3 Matrix3::rotation( const float angle_in_rad, const Vector3 axis_in ) {
+	Vector3 axis(axis_in);
+
+	// from http://en.wikipedia.org/wiki/Rotation_matrix#Axis_and_angle
+
+	float c = cos(angle_in_rad); 
+	float s = sin(angle_in_rad); 
+	float C = 1-c;
+	float x = axis[0], y = axis[1], z = axis[2];
+
+	float xs = x*s;   
+	float ys = y*s;   
+	float zs = z*s;
+	float xC = x*C;   
+	float yC = y*C;   
+	float zC = z*C;
+	float xyC = x*yC; 
+	float yzC = y*zC; 
+	float zxC = z*xC;
+
+	return Matrix3(	
+		Vector3( x*xC+c, xyC+zs, zxC-ys ), 
+		Vector3( xyC-zs, y*yC+c, yzC+xs ), 
+		Vector3( zxC+ys, yzC-xs, z*zC+c ) );
+}
+
+
+inline Matrix3 Matrix3::rotation( const Quat rot_in ) {
+	Quat rot(rot_in);
+
+	// from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+
+	float xx, xy, xz, xw, yy, yz, yw, zz, zw;
+	float X = rot[0], Y = rot[1], Z = rot[2], W = rot[3];
+
+	xx      = X * X;
+	xy      = X * Y;
+	xz      = X * Z;
+	xw      = X * W;
+	yy      = Y * Y;
+	yz      = Y * Z;
+	yw      = Y * W;
+	zz      = Z * Z;
+	zw      = Z * W;
+
+	Matrix3 mat;
+
+	mat[0][0]  = 1.0f - 2.0f * ( yy + zz );
+	mat[1][0]  =     2.0f * ( xy - zw );
+	mat[2][0]  =     2.0f * ( xz + yw );
+	mat[0][1]  =     2.0f * ( xy + zw );
+	mat[1][1]  = 1.0f - 2.0f * ( xx + zz );
+	mat[2][1]  =     2.0f * ( yz - xw );
+	mat[0][2]  =     2.0f * ( xz - yw );
+	mat[1][2]  =     2.0f * ( yz + xw );
+	mat[2][2]  = 1.0f - 2.0f * ( xx + yy );
+
+	return mat;
+
+}
+
+inline Matrix3 Matrix3::translation( const Vector3 trans_in ) {
+	Vector3 trans(trans_in);
+
+	Matrix3 mat;
+	mat._cols[2][0] = trans[0];
+	mat._cols[2][1] = trans[1];
+	mat._cols[2][2] = trans[2];
+
+	return mat;
+}
+
+inline void Matrix3::print( const std::string name ) {
+	if( name != "" ) {
+		std::cout << "Matrix3 " << name << " = { " << std::endl;
+	} else {
+		std::cout << "Matrix3 = { " << std::endl;
+	}
+
+	std::cout << _cols[0][0] << ", " << _cols[1][0] << ", " << _cols[2][0] << std::endl;
+	std::cout << _cols[0][1] << ", " << _cols[1][1] << ", " << _cols[2][1] << std::endl;
+	std::cout << _cols[0][2] << ", " << _cols[1][2] << ", " << _cols[2][2] << std::endl;
+	std::cout << "}" << std::endl << std::endl;
+}
+
+inline Matrix3 Matrix3::rotation( const Vector3 from, const Vector3 to ) {
+	return Matrix3( Matrix3::rotation( Quat::rotation( from, to ) ) );
+}
+
+inline Matrix3 Matrix3::scale( const float xs, const float ys, const float zs ) {
+	Matrix3 scale_mat;
+	scale_mat[0][0] = xs;
+	scale_mat[1][1] = ys;
+	scale_mat[2][2] = zs;
+	return scale_mat;
+}
+
+inline Matrix3 Matrix3::scale( const float s ) {
+	Matrix3 scale_mat;
+	scale_mat[0][0] = s;
+	scale_mat[1][1] = s;
+	scale_mat[2][2] = s;
+	return scale_mat;
+}
+
+inline Matrix3 Matrix3::transpose( const Matrix3 m ) {
+	Matrix3 transp(m);
+	transp.transpose();
+	return transp;
+}
+
+
+inline Vector3 Matrix3::getEulerAngles() {
+#if 1
+
+	Vector3 retval;
+
+	float h, p, r;
+	float A, B;
+
+	B = _cols[1][2];
+	p = asinf( B );
+	A = cosf( p );
+
+	
+	if( fabs( A ) > 0.005f ) {
+		h = atan2f( -_cols[0][2]/A, _cols[2][2]/A ); // atan2( D, C )
+		r = atan2f( -_cols[1][0]/A, _cols[1][1]/A ); // atan2( F, E )
+	} else {
+		h = 0;
+		r = atan2f( _cols[2][1], _cols[2][0] ); // atan2( F, E ) when B=0, D=1
+	}
+
+	retval[0] = h;
+	retval[1] = p;
+	retval[2] = r;
+
+	return retval;
+
+#endif
+
+#if 0
+
+
+	Vector3 retval;
+
+#if 0
+
+	float sin_az, sin_el, sin_rl;
+	float cos_az, cos_el, cos_rl;
+
+	sin_el = - _cols[0][2];
+	cos_el = (float)sqrt( 1.0 - (sin_el * sin_el) );
+
+	if (cos_el == 0.0) {
+		sin_az = 0.0;
+		cos_az = 1.0;	
+	}
+	else {
+		sin_az = _cols[0][1] / cos_el;
+		cos_az = _cols[0][0] / cos_el;
+	}
+
+	sin_rl =   sin_az * _cols[2][0] - cos_az * _cols[2][1];
+	cos_rl = - sin_az * _cols[1][0] + cos_az * _cols[1][1];
+
+	retval[0] = (float)atan2 (sin_az , cos_az);
+	retval[1] = (float)atan2 (sin_el , cos_el);
+	retval[2] = (float)atan2 (sin_rl , cos_rl);
+
+#else
+
+	float h, p, r;
+	float A, B;
+
+	B = _cols[2][1];
+	p = asinf( B );
+	A = cosf( p );
+
+	
+	if( fabs( A ) > 0.005 ) {
+		h = atan2f( -_cols[2][0]/A, _cols[2][2]/A ); // atan2( D, C )
+		r = atan2f( -_cols[0][1]/A, _cols[1][1]/A ); // atan2( F, E )
+	} else {
+		h = 0;
+		r = atan2f( _cols[1][2], _cols[0][2] ); // atan2( F, E ) when B=0, D=1
+	}
+
+	retval[0] = -h;
+	retval[1] = p;
+	retval[2] = r;
+
+#endif
+
+#endif
+
+	return retval;
+
+}
+
+
+
+
+
+
+
+
+
+// Matrix4
+
+inline Matrix4::Matrix4() {
+	_cols[0] = Vector4( 1, 0, 0, 0 );
+	_cols[1] = Vector4( 0, 1, 0, 0 );
+	_cols[2] = Vector4( 0, 0, 1, 0 );
+	_cols[3] = Vector4( 0, 0, 0, 1 );
+}
+
+
+inline Matrix4::Matrix4( const float mat[4][4] ) {
+	_cols[0] = Vector4( mat[0][0], mat[0][1], mat[0][2], mat[0][3] );
+	_cols[1] = Vector4( mat[1][0], mat[1][1], mat[1][2], mat[1][3] );
+	_cols[2] = Vector4( mat[2][0], mat[2][1], mat[2][2], mat[2][3] );
+	_cols[3] = Vector4( mat[3][0], mat[3][1], mat[3][2], mat[3][3] );
+
+}
+
+inline Matrix4::Matrix4( const Matrix4& rhs ) {
+	_cols[0] = rhs._cols[0];
+	_cols[1] = rhs._cols[1];
+	_cols[2] = rhs._cols[2];
+	_cols[3] = rhs._cols[3];
+
+}
+
+inline Matrix4::Matrix4( const Matrix3& rhs ) {
+	Matrix3 rhs_c(rhs);
+	_cols[0] = Vector4( rhs_c[0], 0.0f );
+	_cols[1] = Vector4( rhs_c[1], 0.0f );
+	_cols[2] = Vector4( rhs_c[2], 0.0f );
+	_cols[3] = Vector4( 0.0f, 0.0f, 0.0f, 1.0f );
+
+}
+
+inline Matrix4::Matrix4( const float m00, const float m10, const float m20, const float m30, const float m01, const float m11, const float m21, const float m31, const float m02, const float m12, const float m22, const float m32, const float m03, const float m13, const float m23, const float m33 ) {
+	
+	_cols[0] = Vector4( m00, m01, m02, m03 );
+	_cols[1] = Vector4( m10, m11, m12, m13 );
+	_cols[2] = Vector4( m20, m21, m22, m23 );
+	_cols[3] = Vector4( m30, m31, m32, m33 );
+
+}
+
+
+inline void Matrix4::fill( float mat[4][4] ) {
+	for( unsigned int i=0; i<4; i++ ) {
+		for( int j=0; j<4; j++ ) {
+			mat[i][j] = _cols[i][j];
+		}
+	}
+}
+
+
+inline Matrix4::Matrix4( const Vector4 col0, const Vector4 col1, const Vector4 col2, const Vector4 col3 ) {
+	_cols[0] = col0;
+	_cols[1] = col1;
+	_cols[2] = col2;
+	_cols[3] = col3;
+
+}
+
+
+inline Vector4& Matrix4::operator [](const int idx) {
+	return _cols[idx]; 
+}
+
+
+inline Matrix4 Matrix4::operator *(const Matrix4 rhs_in) {
+	Matrix4 rhs(rhs_in);
+	Matrix4 retmat( 
+		Vector4( rhs[0][0]*_cols[0][0] + rhs[1][0]*_cols[0][1] + rhs[2][0]*_cols[0][2] + rhs[3][0]*_cols[0][3],
+		         rhs[0][1]*_cols[0][0] + rhs[1][1]*_cols[0][1] + rhs[2][1]*_cols[0][2] + rhs[3][1]*_cols[0][3],
+		         rhs[0][2]*_cols[0][0] + rhs[1][2]*_cols[0][1] + rhs[2][2]*_cols[0][2] + rhs[3][2]*_cols[0][3],
+		         rhs[0][3]*_cols[0][0] + rhs[1][3]*_cols[0][1] + rhs[2][3]*_cols[0][2] + rhs[3][3]*_cols[0][3] ),
+
+		Vector4( rhs[0][0]*_cols[1][0] + rhs[1][0]*_cols[1][1] + rhs[2][0]*_cols[1][2] + rhs[3][0]*_cols[1][3],
+		         rhs[0][1]*_cols[1][0] + rhs[1][1]*_cols[1][1] + rhs[2][1]*_cols[1][2] + rhs[3][1]*_cols[1][3], 
+		         rhs[0][2]*_cols[1][0] + rhs[1][2]*_cols[1][1] + rhs[2][2]*_cols[1][2] + rhs[3][2]*_cols[1][3],
+		         rhs[0][3]*_cols[1][0] + rhs[1][3]*_cols[1][1] + rhs[2][3]*_cols[1][2] + rhs[3][3]*_cols[1][3] ),
+
+		Vector4( rhs[0][0]*_cols[2][0] + rhs[1][0]*_cols[2][1] + rhs[2][0]*_cols[2][2] + rhs[3][0]*_cols[2][3],
+		         rhs[0][1]*_cols[2][0] + rhs[1][1]*_cols[2][1] + rhs[2][1]*_cols[2][2] + rhs[3][1]*_cols[2][3], 
+		         rhs[0][2]*_cols[2][0] + rhs[1][2]*_cols[2][1] + rhs[2][2]*_cols[2][2] + rhs[3][2]*_cols[2][3],
+		         rhs[0][3]*_cols[2][0] + rhs[1][3]*_cols[2][1] + rhs[2][3]*_cols[2][2] + rhs[3][3]*_cols[2][3] ),
+
+		Vector4( rhs[0][0]*_cols[3][0] + rhs[1][0]*_cols[3][1] + rhs[2][0]*_cols[3][2] + rhs[3][0]*_cols[3][3],
+		         rhs[0][1]*_cols[3][0] + rhs[1][1]*_cols[3][1] + rhs[2][1]*_cols[3][2] + rhs[3][1]*_cols[3][3], 
+		         rhs[0][2]*_cols[3][0] + rhs[1][2]*_cols[3][1] + rhs[2][2]*_cols[3][2] + rhs[3][2]*_cols[3][3],
+		         rhs[0][3]*_cols[3][0] + rhs[1][3]*_cols[3][1] + rhs[2][3]*_cols[3][2] + rhs[3][3]*_cols[3][3] ) );
+
+	return retmat;
+}
+
+inline Matrix4 Matrix4::operator *(const Quat rhs) {
+	return *this * Matrix4::rotation( rhs );
+}
+
+
+inline Vector4 Matrix4::operator *(const Vector4 rhs_in) {
+	Vector4 rhs(rhs_in);
+	return Vector4( _cols[0][0]*rhs[0] + _cols[1][0]*rhs[1] + _cols[2][0]*rhs[2] + _cols[3][0]*rhs[3],
+		              _cols[0][1]*rhs[0] + _cols[1][1]*rhs[1] + _cols[2][1]*rhs[2] + _cols[3][1]*rhs[3], 
+		              _cols[0][2]*rhs[0] + _cols[1][2]*rhs[1] + _cols[2][2]*rhs[2] + _cols[3][2]*rhs[3],
+		              _cols[0][3]*rhs[0] + _cols[1][3]*rhs[1] + _cols[2][3]*rhs[2] + _cols[3][3]*rhs[3] );
+}
+
+inline Matrix4 Matrix4::operator *(const float rhs) {
+	return Matrix4( _cols[0] * rhs, _cols[1] * rhs, _cols[2] * rhs, _cols[3] * rhs );
+}
+
+inline Matrix4 Matrix4::operator /(const float rhs) {
+	return Matrix4( _cols[0] / rhs, _cols[1] / rhs, _cols[2] / rhs, _cols[3] / rhs );
+}
+
+inline Matrix4 Matrix4:: operator +(const Matrix4 rhs) {
+	return Matrix4( _cols[0] + rhs._cols[0], _cols[1] + rhs._cols[1], _cols[2] + rhs._cols[2], _cols[3] + rhs._cols[3] );
+
+}
+
+inline Matrix4 Matrix4:: operator -(const Matrix4 rhs) {
+	return Matrix4( _cols[0] - rhs._cols[0], _cols[1] - rhs._cols[1], _cols[2] - rhs._cols[2], _cols[3] - rhs._cols[3] );
+
+}
+
+
+inline void Matrix4::transpose() {
+	Matrix4 mat( 
+		Vector4( _cols[0][0], _cols[1][0], _cols[2][0], _cols[3][0] ),
+		Vector4( _cols[0][1], _cols[1][1], _cols[2][1], _cols[3][1] ),
+		Vector4( _cols[0][2], _cols[1][2], _cols[2][2], _cols[3][2] ),
+		Vector4( _cols[0][3], _cols[1][3], _cols[2][3], _cols[3][3] )
+		);
+
+	_cols[0] = mat._cols[0];
+	_cols[1] = mat._cols[1];
+	_cols[2] = mat._cols[2];
+	_cols[3] = mat._cols[3];
+}
+
+inline Vector4 Matrix4::col( const int i ) {
+	return _cols[i];
+}
+
+inline Vector4 Matrix4::row( const int i ) {
+	return Vector4( _cols[0][i], _cols[1][i], _cols[2][i], _cols[3][i] );
+}
+
+inline void Matrix4::set_col( const int which, const Vector4 col ) {
+	_cols[which] = col;
+}
+
+inline float Matrix4::trace() {
+	return _cols[0][0] + _cols[1][1] + _cols[2][2] + _cols[3][3];
+}
+
+inline bool Matrix4::is_identity() {
+	if( _cols[0][0] == 1.0f &&
+		  _cols[0][1] == 0.0f &&
+			_cols[0][2] == 0.0f &&
+			_cols[0][3] == 0.0f &&
+
+			_cols[1][0] == 0.0f &&
+			_cols[1][1] == 1.0f &&
+			_cols[1][2] == 0.0f &&
+			_cols[1][3] == 0.0f &&
+
+			_cols[2][0] == 0.0f &&
+			_cols[2][1] == 0.0f &&
+			_cols[2][2] == 1.0f &&
+			_cols[2][3] == 0.0f &&
+
+			_cols[3][0] == 0.0f &&
+			_cols[3][1] == 0.0f &&
+			_cols[3][2] == 0.0f &&
+			_cols[3][3] == 1.0f ) {
+		return true;
+	} else {
+		return false;
+	}
+}
+
+
+inline Matrix4 Matrix4::rotation( const float angle_in_rad, const Vector3 axis_in ) {
+	return Matrix4( Matrix3::rotation( angle_in_rad, axis_in ) );
+}
+
+
+inline Matrix4 Matrix4::rotation( const Quat rot_in ) {
+	return Matrix4( Matrix3::rotation( rot_in ) );
+}
+
+inline Matrix4 Matrix4::rotation( const Vector3 hpr_in_deg ) {
+	return Matrix4( Matrix3::rotation( hpr_in_deg ) );
+}
+
+inline Matrix4 Matrix4::translation( const Vector3 trans_in ) {
+	return Matrix4( Matrix3::translation( trans_in ) );
+}
+
+inline void Matrix4::print( const std::string name ) {
+	if( name != "" ) {
+		std::cout << "Matrix4 " << name << " = { " << std::endl;
+	} else {
+		std::cout << "Matrix4 = { " << std::endl;
+	}
+
+	std::cout << _cols[0][0] << ", " << _cols[1][0] << ", " << _cols[2][0] << ", " << _cols[3][0] << std::endl;
+	std::cout << _cols[0][1] << ", " << _cols[1][1] << ", " << _cols[2][1] << ", " << _cols[3][1] << std::endl;
+	std::cout << _cols[0][2] << ", " << _cols[1][2] << ", " << _cols[2][2] << ", " << _cols[3][2] << std::endl;
+	std::cout << _cols[0][3] << ", " << _cols[1][3] << ", " << _cols[2][3] << ", " << _cols[3][3] << std::endl;
+	std::cout << "}" << std::endl << std::endl;
+
+
+}
+
+inline Matrix4 Matrix4::rotation( const Vector3 from, const Vector3 to ) {
+	return Matrix4( Matrix3::rotation( from, to ) );
+}
+
+inline Matrix4 Matrix4::scale( const float xs, const float ys, const float zs ) {
+	return Matrix4( Matrix3::scale( xs, ys, zs ) );
+}
+
+inline Matrix4 Matrix4::scale( const float s ) {
+	return Matrix4( Matrix3::scale( s ) );
+}
+
+inline Matrix4 Matrix4::transpose( const Matrix4 m ) {
+	Matrix4 transp(m);
+	transp.transpose();
+	return transp;
+}
+
+inline Vector3 Matrix4::getEulerAngles() {
+	Vector3 retval;
+
+	float h, p, r;
+	float A, B;
+
+	B = _cols[1][2];
+	p = asinf( B );
+	A = cosf( p );
+
+	
+	if( fabs( A ) > 0.005f ) {
+		h = atan2f( -_cols[0][2]/A, _cols[2][2]/A ); // atan2( D, C )
+		r = atan2f( -_cols[1][0]/A, _cols[1][1]/A ); // atan2( F, E )
+	} else {
+		h = 0;
+		r = atan2f( _cols[2][1], _cols[2][0] ); // atan2( F, E ) when B=0, D=1
+	}
+
+	retval[0] = h;
+	retval[1] = p;
+	retval[2] = r;
+
+	return retval;
+
+}
+
+
+
+
+
+// Line
+
+inline Line::Line ( const Vector3& d, const Vector3& p ) : _dir( d ),  _pos1( p ) {
+	_dir.normalize();
+	_pos2 = _pos1 + _dir;
+}
+
+inline Line::Line ( const Line& line ) {
+	_dir = line._dir;
+	_pos1 = line._pos1;
+}
+
+inline Vector3 Line::getClosestPoint( const Vector3& pos ) {
+
+	Vector3 ncp( pos );
+
+	Vector3 u = _pos2 - _pos1;
+	Vector3 v = ncp - _pos1;
+	Vector3 closest = _pos1 + u * ( (u*v) / (u*u) );
+	
+	return closest;
+}
+
+
+
+
+
+// Plane
+
+
+inline Plane::Plane () : _p0( 0.0f, 0.0f, 0.0f ), _p1( 1.0f, 0.0f, 0.0f ), _p2( 1.0f, 1.0f, 0.0f ) {
+	init();
+}
+
+inline Plane::Plane ( const Plane& p ) : _p0(p._p0), _p1(p._p1), _p2(p._p2) {
+	init();
+}
+
+inline Plane::Plane ( Vector3 p0, Vector3 p1, Vector3 p2 ) : _p0( p0 ), _p1( p1 ), _p2( p2 ) {
+	init();
+}
+
+inline Plane::Plane ( Vector3 p, Vector3 norm ) : _p0( p ), _norm( norm ) {
+	
+	_a = _norm[0];
+	_b = _norm[1];
+	_c = _norm[2];
+	_d = -( _norm[0] * _p0[0] + _norm[1] * _p0[1] +  _norm[2] * _p0[2] );
+
+	_norm.normalize();
+
+	// Make up some valid points for pos2 and pos3
+	_p1 = Vector3( _norm[1], _norm[2], _norm[0] ) ^ _norm;
+	_p2 = _p1 ^ _norm;
+
+	_p1+=_p0;
+	_p2+=_p0;
+} 
+
+inline void Plane::init() {
+	_a = ( _p1[1] - _p0[1] ) * ( _p2[2] - _p0[2] ) - ( _p1[2] - _p0[2] ) * ( _p2[1] - _p0[1] );
+	_b = ( _p1[2] - _p0[2] ) * ( _p2[0] - _p0[0] ) - ( _p1[0] - _p0[0] ) * ( _p2[2] - _p0[2] );
+	_c = ( _p1[0] - _p0[0] ) * ( _p2[1] - _p0[1] ) - ( _p1[1] - _p0[1] ) * ( _p2[0] - _p0[0] );
+	_d = -( _p1[0] * _a + _p1[1] * _b + _p1[2] * _c );
+	
+	double denom = sqrt( _a * _a + _b * _b + _c * _c );
+	_a /= denom;
+	_b /= denom;
+	_c /= denom;
+	_d /= denom;
+	
+	Vector3 dir1 = _p1 - _p0;
+	dir1.normalize();
+	
+	Vector3 dir2 = _p2 - _p0;
+	dir2.normalize();
+	
+	_norm = dir1^dir2;
+	_norm.normalize();
+}
+
+inline Vector3 Plane::getClosestPoint( Vector3 in ) {
+	
+	// Create a vector from the surface to  point src
+	Vector3 w = in - _p0;
+	
+	// The closest point in the plane to point p is in the negative normal
+	// direction a distance w dot p
+	Vector3 out = in - _norm * (w * _norm); // (w dot normal)
+
+	return out;
+}
+
+inline Vector3 Plane::getNormal() {
+	return _norm;
+}
+ 
+ 
+inline Vector3 Plane::intersect( const Line line ) {
+	Vector3 nc_dir( line._dir );
+	Vector3 nc_pos1( line._pos1 );
+
+	double denom = ( _a * nc_dir[0] + _b * nc_dir[1] + _c * nc_dir[2] );
+	if ( fabs( denom ) < .001f ) {
+		return Vector3();
+	}
+
+	double t = -( _a * nc_pos1[0] + _b * nc_pos1[1] + _c * nc_pos1[2] + _d ) / denom;
+			
+	Vector3 pos;
+
+	pos[0] = nc_pos1[0] + nc_dir[0] * (float)t;
+	pos[1] = nc_pos1[1] + nc_dir[1] * (float)t;
+	pos[2] = nc_pos1[2] + nc_dir[2] * (float)t;
+
+	return pos;							
+}
+
+inline double	Plane::whichSide	(	Vector3 p )	{
+	return _a * p[0] + _b * p[1] + _c * p[2] + _d;
+}
+
diff --git a/SixenseSDK/include/sixense_math.hpp b/SixenseSDK/include/sixense_math.hpp
new file mode 100755
index 0000000..2db6a54
--- /dev/null
+++ b/SixenseSDK/include/sixense_math.hpp
@@ -0,0 +1,248 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+#ifndef SIXENSE_MATH_HPP
+#define SIXENSE_MATH_HPP
+
+#include <math.h>
+#include <iostream>
+#include <string>
+
+namespace sixenseMath {
+
+	class Vector2 {
+	public:
+		Vector2();
+		Vector2( const float x, const float y );
+		float& operator [](const int idx);
+		Vector2 operator -(const Vector2 rhs);
+		Vector2 operator +(const Vector2 rhs);
+
+		Vector2& operator +=(const Vector2& rhs);
+		Vector2& operator *=(const float& rhs);
+		Vector2& operator -=(const Vector2& rhs);
+		float operator *(const Vector2 rhs); // dot product
+		Vector2 operator *(const float rhs);
+		Vector2 operator /(const float rhs);
+
+		bool operator ==(const Vector2& rhs);
+
+		void normalize();
+		float length();
+		void print( const std::string name=std::string() );
+		void fill( float vec[2] );
+
+	protected:
+		float _vec[2];
+	};
+
+	class Vector3 {
+	public:
+		Vector3();
+		Vector3( const Vector3& );
+		Vector3( const float vec[3] );
+		Vector3( const float x, const float y, const float z );
+		float& operator [](const int idx);
+		Vector3 operator -(const Vector3 rhs);
+		Vector3 operator +(const Vector3 rhs);
+		Vector3 operator ^(const Vector3 rhs); // cross product
+		Vector3& operator +=(const Vector3& rhs);
+		Vector3& operator *=(const float& rhs);
+		Vector3& operator -=(const Vector3& rhs);
+		float operator *(const Vector3 rhs); // dot product
+		Vector3 operator *(const float rhs);
+		Vector3 operator /(const float rhs);
+		void normalize();
+		float length();
+		void print( const std::string name=std::string() );
+		void fill( float vec[3] );
+
+		bool operator ==(const Vector3& rhs);
+
+		// Construction helpers
+		static Vector3 normalize( const Vector3 );
+
+	protected:
+		float _vec[3];
+	};
+
+	class Vector4 {
+	public:
+		Vector4();
+		Vector4( const Vector4& );
+		Vector4( const Vector3&, float w );
+		Vector4( const float vec[4] );
+		Vector4( const float x, const float y, const float z, const float w );
+		float& operator [](const int idx);
+		Vector4 operator -(const Vector4 rhs) const;
+		Vector4 operator ^(const Vector4 rhs) const; // cross product
+		Vector4 operator +(const Vector4 rhs) const;
+		float operator *(const Vector4 rhs); // dot product
+		Vector4 operator *(const float rhs) const;
+		Vector4 operator -(const float rhs) const;
+		Vector4 operator /(const float rhs) const;
+
+		bool operator ==(const Vector4& rhs);
+
+		void normalize();
+		float length();
+		void print( const std::string name=std::string() );
+		Vector4 operator *(const class Matrix4 rhs) const;
+		void fill( float vec[4] );
+
+	protected:
+		float _vec[4];
+	};
+
+	class Quat : public Vector4 {
+	public:
+		Quat();
+		Quat( const float x, const float y, const float z, const float w );
+		Quat( const class Matrix3& );
+		Quat( const Vector4& );
+		Quat( const Vector3 xyz, float w );
+		void print( const std::string name=std::string() );
+		Vector3 operator *(const Vector3 rhs) const;
+		Quat operator *(const Quat rhs) const;
+		float dot(const Quat rhs) const; // dot product (operator * is already used for quat...)
+		void invert(); // invert in place
+		Quat inverse(); // leave this alone and return inverted copy
+		Vector3 getEulerAngles();
+
+		// Construction helpers
+		static Quat rotation( const Vector3 from_vec, const Vector3 to_vec );
+		static Quat rotation( const float angle_in_rad, const Vector3 axis );
+		static Quat rotation( const Vector3 hpr_in_rad );
+		static Quat rotation( const float heading, const float pitch, const float roll );
+		static Quat slerp( const float t, const Quat a, const Quat b);
+	};
+
+	class Matrix3 {
+	public:
+		Matrix3();
+		Matrix3( const Matrix3& );
+		Matrix3( const float mat[3][3] );
+		Matrix3( const float m00, const float m10, const float m20, const float m01, const float m11, const float m21, const float m02, const float m12, const float m22 );
+		void fill( float mat[3][3] );
+		Matrix3( const Vector3 col0, const Vector3 col1, const Vector3 col2 );
+		Vector3& operator [](const int idx);
+		Matrix3 operator *(const Matrix3 rhs);
+		Matrix3 operator *(const float rhs);
+		Matrix3 operator /(const float rhs);
+		Matrix3 operator +(const Matrix3 rhs);
+		Matrix3 operator -(const Matrix3 rhs);
+		Matrix3 operator *(const Quat rhs);
+		Vector3 operator *(const Vector3 rhs);
+		Vector3 col( const int );
+		Vector3 row( const int );
+		void set_col( const int which, const Vector3 col );
+		float trace();
+		bool is_identity();
+		void transpose();
+		void print( const std::string name=std::string() );
+		Vector3 getEulerAngles();
+
+		// Construction helpers
+		static Matrix3 rotation( const float angle_in_rad, const Vector3 axis );
+		static Matrix3 rotation( const Vector3 hpr_in_rad );
+		static Matrix3 rotation( const Quat rot );
+		static Matrix3 rotation( const Vector3 from, const Vector3 to );
+		static Matrix3 translation( const Vector3 trans );
+		static Matrix3 scale( const float, const float, const float );
+		static Matrix3 scale( const float );
+		static Matrix3 transpose( const Matrix3 );
+
+	protected:
+		Vector3 _cols[3];
+	};
+
+	class Matrix4 {
+	public:
+		Matrix4();
+		Matrix4( const Matrix4& );
+		Matrix4( const Matrix3& );
+		Matrix4( const float mat[4][4] );
+		Matrix4( const float m00, const float m10, const float m20, const float m30, const float m01, const float m11, const float m21, const float m31, const float m02, const float m12, const float m22, const float m32, const float m03, const float m13, const float m23, const float m33 );
+		void fill( float mat[4][4] );
+		Matrix4( const Vector4 col0, const Vector4 col1, const Vector4 col2, const Vector4 col3 );
+		Vector4& operator [](const int idx);
+		Matrix4 operator *(const Matrix4 rhs);
+		Matrix4 operator *(const float rhs);
+		Matrix4 operator /(const float rhs);
+		Matrix4 operator +(const Matrix4 rhs);
+		Matrix4 operator -(const Matrix4 rhs);
+		Matrix4 operator *(const Quat rhs);
+		Vector4 operator *(const Vector4 rhs);
+		Vector4 col( const int );
+		Vector4 row( const int );
+		void set_col( const int which, const Vector4 col );
+		float trace();
+		bool is_identity();
+		void transpose();
+		void print( const std::string name=std::string() );
+		Vector3 getEulerAngles();
+
+		// Construction helpers
+		static Matrix4 rotation( const float angle_in_rad, const Vector3 axis );
+		static Matrix4 rotation( const Quat rot );
+		static Matrix4 rotation( const Vector3 from, const Vector3 to );
+		static Matrix4 rotation( const Vector3 hpr_in_rad );
+		static Matrix4 translation( const Vector3 trans );
+		static Matrix4 scale( const float, const float, const float );
+		static Matrix4 scale( const float );
+		static Matrix4 transpose( const Matrix4 );
+
+	protected:
+		Vector4 _cols[4];
+	};
+
+	class Line {
+		friend class Plane;
+		public:
+			Line( const Line& );
+			Line( const Vector3& dir, const Vector3& pos ); 
+
+			Vector3 getClosestPoint( const Vector3& );
+			
+		private:
+			Vector3 _dir;
+			Vector3 _pos1;
+			Vector3 _pos2;
+	};
+
+
+	class Plane	{
+	public:
+		Plane(); 
+		Plane( const Plane&	);
+		Plane( Vector3 p0, Vector3 p1, Vector3 p2 );
+		Plane( Vector3 point, Vector3 normal );
+
+		void init();
+		double whichSide( Vector3 p );
+		Vector3 getClosestPoint( Vector3 in );
+		Vector3 intersect( const Line line );
+		Vector3 getNormal();
+
+	private:
+		double _a, _b, _c, _d;
+		Vector3	_norm;
+		Vector3	_p0, _p1, _p2;
+
+	};
+
+
+
+
+	#include "sixense_math.cpp"
+
+}
+
+
+#endif
diff --git a/SixenseSDK/include/sixense_utils/accelerometer.hpp b/SixenseSDK/include/sixense_utils/accelerometer.hpp
new file mode 100755
index 0000000..e69de29
--- /dev/null
+++ b/SixenseSDK/include/sixense_utils/accelerometer.hpp
diff --git a/SixenseSDK/include/sixense_utils/button_states.hpp b/SixenseSDK/include/sixense_utils/button_states.hpp
new file mode 100755
index 0000000..a1d1936
--- /dev/null
+++ b/SixenseSDK/include/sixense_utils/button_states.hpp
@@ -0,0 +1,106 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+#ifndef SIXENSE_UTILS_BUTTON_STATES_HPP
+#define SIXENSE_UTILS_BUTTON_STATES_HPP
+
+#pragma warning(push)
+#pragma warning( disable:4251 )
+
+#include "sixense_utils/export.hpp"
+#include "sixense_utils/interfaces.hpp"
+
+#include <sixense.h>
+#include <sixense_math.hpp>
+
+namespace sixenseUtils {
+
+	// LaserPointer computes a ray that shoots from the controller and intersects with the screen. 
+	class SIXENSE_UTILS_EXPORT ButtonStates : public IButtonStates {
+
+	public:
+
+		ButtonStates();
+		void update( sixenseControllerData *cd );
+		bool buttonJustPressed( unsigned short which_button );
+		bool buttonJustReleased( unsigned short which_button );
+
+		void setTriggerThreshold( float thresh );
+		bool triggerJustPressed();
+		bool triggerJustReleased();
+
+		void setStickThreshold( float thresh );
+		bool stickJustPressed( Direction which );
+		bool stickJustReleased( Direction which );
+
+		void setAbsoluteTiltAngleThresholdInDeg( float thresh );
+		void setRelativeTiltAngleThresholdInDeg( float thresh );
+
+		// Relative tilts are an orientation change relative to the orientation last time setRelativeOrigin() was called
+		void setRelativeOrigin();
+		void startPointGesture();
+		void stopPointGesture();
+		bool relativeTiltJustStarted( Direction which );
+		bool relativeTiltJustStopped( Direction which );
+
+		// Absolute gestures are just relative to the world
+		bool absoluteTiltJustStarted( Direction which );
+		bool absoluteTiltJustStopped( Direction which );
+
+		float _trigger_threshold;
+
+		bool justStarted( ActionType action, int arg );
+		bool justStopped( ActionType action, int arg );
+
+	protected:
+		void updateJoystick( sixenseControllerData *cd );
+		void updateTrigger( sixenseControllerData *cd );
+		void updateButtons( sixenseControllerData *cd );
+		void updateTilt( sixenseControllerData *cd );
+
+	private:
+
+		// buttons
+		unsigned short _just_pressed;
+		unsigned short _just_released;
+		unsigned short _last_buttons;
+
+		// trigger
+		bool _trigger_just_pressed;
+		bool _trigger_just_released;
+		float _last_trigger_state;
+
+		// joystick
+		float _joystick_threshold;
+		float _last_joystick[2];
+		bool _joystick_just_pressed[4], _joystick_just_released[4]; // 4 stick directions
+
+
+		// tilt
+		float _absolute_tilt_angle_threshold_in_deg, _relative_tilt_angle_threshold_in_deg;
+		sixenseMath::Vector3 _last_point_vec;
+
+		bool _point_gesture_active;
+
+		bool _last_relative_tilt_states[6];
+		bool _relative_tilt_just_started[6], _relative_tilt_just_stopped[6]; // 6 tilt directions
+
+		bool _last_absolute_tilt_states[6];
+		bool _absolute_tilt_just_started[6], _absolute_tilt_just_stopped[6]; // 6 tilt directions
+		bool _should_zero_relative_origin;
+		sixenseMath::Matrix3 _relative_origin;
+
+	};
+
+}
+
+#pragma warning(pop)
+
+#endif
+
diff --git a/SixenseSDK/include/sixense_utils/controller_manager/controller_manager.hpp b/SixenseSDK/include/sixense_utils/controller_manager/controller_manager.hpp
new file mode 100755
index 0000000..f6b674d
--- /dev/null
+++ b/SixenseSDK/include/sixense_utils/controller_manager/controller_manager.hpp
@@ -0,0 +1,91 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+#ifndef CONTROLLER_MANAGER_HPP
+#define CONTROLLER_MANAGER_HPP
+
+#pragma warning(push)
+#pragma warning( disable:4251 )
+
+#include "sixense_utils/export.hpp"
+#include "sixense_utils/interfaces.hpp"
+#include "sixense_utils/sixense_utils_string.hpp"
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include <sixense.h>
+
+namespace sixenseUtils {
+
+	class SIXENSE_UTILS_EXPORT ControllerManager : public IControllerManager {
+
+	public:
+		~ControllerManager();
+
+		void setGameType( game_type gt );
+		game_type getGameType();
+
+		// Update the ControllerManager. Should be called each frame.
+		void update( sixenseAllControllerData * );
+
+		// Get the controller index for the given description, ie player 1's left hand is controller index 3
+		int getIndex( controller_desc );
+
+		// Force the user to rebind the controllers to the player slots.
+		void rebind();
+
+		// Register a callback that will get called when the mode changes
+		void registerSetupCallback( setup_callback );
+
+		// Returns true if the application should show the menu system
+		bool isMenuVisible();
+
+		// Returns the filename of the recommended texture for this step
+		const char* getTextureFileName();
+
+		// Returns a string describing this step
+		const char* getStepString();
+
+
+		ControllerManager::sound_type shouldPlaySound();
+
+		static ControllerManager *getTheControllerManager();
+
+		static controller_desc controllerDescFromString( sixense_utils_string str );
+
+		setup_step getCurrentStep();
+
+	protected:
+		ControllerManager();
+
+		game_type _game_type;
+
+		// A pointer to the current step
+		setup_step _current_step;
+
+		// A list of maps, one map of steps for each of the game_types.
+		std::vector< std::map< setup_step, class base_step*> >_steps;
+
+		int _controller_map[LAST_CONTROLLER_DESC];
+
+		setup_callback _callback;
+
+	};
+
+	// Singleton
+	SIXENSE_UTILS_EXPORT ControllerManager *getTheControllerManager();
+
+}
+
+#pragma warning(pop)
+
+#endif
+
diff --git a/SixenseSDK/include/sixense_utils/derivatives.hpp b/SixenseSDK/include/sixense_utils/derivatives.hpp
new file mode 100755
index 0000000..cd3c31f
--- /dev/null
+++ b/SixenseSDK/include/sixense_utils/derivatives.hpp
@@ -0,0 +1,63 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+#ifndef SIXENSE_UTILS_DERIVATIVES_HPP
+#define SIXENSE_UTILS_DERIVATIVES_HPP
+
+#pragma warning(push)
+#pragma warning( disable:4251 )
+
+#include "sixense.h"
+#include "sixense_math.hpp"
+
+#include <deque>
+
+#include "sixense_utils/export.hpp"
+#include "sixense_utils/interfaces.hpp"
+
+namespace sixenseUtils {
+
+	class SIXENSE_UTILS_EXPORT Derivatives : public IDerivatives {
+
+	public:
+		Derivatives( sixenseMath::Vector3 offset_vec = sixenseMath::Vector3() );
+
+		void update( sixenseControllerData *cd );
+
+		sixenseMath::Vector3 getPosition();
+		sixenseMath::Vector3 getVelocity();
+		sixenseMath::Vector3 getAcceleration();
+
+		sixenseMath::Vector3 getRSquared();
+
+		void setOffset( sixenseMath::Vector3 offset );
+		sixenseMath::Vector3 getOffset();
+
+	protected:
+		std::deque<unsigned char> _last_n_sequence;
+
+		std::deque<float> _last_n_times;
+
+		std::deque<sixenseMath::Vector3> _last_n_pos;
+
+		sixenseMath::Vector3 _pos;
+		sixenseMath::Vector3 _vel;
+		sixenseMath::Vector3 _accel;
+
+		sixenseMath::Vector3 _r_squared;
+
+		sixenseMath::Vector3 _offset_vec;
+
+	};
+
+}
+
+#pragma warning(pop)
+
+#endif
diff --git a/SixenseSDK/include/sixense_utils/event_triggers.hpp b/SixenseSDK/include/sixense_utils/event_triggers.hpp
new file mode 100755
index 0000000..53454ef
--- /dev/null
+++ b/SixenseSDK/include/sixense_utils/event_triggers.hpp
@@ -0,0 +1,110 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+#ifndef EVENT_TRIGGERS_HPP
+#define EVENT_TRIGGERS_HPP
+
+#pragma warning(push)
+#pragma warning( disable:4251 )
+
+#include <sixense_math.hpp>
+
+namespace sixenseUtils {
+
+		// Classes for triggering events based on joystick or trigger positions
+
+	// These are classes that call the supplied EventTriggerBase's 'trigger()' events when triggered. They can be 
+	// created and bound to a switch.
+
+	class EventTriggerBase {
+	public:
+		virtual void trigger() const = 0;
+	};
+
+	// Do nothing
+	class NullEventTrigger : public EventTriggerBase {
+	public:
+		virtual void trigger() const {};
+	};
+
+
+
+	// These classes monitor a floating point value, and trigger their events when the value crosses
+	// a threshold, both rising and falling.
+
+	class EventSwitchBase { 
+	public:
+		EventSwitchBase( float thresh, const EventTriggerBase *positive_transition_event, const EventTriggerBase *negative_transition_event ) : _last_state(false), _thresh(thresh), _positive_transition_event(positive_transition_event), _negative_transition_event(negative_transition_event) {}
+		~EventSwitchBase() {
+			delete _positive_transition_event;
+			delete _negative_transition_event;
+		}
+		void test( float val ) {
+			if( val > _thresh  &&  !_last_state ) {
+				if( _positive_transition_event ) {
+					_positive_transition_event->trigger();
+				}
+				_last_state = true;
+			}
+			if( val < _thresh  &&  _last_state ) {
+				if( _negative_transition_event ) {
+					_negative_transition_event->trigger();
+				}
+				_last_state = false;
+			}
+		}
+
+		bool getLastState( void ) {
+			return _last_state;
+		}
+
+	protected:
+		bool _last_state;
+		float _thresh;
+		const EventTriggerBase *_positive_transition_event;
+		const EventTriggerBase *_negative_transition_event;
+
+	private:
+		EventSwitchBase() {}
+	};
+
+	class PlaneCrossEventSwitch : public EventSwitchBase {
+	public:
+		PlaneCrossEventSwitch( sixenseMath::Plane plane, float thresh, const EventTriggerBase *positive_transition_event, const EventTriggerBase *negative_transition_event ) : EventSwitchBase( thresh, positive_transition_event, negative_transition_event ),
+			_plane(plane) {}
+
+		void test( sixenseMath::Vector3 pt ) {
+			EventSwitchBase::test( -1.0f * (float)_plane.whichSide( pt ) );
+		}
+	private:
+		sixenseMath::Plane _plane;
+	};
+
+	class BinaryEventSwitch : public EventSwitchBase {
+	public:
+		BinaryEventSwitch( const EventTriggerBase *positive_transition_event, const EventTriggerBase *negative_transition_event ) : EventSwitchBase( 0.5f, positive_transition_event, negative_transition_event ) {}
+
+		void test( bool val ) {
+			EventSwitchBase::test( val ? 1.0f : 0.0f );
+		}
+	};
+
+	class ValuatorEventSwitch : public EventSwitchBase {
+	public:
+		ValuatorEventSwitch( float thresh, const EventTriggerBase *positive_transition_event, const EventTriggerBase *negative_transition_event ) : EventSwitchBase( thresh, positive_transition_event, negative_transition_event ){}
+		void test( float val ) {
+			EventSwitchBase::test( val );
+		}
+	};
+
+
+}
+#pragma warning(pop)
+
+#endif
diff --git a/SixenseSDK/include/sixense_utils/export.hpp b/SixenseSDK/include/sixense_utils/export.hpp
new file mode 100755
index 0000000..3f22ce7
--- /dev/null
+++ b/SixenseSDK/include/sixense_utils/export.hpp
@@ -0,0 +1,27 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+#ifndef SIXENSE_UTILS_EXPORT_HPP
+#define SIXENSE_UTILS_EXPORT_HPP
+
+#if defined(WIN32)
+  #ifdef SIXENSE_UTILS_STATIC_LIB
+    #define SIXENSE_UTILS_EXPORT 
+  #else
+		#ifdef BUILDING_SIXENSE_UTILS
+			#define SIXENSE_UTILS_EXPORT __declspec(dllexport)
+		#else
+			#define SIXENSE_UTILS_EXPORT __declspec(dllimport)
+		#endif
+	#endif
+#else
+  #define SIXENSE_UTILS_EXPORT 
+#endif
+
+#endif
diff --git a/SixenseSDK/include/sixense_utils/fps.hpp b/SixenseSDK/include/sixense_utils/fps.hpp
new file mode 100755
index 0000000..44bcb51
--- /dev/null
+++ b/SixenseSDK/include/sixense_utils/fps.hpp
@@ -0,0 +1,327 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+#ifndef SIXENSE_PORT_UTILS_FPS_HPP
+#define SIXENSE_PORT_UTILS_FPS_HPP
+
+#pragma warning(push)
+#pragma warning( disable:4251 )
+
+#include "sixense.h"
+#include "sixense_math.hpp"
+#include "sixense_utils/interfaces.hpp"
+
+#include <vector>
+#include <map>
+
+using sixenseMath::Vector2;
+using sixenseMath::Vector3;
+using sixenseMath::Matrix3;
+using sixenseMath::Matrix4;
+using sixenseMath::Quat;
+
+#include "export.hpp"
+
+#include "sixense_utils/derivatives.hpp"
+#include "sixense_utils/button_states.hpp"
+#include "sixense_utils/sixense_utils_string.hpp"
+
+namespace sixenseUtils {
+
+	class SIXENSE_UTILS_EXPORT FPSViewAngles : public IFPSViewAngles {
+
+	public:
+		FPSViewAngles();
+
+		void setGame( const char* game_name );
+
+		void setMode( fps_mode mode );
+		fps_mode getMode();
+
+		int update( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms=0.0f );
+
+		Vector3 getViewAngles(); // The final view angles, ie the feet plus the aiming from the controller
+		Vector3 getViewAngleOffset(); // The negative controller aim, used to keep the camera stationary in metroid mode
+
+		Vector3 getSpinSpeed();
+
+		void forceViewAngles( fps_mode mode, Vector3 ); // Used to initialize the view direction when switching into stick spin mode
+
+		void setFeetAnglesMetroid( Vector3 angles );
+		Vector3 getFeetAnglesMetroid();
+
+		float getTestVal();
+
+		void setParameter( fps_params param, float val );
+		float getParameter( fps_params param );
+
+		void setFov( float hfov, float vfov );
+		void getFov( float *hfov, float *vfov );
+
+		void setHoldingTurnSpeed( float horiz, float vert );
+
+		void setRatcheting( bool );
+		bool isRatcheting();
+
+		void reset();
+
+		void forceMetroidBlend( float blend_fraction );
+
+	protected:
+		void computeAimOffset1to1( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms );
+		void computeAimOffsetPointer( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms );
+
+		void computeFeetAnglesOffsetStickSpin( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms );
+		void computeFeetAnglesOffsetMetroid( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms );
+		void computeFeetAnglesOffsetRatchet( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms );
+
+		void computeSpringViewOffset();
+
+		Vector3 clampViewAngles( Vector3 angles, Vector3 max_vals );
+
+	private:
+		fps_mode _mode;
+
+		// Keep track of the game so we can do 
+		std::string _game_name;
+
+		// Different types of aim offsets
+		Vector3 _aim_offset_1to1_metroid, _aim_offset_1to1_mouselook; // need two because they're scaled differently
+		Vector3 _aim_offset_pointer;
+
+		Vector3 _spring_view_offset;
+		float _spring_vel;
+		Quat _spring_view;
+
+		// Different types of feet angles
+		Vector3 _feet_angles_stick_spin;
+		Vector3 _feet_angles_metroid;
+		Vector3 _feet_angles_ratchet;
+
+		Vector3 _feet_angles, _aim_offset_1to1;
+
+		// Parameters to control the different modes
+		std::vector<float> _param_vals;
+
+		ButtonStates _left_button_watcher, _right_button_watcher;
+
+		Vector3 _ratchet_base_orientation;
+
+		// Are we flicking? (0-1)
+		float _flicking;
+
+		bool _ratcheting;
+		bool _just_started_ratcheting, _just_stopped_ratcheting;
+
+		float _roll;
+
+		// Keep track of the fov
+		float _hfov, _vfov;
+
+		// This is used to blend pitch smoothly between modes
+		float _blend_pitch_val;
+
+		Vector3 _blend_view_offset;
+		Vector3 _prev_blend_view_offset;
+
+		Vector3 _mode_switch_blend_angle_start, _mode_switch_blend_angle_end, _mode_switch_blend_angles;
+
+		double _mode_switch_blend_duration_ms;
+		double _mode_switch_blend_start_time;
+
+		float _test_val;
+
+		Vector3 _holding_turn_speed;
+
+		// Allow metroid spinning to blend in over time. Useful when switching from 1-to-1 mode.
+		float _metroid_blend_start_time;
+		int _metroid_blend_in_or_out; // 0 == in, 1 == out
+
+		Vector3 _metroid_spin_speed, _stick_spin_speed;
+
+		float _force_blend_aim_metroid_start_time;
+		float _force_blend_aim_metroid_pitch_mult;
+		float _force_blend_aim_metroid_heading_mult;
+	};
+
+
+
+
+	class SIXENSE_UTILS_EXPORT FPSEvents : public IFPSEvents {
+
+	public:
+
+		FPSEvents(); 
+
+		void setGame( const char* game_name );
+
+		void setBinding( fps_event, fps_controller, fps_action, int param );
+
+		void setPointGestureButton( int );
+
+		bool isPointGestureActive();
+
+		int update( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms=0.0f );
+
+		bool eventActive( fps_event event_query );
+		bool eventStarted( fps_event event_query );
+		bool eventStopped( fps_event event_query );
+
+		Vector3 getGrenadeThrowVelocity();
+
+		void setParameter( fps_params param, float val );
+		float getParameter( fps_params param );
+
+	private:
+
+		typedef struct {
+			fps_event _event;
+			fps_controller _controller;
+			fps_action _action;
+			int _param;
+		} fps_binding;
+
+	protected:
+		bool testButtonBinding( fps_binding );
+		bool testJoystickBinding( fps_binding );
+		bool testTriggerBinding( fps_binding );
+		bool testTiltBinding( fps_binding );
+		bool testPointBinding( fps_binding );
+		bool testVelocityBinding( fps_binding );
+
+	private:
+
+		// keep them in a map so there's only one binding per event
+		std::map <fps_event, fps_binding> _bindings;
+
+		ButtonStates _button_states[2]; // one for each controller, in order of fps_controller, so 0=left 1=right
+
+		std::vector<bool> _event_started, _event_stopped, _event_persistent_state;
+		Derivatives _left_deriv, _right_deriv;
+		Derivatives _left_deriv_offset, _right_deriv_offset;
+
+		// Keep track of the velocity of the hand when a grenade is thrown
+		Vector3 _grenade_throw_vel;
+
+		// Keep track of the game so we can do 
+		std::string _game_name;
+
+		// Parameters to control the different modes
+		std::vector<float> _param_vals;
+
+		// The z depth at which one to one mode started. 
+		Vector3 _one_to_one_start_pos;
+
+		// This is set when a point gesture is in process
+		bool _point_gesture_active;
+
+		// The button that is used to engage point gestures
+		unsigned short _point_gesture_button;
+
+	};
+
+
+	class SIXENSE_UTILS_EXPORT FPSPlayerMovement : public IFPSPlayerMovement{
+
+	public:
+		FPSPlayerMovement();
+
+		void setGame( const char* game_name );
+
+		int update( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms=0.0f );
+
+		Vector2 getWalkDir();
+
+		void setParameter( fps_movement_params param, float val );
+		float getParameter( fps_movement_params param );
+
+	private:
+		Vector2 _walk_dir;
+		std::string _game_name;
+
+		// Parameters to control the different modes
+		std::vector<float> _param_vals;
+
+	};
+
+	class SIXENSE_UTILS_EXPORT FPSMeleeWeapon {
+	public:
+		FPSMeleeWeapon();
+
+		int update( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms=0.0f );
+
+		// Weapon movement
+		Vector3 getMeleeWeaponPos();
+		Vector3 getMeleeWeaponBladePos();
+		Matrix3 getMeleeWeaponMatrix();
+
+		// Attack
+		bool swingAttackStarted(); // since last update
+		bool isAttacking();
+		
+		// 1-to-1 mode (when the melee weapon should be moving with the controller and not animated
+		bool OneToOneModeStarted();
+		bool OneToOneModeStopped();
+		bool isInOneToOneMode();
+
+		Vector3 getSwingAttackStartPos();
+		Vector3 getSwingAttackDir();
+
+		void setToggleMode( bool mode );
+
+		void forceOneToOneMode( bool mode );
+
+		// Parameters to control the different modes
+		typedef enum {
+			SWING_START_VELOCITY,
+			SWING_STOP_VELOCITY,
+			SWING_REARM_VELOCITY,
+			BLADE_LENGTH,
+			CONTROLLER_ANGLE,
+
+			LEFT_HANDED,
+
+			LAST_MELEE_EVENTS_PARAM
+		} melee_params;
+
+		void setParameter( melee_params param, float val );
+		float getParameter( melee_params param );
+
+
+	private:
+		Derivatives _left_deriv, _right_deriv;
+		bool _armed; // Can we attack again?
+		bool _just_started_attack, _attacking;
+		bool _just_started_1to1, _just_stopped_1to1, _in_1to1_mode;
+		int _swing_wait_count; // How many frames to wait until starting a swing
+		int _swing_count; // How long have we been swinging?
+		Matrix4 _start_swing_mat;
+		Vector3 _swing_dir_vec; // Direction we are attacking in
+		Vector3 _swing_start_pos; // Position swing started from
+
+		float _last_trigger_pos;
+
+		Vector3 _weap_pos, _last_weap_pos;
+		Matrix3 _weap_mat, _last_weap_mat;
+
+		Vector3 _weap_blade_pos;
+
+		bool _toggle_mode;
+
+		// Parameters to control the different modes
+		std::vector<float> _param_vals;
+
+	};
+
+}
+
+#endif
+
+#pragma warning(pop)
+
diff --git a/SixenseSDK/include/sixense_utils/fps_rewrite.hpp b/SixenseSDK/include/sixense_utils/fps_rewrite.hpp
new file mode 100755
index 0000000..f1ddd19
--- /dev/null
+++ b/SixenseSDK/include/sixense_utils/fps_rewrite.hpp
@@ -0,0 +1,270 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+#ifndef SIXENSE_PORT_UTILS_FPS_HPP
+#define SIXENSE_PORT_UTILS_FPS_HPP
+
+#pragma warning(push)
+#pragma warning( disable:4251 )
+
+#include "sixense.h"
+#include "sixense_math.hpp"
+#include "sixense_utils/interfaces.hpp"
+
+#include <vector>
+#include <map>
+
+using sixenseMath::Vector2;
+using sixenseMath::Vector3;
+using sixenseMath::Matrix3;
+using sixenseMath::Matrix4;
+using sixenseMath::Quat;
+
+#include "export.hpp"
+
+#include "sixense_utils/derivatives.hpp"
+#include "sixense_utils/button_states.hpp"
+#include "sixense_utils/sixense_utils_string.hpp"
+
+namespace sixenseUtils {
+
+	class SIXENSE_UTILS_EXPORT FPSViewAngles : public IFPSViewAngles {
+
+	public:
+		FPSViewAngles();
+
+		void setGame( const char* game_name );
+
+		void setMode( fps_mode mode );
+		fps_mode getMode();
+
+		int update( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms=0.0f );
+
+		Vector3 getViewAngles(); // The final view angles, ie the feet plus the aiming from the controller
+		Vector3 getViewAngleOffset(); // The negative controller aim, used to keep the camera stationary in metroid mode
+
+		Vector3 getSpinSpeed();
+
+		void forceViewAngles( fps_mode mode, Vector3 ); // Used to initialize the view direction when switching into stick spin mode
+
+		void setFeetAnglesMetroid( Vector3 angles );
+		Vector3 getFeetAnglesMetroid();
+
+		float getTestVal();
+
+		void setParameter( fps_params param, float val );
+		float getParameter( fps_params param );
+
+		void setFov( float hfov, float vfov );
+		void getFov( float *hfov, float *vfov );
+
+		void setHoldingTurnSpeed( float horiz, float vert );
+
+		void setRatcheting( bool );
+		bool isRatcheting();
+
+		void reset();
+
+		void forceMetroidBlend( float blend_fraction );
+
+	protected:
+		Vector2 _feet_angles;
+		Vector2 _torso_angles;
+		Vector2 _view_offset;
+
+		Vector2 _ratchet_base_angles;
+
+	private:
+		fps_mode _mode;
+
+		// Keep track of the game so we can do 
+		std::string _game_name;
+
+		// Parameters to control the different modes
+		std::vector<float> _param_vals;
+
+		bool _ratcheting;
+		bool _just_started_ratcheting, _just_stopped_ratcheting;
+	};
+
+
+
+
+	class SIXENSE_UTILS_EXPORT FPSEvents : public IFPSEvents {
+
+	public:
+
+		FPSEvents(); 
+
+		void setGame( const char* game_name );
+
+		void setBinding( fps_event, fps_controller, fps_action, int param );
+
+		void setPointGestureButton( int );
+
+		bool isPointGestureActive();
+
+		int update( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms=0.0f );
+
+		bool eventActive( fps_event event_query );
+		bool eventStarted( fps_event event_query );
+		bool eventStopped( fps_event event_query );
+
+		Vector3 getGrenadeThrowVelocity();
+
+		void setParameter( fps_params param, float val );
+		float getParameter( fps_params param );
+
+	private:
+
+		typedef struct {
+			fps_event _event;
+			fps_controller _controller;
+			fps_action _action;
+			int _param;
+		} fps_binding;
+
+	protected:
+		bool testButtonBinding( fps_binding );
+		bool testJoystickBinding( fps_binding );
+		bool testTriggerBinding( fps_binding );
+		bool testTiltBinding( fps_binding );
+		bool testPointBinding( fps_binding );
+		bool testVelocityBinding( fps_binding );
+
+	private:
+
+		// keep them in a map so there's only one binding per event
+		std::map <fps_event, fps_binding> _bindings;
+
+		ButtonStates _button_states[2]; // one for each controller, in order of fps_controller, so 0=left 1=right
+
+		std::vector<bool> _event_started, _event_stopped, _event_persistent_state;
+		Derivatives _left_deriv, _right_deriv;
+		Derivatives _left_deriv_offset, _right_deriv_offset;
+
+		// Keep track of the velocity of the hand when a grenade is thrown
+		Vector3 _grenade_throw_vel;
+
+		// Keep track of the game so we can do 
+		std::string _game_name;
+
+		// Parameters to control the different modes
+		std::vector<float> _param_vals;
+
+		// The z depth at which one to one mode started. 
+		Vector3 _one_to_one_start_pos;
+
+		// This is set when a point gesture is in process
+		bool _point_gesture_active;
+
+		// The button that is used to engage point gestures
+		unsigned short _point_gesture_button;
+
+	};
+
+
+	class SIXENSE_UTILS_EXPORT FPSPlayerMovement : public IFPSPlayerMovement{
+
+	public:
+		FPSPlayerMovement();
+
+		void setGame( const char* game_name );
+
+		int update( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms=0.0f );
+
+		Vector2 getWalkDir();
+
+		void setParameter( fps_movement_params param, float val );
+		float getParameter( fps_movement_params param );
+
+	private:
+		Vector2 _walk_dir;
+		std::string _game_name;
+
+		// Parameters to control the different modes
+		std::vector<float> _param_vals;
+
+	};
+
+	class SIXENSE_UTILS_EXPORT FPSMeleeWeapon {
+	public:
+		FPSMeleeWeapon();
+
+		int update( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms=0.0f );
+
+		// Weapon movement
+		Vector3 getMeleeWeaponPos();
+		Vector3 getMeleeWeaponBladePos();
+		Matrix3 getMeleeWeaponMatrix();
+
+		// Attack
+		bool swingAttackStarted(); // since last update
+		bool isAttacking();
+		
+		// 1-to-1 mode (when the melee weapon should be moving with the controller and not animated
+		bool OneToOneModeStarted();
+		bool OneToOneModeStopped();
+		bool isInOneToOneMode();
+
+		Vector3 getSwingAttackStartPos();
+		Vector3 getSwingAttackDir();
+
+		void setToggleMode( bool mode );
+
+		void forceOneToOneMode( bool mode );
+
+		// Parameters to control the different modes
+		typedef enum {
+			SWING_START_VELOCITY,
+			SWING_STOP_VELOCITY,
+			SWING_REARM_VELOCITY,
+			BLADE_LENGTH,
+			CONTROLLER_ANGLE,
+
+			LEFT_HANDED,
+
+			LAST_MELEE_EVENTS_PARAM
+		} melee_params;
+
+		void setParameter( melee_params param, float val );
+		float getParameter( melee_params param );
+
+
+	private:
+		Derivatives _left_deriv, _right_deriv;
+		bool _armed; // Can we attack again?
+		bool _just_started_attack, _attacking;
+		bool _just_started_1to1, _just_stopped_1to1, _in_1to1_mode;
+		int _swing_wait_count; // How many frames to wait until starting a swing
+		int _swing_count; // How long have we been swinging?
+		Matrix4 _start_swing_mat;
+		Vector3 _swing_dir_vec; // Direction we are attacking in
+		Vector3 _swing_start_pos; // Position swing started from
+
+		float _last_trigger_pos;
+
+		Vector3 _weap_pos, _last_weap_pos;
+		Matrix3 _weap_mat, _last_weap_mat;
+
+		Vector3 _weap_blade_pos;
+
+		bool _toggle_mode;
+
+		// Parameters to control the different modes
+		std::vector<float> _param_vals;
+
+	};
+
+}
+
+#endif
+
+#pragma warning(pop)
+
diff --git a/SixenseSDK/include/sixense_utils/interfaces.hpp b/SixenseSDK/include/sixense_utils/interfaces.hpp
new file mode 100755
index 0000000..5470221
--- /dev/null
+++ b/SixenseSDK/include/sixense_utils/interfaces.hpp
@@ -0,0 +1,442 @@
+#ifndef SIXENSE_UTILS_INTERFACES_HPP
+#define SIXENSE_UTILS_INTERFACES_HPP
+
+#include <sixense.h>
+#include <sixense_math.hpp>
+
+namespace sixenseUtils {
+
+	class IButtonStates {
+	public:
+
+		typedef enum {
+			DIR_UP,
+			DIR_DOWN,
+			DIR_LEFT,
+			DIR_RIGHT,
+			DIR_CW,
+			DIR_CCW,
+			DIR_FORWARD,
+			DIR_BACKWARD
+		} Direction;
+
+		typedef enum {
+			ACTION_BUTTON_PRESS,
+			ACTION_JOYSTICK_MOVE,
+			ACTION_TRIGGER_PRESS,
+			ACTION_TILT_GESTURE,
+			ACTION_POINT_GESTURE, 
+			ACTION_VELOCITY_GESTURE
+		} ActionType;
+
+		virtual void update( sixenseControllerData *cd )=0;
+		virtual bool buttonJustPressed( unsigned short which_button )=0;
+		virtual bool buttonJustReleased( unsigned short which_button )=0;
+
+		virtual void setTriggerThreshold( float thresh )=0;
+		virtual bool triggerJustPressed()=0;
+		virtual bool triggerJustReleased()=0;
+
+		virtual void setStickThreshold( float thresh )=0;
+		virtual bool stickJustPressed( Direction which )=0;
+		virtual bool stickJustReleased( Direction which )=0;
+
+		virtual void setAbsoluteTiltAngleThresholdInDeg( float thresh )=0;
+		virtual void setRelativeTiltAngleThresholdInDeg( float thresh )=0;
+
+		// Relative tilts are an orientation change relative to the orientation last time setRelativeOrigin() was called
+		virtual void setRelativeOrigin()=0;
+		virtual void startPointGesture()=0;
+		virtual void stopPointGesture()=0;
+		virtual bool relativeTiltJustStarted( Direction which )=0;
+		virtual bool relativeTiltJustStopped( Direction which )=0;
+
+		// Absolute gestures are just relative to the world
+		virtual bool absoluteTiltJustStarted( Direction which )=0;
+		virtual bool absoluteTiltJustStopped( Direction which )=0;
+
+		virtual bool justStarted( ActionType action, int arg )=0;
+		virtual bool justStopped( ActionType action, int arg )=0;
+
+	};
+
+	class IDerivatives {
+	public:
+		virtual void update( sixenseControllerData *cd )=0;
+
+		virtual sixenseMath::Vector3 getPosition()=0;
+		virtual sixenseMath::Vector3 getVelocity()=0;
+		virtual sixenseMath::Vector3 getAcceleration()=0;
+
+		virtual sixenseMath::Vector3 getRSquared()=0;
+
+		virtual void setOffset( sixenseMath::Vector3 offset )=0;
+		virtual sixenseMath::Vector3 getOffset()=0;
+	};
+
+	class IFPSViewAngles {
+
+	public:
+		virtual void setGame( const char* game_name )=0;
+
+		typedef enum {
+			MOUSELOOK,
+			FREE_AIM_TWO_CONTROLLER,
+			FREE_AIM_ONE_CONTROLLER,
+			DUAL_ANALOG
+		} fps_mode;
+
+		virtual void setMode( fps_mode mode )=0;
+		virtual fps_mode getMode()=0;
+
+		virtual int update( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms=0.0f )=0;
+
+		virtual sixenseMath::Vector3 getViewAngles()=0; // The final view angles, ie the feet plus the aiming from the controller
+		virtual sixenseMath::Vector3 getViewAngleOffset()=0; // The negative controller aim, used to keep the camera stationary in metroid mode
+
+		virtual sixenseMath::Vector3 getSpinSpeed()=0;
+
+		virtual void forceViewAngles( fps_mode mode, sixenseMath::Vector3 )=0; // Used to initialize the view direction when switching into stick spin mode
+
+		virtual void setFeetAnglesMetroid( sixenseMath::Vector3 angles )=0;
+		virtual sixenseMath::Vector3 getFeetAnglesMetroid()=0;
+
+		virtual float getTestVal()=0;
+
+		// Parameters to control the different modes
+		typedef enum {
+			CONTROLLER_ANGLE_MODE,
+
+			AIM_1TO1_HEADING_MULTIPLIER,
+			AIM_1TO1_PITCH_MULTIPLIER,
+			AIM_1TO1_RATCHET_VERTICAL,
+
+			AIM_METROID_HEADING_MULTIPLIER,
+			AIM_METROID_PITCH_MULTIPLIER,
+			AIM_METROID_DEAD_ZONE_RADIUS,
+			AIM_METROID_ACCEL_BAND_SIZE,
+			AIM_METROID_MAX_SPEED,
+			AIM_METROID_AUTO_LEVEL_RATE,
+			AIM_METROID_ACCEL_BAND_EXPONENT,
+			AIM_METROID_SWITCH_BLEND_TIME_ENTER,
+			AIM_METROID_SWITCH_BLEND_TIME_EXIT,
+
+			FEET_ANGLES_OFFSET_STICK_SPIN_HORIZ_MULTIPLIER,
+			FEET_ANGLES_OFFSET_STICK_SPIN_VERT_MULTIPLIER,
+			FEET_ANGLES_OFFSET_STICK_SPIN_INVERT_PITCH,
+			FEET_ANGLES_OFFSET_STICK_SPIN_EXPONENT,
+
+			PITCH_CHANGE_BLEND_VAL,
+
+			SPRING_VIEW_ENABLED,
+			SPRING_VIEW_MIN_SPRING,
+			SPRING_VIEW_MAX_SPRING,
+			SPRING_VIEW_MIN_ANGLE,
+			SPRING_VIEW_MAX_ANGLE,
+
+			FEET_ANGLES_ALLOW_VERT_STICK_SPIN,
+
+			AIM_METROID_ACCEL_BAND_POWER,
+
+			HOLDING_TURN_SPEED,
+
+			ROLL_CORRECTION_BLEND,
+			EXIT_METROID_BLEND,
+
+			LEFT_HANDED,
+
+			LAST_FPS_VIEW_ANGLES_PARAM
+		} fps_params;
+
+		virtual void setParameter( fps_params param, float val )=0;
+		virtual float getParameter( fps_params param )=0;
+
+		virtual void setFov( float hfov, float vfov )=0;
+		virtual void getFov( float *hfov, float *vfov )=0;
+
+		virtual void setHoldingTurnSpeed( float horiz, float vert )=0;
+
+		virtual void setRatcheting( bool )=0;
+		virtual bool isRatcheting()=0;
+
+		virtual void reset()=0;
+
+		virtual void forceMetroidBlend( float blend_fraction )=0;
+	};
+
+	class IFPSEvents {
+
+	public:
+		typedef enum {
+			WALK_LEFT,
+			WALK_RIGHT,
+			WALK_FORWARD,
+			WALK_BACK,
+			JUMP,
+			USE,
+			PRIMARY_FIRE,
+			SECONDARY_FIRE,
+			ZOOM,
+			MELEE,
+			LEAN_LEFT,
+			LEAN_RIGHT,
+			CROUCH,
+			SPRINT,
+			THROW_GRENADE,
+			FLASHLIGHT,
+			NIGHTVISION,
+			RELOAD,
+			NEXT_WEAPON,
+			PREV_WEAPON,
+			ESC_KEY,
+			EQUIP_GRENADE,
+			MEDPACK_SWITCH,
+			GIVE,
+			NEXT_PRIMARY_WEAPON,
+			ONE_TO_ONE_CARRY,
+			EQUIP_MELEE,
+			EQUIP_MACHINEGUN,
+			EQUIP_PISTOL,
+			RATCHET,
+			LAST_FPS_EVENT
+		} fps_event;
+
+		typedef enum {
+			CONTROLLER_LEFT,
+			CONTROLLER_RIGHT
+		} fps_controller;
+
+		typedef enum {
+			ACTION_BUTTON_PRESS,
+			ACTION_JOYSTICK_MOVE,
+			ACTION_TRIGGER_PRESS,
+			ACTION_TILT_GESTURE,
+			ACTION_POINT_GESTURE, 
+			ACTION_VELOCITY_GESTURE
+		} fps_action;
+
+		typedef enum {
+			DIR_UP,
+			DIR_DOWN,
+			DIR_LEFT,
+			DIR_RIGHT,
+			DIR_FORWARD,
+			DIR_BACKWARD,
+			DIR_CW,
+			DIR_CCW
+		} fps_direction;
+
+		virtual void setGame( const char* game_name )=0;
+
+		virtual void setBinding( fps_event, fps_controller, fps_action, int param )=0;
+
+		virtual void setPointGestureButton( int )=0;
+
+		virtual bool isPointGestureActive()=0;
+
+		virtual int update( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms=0.0f )=0;
+
+		virtual bool eventActive( fps_event event_query )=0;
+		virtual bool eventStarted( fps_event event_query )=0;
+		virtual bool eventStopped( fps_event event_query )=0;
+
+		virtual sixenseMath::Vector3 getGrenadeThrowVelocity()=0;
+
+		// Parameters to control the different modes
+		typedef enum {
+			MELEE_SENSITIVITY,
+			WEAPON_SELECT_SENSITIVITY,
+			CROUCH_SENSITIVITY,
+			JUMP_SENSITIVITY,
+			RELOAD_SENSITIVITY,
+			THROW_SENSITIVITY,
+
+			CONTROLLER_ANGLE_MODE,
+
+			AUTO_ONE_TO_ONE_START_VEL,
+			AUTO_ONE_TO_ONE_START_ACCEL,
+			AUTO_ONE_TO_ONE_START_ACCEL_TIMER,
+			AUTO_ONE_TO_ONE_START_ANGLE_THRESH,
+
+			AUTO_ONE_TO_ONE_STOP_XY_DIST,
+			AUTO_ONE_TO_ONE_STOP_Z_DIST,
+
+			LEFT_HANDED,
+
+			LAST_FPS_EVENTS_PARAM
+		} fps_params;
+
+		virtual void setParameter( fps_params param, float val )=0;
+		virtual float getParameter( fps_params param )=0;
+
+
+	};
+
+	class IFPSPlayerMovement {
+	public:
+		virtual void setGame( const char* game_name )=0;
+
+		virtual int update( sixenseControllerData *left_cd, sixenseControllerData *right_cd, float frametime_in_ms=0.0f )=0;
+
+		virtual sixenseMath::Vector2 getWalkDir()=0;
+
+		// Parameters to control the different modes
+		typedef enum {
+			DEAD_ZONE_PERCENT,
+			EXPONENTIAL,
+			USE_RIGHT_HAND,
+
+			LEFT_HANDED,
+
+			LAST_FPS_MOVEMENT_PARAM
+		} fps_movement_params;
+
+		virtual void setParameter( fps_movement_params param, float val )=0;
+		virtual float getParameter( fps_movement_params param )=0;
+
+	};
+
+	class ILaserPointer {
+	public:
+		virtual void setScreenSize( sixenseMath::Vector2 width_and_height_in_mm )=0;
+		virtual void setScreenCenterOffsetFromBase( sixenseMath::Vector3 offset_in_mm )=0;
+
+		virtual sixenseMath::Vector2 getScreenSize()=0;
+		virtual sixenseMath::Vector3 getScreenCenterOffsetFromBase()=0;
+
+		virtual sixenseMath::Vector2 getIntersection( sixenseMath::Vector3 position, sixenseMath::Matrix3 rotation )=0;
+	};
+
+	class IMousePointer {
+	public:
+		virtual sixenseMath::Vector2 update( sixenseControllerData *cd )=0;
+
+		virtual void setSensitivity( float sensitivity )=0;
+		virtual void setAcceleration( float acceleration )=0;
+		virtual void setSlideEnabled( bool slide_enabled )=0;
+		virtual void setAspectRatio( float aspect_ratio )=0;
+
+		virtual float getRollAngle()=0;
+
+		virtual void setCenter()=0;
+	};
+
+	class IControllerManager {
+	public:
+
+		// Define the steps the manager goes through for the different modes
+		typedef enum {
+
+			SETUP_COMPLETE,
+
+			// P1C1 = One player, 1 controller
+
+			// One player one controller
+			P1C1_START, //1
+			P1C1_POWER_UP_0,
+			P1C1_POWER_UP_DONE,
+			P1C1_AIM_P1L,
+			P1C1_DONE,
+			P1C1_OUT_OF_RANGE,
+			P1C1_IDLE, // 7
+
+			// One player two controllers
+			P1C2_START, // 8 
+			P1C2_POWER_UP_0,
+			P1C2_POWER_UP_1,
+			P1C2_POWER_UP_DONE,
+			P1C2_AIM_P1L, // 12
+			P1C2_AIM_P1R,
+			P1C2_DONE,
+			P1C2_OUT_OF_RANGE,
+			P1C2_IDLE // 16
+
+		} setup_step;
+
+
+		typedef enum {
+			NO_SOUND,
+			SUCCESS_BEEP,
+			FAIL_BEEP
+		} sound_type;
+
+		typedef void (*setup_callback)( setup_step );
+
+		typedef enum {
+			ONE_PLAYER_ONE_CONTROLLER,
+			ONE_PLAYER_TWO_CONTROLLER,
+			TWO_PLAYER_ONE_CONTROLLER,
+			TWO_PLAYER_TWO_CONTROLLER,
+			THREE_PLAYER_ONE_CONTROLLER,
+			THREE_PLAYER_TWO_CONTROLLER,
+			FOUR_PLAYER_ONE_CONTROLLER,
+			FOUR_PLAYER_TWO_CONTROLLER
+		} game_type;
+
+		typedef enum {
+			P1L,
+			P1R,
+			P2L,
+			P2R,
+			P3L,
+			P3R,
+			P4L,
+			P4R, 
+			LAST_CONTROLLER_DESC
+		} controller_desc;
+
+		virtual void setGameType( game_type gt )=0;
+		virtual game_type getGameType()=0;
+
+		// Update the controller_manager. Should be called each frame.
+		virtual void update( sixenseAllControllerData * )=0;
+
+		// Get the controller index for the given description, ie player 1's left hand is controller index 3
+		virtual int getIndex( controller_desc )=0;
+
+		// Force the user to rebind the controllers to the player slots.
+		virtual void rebind()=0;
+
+		// Register a callback that will get called when the mode changes
+		virtual void registerSetupCallback( setup_callback )=0;
+
+		// Returns true if the application should show the menu system
+		virtual bool isMenuVisible()=0;
+
+		// Returns the filename of the recommended texture for this step
+		virtual const char* getTextureFileName()=0;
+
+		// Returns a string describing this step
+		virtual const char* getStepString()=0;
+
+
+		virtual IControllerManager::sound_type shouldPlaySound()=0;
+
+		virtual setup_step getCurrentStep()=0;
+
+	};
+
+	class IMouseAndKeyboard {
+	public:
+		virtual void sendMouseClick( int click, int release ) = 0; // 0 = none, 1 = left, 2 = right
+		virtual void sendMouseClick( int click, int release, int delay) = 0; // 0 = none, 1 = left, 2 = right
+		virtual void sendMouseWheelMove( int dir ) = 0; // in 'clicks', + = forwards - = backwards
+		virtual void sendKeyState( char key, int press, int release ) = 0;
+		virtual void sendKeyState( char key, int press, int release, int delay ) = 0;
+		virtual void releaseKey( char key ) = 0;
+		virtual void releaseMouseButton( int release ) = 0;
+		virtual void sendAbsoluteMouseMove( float x, float y ) = 0;
+		virtual void sendRelativeMouseMove( float x, float y ) = 0;
+		virtual void sendAbsoluteMouseMoveInPixels( int x, int y ) = 0;
+
+		virtual void getMousePos( int *x, int *y ) = 0;
+		virtual void getPrimaryScreenResolution( int *x, int *y ) = 0;
+
+		virtual void sendMouseClick( float x, float y, int click, int release ) = 0;
+
+		// This should be called once per 10ms or so...
+		virtual void processQueue() = 0;
+	};
+}
+
+#endif
diff --git a/SixenseSDK/include/sixense_utils/keyboard_and_mouse_win32.hpp b/SixenseSDK/include/sixense_utils/keyboard_and_mouse_win32.hpp
new file mode 100755
index 0000000..16223b9
--- /dev/null
+++ b/SixenseSDK/include/sixense_utils/keyboard_and_mouse_win32.hpp
@@ -0,0 +1,227 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+#ifndef SIXENSE_UTILS_KEYBOARD_AND_MOUSE_WIN32_HPP
+#define SIXENSE_UTILS_KEYBOARD_AND_MOUSE_WIN32_HPP
+
+#include "export.hpp"
+
+#include "sixense_utils/interfaces.hpp"
+
+namespace sixenseUtils {
+
+	const int key_esc = 0x01;
+
+#define KEY_ESCAPE          0x01
+#define KEY_1               0x02
+#define KEY_2               0x03
+#define KEY_3               0x04
+#define KEY_4               0x05
+#define KEY_5               0x06
+#define KEY_6               0x07
+#define KEY_7               0x08
+#define KEY_8               0x09
+#define KEY_9               0x0A
+#define KEY_0               0x0B
+#define KEY_MINUS           0x0C    /* - on main keyboard */
+#define KEY_EQUALS          0x0D
+#define KEY_BACK            0x0E    /* backspace */
+#define KEY_TAB             0x0F
+#define KEY_Q               0x10
+#define KEY_W               0x11
+#define KEY_E               0x12
+#define KEY_R               0x13
+#define KEY_T               0x14
+#define KEY_Y               0x15
+#define KEY_U               0x16
+#define KEY_I               0x17
+#define KEY_O               0x18
+#define KEY_P               0x19
+#define KEY_LBRACKET        0x1A
+#define KEY_RBRACKET        0x1B
+#define KEY_RETURN          0x1C    /* Enter on main keyboard */
+#define KEY_LCONTROL        0x1D
+#define KEY_A               0x1E
+#define KEY_S               0x1F
+#define KEY_D               0x20
+#define KEY_F               0x21
+#define KEY_G               0x22
+#define KEY_H               0x23
+#define KEY_J               0x24
+#define KEY_K               0x25
+#define KEY_L               0x26
+#define KEY_SEMICOLON       0x27
+#define KEY_APOSTROPHE      0x28
+#define KEY_GRAVE           0x29    /* accent grave */
+#define KEY_LSHIFT          0x2A
+#define KEY_BACKSLASH       0x2B
+#define KEY_Z               0x2C
+#define KEY_X               0x2D
+#define KEY_C               0x2E
+#define KEY_V               0x2F
+#define KEY_B               0x30
+#define KEY_N               0x31
+#define KEY_M               0x32
+#define KEY_COMMA           0x33
+#define KEY_PERIOD          0x34    /* . on main keyboard */
+#define KEY_SLASH           0x35    /* / on main keyboard */
+#define KEY_RSHIFT          0x36
+#define KEY_MULTIPLY        0x37    /* * on numeric keypad */
+#define KEY_LMENU           0x38    /* left Alt */
+#define KEY_SPACE           0x39
+#define KEY_CAPITAL         0x3A
+#define KEY_F1              0x3B
+#define KEY_F2              0x3C
+#define KEY_F3              0x3D
+#define KEY_F4              0x3E
+#define KEY_F5              0x3F
+#define KEY_F6              0x40
+#define KEY_F7              0x41
+#define KEY_F8              0x42
+#define KEY_F9              0x43
+#define KEY_F10             0x44
+#define KEY_NUMLOCK         0x45
+#define KEY_SCROLL          0x46    /* Scroll Lock */
+#define KEY_NUMPAD7         0x47
+#define KEY_NUMPAD8         0x48
+#define KEY_NUMPAD9         0x49
+#define KEY_SUBTRACT        0x4A    /* - on numeric keypad */
+#define KEY_NUMPAD4         0x4B
+#define KEY_NUMPAD5         0x4C
+#define KEY_NUMPAD6         0x4D
+#define KEY_ADD             0x4E    /* + on numeric keypad */
+#define KEY_NUMPAD1         0x4F
+#define KEY_NUMPAD2         0x50
+#define KEY_NUMPAD3         0x51
+#define KEY_NUMPAD0         0x52
+#define KEY_DECIMAL         0x53    /* . on numeric keypad */
+#define KEY_F11             0x57
+#define KEY_F12             0x58
+#define KEY_F13             0x64    /*                     (NEC PC98) */
+#define KEY_F14             0x65    /*                     (NEC PC98) */
+#define KEY_F15             0x66    /*                     (NEC PC98) */
+
+#define KEY_KANA            0x70    /* (Japanese keyboard)            */
+#define KEY_CONVERT         0x79    /* (Japanese keyboard)            */
+#define KEY_NOCONVERT       0x7B    /* (Japanese keyboard)            */
+#define KEY_YEN             0x7D    /* (Japanese keyboard)            */
+#define KEY_BREAK			0x7E
+#define KEY_NUMPADEQUALS    0x8D    /* = on numeric keypad (NEC PC98) */
+#define KEY_CIRCUMFLEX      0x90    /* (Japanese keyboard)            */
+#define KEY_AT              0x91    /*                     (NEC PC98) */
+#define KEY_COLON           0x92    /*                     (NEC PC98) */
+#define KEY_UNDERLINE       0x93    /*                     (NEC PC98) */
+#define KEY_KANJI           0x94    /* (Japanese keyboard)            */
+#define KEY_STOP            0x95    /*                     (NEC PC98) */
+#define KEY_AX              0x96    /*                     (Japan AX) */
+#define KEY_UNLABELED       0x97    /*                        (J3100) */
+#define KEY_NUMPADENTER     0x9C    /* Enter on numeric keypad */
+#define KEY_RCONTROL        0x9D
+#define KEY_NUMPADCOMMA     0xB3    /* , on numeric keypad (NEC PC98) */
+#define KEY_DIVIDE          0xB5    /* / on numeric keypad */
+#define KEY_SYSRQ           0xB7
+#define KEY_RMENU           0xB8    /* right Alt */
+#define KEY_HOME            0xC7    /* Home on arrow keypad */
+#define KEY_UP              0xC8    /* UpArrow on arrow keypad */
+#define KEY_PRIOR           0xC9    /* PgUp on arrow keypad */
+#define KEY_LEFT            0xCB    /* LeftArrow on arrow keypad */
+#define KEY_RIGHT           0xCD    /* RightArrow on arrow keypad */
+#define KEY_END             0xCF    /* End on arrow keypad */
+#define KEY_DOWN            0xD0    /* DownArrow on arrow keypad */
+#define KEY_NEXT            0xD1    /* PgDn on arrow keypad */
+#define KEY_INSERT          0xD2    /* Insert on arrow keypad */
+#define KEY_DELETE          0xD3    /* Delete on arrow keypad */
+#define KEY_LWIN            0xDB    /* Left Windows key */
+#define KEY_RWIN            0xDC    /* Right Windows key */
+#define KEY_APPS            0xDD    /* AppMenu key */
+
+	class SIXENSE_UTILS_EXPORT mouseAndKeyboardWin32 {
+	public:
+		static void sendMouseClick( int click, int release ); // 0 = none, 1 = left, 2 = right
+		static void sendMouseClick( int click, int release, int delay); // 0 = none, 1 = left, 2 = right
+		static void sendMouseWheelMove( int dir ); // in 'clicks', + = forwards - = backwards
+		static void sendKeyState( char key, int press, int release );
+		static void sendKeyState( char key, int press, int release, int delay );
+		static void releaseKey( char key );
+		static void releaseMouseButton( int release );
+		static void sendAbsoluteMouseMove( float x, float y );
+		static void sendRelativeMouseMove( float x, float y );
+		static void sendAbsoluteMouseMoveInPixels( int x, int y );
+
+		static void getMousePos( int *x, int *y );
+		static void getPrimaryScreenResolution( int *x, int *y );
+
+		static void sendMouseClick( float x, float y, int click, int release );
+
+		// This should be called once per 10ms or so...
+		static void processQueue();
+
+	private:
+		static double _last_click_time;
+		static float _last_absolute_mouse_pos_x;
+		static float _last_absolute_mouse_pos_y;
+		static float _last_absolute_mouse_pos_x_when_clicked;
+		static float _last_absolute_mouse_pos_y_when_clicked;
+	};
+
+	// Define a non-static class for use by the factory constructor. These calls just
+	// pass through to the static calls above.
+	class MouseAndKeyboardWin32 : public IMouseAndKeyboard {
+	public:
+		virtual void sendMouseClick( int click, int release ) {
+			mouseAndKeyboardWin32::sendMouseClick( click, release );
+		}
+		virtual void sendMouseClick( int click, int release, int delay) {
+			mouseAndKeyboardWin32::sendMouseClick( click, release, delay );
+		}
+		virtual void sendMouseWheelMove( int dir ) {
+			mouseAndKeyboardWin32::sendMouseWheelMove( dir );
+		}
+		virtual void sendKeyState( char key, int press, int release ) {
+			mouseAndKeyboardWin32::sendKeyState( key, press, release );
+		}
+		virtual void sendKeyState( char key, int press, int release, int delay ) {
+			mouseAndKeyboardWin32::sendKeyState( key, press, release, delay );
+		}
+		virtual void releaseKey( char key ) {
+			mouseAndKeyboardWin32::releaseKey( key );
+		}
+		virtual void releaseMouseButton( int release ) {
+			mouseAndKeyboardWin32::releaseMouseButton( release );
+		}
+		virtual void sendAbsoluteMouseMove( float x, float y ) {
+			mouseAndKeyboardWin32::sendAbsoluteMouseMove( x, y );
+		}
+		virtual void sendRelativeMouseMove( float x, float y ) {
+			mouseAndKeyboardWin32::sendRelativeMouseMove( x, y );
+		}
+		virtual void sendAbsoluteMouseMoveInPixels( int x, int y ) {
+			mouseAndKeyboardWin32::sendAbsoluteMouseMoveInPixels( x, y );
+		}
+		virtual void getMousePos( int *x, int *y ) {
+			mouseAndKeyboardWin32::getMousePos( x, y );
+		}
+		virtual void getPrimaryScreenResolution( int *x, int *y ) {
+			mouseAndKeyboardWin32::getPrimaryScreenResolution( x, y );
+		}
+
+		virtual void sendMouseClick( float x, float y, int click, int release ) {
+			mouseAndKeyboardWin32::sendMouseClick( x, y, click, release );
+		}
+		// This should be called once per 10ms or so...
+		virtual void processQueue() {
+			mouseAndKeyboardWin32::processQueue();
+		}
+
+	};
+
+}
+
+#endif
+
diff --git a/SixenseSDK/include/sixense_utils/laser_pointer.hpp b/SixenseSDK/include/sixense_utils/laser_pointer.hpp
new file mode 100755
index 0000000..d845ae9
--- /dev/null
+++ b/SixenseSDK/include/sixense_utils/laser_pointer.hpp
@@ -0,0 +1,81 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+#ifndef SIXENSE_PORT_UTILS_LASER_POINTER_HPP
+#define SIXENSE_PORT_UTILS_LASER_POINTER_HPP
+
+#pragma warning(push)
+#pragma warning( disable:4251 )
+
+#include "sixense_utils/export.hpp"
+#include "sixense_utils/interfaces.hpp"
+
+#include "sixense_math.hpp"
+
+using sixenseMath::Vector2;
+using sixenseMath::Vector3;
+using sixenseMath::Matrix3;
+
+namespace sixenseUtils {
+
+	// LaserPointer computes a ray that shoots from the controller and intersects with the screen. 
+	class SIXENSE_UTILS_EXPORT LaserPointer {
+
+	public:
+		LaserPointer();
+
+		void setScreenSize( Vector2 width_and_height_in_mm );
+		void setScreenCenterOffsetFromBase( Vector3 offset_in_mm );
+
+		Vector2 getScreenSize();
+		Vector3 getScreenCenterOffsetFromBase();
+
+		Vector2 getIntersection( Vector3 position, Matrix3 rotation );
+
+	private:
+		Vector2 _width_and_height_in_mm;
+		Vector3 _offset_in_mm;
+
+	};
+
+
+	// This class can be used to compute the required parameters for the LaserPointer by having the user aim the controller at the
+	// corners of the screen.
+	class SIXENSE_UTILS_EXPORT LaserPointerCalib {
+	public:
+
+		LaserPointerCalib() { _has_computed = false; }
+
+		// Calibrate using 4 ray casts, bottom left and top right from 2 distinct positions in space
+		bool compute( 
+			Vector3 bottom_left_ray_from_point_A, 
+			Vector3 top_right_ray_from_point_A, 
+			Vector3 bottom_left_ray_from_point_B, 
+			Vector3 top_right_ray_from_point_B );
+
+		// Calibrate using 2 ray casts plus the screen size, bottom left and top right
+		bool compute(
+			float screen_width_in_mm, float screen_aspect, 
+			Vector3 bottom_left_ray_from_point_A, 
+			Vector3 top_right_ray_from_point_A );
+
+		bool hasComputed() { return _has_computed; }
+		Vector2 getScrenSize();
+		Vector3 setScreenCenterOffsetFromBase();
+
+	private:
+		bool _has_computed;
+		Vector2 _width_and_height_in_mm;
+		Vector3 _offset_in_mm;
+	};
+};
+
+#pragma warning(pop)
+
+#endif
diff --git a/SixenseSDK/include/sixense_utils/mouse_pointer.hpp b/SixenseSDK/include/sixense_utils/mouse_pointer.hpp
new file mode 100755
index 0000000..fb16e6a
--- /dev/null
+++ b/SixenseSDK/include/sixense_utils/mouse_pointer.hpp
@@ -0,0 +1,76 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+#ifndef SIXENSE_UTILS_MOUSE_POINTER_HPP
+#define SIXENSE_UTILS_MOUSE_POINTER_HPP
+
+#pragma warning(push)
+#pragma warning( disable:4251 )
+
+#include "sixense_utils/export.hpp"
+#include "sixense_utils/interfaces.hpp"
+
+using sixenseMath::Vector2;
+using sixenseMath::Vector3;
+using sixenseMath::Matrix3;
+
+namespace sixenseUtils {
+
+	class SIXENSE_UTILS_EXPORT MousePointer : public IMousePointer {
+
+	public:
+		MousePointer();
+		sixenseMath::Vector2 update( sixenseControllerData *cd );
+
+		void setSensitivity( float sensitivity );
+		void setAcceleration( float acceleration );
+		void setSlideEnabled( bool slide_enabled );
+		void setAspectRatio( float aspect_ratio );
+		void setLockRelativeToWindow(bool lock);
+		void setLockRelativeToClientBounds(bool client);
+
+		Vector2 getMouseOffset() { return mouse_offset; }
+		void setMouseOffset( Vector2 offset ) { mouse_offset = offset; }
+		float getRollAngle();
+
+		void setCenter();
+		void setWindowCenter();
+
+	private:
+		bool _slide_enabled;
+		float _aspect_ratio;
+		float _sensitivity;
+		float _screen_width_in_mm;
+
+		// velocity params
+		float _min_vel, _max_vel;
+		float _acceleration;
+	
+		// This offset is the position of the center of the virtual screen relative to the base
+		Vector2 mouse_offset;
+
+		// Keep track of the previous mouse pos so we can compute velocity
+		Vector2 _last_mouse_pos;
+
+		// Keep track of the last accel so we can filter it
+		float _last_accel;
+
+		float _roll_angle;
+
+		bool _center_mouse_requested;
+		bool _center_mouse_on_window;
+		
+		bool _lock_relative_to_window;
+		bool _lock_relative_using_client_bounds;
+	};
+}
+
+#pragma warning(pop)
+
+#endif
diff --git a/SixenseSDK/include/sixense_utils/sixense_utils_string.hpp b/SixenseSDK/include/sixense_utils/sixense_utils_string.hpp
new file mode 100755
index 0000000..31ee7c6
--- /dev/null
+++ b/SixenseSDK/include/sixense_utils/sixense_utils_string.hpp
@@ -0,0 +1,77 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+#ifndef SIXENSE_UTILS_STRING_HPP
+#define SIXENSE_UTILS_STRING_HPP
+
+SIXENSE_UTILS_EXPORT void* sixense_utils_alloc(size_t);
+SIXENSE_UTILS_EXPORT void sixense_utils_free(void*); 
+
+#include <string>
+#include <cstddef>
+
+template<class MY_TYPE>
+class sixense_utils_allocator
+{
+public:
+	typedef size_t              size_type;
+	typedef ptrdiff_t   difference_type;
+	typedef MY_TYPE*    pointer;
+	typedef const MY_TYPE*      const_pointer;
+	typedef MY_TYPE&    reference;
+	typedef const MY_TYPE&      const_reference;
+	typedef MY_TYPE             value_type;
+
+	sixense_utils_allocator () {}
+	template <class U> sixense_utils_allocator(const sixense_utils_allocator<U>&) {}
+	template <class U> struct rebind { typedef sixense_utils_allocator<U> other; };
+
+	pointer allocate(size_type n, std::allocator<void>::const_pointer hint = 0)
+	{
+		return reinterpret_cast<pointer>(sixense_utils_alloc(n * sizeof(MY_TYPE)));
+	}
+	void construct(pointer p, const_reference val)
+	{
+		::new(p) MY_TYPE(val);
+	}
+	void destroy(pointer p)
+	{
+		p->~MY_TYPE();
+	}
+	void deallocate(pointer p, size_type n)
+	{
+		sixense_utils_free(p);
+	}
+	size_type max_size() const throw()
+	{
+		return ~size_type(0);
+	}
+};
+
+template<class MY_TYPE>
+bool operator==(const sixense_utils_allocator<MY_TYPE>& left, const sixense_utils_allocator<MY_TYPE>& right) throw()
+{
+	return true;
+}
+
+template<class MY_TYPE>
+bool operator!=(const sixense_utils_allocator<MY_TYPE>& left, const sixense_utils_allocator<MY_TYPE>& right) throw()
+{
+	return false;
+}
+
+namespace sixenseUtils {
+
+	typedef std::basic_string< char, std::char_traits<char>, sixense_utils_allocator<char> > sixense_utils_string;
+
+}
+
+
+#endif
+
diff --git a/SixenseSDK/include/sixense_utils/time.hpp b/SixenseSDK/include/sixense_utils/time.hpp
new file mode 100755
index 0000000..314ed64
--- /dev/null
+++ b/SixenseSDK/include/sixense_utils/time.hpp
@@ -0,0 +1,23 @@
+/*
+ *
+ * SIXENSE CONFIDENTIAL
+ *
+ * Copyright (C) 2011 Sixense Entertainment Inc.
+ * All Rights Reserved
+ *
+ */
+
+#include "sixense_utils/export.hpp"
+
+namespace sixenseUtils {
+
+	class SIXENSE_UTILS_EXPORT Time {
+	public:
+		static double getTimeInMilliseconds();
+
+		// Return the number of milliseconds since this was last called with the given token
+		static double getElapsedMilliseconds( int token );
+	};
+
+}
+