Replaced rotation algorithm & cleanup

2 files changed, 311 insertions, 303 deletions

diff --git a/redist/linmath.c b/redist/linmath.c
index f5f6b22..d5d54e3 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -34,7 +34,7 @@ void scale3d( FLT * out, const FLT * a, FLT scalar )
 
 void normalize3d( FLT * out, const FLT * in )
 {
-    FLT r = ((FLT)1.) / FLT_SQRT(in[0] * in[0] + in[1] * in[1] + in[2] * in[2]);
+	FLT r = ((FLT)1.) / FLT_SQRT(in[0] * in[0] + in[1] * in[1] + in[2] * in[2]);
 	out[0] = in[0] * r;
 	out[1] = in[1] * r;
 	out[2] = in[2] * r;
@@ -66,7 +66,7 @@ void copy3d( FLT * out, const FLT * in )
 
 FLT magnitude3d(const FLT * a )
 {
-    return FLT_SQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
+	return FLT_SQRT(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
 }
 
 FLT anglebetween3d( FLT * a, FLT * b )
@@ -78,7 +78,7 @@ FLT anglebetween3d( FLT * a, FLT * b )
 	FLT dot = dot3d( a, b );
 	if( dot < -0.9999999 ) return LINMATHPI;
 	if( dot >  0.9999999 ) return 0;
-    return FLT_ACOS(dot);
+	return FLT_ACOS(dot);
 }
 
 /////////////////////////////////////QUATERNIONS//////////////////////////////////////////
@@ -90,12 +90,12 @@ FLT anglebetween3d( FLT * a, FLT * b )
 
 void quatsetnone(FLT * q)
 {
-    q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0;
+	q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0;
 }
 
 void quatsetidentity(FLT * q)
 {
-    q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 1;
+	q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 1;
 }
 
 void quatcopy(FLT * qout, const FLT * qin)
@@ -112,12 +112,12 @@ void quatfromeuler( FLT * q, const FLT * euler )
 	FLT Y = euler[1]/2.0f; //pitch
 	FLT Z = euler[2]/2.0f; //yaw
 
-    FLT cx = FLT_COS(X);
-    FLT sx = FLT_SIN(X);
-    FLT cy = FLT_COS(Y);
-    FLT sy = FLT_SIN(Y);
-    FLT cz = FLT_COS(Z);
-    FLT sz = FLT_SIN(Z);
+	FLT cx = FLT_COS(X);
+	FLT sx = FLT_SIN(X);
+	FLT cy = FLT_COS(Y);
+	FLT sy = FLT_SIN(Y);
+	FLT cz = FLT_COS(Z);
+	FLT sz = FLT_SIN(Z);
 
 	//Correct according to
 	//http://en.wikipedia.org/wiki/Conversion_between_MQuaternions_and_Euler_angles
@@ -131,9 +131,9 @@ void quatfromeuler( FLT * q, const FLT * euler )
 void quattoeuler( FLT * euler, const FLT * q )
 {
 	//According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009)
-    euler[0] = FLT_ATAN2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2]));
-    euler[1] = FLT_ASIN(2 * (q[0] * q[2] - q[3] * q[1]));
-    euler[2] = FLT_ATAN2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] * q[2] + q[3] * q[3]));
+	euler[0] = FLT_ATAN2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2]));
+	euler[1] = FLT_ASIN(2 * (q[0] * q[2] - q[3] * q[1]));
+	euler[2] = FLT_ATAN2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] * q[2] + q[3] * q[3]));
 }
 
 void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians )
@@ -141,8 +141,8 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians )
 	FLT v[3];
 	normalize3d( v, axis );
 	
-    FLT sn = FLT_SIN(radians / 2.0f);
-    q[0] = FLT_COS(radians / 2.0f);
+	FLT sn = FLT_SIN(radians / 2.0f);
+	q[0] = FLT_COS(radians / 2.0f);
 	q[1] = sn * v[0];
 	q[2] = sn * v[1];
 	q[3] = sn * v[2];
