distq.h

   1 #ifndef DISTQ_H
   2 #define DISTQ_H
   3
   4 #include "vg_m.h"
   5
   6 enum contact_type
   7 {
   8    k_contact_type_default,
   9    k_contact_type_disabled,
  10    k_contact_type_edge
  11 };
  12
  13 /*
  14  * -----------------------------------------------------------------------------
  15  *                        Closest point functions
  16  * -----------------------------------------------------------------------------
  17  */
  18
  19 /*
  20  * These closest point tests were learned from Real-Time Collision Detection by
  21  * Christer Ericson
  22  */
  23 VG_STATIC float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
  24    float *s, float *t, v3f c1, v3f c2)
  25 {
  26    v3f d1,d2,r;
  27    v3_sub( q1, p1, d1 );
  28    v3_sub( q2, p2, d2 );
  29    v3_sub( p1, p2, r );
  30
  31    float a = v3_length2( d1 ),
  32          e = v3_length2( d2 ),
  33          f = v3_dot( d2, r );
  34
  35    const float kEpsilon = 0.0001f;
  36
  37    if( a <= kEpsilon && e <= kEpsilon )
  38    {
  39       *s = 0.0f;
  40       *t = 0.0f;
  41       v3_copy( p1, c1 );
  42       v3_copy( p2, c2 );
  43
  44       v3f v0;
  45       v3_sub( c1, c2, v0 );
  46
  47       return v3_length2( v0 );
  48    }
  49
  50    if( a<= kEpsilon )
  51    {
  52       *s = 0.0f;
  53       *t = vg_clampf( f / e, 0.0f, 1.0f );
  54    }
  55    else
  56    {
  57       float c = v3_dot( d1, r );
  58       if( e <= kEpsilon )
  59       {
  60          *t = 0.0f;
  61          *s = vg_clampf( -c / a, 0.0f, 1.0f );
  62       }
  63       else
  64       {
  65          float b = v3_dot(d1,d2),
  66                d = a*e-b*b;
  67
  68          if( d != 0.0f )
  69          {
  70             *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
  71          }
  72          else
  73          {
  74             *s = 0.0f;
  75          }
  76
  77          *t = (b*(*s)+f) / e;
  78
  79          if( *t < 0.0f )
  80          {
  81             *t = 0.0f;
  82             *s = vg_clampf( -c / a, 0.0f, 1.0f );
  83          }
  84          else if( *t > 1.0f )
  85          {
  86             *t = 1.0f;
  87             *s = vg_clampf((b-c)/a,0.0f,1.0f);
  88          }
  89       }
  90    }
  91
  92    v3_muladds( p1, d1, *s, c1 );
  93    v3_muladds( p2, d2, *t, c2 );
  94
  95    v3f v0;
  96    v3_sub( c1, c2, v0 );
  97    return v3_length2( v0 );
  98 }
  99
 100 VG_STATIC void closest_point_aabb( v3f p, boxf box, v3f dest )
 101 {
 102    v3_maxv( p, box[0], dest );
 103    v3_minv( dest, box[1], dest );
 104 }
 105
 106 VG_STATIC void closest_point_obb( v3f p, boxf box,
 107                                   m4x3f mtx, m4x3f inv_mtx, v3f dest )
 108 {
 109    v3f local;
 110    m4x3_mulv( inv_mtx, p, local );
 111    closest_point_aabb( local, box, local );
 112    m4x3_mulv( mtx, local, dest );
 113 }
 114
 115 VG_STATIC float closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
 116 {
 117    v3f v0, v1;
 118    v3_sub( b, a, v0 );
 119    v3_sub( point, a, v1 );
 120
 121    float t = v3_dot( v1, v0 ) / v3_length2(v0);
 122    t = vg_clampf(t,0.0f,1.0f);
 123    v3_muladds( a, v0, t, dest );
 124    return t;
 125 }
 126
 127 VG_STATIC void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
 128 {
 129    v3f ab, ac, ap;
 130    float d1, d2;
 131
 132    /* Region outside A */
 133    v3_sub( tri[1], tri[0], ab );
 134    v3_sub( tri[2], tri[0], ac );
 135    v3_sub( p, tri[0], ap );
 136
 137    d1 = v3_dot(ab,ap);
 138    d2 = v3_dot(ac,ap);
 139    if( d1 <= 0.0f && d2 <= 0.0f )
 140    {
 141       v3_copy( tri[0], dest );
 142       v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
 143       return;
 144    }
 145
 146    /* Region outside B */
 147    v3f bp;
 148    float d3, d4;
 149
 150    v3_sub( p, tri[1], bp );
 151    d3 = v3_dot( ab, bp );
 152    d4 = v3_dot( ac, bp );
 153
 154    if( d3 >= 0.0f && d4 <= d3 )
 155    {
 156       v3_copy( tri[1], dest );
 157       v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
 158       return;
 159    }
 160
 161    /* Edge region of AB */
 162    float vc = d1*d4 - d3*d2;
 163    if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
 164    {
 165       float v = d1 / (d1-d3);
 166       v3_muladds( tri[0], ab, v, dest );
 167       v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
 168       return;
 169    }
 170
 171    /* Region outside C */
 172    v3f cp;
 173    float d5, d6;
 174    v3_sub( p, tri[2], cp );
 175    d5 = v3_dot(ab, cp);
 176    d6 = v3_dot(ac, cp);
 177
 178    if( d6 >= 0.0f && d5 <= d6 )
 179    {
 180       v3_copy( tri[2], dest );
 181       v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
 182       return;
 183    }
 184
 185    /* Region of AC */
 186    float vb = d5*d2 - d1*d6;
 187    if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
 188    {
 189       float w = d2 / (d2-d6);
 190       v3_muladds( tri[0], ac, w, dest );
 191       v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
 192       return;
 193    }
 194
 195    /* Region of BC */
 196    float va = d3*d6 - d5*d4;
 197    if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
 198    {
 199       float w = (d4-d3) / ((d4-d3) + (d5-d6));
 200       v3f bc;
 201       v3_sub( tri[2], tri[1], bc );
 202       v3_muladds( tri[1], bc, w, dest );
 203       v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
 204       return;
 205    }
 206
 207    /* P inside region, Q via barycentric coordinates uvw */
 208    float d = 1.0f/(va+vb+vc),
 209          v = vb*d,
 210          w = vc*d;
 211
 212    v3_muladds( tri[0], ab, v, dest );
 213    v3_muladds( dest, ac, w, dest );
 214 }
 215
 216 VG_STATIC enum contact_type closest_on_triangle_1( v3f p, v3f tri[3], v3f dest )
 217 {
 218    v3f ab, ac, ap;
 219    float d1, d2;
 220
 221    /* Region outside A */
 222    v3_sub( tri[1], tri[0], ab );
 223    v3_sub( tri[2], tri[0], ac );
 224    v3_sub( p, tri[0], ap );
 225
 226    d1 = v3_dot(ab,ap);
 227    d2 = v3_dot(ac,ap);
 228    if( d1 <= 0.0f && d2 <= 0.0f )
 229    {
 230       v3_copy( tri[0], dest );
 231       return k_contact_type_default;
 232    }
 233
 234    /* Region outside B */
 235    v3f bp;
 236    float d3, d4;
 237
 238    v3_sub( p, tri[1], bp );
 239    d3 = v3_dot( ab, bp );
 240    d4 = v3_dot( ac, bp );
 241
 242    if( d3 >= 0.0f && d4 <= d3 )
 243    {
 244       v3_copy( tri[1], dest );
 245       return k_contact_type_edge;
 246    }
 247
 248    /* Edge region of AB */
 249    float vc = d1*d4 - d3*d2;
 250    if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
 251    {
 252       float v = d1 / (d1-d3);
 253       v3_muladds( tri[0], ab, v, dest );
 254       return k_contact_type_edge;
 255    }
 256
 257    /* Region outside C */
 258    v3f cp;
 259    float d5, d6;
 260    v3_sub( p, tri[2], cp );
 261    d5 = v3_dot(ab, cp);
 262    d6 = v3_dot(ac, cp);
 263
 264    if( d6 >= 0.0f && d5 <= d6 )
 265    {
 266       v3_copy( tri[2], dest );
 267       return k_contact_type_edge;
 268    }
 269
 270    /* Region of AC */
 271    float vb = d5*d2 - d1*d6;
 272    if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
 273    {
 274       float w = d2 / (d2-d6);
 275       v3_muladds( tri[0], ac, w, dest );
 276       return k_contact_type_edge;
 277    }
 278
 279    /* Region of BC */
 280    float va = d3*d6 - d5*d4;
 281    if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
 282    {
 283       float w = (d4-d3) / ((d4-d3) + (d5-d6));
 284       v3f bc;
 285       v3_sub( tri[2], tri[1], bc );
 286       v3_muladds( tri[1], bc, w, dest );
 287       return k_contact_type_edge;
 288    }
 289
 290    /* P inside region, Q via barycentric coordinates uvw */
 291    float d = 1.0f/(va+vb+vc),
 292          v = vb*d,
 293          w = vc*d;
 294
 295    v3_muladds( tri[0], ab, v, dest );
 296    v3_muladds( dest, ac, w, dest );
 297
 298    return k_contact_type_default;
 299 }
 300
 301 #endif /* DISTQ_H */