+
+/* Quaternions */
+
+static inline void q_identity( v4f q )
+{
+ q[0] = 0.0f; q[1] = 0.0f; q[2] = 0.0f; q[3] = 1.0f;
+}
+
+static inline void q_axis_angle( v4f q, v3f axis, float angle )
+{
+ float a = angle*0.5f,
+ c = cosf(a),
+ s = sinf(a);
+
+ q[0] = s*axis[0];
+ q[1] = s*axis[1];
+ q[2] = s*axis[2];
+ q[3] = c;
+}
+
+static inline void q_mul( v4f q, v4f q1, v4f d )
+{
+ v4f t;
+ t[0] = q[3]*q1[0] + q[0]*q1[3] + q[1]*q1[2] - q[2]*q1[1];
+ t[1] = q[3]*q1[1] - q[0]*q1[2] + q[1]*q1[3] + q[2]*q1[0];
+ t[2] = q[3]*q1[2] + q[0]*q1[1] - q[1]*q1[0] + q[2]*q1[3];
+ t[3] = q[3]*q1[3] - q[0]*q1[0] - q[1]*q1[1] - q[2]*q1[2];
+ 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);
+ d[0] = -q[0]*s;
+ d[1] = -q[1]*s;
+ d[2] = -q[2]*s;
+ d[3] = q[3]*s;
+}
+
+static inline void q_nlerp( v4f a, v4f b, float t, v4f d )
+{
+ if( v4_dot(a,b) < 0.0f )
+ {
+ v4_muls( b, -1.0f, d );
+ v4_lerp( a, d, t, d );
+ }
+ else
+ v4_lerp( a, b, t, d );
+
+ q_normalize( d );
+}
+
+static inline void q_m3x3( v4f q, m3x3f d )
+{
+ float
+ l = v4_length(q),
+ s = l > 0.0f? 2.0f/l: 0.0f,
+
+ 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[0]*q[2], wz = s*q[3]*q[2];
+
+ d[0][0] = 1.0f - yy - zz;
+ d[1][1] = 1.0f - xx - zz;
+ d[2][2] = 1.0f - xx - yy;
+ d[0][1] = xy + wz;
+ d[1][2] = yz + wx;
+ d[2][0] = xz + wy;
+ d[1][0] = xy - wz;
+ 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]);
+ }
+}
+
+static int ray_tri( v3f tri[3], v3f co, v3f dir, float *dist )
+{
+ float const kEpsilon = 0.00001f;
+
+ v3f v0, v1, h, s, q, n;
+ float a,f,u,v,t;
+
+ float *pa = tri[0],
+ *pb = tri[1],
+ *pc = tri[2];
+
+ v3_sub( pb, pa, v0 );
+ v3_sub( pc, pa, v1 );
+ v3_cross( dir, v1, h );
+ v3_cross( v0, v1, n );
+
+ if( v3_dot( n, dir ) > 0.0f ) /* Backface culling */
+ return 0;
+
+ /* Parralel */
+ a = v3_dot( v0, h );
+ if( a > -kEpsilon && a < kEpsilon )
+ return 0;
+
+ f = 1.0f/a;
+ v3_sub( co, pa, s );
+
+ u = f * v3_dot(s, h);
+ if( u < 0.0f || u > 1.0f )
+ return 0;
+
+ v3_cross( s, v0, q );
+ v = f * v3_dot( dir, q );
+ if( v < 0.0f || u+v > 1.0f )
+ return 0;
+
+ t = f * v3_dot(v1, q);
+ if( t > kEpsilon )
+ {
+ *dist = t;
+ return 1;
+ }
+ else return 0;
+}
+
+static inline float vg_randf(void)
+{
+ return (float)rand()/(float)(RAND_MAX);
+}
+
+static inline void vg_rand_dir(v3f dir)
+{
+ dir[0] = vg_randf();
+ dir[1] = vg_randf();
+ dir[2] = vg_randf();
+
+ v3_muls( dir, 2.0f, dir );
+ v3_sub( dir, (v3f){1.0f,1.0f,1.0f}, dir );
+
+ v3_normalize( dir );
+}
+
+static inline void vg_rand_sphere( v3f co )
+{
+ vg_rand_dir(co);
+ v3_muls( co, cbrtf( vg_randf() ), co );
+}
+
+static inline int vg_randint(int max)
+{
+ return rand()%max;
+}
+
+static void eval_bezier_time( v3f p0, v3f p1, v3f h0, v3f h1, float t, v3f p )
+{
+ float tt = t*t,
+ ttt = tt*t;
+
+ v3_muls( p1, ttt, p );
+ v3_muladds( p, h1, 3.0f*tt -3.0f*ttt, p );
+ v3_muladds( p, h0, 3.0f*ttt -6.0f*tt +3.0f*t, p );
+ v3_muladds( p, p0, 3.0f*tt -ttt -3.0f*t +1.0f, p );
+}
+
+#endif /* VG_M_H */