@@ -152,7 +152,7 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians )
 
 FLT quatmagnitude( const FLT * q )
 {
-    return FLT_SQRT((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3]));
+	return FLT_SQRT((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3]));
 }
 
 FLT quatinvsqmagnitude( const FLT * q )
@@ -169,74 +169,74 @@ void quatnormalize( FLT * qout, const FLT * qin )
 
 void quattomatrix(FLT * matrix44, const FLT * qin)
 {
-    FLT q[4];
-    quatnormalize(q, qin);
+	FLT q[4];
+	quatnormalize(q, qin);
 
-    //Reduced calulation for speed
-    FLT xx = 2 * q[0] * q[0];
-    FLT xy = 2 * q[0] * q[1];
-    FLT xz = 2 * q[0] * q[2];
-    FLT xw = 2 * q[0] * q[3];
+	//Reduced calulation for speed
+	FLT xx = 2 * q[0] * q[0];
+	FLT xy = 2 * q[0] * q[1];
+	FLT xz = 2 * q[0] * q[2];
+	FLT xw = 2 * q[0] * q[3];
 
-    FLT yy = 2 * q[1] * q[1];
-    FLT yz = 2 * q[1] * q[2];
-    FLT yw = 2 * q[1] * q[3];
+	FLT yy = 2 * q[1] * q[1];
+	FLT yz = 2 * q[1] * q[2];
+	FLT yw = 2 * q[1] * q[3];
 
-    FLT zz = 2 * q[2] * q[2];
-    FLT zw = 2 * q[2] * q[3];
+	FLT zz = 2 * q[2] * q[2];
+	FLT zw = 2 * q[2] * q[3];
 
-    //opengl major
-    matrix44[0] = 1 - yy - zz;
-    matrix44[1] = xy - zw;
-    matrix44[2] = xz + yw;
-    matrix44[3] = 0;
+	//opengl major
+	matrix44[0] = 1 - yy - zz;
+	matrix44[1] = xy - zw;
+	matrix44[2] = xz + yw;
+	matrix44[3] = 0;
 
-    matrix44[4] = xy + zw;
-    matrix44[5] = 1 - xx - zz;
-    matrix44[6] = yz - xw;
-    matrix44[7] = 0;
+	matrix44[4] = xy + zw;
+	matrix44[5] = 1 - xx - zz;
+	matrix44[6] = yz - xw;
+	matrix44[7] = 0;
 
-    matrix44[8] = xz - yw;
-    matrix44[9] = yz + xw;
-    matrix44[10] = 1 - xx - yy;
-    matrix44[11] = 0;
+	matrix44[8] = xz - yw;
+	matrix44[9] = yz + xw;
+	matrix44[10] = 1 - xx - yy;
+	matrix44[11] = 0;
 
-    matrix44[12] = 0;
-    matrix44[13] = 0;
-    matrix44[14] = 0;
-    matrix44[15] = 1;
+	matrix44[12] = 0;
+	matrix44[13] = 0;
+	matrix44[14] = 0;
+	matrix44[15] = 1;
 }
 
 void quattomatrix33(FLT * matrix33, const FLT * qin)
 {
-    FLT q[4];
-    quatnormalize(q, qin);
+	FLT q[4];
+	quatnormalize(q, qin);
 
-    //Reduced calulation for speed
-    FLT xx = 2 * q[0] * q[0];
-    FLT xy = 2 * q[0] * q[1];
-    FLT xz = 2 * q[0] * q[2];
-    FLT xw = 2 * q[0] * q[3];
+	//Reduced calulation for speed
+	FLT xx = 2 * q[0] * q[0];
+	FLT xy = 2 * q[0] * q[1];
+	FLT xz = 2 * q[0] * q[2];
+	FLT xw = 2 * q[0] * q[3];
 
-    FLT yy = 2 * q[1] * q[1];
-    FLT yz = 2 * q[1] * q[2];
-    FLT yw = 2 * q[1] * q[3];
+	FLT yy = 2 * q[1] * q[1];
+	FLT yz = 2 * q[1] * q[2];
+	FLT yw = 2 * q[1] * q[3];
 
-    FLT zz = 2 * q[2] * q[2];
-    FLT zw = 2 * q[2] * q[3];
+	FLT zz = 2 * q[2] * q[2];
+	FLT zw = 2 * q[2] * q[3];
 
-    //opengl major
-    matrix33[0] = 1 - yy - zz;
-    matrix33[1] = xy - zw;
-    matrix33[2] = xz + yw;
+	//opengl major
+	matrix33[0] = 1 - yy - zz;
+	matrix33[1] = xy - zw;
+	matrix33[2] = xz + yw;
 
-    matrix33[3] = xy + zw;
-    matrix33[4] = 1 - xx - zz;
-    matrix33[5] = yz - xw;
+	matrix33[3] = xy + zw;
+	matrix33[4] = 1 - xx - zz;
+	matrix33[5] = yz - xw;
 
-    matrix33[6] = xz - yw;
-    matrix33[7] = yz + xw;
-    matrix33[8] = 1 - xx - yy;
+	matrix33[6] = xz - yw;
+	matrix33[7] = yz + xw;
+	matrix33[8] = 1 - xx - yy;
 }
 
 void quatgetconjugate(FLT * qout, const FLT * qin)
