some raycasting stuff

diff --git a/vg_lines.h b/vg_lines.h
index 672a59e84d663b76fa407310d7ca0bc57e27c2e8..e2a79fee65efc3540d32300a72c6449e4314840a 100644 (file)
--- a/vg_lines.h
+++ b/vg_lines.h
@@ -3,6 +3,7 @@
 #ifndef VG_LINES_H
 #define VG_LINES_H
 
+#define VG_GAME
 #include "vg/vg.h"
 
 typedef v3f line_co;

diff --git a/vg_m.h b/vg_m.h
index 368f2889aff1f23807325e92f11228f22c654305..d6da3a1b933e2ff14790895728e6a5918d344903 100644 (file)
--- a/vg_m.h
+++ b/vg_m.h
@@ -1353,7 +1353,308 @@ static void m3x3_q( m3x3f m, v4f q )
    }
 }
 
-static int ray_tri( v3f tri[3], v3f co, v3f dir, float *dist )
+enum contact_type
+{
+   k_contact_type_default,
+   k_contact_type_disabled,
+   k_contact_type_edge
+};
+
+/*
+ * -----------------------------------------------------------------------------
+ *                        Closest point functions
+ * -----------------------------------------------------------------------------
+ */
+
+/* 
+ * These closest point tests were learned from Real-Time Collision Detection by 
+ * Christer Ericson 
+ */
+VG_STATIC float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2, 
+   float *s, float *t, v3f c1, v3f c2)
+{
+   v3f d1,d2,r;
+   v3_sub( q1, p1, d1 );
+   v3_sub( q2, p2, d2 );
+   v3_sub( p1, p2, r );
+
+   float a = v3_length2( d1 ),
+         e = v3_length2( d2 ),
+         f = v3_dot( d2, r );
+
+   const float kEpsilon = 0.0001f;
+
+   if( a <= kEpsilon && e <= kEpsilon )
+   {
+      *s = 0.0f;
+      *t = 0.0f;
+      v3_copy( p1, c1 );
+      v3_copy( p2, c2 );
+
+      v3f v0;
+      v3_sub( c1, c2, v0 );
+
+      return v3_length2( v0 );
+   }
+   
+   if( a<= kEpsilon )
+   {
+      *s = 0.0f;
+      *t = vg_clampf( f / e, 0.0f, 1.0f );
+   }
+   else
+   {
+      float c = v3_dot( d1, r );
+      if( e <= kEpsilon )
+      {
+         *t = 0.0f;
+         *s = vg_clampf( -c / a, 0.0f, 1.0f );
+      }
+      else
+      {
+         float b = v3_dot(d1,d2),
+               d = a*e-b*b;
+
+         if( d != 0.0f )
+         {
+            *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
+         }
+         else
+         {
+            *s = 0.0f;
+         }
+
+         *t = (b*(*s)+f) / e;
+
+         if( *t < 0.0f )
+         {
+            *t = 0.0f;
+            *s = vg_clampf( -c / a, 0.0f, 1.0f );
+         }
+         else if( *t > 1.0f )
+         {
+            *t = 1.0f;
+            *s = vg_clampf((b-c)/a,0.0f,1.0f);
+         }
+      }
+   }
+
+   v3_muladds( p1, d1, *s, c1 );
+   v3_muladds( p2, d2, *t, c2 );
+
+   v3f v0;
+   v3_sub( c1, c2, v0 );
+   return v3_length2( v0 );
+}
+
+VG_STATIC void closest_point_aabb( v3f p, boxf box, v3f dest )
+{
+   v3_maxv( p, box[0], dest );
+   v3_minv( dest, box[1], dest );
+}
+
+VG_STATIC void closest_point_obb( v3f p, boxf box, 
+                                  m4x3f mtx, m4x3f inv_mtx, v3f dest )
+{
+   v3f local;
+   m4x3_mulv( inv_mtx, p, local );
+   closest_point_aabb( local, box, local );
+   m4x3_mulv( mtx, local, dest );
+}
+
+VG_STATIC float closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
+{
+   v3f v0, v1;
+   v3_sub( b, a, v0 );
+   v3_sub( point, a, v1 );
+
+   float t = v3_dot( v1, v0 ) / v3_length2(v0);
+   t = vg_clampf(t,0.0f,1.0f);
+   v3_muladds( a, v0, t, dest );
+   return t;
+}
+
+VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
+{
+   v3f ab, ac, ap;
+   float d1, d2;
+
+   /* Region outside A */
+   v3_sub( tri[1], tri[0], ab );
+   v3_sub( tri[2], tri[0], ac );
+   v3_sub( p, tri[0], ap );
+   
+   d1 = v3_dot(ab,ap);
+   d2 = v3_dot(ac,ap);
+   if( d1 <= 0.0f && d2 <= 0.0f ) 
+   {
+      v3_copy( tri[0], dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* Region outside B */
+   v3f bp;
+   float d3, d4;
+
+   v3_sub( p, tri[1], bp );
+   d3 = v3_dot( ab, bp );
+   d4 = v3_dot( ac, bp );
+
+   if( d3 >= 0.0f && d4 <= d3 )
+   {
+      v3_copy( tri[1], dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+   
+   /* Edge region of AB */
+   float vc = d1*d4 - d3*d2;
+   if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
+   {
+      float v = d1 / (d1-d3);
+      v3_muladds( tri[0], ab, v, dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* Region outside C */
+   v3f cp;
+   float d5, d6;
+   v3_sub( p, tri[2], cp );
+   d5 = v3_dot(ab, cp);
+   d6 = v3_dot(ac, cp);
+   
+   if( d6 >= 0.0f && d5 <= d6 )
+   {
+      v3_copy( tri[2], dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* Region of AC */
+   float vb = d5*d2 - d1*d6;
+   if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+   {
+      float w = d2 / (d2-d6);
+      v3_muladds( tri[0], ac, w, dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* Region of BC */
+   float va = d3*d6 - d5*d4;
+   if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
+   {
+      float w = (d4-d3) / ((d4-d3) + (d5-d6));
+      v3f bc;
+      v3_sub( tri[2], tri[1], bc );
+      v3_muladds( tri[1], bc, w, dest );
+      v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+      return;
+   }
+
+   /* P inside region, Q via barycentric coordinates uvw */
+   float d = 1.0f/(va+vb+vc),
+         v = vb*d,
+         w = vc*d;
+
+   v3_muladds( tri[0], ab, v, dest );
+   v3_muladds( dest, ac, w, dest );
+}
+
+VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest )
+{
+   v3f ab, ac, ap;
+   float d1, d2;
+
+   /* Region outside A */
+   v3_sub( tri[1], tri[0], ab );
+   v3_sub( tri[2], tri[0], ac );
+   v3_sub( p, tri[0], ap );
+   
+   d1 = v3_dot(ab,ap);
+   d2 = v3_dot(ac,ap);
+   if( d1 <= 0.0f && d2 <= 0.0f ) 
+   {
+      v3_copy( tri[0], dest );
+      return k_contact_type_default;
+   }
+
+   /* Region outside B */
+   v3f bp;
+   float d3, d4;
+
+   v3_sub( p, tri[1], bp );
+   d3 = v3_dot( ab, bp );
+   d4 = v3_dot( ac, bp );
+
+   if( d3 >= 0.0f && d4 <= d3 )
+   {
+      v3_copy( tri[1], dest );
+      return k_contact_type_edge;
+   }
+   
+   /* Edge region of AB */
+   float vc = d1*d4 - d3*d2;
+   if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
+   {
+      float v = d1 / (d1-d3);
+      v3_muladds( tri[0], ab, v, dest );
+      return k_contact_type_edge;
+   }
+
+   /* Region outside C */
+   v3f cp;
+   float d5, d6;
+   v3_sub( p, tri[2], cp );
+   d5 = v3_dot(ab, cp);
+   d6 = v3_dot(ac, cp);
+   
+   if( d6 >= 0.0f && d5 <= d6 )
+   {
+      v3_copy( tri[2], dest );
+      return k_contact_type_edge;
+   }
+
+   /* Region of AC */
+   float vb = d5*d2 - d1*d6;
+   if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+   {
+      float w = d2 / (d2-d6);
+      v3_muladds( tri[0], ac, w, dest );
+      return k_contact_type_edge;
+   }
+
+   /* Region of BC */
+   float va = d3*d6 - d5*d4;
+   if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
+   {
+      float w = (d4-d3) / ((d4-d3) + (d5-d6));
+      v3f bc;
+      v3_sub( tri[2], tri[1], bc );
+      v3_muladds( tri[1], bc, w, dest );
+      return k_contact_type_edge;
+   }
+
+   /* P inside region, Q via barycentric coordinates uvw */
+   float d = 1.0f/(va+vb+vc),
+         v = vb*d,
+         w = vc*d;
+
+   v3_muladds( tri[0], ab, v, dest );
+   v3_muladds( dest, ac, w, dest );
+
+   return k_contact_type_default;
+}
+
+/*
+ * Raycasts
+ */
+
+/* Time of intersection with ray vs triangle */
+static int ray_tri( v3f tri[3], v3f co, 
+                    v3f dir, float *dist )
 {
    float const kEpsilon = 0.00001f;
 
@@ -1398,6 +1699,162 @@ static int ray_tri( v3f tri[3], v3f co, v3f dir, float *dist )
    else return 0;
 }
 
+/* time of intersection with ray vs sphere */
+static int ray_sphere( v3f c, float r, 
+                       v3f co, v3f dir, float *t )
+{
+   v3f m;
+   v3_sub( co, c, m );
+
+   float b  = v3_dot( m, dir ),
+         c1 = v3_dot( m, m ) - r*r;
+
+   /* Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0) */
+   if( c1 > 0.0f && b > 0.0f ) 
+      return 0;
+   
+   float discr = b*b - c1;
+
+   /* A negative discriminant corresponds to ray missing sphere */
+   if( discr < 0.0f )
+      return 0;
+   
+   /* 
+    * Ray now found to intersect sphere, compute smallest t value of 
+    * intersection 
+    */
+   *t = -b - sqrtf( discr );
+   
+   /* If t is negative, ray started inside sphere so clamp t to zero */
+   if( *t < 0.0f ) 
+      *t = 0.0f;
+
+   return 1;
+}
+
+/* 
+ * time of intersection of ray vs cylinder
+ * The cylinder does not have caps but is finite
+ *
+ * Heavily adapted from regular segment vs cylinder from:
+ *    Real-Time Collision Detection
+ */
+static int ray_uncapped_finite_cylinder( v3f q, v3f p, float r, 
+                                         v3f co, v3f dir, float *t )
+{
+   v3f d, m, n, sb;
+   v3_muladds( co, dir, 1.0f, sb );
+
+   v3_sub( q, p, d );
+   v3_sub( co, p, m );
+   v3_sub( sb, co, n );
+   
+   float md = v3_dot( m, d ),
+         nd = v3_dot( n, d ),
+         dd = v3_dot( d, d ),
+         nn = v3_dot( n, n ),
+         mn = v3_dot( m, n ),
+         a  = dd*nn - nd*nd,
+         k  = v3_dot( m, m ) - r*r,
+         c  = dd*k - md*md;
+
+   if( fabsf(a) < 0.00001f ) 
+   {
+      /* Segment runs parallel to cylinder axis */
+      return 0;
+   }
+
+   float b     = dd*mn - nd*md,
+         discr = b*b - a*c;
+
+   if( discr < 0.0f ) 
+      return 0; /* No real roots; no intersection */
+
+   *t = (-b - sqrtf(discr)) / a;
+   if( *t < 0.0f )
+      return 0; /* Intersection behind ray */
+   
+   /* Check within cylinder segment */
+   if( md + (*t)*nd < 0.0f ) 
+      return 0;
+
+   if( md + (*t)*nd > dd ) 
+      return 0;
+
+   /* Segment intersects cylinder between the endcaps; t is correct */
+   return 1;
+}
+
+/*
+ * Time of intersection of sphere and triangle. Origin must be outside the 
+ * colliding area. This is a fairly long procedure.
+ */
+static int spherecast_triangle( v3f tri[3],
+                                v3f co, v3f dir, float r, float *t, v3f n )
+{
+   v3f sum[3];
+   v3f v0, v1;
+
+   v3_sub( tri[1], tri[0], v0 );
+   v3_sub( tri[2], tri[0], v1 );
+   v3_cross( v0, v1, n );
+   v3_normalize( n );
+   v3_muladds( tri[0], n, r, sum[0] );
+   v3_muladds( tri[1], n, r, sum[1] );
+   v3_muladds( tri[2], n, r, sum[2] );
+
+   int hit = 0;
+   float t_min = INFINITY,
+         t1;
+
+   if( ray_tri( sum, co, dir, &t1 ) )
+   {
+      t_min = vg_minf( t_min, t1 );
+      hit = 1;
+   }
+
+   /* 
+    * Currently disabled; ray_sphere requires |d| = 1. it is not very important.
+    */
+#if 0
+   for( int i=0; i<3; i++ )
+   {
+      if( ray_sphere( tri[i], r, co, dir, &t1 ) )
+      {
+         t_min = vg_minf( t_min, t1 );
+         hit = 1;
+      }
+   }
+#endif
+
+   for( int i=0; i<3; i++ )
+   {
+      int i0 =  i,
+          i1 = (i+1)%3;
+
+      if( ray_uncapped_finite_cylinder( tri[i0], tri[i1], r, co, dir, &t1 ) )
+      {
+         if( t1 < t_min )
+         {
+            t_min = t1;
+            
+            v3f co1, ct, cx;
+            v3_add( dir, co, co1 );
+            v3_lerp( co, co1, t_min, ct );
+
+            closest_point_segment( tri[i0], tri[i1], ct, cx );
+            v3_sub( ct, cx, n );
+            v3_normalize( n );
+         }
+
+         hit = 1;
+      }
+   }
+
+   *t = t_min;
+   return hit;
+}
+
 static inline float vg_randf(void)
 {
    return (float)rand()/(float)(RAND_MAX);