new maths functions

[vg.git] / src / vg / vg_m.h

diff --git a/src/vg/vg_m.h b/src/vg/vg_m.h
index 7a5a7c22aa11caa4448bf81a938595eca2bbefd0..a61c6f0565575a442a98fa43fb675b3ef2edb834 100644 (file)
--- a/src/vg/vg_m.h
+++ b/src/vg/vg_m.h
@@ -767,6 +767,21 @@ static inline void m4x3_transform_aabb( m4x3f m, boxf box )
        m4x3_expand_aabb_point( m, box, (v3f){ b[0], b[1], b[2] } );
 }
 
+static inline void m4x3_lookat( m4x3f m, v3f pos, v3f target, v3f up )
+{
+   v3f dir;
+   v3_sub( target, pos, dir );
+   v3_normalize( dir );
+
+   v3_negate( dir, m[2] );
+
+   v3_cross( up, m[2], m[0] );
+   v3_normalize( m[0] );
+
+   v3_cross( m[2], m[0], m[1] );
+   v3_copy( pos, m[3] );
+}
+
 /*
  * Matrix 4x4
  */
@@ -956,6 +971,15 @@ static inline void q_mul( v4f q, v4f q1, v4f d )
    v4_copy( t, d );
 }
 
+static inline void q_normalize( v4f q )
+{
+   float s = 1.0f/ sqrtf(v4_dot(q,q));
+   q[0] *= s;
+   q[1] *= s;
+   q[2] *= s;
+   q[3] *= s;
+}
+
 static inline void q_inv( v4f q, v4f d )
 {
    float s = 1.0f / v4_dot(q,q);
@@ -971,9 +995,9 @@ static inline void q_m3x3( v4f q, m3x3f d )
       l = v4_length(q),
       s = l > 0.0f? 2.0f/l: 0.0f,
 
-      xx = s*q[0]*q[0], xy = s*q[0]*q[2], wx = s*q[3]*q[0],
+      xx = s*q[0]*q[0], xy = s*q[0]*q[1], wx = s*q[3]*q[0],
       yy = s*q[1]*q[1], yz = s*q[1]*q[2], wy = s*q[3]*q[1],
-      zz = s*q[2]*q[2], xz = s*q[2]*q[2], wz = s*q[3]*q[2];
+      zz = s*q[2]*q[2], xz = s*q[0]*q[2], wz = s*q[3]*q[2];
 
    d[0][0] = 1.0f - yy - zz;
    d[1][1] = 1.0f - xx - zz;