@@ -334,13 +334,13 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t )
 	if ( 1 - (cosTheta*cosTheta) <= 0 )
 		sinTheta = 0;
 	else
-        sinTheta = FLT_SQRT(1 - (cosTheta*cosTheta));
+		sinTheta = FLT_SQRT(1 - (cosTheta*cosTheta));
 
-    FLT Theta = FLT_ACOS(cosTheta); //Theta is half the angle between the 2 MQuaternions
+	FLT Theta = FLT_ACOS(cosTheta); //Theta is half the angle between the 2 MQuaternions
 
-    if (FLT_FABS(Theta) < DEFAULT_EPSILON)
+	if (FLT_FABS(Theta) < DEFAULT_EPSILON)
 		quatcopy( q, qa );
-    else if (FLT_FABS(sinTheta) < DEFAULT_EPSILON)
+	else if (FLT_FABS(sinTheta) < DEFAULT_EPSILON)
 	{
 		quatadd( q, qa, qb );
 		quatscale( q, q, 0.5 );
@@ -349,8 +349,8 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t )
 	{
 		FLT aside[4];
 		FLT bside[4];
-        quatscale( bside, qb, FLT_SIN(t * Theta));
-        quatscale( aside, qa, FLT_SIN((1 - t)*Theta));
+		quatscale( bside, qb, FLT_SIN(t * Theta));
+		quatscale( aside, qa, FLT_SIN((1 - t)*Theta));
 		quatadd( q, aside, bside );
 		quatscale( q, q, ((FLT)1.)/sinTheta );
 	}
@@ -380,37 +380,47 @@ void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in )
 
 Matrix3x3 inverseM33(const Matrix3x3 mat)
 {
-    Matrix3x3 newMat;
-    for (int a = 0; a < 3; a++)
-    {
-        for (int b = 0; b < 3; b++)
-        {
-            newMat.val[a][b] = mat.val[a][b];
-        }
-    }
+	Matrix3x3 newMat;
+	for (int a = 0; a < 3; a++)
+	{
+		for (int b = 0; b < 3; b++)
+		{
+			newMat.val[a][b] = mat.val[a][b];
+		}
+	}
 
-    for (int i = 0; i < 3; i++)
-    {
-        for (int j = i + 1; j < 3; j++)
-        {
-            FLT tmp = newMat.val[i][j];
-            newMat.val[i][j] = newMat.val[j][i];
-            newMat.val[j][i] = tmp;
-        }
-    }
+	for (int i = 0; i < 3; i++)
+	{
+		for (int j = i + 1; j < 3; j++)
+		{
+			FLT tmp = newMat.val[i][j];
+			newMat.val[i][j] = newMat.val[j][i];
+			newMat.val[j][i] = tmp;
+		}
+	}
 
-    return newMat;
+	return newMat;
 }
 
