Fix library add two more utility functions

2 files changed, 24 insertions, 2 deletions

diff --git a/redist/linmath.c b/redist/linmath.c
index 808bfbf..60fbc21 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -63,7 +63,22 @@ void copy3d( FLT * out, const FLT * in )
 	out[2] = in[2];
 }
 
+FLT magnitude3d( FLT * a )
+{
+	return sqrt( a[0]*a[0] + a[1]*a[1] + a[2]*a[2] );
+}
 
+FLT anglebetween3d( FLT * a, FLT * b )
+{
+	FLT an[3];
+	FLT bn[3];
+	normalize3d( an, a );
+	normalize3d( bn, b );
+	FLT dot = dot3d( a, b );
+	if( dot < -0.9999999 ) return LINMATHPI;
+	if( dot >  0.9999999 ) return 0;
+	return acos( dot );
+}
 
 /////////////////////////////////////QUATERNIONS//////////////////////////////////////////
 //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman)
@@ -74,7 +89,7 @@ void copy3d( FLT * out, const FLT * in )
 
 void quatsetnone( FLT * q )
 {
-	q[0] = 0; q[1] = 0; q[2] = 0; q[3] = 1;
+	q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0;
 }
 
 void quatcopy( FLT * qout, const FLT * qin )
diff --git a/redist/linmath.h b/redist/linmath.h
index 9b21347..5cc7b7d 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -6,6 +6,12 @@
 //Yes, I know it's kind of arbitrary.
 #define DEFAULT_EPSILON 0.001
 
+//For printf
+#define PFTHREE(x) x[0], x[1], x[2]
+#define PFFOUR(x) x[0], x[1], x[2], x[3]
+
+#define LINMATHPI 3.141592653589
+
 //If you want, you can define FLT to be double for double precision.
 #ifndef FLT
 #define FLT float
@@ -30,8 +36,9 @@ int compare3d( const FLT * a, const FLT * b, FLT epsilon );
 
 void copy3d( FLT * out, const FLT * in );
 
+FLT magnitude3d( FLT * a );
 
-
+FLT anglebetween3d( FLT * a, FLT * b );
 
 //Quaternion things...