rigidbody.h

   1 /*
   2  * Resources: Box2D - Erin Catto
   3  *            qu3e  - Randy Gaul
   4  */
   5
   6 #include "common.h"
   7 #include "bvh.h"
   8
   9 static void rb_tangent_basis( v3f n, v3f tx, v3f ty );
  10 static bh_system bh_system_rigidbodies;
  11
  12 #ifndef RIGIDBODY_H
  13 #define RIGIDBODY_H
  14
  15 #define RB_DEPR
  16 #define k_rb_rate  60.0f
  17 #define k_rb_delta (1.0f/k_rb_rate)
  18
  19 typedef struct rigidbody rigidbody;
  20 struct rigidbody
  21 {
  22    v3f co, v, I;
  23    v4f q;
  24
  25    enum rb_shape
  26    {
  27       k_rb_shape_box,
  28       k_rb_shape_capsule
  29    }
  30    type;
  31    v3f top, bottom;
  32    float radius;
  33
  34    boxf bbx, bbx_world;
  35    float inv_mass;
  36
  37    struct contact
  38    {
  39       v3f co, n, delta;
  40       v3f t[2];
  41       float bias, norm_impulse, tangent_impulse[2];
  42    }
  43    manifold[12];
  44    int manifold_count;
  45
  46    v3f delta;  /* where is the origin of this in relation to a parent body */
  47    m4x3f to_world, to_local;
  48 };
  49
  50 static void rb_update_transform( rigidbody *rb )
  51 {
  52    q_normalize( rb->q );
  53    q_m3x3( rb->q, rb->to_world );
  54    v3_copy( rb->co, rb->to_world[3] );
  55
  56    m4x3_invert_affine( rb->to_world, rb->to_local );
  57
  58    box_copy( rb->bbx, rb->bbx_world );
  59    m4x3_transform_aabb( rb->to_world, rb->bbx_world );
  60 }
  61
  62 static void rb_init( rigidbody *rb )
  63 {
  64    q_identity( rb->q );
  65    v3_zero( rb->v );
  66    v3_zero( rb->I );
  67
  68    v3f dims;
  69    v3_sub( rb->bbx[1], rb->bbx[0], dims );
  70
  71    rb->inv_mass = 1.0f/(8.0f*dims[0]*dims[1]*dims[2]);
  72
  73    rb_update_transform( rb );
  74 }
  75
  76 static void rb_iter( rigidbody *rb )
  77 {
  78    v3f gravity = { 0.0f, -9.6f, 0.0f };
  79    v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
  80
  81    /* intergrate velocity */
  82    v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
  83    v3_lerp( rb->I, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->I );
  84
  85    /* inegrate inertia */
  86    if( v3_length2( rb->I ) > 0.0f )
  87    {
  88       v4f rotation;
  89       v3f axis;
  90       v3_copy( rb->I, axis );
  91
  92       float mag = v3_length( axis );
  93       v3_divs( axis, mag, axis );
  94       q_axis_angle( rotation, axis, mag*k_rb_delta );
  95       q_mul( rotation, rb->q, rb->q );
  96    }
  97 }
  98
  99 static void rb_torque( rigidbody *rb, v3f axis, float mag )
 100 {
 101    v3_muladds( rb->I, axis, mag*k_rb_delta, rb->I );
 102 }
 103
 104 static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
 105 {
 106    /* Compute tangent basis (box2d) */
 107    if( fabsf( n[0] ) >= 0.57735027f )
 108    {
 109       tx[0] =  n[1];
 110       tx[1] = -n[0];
 111       tx[2] =  0.0f;
 112    }
 113    else
 114    {
 115       tx[0] =  0.0f;
 116       tx[1] =  n[2];
 117       tx[2] = -n[1];
 118    }
 119
 120    v3_normalize( tx );
 121    v3_cross( n, tx, ty );
 122 }
 123
 124 #include "world.h"
 125
 126 static void rb_manifold_reset( rigidbody *rb )
 127 {
 128    rb->manifold_count = 0;
 129 }
 130
 131 static void rb_build_manifold_terrain( rigidbody *rb )
 132 {
 133    v3f *box = rb->bbx;
 134    v3f pts[8];
 135    float *p000 = pts[0], *p001 = pts[1], *p010 = pts[2], *p011 = pts[3],
 136          *p100 = pts[4], *p101 = pts[5], *p110 = pts[6], *p111 = pts[7];
 137
 138    p000[0]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2];
 139    p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2];
 140    p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2];
 141    p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2];
 142
 143    p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2];
 144    p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2];
 145    p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2];
 146    p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2];
 147
 148    m4x3_mulv( rb->to_world, p000, p000 );
 149    m4x3_mulv( rb->to_world, p001, p001 );
 150    m4x3_mulv( rb->to_world, p010, p010 );
 151    m4x3_mulv( rb->to_world, p011, p011 );
 152    m4x3_mulv( rb->to_world, p100, p100 );
 153    m4x3_mulv( rb->to_world, p101, p101 );
 154    m4x3_mulv( rb->to_world, p110, p110 );
 155    m4x3_mulv( rb->to_world, p111, p111 );
 156
 157    int count = 0;
 158
 159    for( int i=0; i<8; i++ )
 160    {
 161       float *point = pts[i];
 162       struct contact *ct = &rb->manifold[rb->manifold_count];
 163
 164       v3f surface;
 165       v3_copy( point, surface );
 166       surface[1] += 4.0f;
 167
 168       ray_hit hit;
 169       hit.dist = INFINITY;
 170       if( !ray_world( surface, (v3f){0.0f,-1.0f,0.0f}, &hit ))
 171          continue;
 172
 173       v3_copy( hit.normal, ct->n );
 174       v3_copy( hit.pos, surface );
 175
 176       float p = vg_minf( surface[1] - point[1], 1.0f );
 177
 178       if( p > 0.0f )
 179       {
 180          v3_add( point, surface, ct->co );
 181          v3_muls( ct->co, 0.5f, ct->co );
 182
 183          //vg_line_pt3( ct->co, 0.0125f, 0xff0000ff );
 184
 185          v3_sub( ct->co, rb->co, ct->delta );
 186          ct->bias = -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -p+0.04f );
 187          rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
 188
 189          ct->norm_impulse = 0.0f;
 190          ct->tangent_impulse[0] = 0.0f;
 191          ct->tangent_impulse[1] = 0.0f;
 192
 193          rb->manifold_count ++;
 194          count ++;
 195          if( count == 4 )
 196             break;
 197       }
 198    }
 199 }
 200
 201 static void rb_constraint_manifold( rigidbody *rb )
 202 {
 203    float k_friction = 0.1f;
 204
 205    /* Friction Impulse */
 206    for( int i=0; i<rb->manifold_count; i++ )
 207    {
 208       struct contact *ct = &rb->manifold[i];
 209
 210       v3f dv;
 211       v3_cross( rb->I, ct->delta, dv );
 212       v3_add( rb->v, dv, dv );
 213
 214       for( int j=0; j<2; j++ )
 215       {
 216          float vt = vg_clampf( -v3_dot( dv, ct->t[j] ),
 217                -k_friction, k_friction );
 218
 219          vt = -v3_dot( dv, ct->t[j] );
 220
 221          float temp = ct->tangent_impulse[j];
 222          ct->tangent_impulse[j] = vg_clampf( temp+vt, -k_friction, k_friction );
 223          vt = ct->tangent_impulse[j] - temp;
 224
 225          v3f impulse;
 226
 227          v3_muls( ct->t[j], vt, impulse );
 228          v3_add( impulse, rb->v, rb->v );
 229          v3_cross( ct->delta, impulse, impulse );
 230          v3_add( impulse, rb->I, rb->I );
 231       }
 232    }
 233
 234    /* Normal Impulse */
 235    for( int i=0; i<rb->manifold_count; i++ )
 236    {
 237       struct contact *ct = &rb->manifold[i];
 238
 239       v3f dv;
 240       v3_cross( rb->I, ct->delta, dv );
 241       v3_add( rb->v, dv, dv );
 242
 243       float vn = -v3_dot( dv, ct->n );
 244       vn += ct->bias;
 245
 246       float temp = ct->norm_impulse;
 247       ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
 248       vn = ct->norm_impulse - temp;
 249
 250       v3f impulse;
 251
 252       v3_muls( ct->n, vn, impulse );
 253       v3_add( impulse, rb->v, rb->v );
 254       v3_cross( ct->delta, impulse, impulse );
 255       v3_add( impulse, rb->I, rb->I );
 256    }
 257 }
 258
 259 struct rb_angle_limit
 260 {
 261    rigidbody *rba, *rbb;
 262    v3f axis;
 263    float impulse, bias;
 264 };
 265
 266 static int rb_angle_limit_force(  rigidbody *rba, v3f va,
 267                                   rigidbody *rbb, v3f vb,
 268                                   float max )
 269 {
 270    v3f wva, wvb;
 271    m3x3_mulv( rba->to_world, va, wva );
 272    m3x3_mulv( rbb->to_world, vb, wvb );
 273
 274    float dt = v3_dot(wva,wvb)*0.999f,
 275         ang = fabsf(dt);
 276    ang = acosf( dt );
 277    if( ang > max )
 278    {
 279       float correction = max-ang;
 280
 281       v3f axis;
 282       v3_cross( wva, wvb, axis );
 283
 284       v4f rotation;
 285       q_axis_angle( rotation, axis, -correction*0.25f );
 286       q_mul( rotation, rba->q, rba->q );
 287
 288       q_axis_angle( rotation, axis,  correction*0.25f );
 289       q_mul( rotation, rbb->q, rbb->q );
 290
 291       return 1;
 292    }
 293
 294    return 0;
 295 }
 296
 297 static void rb_constraint_angle_limit( struct rb_angle_limit *limit )
 298 {
 299
 300 }
 301
 302 RB_DEPR
 303 static void rb_constraint_angle( rigidbody *rba, v3f va,
 304                                  rigidbody *rbb, v3f vb,
 305                                  float max, float spring )
 306 {
 307    v3f wva, wvb;
 308    m3x3_mulv( rba->to_world, va, wva );
 309    m3x3_mulv( rbb->to_world, vb, wvb );
 310
 311    float dt = v3_dot(wva,wvb)*0.999f,
 312         ang = fabsf(dt);
 313
 314    v3f axis;
 315    v3_cross( wva, wvb, axis );
 316    v3_muladds( rba->I, axis,  ang*spring*0.5f, rba->I );
 317    v3_muladds( rbb->I, axis, -ang*spring*0.5f, rbb->I );
 318
 319    return;
 320
 321    /* TODO: convert max into the dot product value so we dont have to always
 322     *       evaluate acosf, only if its greater than the angle specified */
 323    ang = acosf( dt );
 324    if( ang > max )
 325    {
 326       float correction = max-ang;
 327
 328       v4f rotation;
 329       q_axis_angle( rotation, axis, -correction*0.125f );
 330       q_mul( rotation, rba->q, rba->q );
 331
 332       q_axis_angle( rotation, axis,  correction*0.125f );
 333       q_mul( rotation, rbb->q, rbb->q );
 334    }
 335 }
 336
 337 static void rb_relative_velocity( rigidbody *ra, v3f lca,
 338                                   rigidbody *rb, v3f lcb, v3f rcv )
 339 {
 340    v3f wca, wcb;
 341    m3x3_mulv( ra->to_world, lca, wca );
 342    m3x3_mulv( rb->to_world, lcb, wcb );
 343
 344    v3_sub( ra->v, rb->v, rcv );
 345
 346    v3f rcv_Ra, rcv_Rb;
 347    v3_cross( ra->I, wca, rcv_Ra );
 348    v3_cross( rb->I, wcb, rcv_Rb );
 349    v3_add( rcv_Ra, rcv, rcv );
 350    v3_sub( rcv, rcv_Rb, rcv );
 351 }
 352
 353 static void rb_constraint_position( rigidbody *ra, v3f lca,
 354                                     rigidbody *rb, v3f lcb )
 355 {
 356    /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
 357    v3f wca, wcb;
 358    m3x3_mulv( ra->to_world, lca, wca );
 359    m3x3_mulv( rb->to_world, lcb, wcb );
 360
 361    v3f delta;
 362    v3_add( wcb, rb->co, delta );
 363    v3_sub( delta, wca, delta );
 364    v3_sub( delta, ra->co, delta );
 365
 366    v3_muladds( ra->co, delta,  0.5f, ra->co );
 367    v3_muladds( rb->co, delta, -0.5f, rb->co );
 368
 369    v3f rcv;
 370    v3_sub( ra->v, rb->v, rcv );
 371
 372    v3f rcv_Ra, rcv_Rb;
 373    v3_cross( ra->I, wca, rcv_Ra );
 374    v3_cross( rb->I, wcb, rcv_Rb );
 375    v3_add( rcv_Ra, rcv, rcv );
 376    v3_sub( rcv, rcv_Rb, rcv );
 377
 378    float nm = 0.5f/(rb->inv_mass + ra->inv_mass);
 379
 380    float mass_a = 1.0f/ra->inv_mass,
 381          mass_b = 1.0f/rb->inv_mass,
 382          total_mass = mass_a+mass_b;
 383
 384    v3f impulse;
 385    v3_muls( rcv, 1.0f, impulse );
 386    v3_muladds( rb->v, impulse, mass_b/total_mass, rb->v );
 387    v3_cross( wcb, impulse, impulse );
 388    v3_add( impulse, rb->I, rb->I );
 389
 390    v3_muls( rcv, -1.0f, impulse );
 391    v3_muladds( ra->v, impulse, mass_a/total_mass, ra->v );
 392    v3_cross( wca, impulse, impulse );
 393    v3_add( impulse, ra->I, ra->I );
 394
 395 #if 0
 396    /*
 397     * this could be used for spring joints
 398     * its not good for position constraint
 399     */
 400    v3f impulse;
 401    v3_muls( delta, 0.5f*spring, impulse );
 402
 403    v3_add( impulse, ra->v, ra->v );
 404    v3_cross( wca, impulse, impulse );
 405    v3_add( impulse, ra->I, ra->I );
 406
 407    v3_muls( delta, -0.5f*spring, impulse );
 408
 409    v3_add( impulse, rb->v, rb->v );
 410    v3_cross( wcb, impulse, impulse );
 411    v3_add( impulse, rb->I, rb->I );
 412 #endif
 413 }
 414
 415 static void rb_debug( rigidbody *rb, u32 colour )
 416 {
 417    v3f *box = rb->bbx;
 418    vg_line_boxf_transformed( rb->to_world, rb->bbx, colour );
 419 }
 420
 421 /*
 422  * out penetration distance, normal
 423  */
 424 static int rb_point_in_body( rigidbody *rb, v3f pos, float *pen, v3f normal )
 425 {
 426    v3f local;
 427    m4x3_mulv( rb->to_local, pos, local );
 428
 429    if( local[0] > rb->bbx[0][0] && local[0] < rb->bbx[1][0] &&
 430        local[1] > rb->bbx[0][1] && local[1] < rb->bbx[1][1] &&
 431        local[2] > rb->bbx[0][2] && local[2] < rb->bbx[1][2] )
 432    {
 433       v3f area, com, comrel;
 434       v3_add( rb->bbx[0], rb->bbx[1], com );
 435       v3_muls( com, 0.5f, com );
 436
 437       v3_sub( rb->bbx[1], rb->bbx[0], area );
 438       v3_sub( local, com, comrel );
 439       v3_div( comrel, area, comrel );
 440
 441       int axis = 0;
 442       float max_mag = fabsf(comrel[0]);
 443
 444       if( fabsf(comrel[1]) > max_mag )
 445       {
 446          axis = 1;
 447          max_mag = fabsf(comrel[1]);
 448       }
 449       if( fabsf(comrel[2]) > max_mag )
 450       {
 451          axis = 2;
 452          max_mag = fabsf(comrel[2]);
 453       }
 454
 455       v3_zero( normal );
 456       normal[axis] = vg_signf(comrel[axis]);
 457
 458       if( normal[axis] < 0.0f )
 459          *pen = local[axis] - rb->bbx[0][axis];
 460       else
 461          *pen = rb->bbx[1][axis] - local[axis];
 462
 463       m3x3_mulv( rb->to_world, normal, normal );
 464       return 1;
 465    }
 466
 467    return 0;
 468 }
 469
 470 static void rb_build_manifold_rb_static( rigidbody *ra, rigidbody *rb_static )
 471 {
 472    v3f verts[8];
 473
 474    v3f a, b;
 475    v3_copy( ra->bbx[0], a );
 476    v3_copy( ra->bbx[1], b );
 477
 478         m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], a[2] }, verts[0] );
 479         m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], a[2] }, verts[1] );
 480         m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], a[2] }, verts[2] );
 481         m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], a[2] }, verts[3] );
 482         m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], b[2] }, verts[4] );
 483         m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], b[2] }, verts[5] );
 484         m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], b[2] }, verts[6] );
 485         m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], b[2] }, verts[7] );
 486
 487    vg_line_boxf_transformed( rb_static->to_world, rb_static->bbx, 0xff0000ff );
 488
 489    int count = 0;
 490
 491    for( int i=0; i<8; i++ )
 492    {
 493       if( ra->manifold_count == vg_list_size(ra->manifold) )
 494          return;
 495
 496       struct contact *ct = &ra->manifold[ ra->manifold_count ];
 497
 498       float p;
 499       v3f normal;
 500
 501       if( rb_point_in_body( rb_static, verts[i], &p, normal ))
 502       {
 503          v3_copy( normal, ct->n );
 504          v3_muladds( verts[i], ct->n, p*0.5f, ct->co );
 505          v3_sub( ct->co, ra->co, ct->delta );
 506
 507          vg_line_pt3( ct->co, 0.0125f, 0xffff00ff );
 508
 509          ct->bias = -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -p+0.04f );
 510          rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
 511
 512          ct->norm_impulse = 0.0f;
 513          ct->tangent_impulse[0] = 0.0f;
 514          ct->tangent_impulse[1] = 0.0f;
 515
 516          ra->manifold_count ++;
 517          count ++;
 518          if( count == 4 )
 519             return;
 520       }
 521    }
 522 }
 523
 524 /*
 525  * Capsule phyics
 526  */
 527
 528 static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
 529    float *s, float *t, v3f c1, v3f c2)
 530 {
 531    v3f d1,d2,r;
 532    v3_sub( q1, p1, d1 );
 533    v3_sub( q2, p2, d2 );
 534    v3_sub( p1, p2, r );
 535
 536    float a = v3_length2( d1 ),
 537          e = v3_length2( d2 ),
 538          f = v3_dot( d2, r );
 539
 540    const float kEpsilon = 0.0001f;
 541
 542    if( a <= kEpsilon && e <= kEpsilon )
 543    {
 544       *s = 0.0f;
 545       *t = 0.0f;
 546       v3_copy( p1, c1 );
 547       v3_copy( p2, c2 );
 548
 549       v3f v0;
 550       v3_sub( c1, c2, v0 );
 551
 552       return v3_length2( v0 );
 553    }
 554
 555    if( a<= kEpsilon )
 556    {
 557       *s = 0.0f;
 558       *t = vg_clampf( f / e, 0.0f, 1.0f );
 559    }
 560    else
 561    {
 562       float c = v3_dot( d1, r );
 563       if( e <= kEpsilon )
 564       {
 565          *t = 0.0f;
 566          *s = vg_clampf( -c / a, 0.0f, 1.0f );
 567       }
 568       else
 569       {
 570          float b = v3_dot(d1,d2),
 571                d = a*e-b*b;
 572
 573          if( d != 0.0f )
 574          {
 575             *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
 576          }
 577          else
 578          {
 579             *s = 0.0f;
 580          }
 581
 582          *t = (b*(*s)+f) / e;
 583
 584          if( *t < 0.0f )
 585          {
 586             *t = 0.0f;
 587             *s = vg_clampf( -c / a, 0.0f, 1.0f );
 588          }
 589          else if( *t > 1.0f )
 590          {
 591             *t = 1.0f;
 592             *s = vg_clampf((b-c)/a,0.0f,1.0f);
 593          }
 594       }
 595    }
 596
 597    v3_muladds( p1, d1, *s, c1 );
 598    v3_muladds( p2, d2, *t, c2 );
 599
 600    v3f v0;
 601    v3_sub( c1, c2, v0 );
 602    return v3_length2( v0 );
 603 }
 604
 605 static void closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
 606 {
 607    v3f v0, v1;
 608    v3_sub( b, a, v0 );
 609    v3_sub( point, a, v1 );
 610
 611    float t = v3_dot( v1, v0 ) / v3_length2(v0);
 612    v3_muladds( a, v0, vg_clampf(t,0.0f,1.0f), dest );
 613 }
 614
 615 /* Real-Time Collision Detection */
 616 static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
 617 {
 618    v3f ab, ac, ap;
 619    float d1, d2;
 620
 621    /* Region outside A */
 622    v3_sub( tri[1], tri[0], ab );
 623    v3_sub( tri[2], tri[0], ac );
 624    v3_sub( p, tri[0], ap );
 625
 626    d1 = v3_dot(ab,ap);
 627    d2 = v3_dot(ac,ap);
 628    if( d1 <= 0.0f && d2 <= 0.0f )
 629    {
 630       v3_copy( tri[0], dest );
 631       v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
 632       return;
 633    }
 634
 635    /* Region outside B */
 636    v3f bp;
 637    float d3, d4;
 638
 639    v3_sub( p, tri[1], bp );
 640    d3 = v3_dot( ab, bp );
 641    d4 = v3_dot( ac, bp );
 642
 643    if( d3 >= 0.0f && d4 <= d3 )
 644    {
 645       v3_copy( tri[1], dest );
 646       v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
 647       return;
 648    }
 649
 650    /* Edge region of AB */
 651    float vc = d1*d4 - d3*d2;
 652    if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
 653    {
 654       float v = d1 / (d1-d3);
 655       v3_muladds( tri[0], ab, v, dest );
 656       v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
 657       return;
 658    }
 659
 660    /* Region outside C */
 661    v3f cp;
 662    float d5, d6;
 663    v3_sub( p, tri[2], cp );
 664    d5 = v3_dot(ab, cp);
 665    d6 = v3_dot(ac, cp);
 666
 667    if( d6 >= 0.0f && d5 <= d6 )
 668    {
 669       v3_copy( tri[2], dest );
 670       v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
 671       return;
 672    }
 673
 674    /* Region of AC */
 675    float vb = d5*d2 - d1*d6;
 676    if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
 677    {
 678       float w = d2 / (d2-d6);
 679       v3_muladds( tri[0], ac, w, dest );
 680       v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
 681       return;
 682    }
 683
 684    /* Region of BC */
 685    float va = d3*d6 - d5*d4;
 686    if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
 687    {
 688       float w = (d4-d3) / ((d4-d3) + (d5-d6));
 689       v3f bc;
 690       v3_sub( tri[2], tri[1], bc );
 691       v3_muladds( tri[1], bc, w, dest );
 692       v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
 693       return;
 694    }
 695
 696    /* P inside region, Q via barycentric coordinates uvw */
 697    float d = 1.0f/(va+vb+vc),
 698          v = vb*d,
 699          w = vc*d;
 700
 701    v3_muladds( tri[0], ab, v, dest );
 702    v3_muladds( dest, ac, w, dest );
 703 }
 704
 705 static int sphere_vs_triangle( v3f c, float r, v3f tri[3],
 706       v3f co, v3f norm, float *p )
 707 {
 708    v3f delta;
 709    closest_on_triangle( c, tri, co );
 710
 711    v3_sub( c, co, delta );
 712
 713
 714    float d = v3_length2( delta );
 715    if( d < r*r )
 716    {
 717       v3f ab, ac, tn;
 718       v3_sub( tri[1], tri[0], ab );
 719       v3_sub( tri[2], tri[0], ac );
 720       v3_cross( ac, ab, tn );
 721
 722       if( v3_dot( delta, tn ) > 0.0f )
 723          v3_muls( delta, -1.0f, delta );
 724
 725       vg_line_pt3( co, 0.05f, 0xff00ff00 );
 726
 727       d = sqrtf(d);
 728       v3_muls( delta, 1.0f/d, norm );
 729
 730       *p = r-d;
 731       return 1;
 732    }
 733
 734    return 0;
 735 }
 736
 737 static void debug_capsule( m4x3f m, float height, float radius, u32 colour )
 738 {
 739    v3f last = { 0.0f, 0.0f, radius };
 740    m4x3f lower, upper;
 741    m3x3_copy( m, lower );
 742    m3x3_copy( m, upper );
 743    m4x3_mulv( m, (v3f){0.0f,-height*0.5f+radius,0.0f}, lower[3] );
 744    m4x3_mulv( m, (v3f){0.0f, height*0.5f-radius,0.0f}, upper[3] );
 745
 746    for( int i=0; i<16; i++ )
 747    {
 748       float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
 749             s = sinf(t),
 750             c = cosf(t);
 751
 752       v3f p = { s*radius, 0.0f, c*radius };
 753
 754       v3f p0, p1;
 755       m4x3_mulv( lower, p, p0 );
 756       m4x3_mulv( lower, last, p1 );
 757       vg_line( p0, p1, colour );
 758
 759       m4x3_mulv( upper, p, p0 );
 760       m4x3_mulv( upper, last, p1 );
 761       vg_line( p0, p1, colour );
 762
 763       v3_copy( p, last );
 764    }
 765
 766    for( int i=0; i<4; i++ )
 767    {
 768       float t = ((float)(i) * (1.0f/4.0f)) * VG_PIf * 2.0f,
 769             s = sinf(t),
 770             c = cosf(t);
 771
 772       v3f p = { s*radius, 0.0f, c*radius };
 773
 774       v3f p0, p1;
 775       m4x3_mulv( lower, p, p0 );
 776       m4x3_mulv( upper, p, p1 );
 777       vg_line( p0, p1, colour );
 778
 779       m4x3_mulv( lower, (v3f){0.0f,-radius,0.0f}, p0 );
 780       m4x3_mulv( upper, (v3f){0.0f, radius,0.0f}, p1 );
 781       vg_line( p0, p1, colour );
 782    }
 783 }
 784
 785 /*
 786  * BVH implementation, this is ONLY for static rigidbodies, its to slow for
 787  * realtime use.
 788  */
 789
 790 static void rb_bh_expand_bound( void *user, boxf bound, u32 item_index )
 791 {
 792    rigidbody *rb = &((rigidbody *)user)[ item_index ];
 793    box_concat( bound, rb->bbx_world );
 794 }
 795
 796 static float rb_bh_centroid( void *user, u32 item_index, int axis )
 797 {
 798    rigidbody *rb = &((rigidbody *)user)[ item_index ];
 799    return (rb->bbx_world[axis][0] + rb->bbx_world[1][axis]) * 0.5f;
 800 }
 801
 802 static void rb_bh_swap( void *user, u32 ia, u32 ib )
 803 {
 804    rigidbody temp, *rba, *rbb;
 805    rba = &((rigidbody *)user)[ ia ];
 806    rbb = &((rigidbody *)user)[ ib ];
 807
 808    temp = *rba;
 809    *rba = *rbb;
 810    *rbb = temp;
 811 }
 812
 813 static void rb_bh_debug( void *user, u32 item_index )
 814 {
 815    rigidbody *rb = &((rigidbody *)user)[ item_index ];
 816    rb_debug( rb, 0xff00ffff );
 817 }
 818
 819 static bh_system bh_system_rigidbodies =
 820 {
 821    .expand_bound = rb_bh_expand_bound,
 822    .item_centroid = rb_bh_centroid,
 823    .item_swap = rb_bh_swap,
 824    .item_debug = rb_bh_debug,
 825    .cast_ray = NULL
 826 };
 827
 828 #endif /* RIGIDBODY_H */