-void rotation_between_vecs_to_m3(FLT m[3][3], const FLT v1[3], const FLT v2[3])
+void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3])
 {
-    FLT q[4];
+	FLT q[4];
 
-    getRotationTo(q, v1, v2);
+	quatfrom2vectors(q, v1, v2);
 
-    quattomatrix33(&(m[0][0]), q);
+	quattomatrix33(&(m->val[0][0]), q);
 }
 
+void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot)
+{
+	out[0] = rot.val[0][0] * in[0] + rot.val[1][0] * in[1] + rot.val[2][0] * in[2];
+	out[1] = rot.val[0][1] * in[0] + rot.val[1][1] * in[1] + rot.val[2][1] * in[2];
+	out[2] = rot.val[0][2] * in[0] + rot.val[1][2] * in[1] + rot.val[2][2] * in[2];
+
+	return;
+}
+
+
 // This function based on code from Object-oriented Graphics Rendering Engine
 // Copyright(c) 2000 - 2012 Torus Knot Software Ltd
 // under MIT license
@@ -423,201 +433,203 @@ If you call this with a dest vector that is close to the inverse
 of this vector, we will rotate 180 degrees around a generated axis if
 since in this case ANY axis of rotation is valid.
 */
-void getRotationTo(FLT *q, const FLT *src, const FLT *dest)
-{
-    // Based on Stan Melax's article in Game Programming Gems
-
-    // Copy, since cannot modify local
-    FLT v0[3];
-    FLT v1[3];
-    normalize3d(v0, src);
-    normalize3d(v1, dest);
-
-    FLT d = dot3d(v0, v1);// v0.dotProduct(v1);
-    // If dot == 1, vectors are the same
-    if (d >= 1.0f)
-    {
-        quatsetidentity(q);
-        return;
-    }
-    if (d < (1e-6f - 1.0f))
-    {
-        // Generate an axis
-        FLT unitX[3] = { 1, 0, 0 };
-        FLT unitY[3] = { 0, 1, 0 };
-        
-        FLT axis[3];
-        cross3d(axis, unitX, src); // pick an angle
-        if ((axis[0] < 1.0e-35f) &&
-            (axis[1] < 1.0e-35f) &&
-            (axis[2] < 1.0e-35f)) // pick another if colinear
-        {
-            cross3d(axis, unitY, src);
-        }
-        normalize3d(axis, axis);
-        quatfromaxisangle(q, axis, LINMATHPI);
-    }
-    else
-    {
-        FLT s = FLT_SQRT((1 + d) * 2);
-        FLT invs = 1 / s;
-
-        FLT c[3];
-        cross3d(c, v0, v1);
-
-        q[0] = c[0] * invs;
-        q[1] = c[1] * invs;
-        q[2] = c[2] * invs;
-        q[3] = s * 0.5f;
-
-        quatnormalize(q, q);
-    }
-}
-
-/////////////////////////////////////Matrix Rotations////////////////////////////////////
-//Originally from Stack Overflow 
-//Under cc by-sa 3.0
-//  http://stackoverflow.com/questions/23166898/efficient-way-to-calculate-a-3x3-rotation-matrix-from-the-rotation-defined-by-tw
-// Copyright 2014 by Campbell Barton
-// Copyright 2017 by Michael Turvey
-
-/**
-* Calculate a rotation matrix from 2 normalized vectors.
-*
-* v1 and v2 must be unit length.
-*/
-void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3])
-{
-    FLT axis[3];
-    /* avoid calculating the angle */
-    FLT angle_sin;
-    FLT angle_cos;
-
-    cross3d(axis, v1, v2);
-
-    angle_sin = normalize_v3(axis);
-    angle_cos = dot3d(v1, v2);
-
-    if (angle_sin > FLT_EPSILON) {
-    axis_calc:
-        axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos);
-    }
-    else {
-        /* Degenerate (co-linear) vectors */
-        if (angle_cos > 0.0f) {
-            /* Same vectors, zero rotation... */
-            unit_m3(m);
-        }
-        else {
-            /* Colinear but opposed vectors, 180 rotation... */
-            get_orthogonal_vector(axis, v1);
-            normalize_v3(axis);
-            angle_sin = 0.0f;  /* sin(M_PI) */
-            angle_cos = -1.0f;  /* cos(M_PI) */
-            goto axis_calc;
-        }
-    }
-}
-
-void get_orthogonal_vector(FLT out[3], const FLT in[3])
-{
-#ifdef USE_DOUBLE
-    const FLT x = fabs(in[0]);
-    const FLT y = fabs(in[1]);
-    const FLT z = fabs(in[2]);
-#else
-    const FLT x = fabsf(in[0]);
-    const FLT y = fabsf(in[1]);
-    const FLT z = fabsf(in[2]);
-#endif
-
-    if (x > y && x > z)
-    {
-        // x is dominant
-        out[0] = -in[1] - in[2];
-        out[1] = in[0];
-        out[2] = in[0];
-    }
-    else if (y > z)
-    {
-        // y is dominant
-        out[0] = in[1];
-        out[1] = -in[0] - in[2];
-        out[2] = in[1];
-    }
-    else
-    {
-        // z is dominant
-        out[0] = in[2];
-        out[1] = in[2];
-        out[2] = -in[0] - in[1];
-    }
-}
-
-void unit_m3(FLT mat[3][3])
-{
-    mat[0][0] = 1;
-    mat[0][1] = 0;
-    mat[0][2] = 0;
-    mat[1][0] = 0;
-    mat[1][1] = 1;
-    mat[1][2] = 0;
-    mat[2][0] = 0;
-    mat[2][1] = 0;
-    mat[2][2] = 1;
-}
-
-
-FLT normalize_v3(FLT vect[3])
-{
-    FLT distance = dot3d(vect, vect);
-
-    if (distance < 1.0e-35f)
-    {
-        // distance is too short, just go to zero.
-        vect[0] = 0;
-        vect[1] = 0;
-        vect[2] = 0;
-        distance = 0;
-    }
-    else
-    {
-        distance = FLT_SQRT((FLT)distance);
-        scale3d(vect, vect, 1.0f / distance);
-    }
-
-    return distance;
-}
-
-/* axis must be unit length */
-void axis_angle_normalized_to_mat3_ex(
-    FLT mat[3][3], const FLT axis[3],
-    const FLT angle_sin, const FLT angle_cos)
-{
-    FLT nsi[3], ico;
-    FLT n_00, n_01, n_11, n_02, n_12, n_22;
-
-    ico = (1.0f - angle_cos);
-    nsi[0] = axis[0] * angle_sin;
-    nsi[1] = axis[1] * angle_sin;
-    nsi[2] = axis[2] * angle_sin;
-
-    n_00 = (axis[0] * axis[0]) * ico;
-    n_01 = (axis[0] * axis[1]) * ico;
-    n_11 = (axis[1] * axis[1]) * ico;
-    n_02 = (axis[0] * axis[2]) * ico;
-    n_12 = (axis[1] * axis[2]) * ico;
-    n_22 = (axis[2] * axis[2]) * ico;
-
-    mat[0][0] = n_00 + angle_cos;
-    mat[0][1] = n_01 + nsi[2];
-    mat[0][2] = n_02 - nsi[1];
-    mat[1][0] = n_01 - nsi[2];
-    mat[1][1] = n_11 + angle_cos;
-    mat[1][2] = n_12 + nsi[0];
-    mat[2][0] = n_02 + nsi[1];
-    mat[2][1] = n_12 - nsi[0];
-    mat[2][2] = n_22 + angle_cos;
+void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest)
+{
+	// Based on Stan Melax's article in Game Programming Gems
+
+	// Copy, since cannot modify local
+	FLT v0[3];
+	FLT v1[3];
+	normalize3d(v0, src);
+	normalize3d(v1, dest);
+
+	FLT d = dot3d(v0, v1);// v0.dotProduct(v1);
+	// If dot == 1, vectors are the same
+	if (d >= 1.0f)
+	{
+		quatsetidentity(q);
+		return;
+	}
+	if (d < (1e-6f - 1.0f))
+	{
+		// Generate an axis
+		FLT unitX[3] = { 1, 0, 0 };
+		FLT unitY[3] = { 0, 1, 0 };
+		
+		FLT axis[3];
+		cross3d(axis, unitX, src); // pick an angle
+		if ((axis[0] < 1.0e-35f) &&
+			(axis[1] < 1.0e-35f) &&
+			(axis[2] < 1.0e-35f)) // pick another if colinear
+		{
+			cross3d(axis, unitY, src);
+		}
+		normalize3d(axis, axis);
+		quatfromaxisangle(q, axis, LINMATHPI);
+	}
+	else
+	{
+		FLT s = FLT_SQRT((1 + d) * 2);
+		FLT invs = 1 / s;
+
+		FLT c[3];
+		//cross3d(c, v0, v1);
+		cross3d(c, v1, v0);
+
+		q[0] = c[0] * invs;
+		q[1] = c[1] * invs;
+		q[2] = c[2] * invs;
+		q[3] = s * 0.5f;
+
+		quatnormalize(q, q);
+	}
+
 }
 
