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_delta (1.0f/60.0f)
  17
  18 typedef struct rigidbody rigidbody;
  19 struct rigidbody
  20 {
  21    v3f co, v, I;
  22    v4f q;
  23    boxf bbx, bbx_world;
  24    float inv_mass;
  25
  26    struct contact
  27    {
  28       v3f co, n, delta;
  29       v3f t[2];
  30       float bias, norm_impulse, tangent_impulse[2];
  31    }
  32    manifold[12];
  33    int manifold_count;
  34
  35    v3f delta;  /* where is the origin of this in relation to a parent body */
  36    m4x3f to_world, to_local;
  37 };
  38
  39 static void rb_update_transform( rigidbody *rb )
  40 {
  41    q_normalize( rb->q );
  42    q_m3x3( rb->q, rb->to_world );
  43    v3_copy( rb->co, rb->to_world[3] );
  44
  45    m4x3_invert_affine( rb->to_world, rb->to_local );
  46
  47    box_copy( rb->bbx, rb->bbx_world );
  48    m4x3_transform_aabb( rb->to_world, rb->bbx_world );
  49 }
  50
  51 static void rb_init( rigidbody *rb )
  52 {
  53    q_identity( rb->q );
  54    v3_zero( rb->v );
  55    v3_zero( rb->I );
  56
  57    v3f dims;
  58    v3_sub( rb->bbx[1], rb->bbx[0], dims );
  59
  60    rb->inv_mass = 1.0f/(8.0f*dims[0]*dims[1]*dims[2]);
  61
  62    rb_update_transform( rb );
  63 }
  64
  65 static void rb_iter( rigidbody *rb )
  66 {
  67    v3f gravity = { 0.0f, -9.6f, 0.0f };
  68    v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
  69
  70    /* intergrate velocity */
  71    v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
  72
  73    v3_lerp( rb->I, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->I );
  74
  75    /* inegrate inertia */
  76    if( v3_length2( rb->I ) > 0.0f )
  77    {
  78       v4f rotation;
  79       v3f axis;
  80       v3_copy( rb->I, axis );
  81
  82       float mag = v3_length( axis );
  83       v3_divs( axis, mag, axis );
  84       q_axis_angle( rotation, axis, mag*k_rb_delta );
  85       q_mul( rotation, rb->q, rb->q );
  86    }
  87 }
  88
  89 static void rb_torque( rigidbody *rb, v3f axis, float mag )
  90 {
  91    v3_muladds( rb->I, axis, mag*k_rb_delta, rb->I );
  92 }
  93
  94 static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
  95 {
  96    /* Compute tangent basis (box2d) */
  97    if( fabsf( n[0] ) >= 0.57735027f )
  98    {
  99       tx[0] =  n[1];
 100       tx[1] = -n[0];
 101       tx[2] =  0.0f;
 102    }
 103    else
 104    {
 105       tx[0] =  0.0f;
 106       tx[1] =  n[2];
 107       tx[2] = -n[1];
 108    }
 109
 110    v3_normalize( tx );
 111    v3_cross( n, tx, ty );
 112 }
 113
 114 #include "world.h"
 115
 116 static void rb_manifold_reset( rigidbody *rb )
 117 {
 118    rb->manifold_count = 0;
 119 }
 120
 121 static void rb_build_manifold_terrain( rigidbody *rb )
 122 {
 123    v3f *box = rb->bbx;
 124    v3f pts[8];
 125    float *p000 = pts[0], *p001 = pts[1], *p010 = pts[2], *p011 = pts[3],
 126          *p100 = pts[4], *p101 = pts[5], *p110 = pts[6], *p111 = pts[7];
 127
 128    p000[0]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2];
 129    p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2];
 130    p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2];
 131    p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2];
 132
 133    p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2];
 134    p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2];
 135    p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2];
 136    p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2];
 137
 138    m4x3_mulv( rb->to_world, p000, p000 );
 139    m4x3_mulv( rb->to_world, p001, p001 );
 140    m4x3_mulv( rb->to_world, p010, p010 );
 141    m4x3_mulv( rb->to_world, p011, p011 );
 142    m4x3_mulv( rb->to_world, p100, p100 );
 143    m4x3_mulv( rb->to_world, p101, p101 );
 144    m4x3_mulv( rb->to_world, p110, p110 );
 145    m4x3_mulv( rb->to_world, p111, p111 );
 146
 147    int count = 0;
 148
 149    for( int i=0; i<8; i++ )
 150    {
 151       float *point = pts[i];
 152       struct contact *ct = &rb->manifold[rb->manifold_count];
 153
 154       v3f surface;
 155       v3_copy( point, surface );
 156       surface[1] += 4.0f;
 157
 158       ray_hit hit;
 159       hit.dist = INFINITY;
 160       if( !ray_world( surface, (v3f){0.0f,-1.0f,0.0f}, &hit ))
 161          continue;
 162
 163       v3_copy( hit.normal, ct->n );
 164       v3_copy( hit.pos, surface );
 165
 166       float p = vg_minf( surface[1] - point[1], 1.0f );
 167
 168       if( p > 0.0f )
 169       {
 170          v3_add( point, surface, ct->co );
 171          v3_muls( ct->co, 0.5f, ct->co );
 172
 173          //vg_line_pt3( ct->co, 0.0125f, 0xff0000ff );
 174
 175          v3_sub( ct->co, rb->co, ct->delta );
 176          ct->bias = -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -p+0.04f );
 177          rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
 178
 179          ct->norm_impulse = 0.0f;
 180          ct->tangent_impulse[0] = 0.0f;
 181          ct->tangent_impulse[1] = 0.0f;
 182
 183          rb->manifold_count ++;
 184          count ++;
 185          if( count == 4 )
 186             break;
 187       }
 188    }
 189 }
 190
 191 static void rb_constraint_manifold( rigidbody *rb )
 192 {
 193    float k_friction = 0.1f;
 194
 195    /* Friction Impulse */
 196    for( int i=0; i<rb->manifold_count; i++ )
 197    {
 198       struct contact *ct = &rb->manifold[i];
 199
 200       v3f dv;
 201       v3_cross( rb->I, ct->delta, dv );
 202       v3_add( rb->v, dv, dv );
 203
 204       for( int j=0; j<2; j++ )
 205       {
 206          float vt = vg_clampf( -v3_dot( dv, ct->t[j] ),
 207                -k_friction, k_friction );
 208
 209          vt = -v3_dot( dv, ct->t[j] );
 210
 211          float temp = ct->tangent_impulse[j];
 212          ct->tangent_impulse[j] = vg_clampf( temp+vt, -k_friction, k_friction );
 213          vt = ct->tangent_impulse[j] - temp;
 214
 215          v3f impulse;
 216
 217          v3_muls( ct->t[j], vt, impulse );
 218          v3_add( impulse, rb->v, rb->v );
 219          v3_cross( ct->delta, impulse, impulse );
 220          v3_add( impulse, rb->I, rb->I );
 221       }
 222    }
 223
 224    /* Normal Impulse */
 225    for( int i=0; i<rb->manifold_count; i++ )
 226    {
 227       struct contact *ct = &rb->manifold[i];
 228
 229       v3f dv;
 230       v3_cross( rb->I, ct->delta, dv );
 231       v3_add( rb->v, dv, dv );
 232
 233       float vn = -v3_dot( dv, ct->n );
 234       vn += ct->bias;
 235
 236       float temp = ct->norm_impulse;
 237       ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
 238       vn = ct->norm_impulse - temp;
 239
 240       v3f impulse;
 241
 242       v3_muls( ct->n, vn, impulse );
 243       v3_add( impulse, rb->v, rb->v );
 244       v3_cross( ct->delta, impulse, impulse );
 245       v3_add( impulse, rb->I, rb->I );
 246    }
 247 }
 248
 249 struct rb_angle_limit
 250 {
 251    rigidbody *rba, *rbb;
 252    v3f axis;
 253    float impulse, bias;
 254 };
 255
 256 static int rb_angle_limit_force(  rigidbody *rba, v3f va,
 257                                   rigidbody *rbb, v3f vb,
 258                                   float max )
 259 {
 260    v3f wva, wvb;
 261    m3x3_mulv( rba->to_world, va, wva );
 262    m3x3_mulv( rbb->to_world, vb, wvb );
 263
 264    float dt = v3_dot(wva,wvb)*0.999f,
 265         ang = fabsf(dt);
 266    ang = acosf( dt );
 267    if( ang > max )
 268    {
 269       float correction = max-ang;
 270
 271       v3f axis;
 272       v3_cross( wva, wvb, axis );
 273
 274       v4f rotation;
 275       q_axis_angle( rotation, axis, -correction*0.25f );
 276       q_mul( rotation, rba->q, rba->q );
 277
 278       q_axis_angle( rotation, axis,  correction*0.25f );
 279       q_mul( rotation, rbb->q, rbb->q );
 280
 281       return 1;
 282    }
 283
 284    return 0;
 285 }
 286
 287 static void rb_constraint_angle_limit( struct rb_angle_limit *limit )
 288 {
 289
 290 }
 291
 292 RB_DEPR
 293 static void rb_constraint_angle( rigidbody *rba, v3f va,
 294                                  rigidbody *rbb, v3f vb,
 295                                  float max, float spring )
 296 {
 297    v3f wva, wvb;
 298    m3x3_mulv( rba->to_world, va, wva );
 299    m3x3_mulv( rbb->to_world, vb, wvb );
 300
 301    float dt = v3_dot(wva,wvb)*0.999f,
 302         ang = fabsf(dt);
 303
 304    v3f axis;
 305    v3_cross( wva, wvb, axis );
 306    v3_muladds( rba->I, axis,  ang*spring*0.5f, rba->I );
 307    v3_muladds( rbb->I, axis, -ang*spring*0.5f, rbb->I );
 308
 309    return;
 310
 311    /* TODO: convert max into the dot product value so we dont have to always
 312     *       evaluate acosf, only if its greater than the angle specified */
 313    ang = acosf( dt );
 314    if( ang > max )
 315    {
 316       float correction = max-ang;
 317
 318       v4f rotation;
 319       q_axis_angle( rotation, axis, -correction*0.125f );
 320       q_mul( rotation, rba->q, rba->q );
 321
 322       q_axis_angle( rotation, axis,  correction*0.125f );
 323       q_mul( rotation, rbb->q, rbb->q );
 324    }
 325 }
 326
 327 static void rb_relative_velocity( rigidbody *ra, v3f lca,
 328                                   rigidbody *rb, v3f lcb, v3f rcv )
 329 {
 330    v3f wca, wcb;
 331    m3x3_mulv( ra->to_world, lca, wca );
 332    m3x3_mulv( rb->to_world, lcb, wcb );
 333
 334    v3_sub( ra->v, rb->v, rcv );
 335
 336    v3f rcv_Ra, rcv_Rb;
 337    v3_cross( ra->I, wca, rcv_Ra );
 338    v3_cross( rb->I, wcb, rcv_Rb );
 339    v3_add( rcv_Ra, rcv, rcv );
 340    v3_sub( rcv, rcv_Rb, rcv );
 341 }
 342
 343 static void rb_constraint_position( rigidbody *ra, v3f lca,
 344                                     rigidbody *rb, v3f lcb )
 345 {
 346    /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
 347    v3f wca, wcb;
 348    m3x3_mulv( ra->to_world, lca, wca );
 349    m3x3_mulv( rb->to_world, lcb, wcb );
 350
 351    v3f delta;
 352    v3_add( wcb, rb->co, delta );
 353    v3_sub( delta, wca, delta );
 354    v3_sub( delta, ra->co, delta );
 355
 356    v3_muladds( ra->co, delta,  0.5f, ra->co );
 357    v3_muladds( rb->co, delta, -0.5f, rb->co );
 358
 359    v3f rcv;
 360    v3_sub( ra->v, rb->v, rcv );
 361
 362    v3f rcv_Ra, rcv_Rb;
 363    v3_cross( ra->I, wca, rcv_Ra );
 364    v3_cross( rb->I, wcb, rcv_Rb );
 365    v3_add( rcv_Ra, rcv, rcv );
 366    v3_sub( rcv, rcv_Rb, rcv );
 367
 368    float nm = 0.5f/(rb->inv_mass + ra->inv_mass);
 369
 370    float mass_a = 1.0f/ra->inv_mass,
 371          mass_b = 1.0f/rb->inv_mass,
 372          total_mass = mass_a+mass_b;
 373
 374    v3f impulse;
 375    v3_muls( rcv, 1.0f, impulse );
 376    v3_muladds( rb->v, impulse, mass_b/total_mass, rb->v );
 377    v3_cross( wcb, impulse, impulse );
 378    v3_add( impulse, rb->I, rb->I );
 379
 380    v3_muls( rcv, -1.0f, impulse );
 381    v3_muladds( ra->v, impulse, mass_a/total_mass, ra->v );
 382    v3_cross( wca, impulse, impulse );
 383    v3_add( impulse, ra->I, ra->I );
 384
 385 #if 0
 386    /*
 387     * this could be used for spring joints
 388     * its not good for position constraint
 389     */
 390    v3f impulse;
 391    v3_muls( delta, 0.5f*spring, impulse );
 392
 393    v3_add( impulse, ra->v, ra->v );
 394    v3_cross( wca, impulse, impulse );
 395    v3_add( impulse, ra->I, ra->I );
 396
 397    v3_muls( delta, -0.5f*spring, impulse );
 398
 399    v3_add( impulse, rb->v, rb->v );
 400    v3_cross( wcb, impulse, impulse );
 401    v3_add( impulse, rb->I, rb->I );
 402 #endif
 403 }
 404
 405 static void rb_debug( rigidbody *rb, u32 colour )
 406 {
 407    v3f *box = rb->bbx;
 408    v3f p000, p001, p010, p011, p100, p101, p110, p111;
 409
 410    p000[0]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2];
 411    p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2];
 412    p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2];
 413    p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2];
 414
 415    p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2];
 416    p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2];
 417    p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2];
 418    p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2];
 419
 420    m4x3_mulv( rb->to_world, p000, p000 );
 421    m4x3_mulv( rb->to_world, p001, p001 );
 422    m4x3_mulv( rb->to_world, p010, p010 );
 423    m4x3_mulv( rb->to_world, p011, p011 );
 424    m4x3_mulv( rb->to_world, p100, p100 );
 425    m4x3_mulv( rb->to_world, p101, p101 );
 426    m4x3_mulv( rb->to_world, p110, p110 );
 427    m4x3_mulv( rb->to_world, p111, p111 );
 428
 429    vg_line( p000, p001, colour );
 430    vg_line( p001, p011, colour );
 431    vg_line( p011, p010, colour );
 432    vg_line( p010, p000, colour );
 433
 434    vg_line( p100, p101, colour );
 435    vg_line( p101, p111, colour );
 436    vg_line( p111, p110, colour );
 437    vg_line( p110, p100, colour );
 438
 439    vg_line( p100, p000, colour );
 440    vg_line( p101, p001, colour );
 441    vg_line( p110, p010, colour );
 442    vg_line( p111, p011, colour );
 443
 444    vg_line( p000, p110, colour );
 445    vg_line( p100, p010, colour );
 446 }
 447
 448 /*
 449  * out penetration distance, normal
 450  */
 451 static int rb_point_in_body( rigidbody *rb, v3f pos, float *pen, v3f normal )
 452 {
 453    v3f local;
 454    m4x3_mulv( rb->to_local, pos, local );
 455
 456    if( local[0] > rb->bbx[0][0] && local[0] < rb->bbx[1][0] &&
 457        local[1] > rb->bbx[0][1] && local[1] < rb->bbx[1][1] &&
 458        local[2] > rb->bbx[0][2] && local[2] < rb->bbx[1][2] )
 459    {
 460       v3f area, com, comrel;
 461       v3_add( rb->bbx[0], rb->bbx[1], com );
 462       v3_muls( com, 0.5f, com );
 463
 464       v3_sub( rb->bbx[1], rb->bbx[0], area );
 465       v3_sub( local, com, comrel );
 466       v3_div( comrel, area, comrel );
 467
 468       int axis = 0;
 469       float max_mag = fabsf(comrel[0]);
 470
 471       if( fabsf(comrel[1]) > max_mag )
 472       {
 473          axis = 1;
 474          max_mag = fabsf(comrel[1]);
 475       }
 476       if( fabsf(comrel[2]) > max_mag )
 477       {
 478          axis = 2;
 479          max_mag = fabsf(comrel[2]);
 480       }
 481
 482       v3_zero( normal );
 483       normal[axis] = vg_signf(comrel[axis]);
 484
 485       if( normal[axis] < 0.0f )
 486          *pen = local[axis] - rb->bbx[0][axis];
 487       else
 488          *pen = rb->bbx[1][axis] - local[axis];
 489
 490       m3x3_mulv( rb->to_world, normal, normal );
 491       return 1;
 492    }
 493
 494    return 0;
 495 }
 496
 497 static void rb_build_manifold_rb_static( rigidbody *ra, rigidbody *rb_static )
 498 {
 499    v3f verts[8];
 500
 501    v3f a, b;
 502    v3_copy( ra->bbx[0], a );
 503    v3_copy( ra->bbx[1], b );
 504
 505         m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], a[2] }, verts[0] );
 506         m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], a[2] }, verts[1] );
 507         m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], a[2] }, verts[2] );
 508         m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], a[2] }, verts[3] );
 509         m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], b[2] }, verts[4] );
 510         m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], b[2] }, verts[5] );
 511         m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], b[2] }, verts[6] );
 512         m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], b[2] }, verts[7] );
 513
 514    int count = 0;
 515
 516    for( int i=0; i<8; i++ )
 517    {
 518       if( ra->manifold_count == vg_list_size(ra->manifold) )
 519          return;
 520
 521       struct contact *ct = &ra->manifold[ ra->manifold_count ];
 522
 523       float p;
 524       v3f normal;
 525
 526       if( rb_point_in_body( rb_static, verts[i], &p, normal ))
 527       {
 528          v3_copy( normal, ct->n );
 529          v3_muladds( verts[i], ct->n, p*0.5f, ct->co );
 530          v3_sub( ct->co, ra->co, ct->delta );
 531
 532          vg_line_pt3( ct->co, 0.0125f, 0xffff00ff );
 533
 534          ct->bias = -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -p+0.04f );
 535          rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
 536
 537          ct->norm_impulse = 0.0f;
 538          ct->tangent_impulse[0] = 0.0f;
 539          ct->tangent_impulse[1] = 0.0f;
 540
 541          ra->manifold_count ++;
 542          count ++;
 543          if( count == 4 )
 544             return;
 545       }
 546    }
 547 }
 548
 549 /*
 550  * BVH implementation, this is ONLY for static rigidbodies, its to slow for
 551  * realtime use.
 552  */
 553
 554 static void rb_bh_expand_bound( void *user, boxf bound, u32 item_index )
 555 {
 556    rigidbody *rb = &((rigidbody *)user)[ item_index ];
 557    box_concat( bound, rb->bbx_world );
 558 }
 559
 560 static float rb_bh_centroid( void *user, u32 item_index, int axis )
 561 {
 562    rigidbody *rb = &((rigidbody *)user)[ item_index ];
 563    return (rb->bbx_world[axis][0] + rb->bbx_world[1][axis]) * 0.5f;
 564 }
 565
 566 static void rb_bh_swap( void *user, u32 ia, u32 ib )
 567 {
 568    rigidbody temp, *rba, *rbb;
 569    rba = &((rigidbody *)user)[ ia ];
 570    rbb = &((rigidbody *)user)[ ib ];
 571
 572    temp = *rba;
 573    *rba = *rbb;
 574    *rbb = temp;
 575 }
 576
 577 static void rb_bh_debug( void *user, u32 item_index )
 578 {
 579    rigidbody *rb = &((rigidbody *)user)[ item_index ];
 580    rb_debug( rb, 0xff00ffff );
 581 }
 582
 583 static bh_system bh_system_rigidbodies =
 584 {
 585    .expand_bound = rb_bh_expand_bound,
 586    .item_centroid = rb_bh_centroid,
 587    .item_swap = rb_bh_swap,
 588    .item_debug = rb_bh_debug,
 589    .cast_ray = NULL
 590 };
 591
 592 #endif /* RIGIDBODY_H */