qm3x3,boxf debug, bugfixes

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

diff --git a/src/vg/vg_m.h b/src/vg/vg_m.h
index 9f934163e42bd7c5204566eeb11eb637f93e4206..c71f3abd5af081f077c6547855d55de8a1841208 100644 (file)
--- a/src/vg/vg_m.h
+++ b/src/vg/vg_m.h
@@ -103,7 +103,7 @@ static inline float v2_dot( v2f a, v2f b )
 
 static inline float v2_cross( v2f a, v2f b )
 {
-       return a[0] * b[1] - a[1] * b[0];
+       return a[0]*b[1] - a[1]*b[0];
 }
 
 static inline void v2_add( v2f a, v2f b, v2f d )
@@ -237,11 +237,13 @@ static inline float v3_dot( v3f a, v3f b )
        return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
 }
 
-static inline void v3_cross( v3f a, v3f b, v3f d )
+static inline void v3_cross( v3f a, v3f b, v3f dest )
 {
-  d[0] = a[1] * b[2] - a[2] * b[1];
-  d[1] = a[2] * b[0] - a[0] * b[2];
-  d[2] = a[0] * b[1] - a[1] * b[0];
+   v3f d;
+   d[0] = a[1]*b[2] - a[2]*b[1];
+   d[1] = a[2]*b[0] - a[0]*b[2];
+   d[2] = a[0]*b[1] - a[1]*b[0];
+   v3_copy( d, dest );
 }
 
 static inline float v3_length2( v3f a )
@@ -743,6 +745,12 @@ static inline void m4x3_expand_aabb_point( m4x3f m, boxf box, v3f point )
        v3_maxv( box[1], v, box[1] );
 }
 
+static inline void box_addpt( boxf a, v3f pt )
+{
+       v3_minv( a[0], pt, a[0] );
+       v3_maxv( a[1], pt, a[1] );
+}
+
 static inline void box_concat( boxf a, boxf b )
 {
        v3_minv( a[0], b[0], a[0] );
@@ -755,6 +763,12 @@ static inline void box_copy( boxf a, boxf b )
        v3_copy( a[1], b[1] );
 }
 
+static inline void box_init_inf( boxf box )
+{
+       v3_fill( box[0],  INFINITY );
+       v3_fill( box[1], -INFINITY );
+}
+
 static inline void m4x3_transform_aabb( m4x3f m, boxf box )
 {
        v3f a; v3f b;
@@ -780,7 +794,7 @@ static inline void m4x3_lookat( m4x3f m, v3f pos, v3f target, v3f up )
    v3_sub( target, pos, dir );
    v3_normalize( dir );
 
-   v3_negate( dir, m[2] );
+   v3_copy( dir, m[2] );
 
    v3_cross( up, m[2], m[0] );
    v3_normalize( m[0] );
@@ -1016,3 +1030,46 @@ static inline void q_m3x3( v4f q, m3x3f d )
    d[2][1] = yz - wx;
    d[0][2] = xz - wy;
 }
+
+static void m3x3_q( m3x3f m, v4f q )
+{
+   float diag, r, rinv;
+
+   diag = m[0][0] + m[1][1] + m[2][2];
+   if( diag >= 0.0f )
+   {
+      r    = sqrtf( 1.0f + diag );
+      rinv = 0.5f / r;
+      q[0] = rinv * (m[1][2] - m[2][1]);
+      q[1] = rinv * (m[2][0] - m[0][2]);
+      q[2] = rinv * (m[0][1] - m[1][0]);
+      q[3] = r    * 0.5f;
+   } 
+   else if( m[0][0] >= m[1][1] && m[0][0] >= m[2][2] )
+   {
+      r    = sqrtf( 1.0f - m[1][1] - m[2][2] + m[0][0] );
+      rinv = 0.5f / r;
+      q[0] = r    * 0.5f;
+      q[1] = rinv * (m[0][1] + m[1][0]);
+      q[2] = rinv * (m[0][2] + m[2][0]);
+      q[3] = rinv * (m[1][2] - m[2][1]);
+   } 
+   else if( m[1][1] >= m[2][2] )
+   {
+      r    = sqrtf( 1.0f - m[0][0] - m[2][2] + m[1][1] );
+      rinv = 0.5f / r;
+      q[0] = rinv * (m[0][1] + m[1][0]);
+      q[1] = r    * 0.5f;
+      q[2] = rinv * (m[1][2] + m[2][1]);
+      q[3] = rinv * (m[2][0] - m[0][2]);
+   } 
+   else 
+   {
+      r    = sqrtf( 1.0f - m[0][0] - m[1][1] + m[2][2] );
+      rinv = 0.5f / r;
+      q[0] = rinv * (m[0][2] + m[2][0]);
+      q[1] = rinv * (m[1][2] + m[2][1]);
+      q[2] = r    * 0.5f;
+      q[3] = rinv * (m[0][1] - m[1][0]);
+   }
+}