+///////////////////////////////////////Matrix Rotations////////////////////////////////////
+////Originally from Stack Overflow 
+////Under cc by-sa 3.0
+////  http://stackoverflow.com/questions/23166898/efficient-way-to-calculate-a-3x3-rotation-matrix-from-the-rotation-defined-by-tw
+//// Copyright 2014 by Campbell Barton
+//// Copyright 2017 by Michael Turvey
+//
+///**
+//* Calculate a rotation matrix from 2 normalized vectors.
+//*
+//* v1 and v2 must be unit length.
+//*/
+//void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3])
+//{
+//	FLT axis[3];
+//	/* avoid calculating the angle */
+//	FLT angle_sin;
+//	FLT angle_cos;
+//
+//	cross3d(axis, v1, v2);
+//
+//	angle_sin = normalize_v3(axis);
+//	angle_cos = dot3d(v1, v2);
+//
+//	if (angle_sin > FLT_EPSILON) {
+//	axis_calc:
+//		axis_angle_normalized_to_mat3_ex(m, axis, angle_sin, angle_cos);
+//	}
+//	else {
+//		/* Degenerate (co-linear) vectors */
+//		if (angle_cos > 0.0f) {
+//			/* Same vectors, zero rotation... */
+//			unit_m3(m);
+//		}
+//		else {
+//			/* Colinear but opposed vectors, 180 rotation... */
+//			get_orthogonal_vector(axis, v1);
+//			normalize_v3(axis);
+//			angle_sin = 0.0f;  /* sin(M_PI) */
+//			angle_cos = -1.0f;  /* cos(M_PI) */
+//			goto axis_calc;
+//		}
+//	}
+//}
+
+//void get_orthogonal_vector(FLT out[3], const FLT in[3])
+//{
+//#ifdef USE_DOUBLE
+//	const FLT x = fabs(in[0]);
+//	const FLT y = fabs(in[1]);
+//	const FLT z = fabs(in[2]);
+//#else
+//	const FLT x = fabsf(in[0]);
+//	const FLT y = fabsf(in[1]);
+//	const FLT z = fabsf(in[2]);
+//#endif
+//
+//	if (x > y && x > z)
+//	{
+//		// x is dominant
+//		out[0] = -in[1] - in[2];
+//		out[1] = in[0];
+//		out[2] = in[0];
+//	}
+//	else if (y > z)
+//	{
+//		// y is dominant
+//		out[0] = in[1];
+//		out[1] = -in[0] - in[2];
+//		out[2] = in[1];
+//	}
+//	else
+//	{
+//		// z is dominant
+//		out[0] = in[2];
+//		out[1] = in[2];
+//		out[2] = -in[0] - in[1];
+//	}
+//}
+//
+//void unit_m3(FLT mat[3][3])
+//{
+//	mat[0][0] = 1;
+//	mat[0][1] = 0;
+//	mat[0][2] = 0;
+//	mat[1][0] = 0;
+//	mat[1][1] = 1;
+//	mat[1][2] = 0;
+//	mat[2][0] = 0;
+//	mat[2][1] = 0;
+//	mat[2][2] = 1;
+//}
+
+
+//FLT normalize_v3(FLT vect[3])
+//{
+//	FLT distance = dot3d(vect, vect);
+//
+//	if (distance < 1.0e-35f)
+//	{
+//		// distance is too short, just go to zero.
+//		vect[0] = 0;
+//		vect[1] = 0;
+//		vect[2] = 0;
+//		distance = 0;
+//	}
+//	else
+//	{
+//		distance = FLT_SQRT((FLT)distance);
+//		scale3d(vect, vect, 1.0f / distance);
+//	}
+//
+//	return distance;
+//}
+
+///* axis must be unit length */
+//void axis_angle_normalized_to_mat3_ex(
+//	FLT mat[3][3], const FLT axis[3],
+//	const FLT angle_sin, const FLT angle_cos)
+//{
+//	FLT nsi[3], ico;
+//	FLT n_00, n_01, n_11, n_02, n_12, n_22;
+//
+//	ico = (1.0f - angle_cos);
+//	nsi[0] = axis[0] * angle_sin;
+//	nsi[1] = axis[1] * angle_sin;
+//	nsi[2] = axis[2] * angle_sin;
+//
+//	n_00 = (axis[0] * axis[0]) * ico;
+//	n_01 = (axis[0] * axis[1]) * ico;
+//	n_11 = (axis[1] * axis[1]) * ico;
+//	n_02 = (axis[0] * axis[2]) * ico;
+//	n_12 = (axis[1] * axis[2]) * ico;
+//	n_22 = (axis[2] * axis[2]) * ico;
+//
+//	mat[0][0] = n_00 + angle_cos;
+//	mat[0][1] = n_01 + nsi[2];
+//	mat[0][2] = n_02 - nsi[1];
+//	mat[1][0] = n_01 - nsi[2];
+//	mat[1][1] = n_11 + angle_cos;
+//	mat[1][2] = n_12 + nsi[0];
+//	mat[2][0] = n_02 + nsi[1];
+//	mat[2][1] = n_12 - nsi[0];
+//	mat[2][2] = n_22 + angle_cos;
+//}
+
 
diff --git a/redist/linmath.h b/redist/linmath.h
index 3b11c1b..20a7848 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -87,26 +87,22 @@ void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb );
 void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t );
 void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in );
 
-void getRotationTo(FLT *q, const FLT *src, const FLT *dest);
+void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest);
 
 // Matrix Stuff
 
 typedef struct
 {
-    // row, column, (0,0) in upper left
-    FLT val[3][3];
+	FLT val[3][3]; // row, column
 } Matrix3x3;
 
+void rotate_vec(FLT *out, const FLT *in, Matrix3x3 rot);
+void rotation_between_vecs_to_m3(Matrix3x3 *m, const FLT v1[3], const FLT v2[3]);
 Matrix3x3 inverseM33(const Matrix3x3 mat);
-void get_orthogonal_vector(FLT out[3], const FLT in[3]);
-void rotation_between_vecs_to_mat3(FLT m[3][3], const FLT v1[3], const FLT v2[3]);
-void unit_m3(FLT m[3][3]);
-FLT normalize_v3(FLT n[3]);
-void axis_angle_normalized_to_mat3_ex(
-    FLT mat[3][3],
-    const FLT axis[3],
-    const FLT angle_sin,
-    const FLT angle_cos);
+
+
+
+
 
 
 #endif