rigidbody.h

   1 /*
   2  * Resources: Box2D - Erin Catto
   3  *            qu3e  - Randy Gaul
   4  */
   5
   6 #include "vg/vg.h"
   7 static void rb_tangent_basis( v3f n, v3f tx, v3f ty );
   8
   9 #ifndef RIGIDBODY_H
  10 #define RIGIDBODY_H
  11
  12 #define RB_DEPR
  13
  14 #include "world.h"
  15
  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;
  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[4];
  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
  48 static void rb_init( rigidbody *rb )
  49 {
  50    q_identity( rb->q );
  51    v3_zero( rb->v );
  52    v3_zero( rb->I );
  53
  54    v3f dims;
  55    v3_sub( rb->bbx[1], rb->bbx[0], dims );
  56
  57    rb->inv_mass = 1.0f/(8.0f*dims[0]*dims[1]*dims[2]);
  58
  59    rb_update_transform( rb );
  60 }
  61
  62 static void rb_iter( rigidbody *rb )
  63 {
  64    v3f gravity = { 0.0f, -9.6f, 0.0f };
  65    v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
  66
  67    /* intergrate velocity */
  68    v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
  69
  70    v3_lerp( rb->I, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->I );
  71
  72    /* inegrate inertia */
  73    if( v3_length2( rb->I ) > 0.0f )
  74    {
  75       v4f rotation;
  76       v3f axis;
  77       v3_copy( rb->I, axis );
  78
  79       float mag = v3_length( axis );
  80       v3_divs( axis, mag, axis );
  81       q_axis_angle( rotation, axis, mag*k_rb_delta );
  82       q_mul( rotation, rb->q, rb->q );
  83    }
  84 }
  85
  86 static void rb_torque( rigidbody *rb, v3f axis, float mag )
  87 {
  88    v3_muladds( rb->I, axis, mag*k_rb_delta, rb->I );
  89 }
  90
  91 static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
  92 {
  93    /* Compute tangent basis (box2d) */
  94    if( fabsf( n[0] ) >= 0.57735027f )
  95    {
  96       tx[0] =  n[1];
  97       tx[1] = -n[0];
  98       tx[2] =  0.0f;
  99    }
 100    else
 101    {
 102       tx[0] =  0.0f;
 103       tx[1] =  n[2];
 104       tx[2] = -n[1];
 105    }
 106
 107    v3_normalize( tx );
 108    v3_cross( n, tx, ty );
 109 }
 110
 111 static void rb_build_manifold( rigidbody *rb )
 112 {
 113    v3f *box = rb->bbx;
 114    v3f pts[8];
 115    float *p000 = pts[0], *p001 = pts[1], *p010 = pts[2], *p011 = pts[3],
 116          *p100 = pts[4], *p101 = pts[5], *p110 = pts[6], *p111 = pts[7];
 117
 118    p000[0]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2];
 119    p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2];
 120    p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2];
 121    p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2];
 122
 123    p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2];
 124    p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2];
 125    p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2];
 126    p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2];
 127
 128    m4x3_mulv( rb->to_world, p000, p000 );
 129    m4x3_mulv( rb->to_world, p001, p001 );
 130    m4x3_mulv( rb->to_world, p010, p010 );
 131    m4x3_mulv( rb->to_world, p011, p011 );
 132    m4x3_mulv( rb->to_world, p100, p100 );
 133    m4x3_mulv( rb->to_world, p101, p101 );
 134    m4x3_mulv( rb->to_world, p110, p110 );
 135    m4x3_mulv( rb->to_world, p111, p111 );
 136
 137    rb->manifold_count = 0;
 138
 139    for( int i=0; i<8; i++ )
 140    {
 141       float *point = pts[i];
 142       struct contact *ct = &rb->manifold[rb->manifold_count];
 143
 144       v3f surface;
 145       v3_copy( point, surface );
 146       surface[1] += 4.0f;
 147
 148       ray_hit hit;
 149       hit.dist = INFINITY;
 150       if( !ray_world( surface, (v3f){0.0f,-1.0f,0.0f}, &hit ))
 151          continue;
 152
 153       v3_copy( hit.normal, ct->n );
 154       v3_copy( hit.pos, surface );
 155
 156       float p = vg_minf( surface[1] - point[1], 1.0f );
 157
 158       if( p > 0.0f )
 159       {
 160          v3_add( point, surface, ct->co );
 161          v3_muls( ct->co, 0.5f, ct->co );
 162
 163          //vg_line_pt3( ct->co, 0.0125f, 0xff0000ff );
 164
 165          v3_sub( ct->co, rb->co, ct->delta );
 166          ct->bias = -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -p+0.04f );
 167          rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
 168
 169          ct->norm_impulse = 0.0f;
 170          ct->tangent_impulse[0] = 0.0f;
 171          ct->tangent_impulse[1] = 0.0f;
 172
 173          rb->manifold_count ++;
 174          if( rb->manifold_count == 4 )
 175             break;
 176       }
 177    }
 178 }
 179
 180 static void rb_constraint_manifold( rigidbody *rb )
 181 {
 182    float k_friction = 0.1f;
 183
 184    /* Friction Impulse */
 185    for( int i=0; i<rb->manifold_count; i++ )
 186    {
 187       struct contact *ct = &rb->manifold[i];
 188
 189       v3f dv;
 190       v3_cross( rb->I, ct->delta, dv );
 191       v3_add( rb->v, dv, dv );
 192
 193       for( int j=0; j<2; j++ )
 194       {
 195          float vt = vg_clampf( -v3_dot( dv, ct->t[j] ),
 196                -k_friction, k_friction );
 197
 198          vt = -v3_dot( dv, ct->t[j] );
 199
 200          float temp = ct->tangent_impulse[j];
 201          ct->tangent_impulse[j] = vg_clampf( temp+vt, -k_friction, k_friction );
 202          vt = ct->tangent_impulse[j] - temp;
 203
 204          v3f impulse;
 205
 206          v3_muls( ct->t[j], vt, impulse );
 207          v3_add( impulse, rb->v, rb->v );
 208          v3_cross( ct->delta, impulse, impulse );
 209          v3_add( impulse, rb->I, rb->I );
 210       }
 211    }
 212
 213    /* Normal Impulse */
 214    for( int i=0; i<rb->manifold_count; i++ )
 215    {
 216       struct contact *ct = &rb->manifold[i];
 217
 218       v3f dv;
 219       v3_cross( rb->I, ct->delta, dv );
 220       v3_add( rb->v, dv, dv );
 221
 222       float vn = -v3_dot( dv, ct->n );
 223       vn += ct->bias;
 224
 225       float temp = ct->norm_impulse;
 226       ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
 227       vn = ct->norm_impulse - temp;
 228
 229       v3f impulse;
 230
 231       v3_muls( ct->n, vn, impulse );
 232       v3_add( impulse, rb->v, rb->v );
 233       v3_cross( ct->delta, impulse, impulse );
 234       v3_add( impulse, rb->I, rb->I );
 235    }
 236 }
 237
 238 struct rb_angle_limit
 239 {
 240    rigidbody *rba, *rbb;
 241    v3f axis;
 242    float impulse, bias;
 243 };
 244
 245 static int rb_angle_limit_force(
 246                                   rigidbody *rba, v3f va,
 247                                   rigidbody *rbb, v3f vb,
 248                                   float max )
 249 {
 250    v3f wva, wvb;
 251    m3x3_mulv( rba->to_world, va, wva );
 252    m3x3_mulv( rbb->to_world, vb, wvb );
 253
 254    float dt = v3_dot(wva,wvb)*0.999f,
 255         ang = fabsf(dt);
 256    ang = acosf( dt );
 257    if( ang > max )
 258    {
 259       float correction = max-ang;
 260
 261       v3f axis;
 262       v3_cross( wva, wvb, axis );
 263
 264       v4f rotation;
 265       q_axis_angle( rotation, axis, -correction*0.25f );
 266       q_mul( rotation, rba->q, rba->q );
 267
 268       q_axis_angle( rotation, axis,  correction*0.25f );
 269       q_mul( rotation, rbb->q, rbb->q );
 270
 271       return 1;
 272    }
 273
 274    return 0;
 275 }
 276
 277 static void rb_constraint_angle_limit( struct rb_angle_limit *limit )
 278 {
 279
 280 }
 281
 282
 283 RB_DEPR
 284 static void rb_constraint_angle( rigidbody *rba, v3f va,
 285                                  rigidbody *rbb, v3f vb,
 286                                  float max, float spring )
 287 {
 288    v3f wva, wvb;
 289    m3x3_mulv( rba->to_world, va, wva );
 290    m3x3_mulv( rbb->to_world, vb, wvb );
 291
 292    float dt = v3_dot(wva,wvb)*0.999f,
 293         ang = fabsf(dt);
 294
 295    v3f axis;
 296    v3_cross( wva, wvb, axis );
 297    v3_muladds( rba->I, axis,  ang*spring*0.5f, rba->I );
 298    v3_muladds( rbb->I, axis, -ang*spring*0.5f, rbb->I );
 299
 300    return;
 301
 302    /* TODO: convert max into the dot product value so we dont have to always
 303     *       evaluate acosf, only if its greater than the angle specified */
 304    ang = acosf( dt );
 305    if( ang > max )
 306    {
 307       float correction = max-ang;
 308
 309       v4f rotation;
 310       q_axis_angle( rotation, axis, -correction*0.125f );
 311       q_mul( rotation, rba->q, rba->q );
 312
 313       q_axis_angle( rotation, axis,  correction*0.125f );
 314       q_mul( rotation, rbb->q, rbb->q );
 315    }
 316 }
 317
 318 static void rb_relative_velocity( rigidbody *ra, v3f lca,
 319                                   rigidbody *rb, v3f lcb, v3f rcv )
 320 {
 321    v3f wca, wcb;
 322    m3x3_mulv( ra->to_world, lca, wca );
 323    m3x3_mulv( rb->to_world, lcb, wcb );
 324
 325    v3_sub( ra->v, rb->v, rcv );
 326
 327    v3f rcv_Ra, rcv_Rb;
 328    v3_cross( ra->I, wca, rcv_Ra );
 329    v3_cross( rb->I, wcb, rcv_Rb );
 330    v3_add( rcv_Ra, rcv, rcv );
 331    v3_sub( rcv, rcv_Rb, rcv );
 332 }
 333
 334 static void rb_constraint_position( rigidbody *ra, v3f lca,
 335                                     rigidbody *rb, v3f lcb )
 336 {
 337    /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
 338    v3f wca, wcb;
 339    m3x3_mulv( ra->to_world, lca, wca );
 340    m3x3_mulv( rb->to_world, lcb, wcb );
 341
 342    v3f delta;
 343    v3_add( wcb, rb->co, delta );
 344    v3_sub( delta, wca, delta );
 345    v3_sub( delta, ra->co, delta );
 346
 347    v3_muladds( ra->co, delta,  0.5f, ra->co );
 348    v3_muladds( rb->co, delta, -0.5f, rb->co );
 349
 350    v3f rcv;
 351    v3_sub( ra->v, rb->v, rcv );
 352
 353    v3f rcv_Ra, rcv_Rb;
 354    v3_cross( ra->I, wca, rcv_Ra );
 355    v3_cross( rb->I, wcb, rcv_Rb );
 356    v3_add( rcv_Ra, rcv, rcv );
 357    v3_sub( rcv, rcv_Rb, rcv );
 358
 359    float nm = 0.5f/(rb->inv_mass + ra->inv_mass);
 360
 361    float mass_a = 1.0f/ra->inv_mass,
 362          mass_b = 1.0f/rb->inv_mass,
 363          total_mass = mass_a+mass_b;
 364
 365    v3f impulse;
 366    v3_muls( rcv, 1.0f, impulse );
 367    v3_muladds( rb->v, impulse, mass_b/total_mass, rb->v );
 368    v3_cross( wcb, impulse, impulse );
 369    v3_add( impulse, rb->I, rb->I );
 370
 371    v3_muls( rcv, -1.0f, impulse );
 372    v3_muladds( ra->v, impulse, mass_a/total_mass, ra->v );
 373    v3_cross( wca, impulse, impulse );
 374    v3_add( impulse, ra->I, ra->I );
 375
 376 #if 0
 377    v3f impulse;
 378    v3_muls( delta, 0.5f*spring, impulse );
 379
 380    v3_add( impulse, ra->v, ra->v );
 381    v3_cross( wca, impulse, impulse );
 382    v3_add( impulse, ra->I, ra->I );
 383
 384    v3_muls( delta, -0.5f*spring, impulse );
 385
 386    v3_add( impulse, rb->v, rb->v );
 387    v3_cross( wcb, impulse, impulse );
 388    v3_add( impulse, rb->I, rb->I );
 389 #endif
 390 }
 391
 392 static void rb_debug( rigidbody *rb, u32 colour )
 393 {
 394    v3f *box = rb->bbx;
 395    v3f p000, p001, p010, p011, p100, p101, p110, p111;
 396
 397    p000[0]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2];
 398    p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2];
 399    p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2];
 400    p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2];
 401
 402    p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2];
 403    p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2];
 404    p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2];
 405    p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2];
 406
 407    m4x3_mulv( rb->to_world, p000, p000 );
 408    m4x3_mulv( rb->to_world, p001, p001 );
 409    m4x3_mulv( rb->to_world, p010, p010 );
 410    m4x3_mulv( rb->to_world, p011, p011 );
 411    m4x3_mulv( rb->to_world, p100, p100 );
 412    m4x3_mulv( rb->to_world, p101, p101 );
 413    m4x3_mulv( rb->to_world, p110, p110 );
 414    m4x3_mulv( rb->to_world, p111, p111 );
 415
 416    vg_line( p000, p001, colour );
 417    vg_line( p001, p011, colour );
 418    vg_line( p011, p010, colour );
 419    vg_line( p010, p000, colour );
 420
 421    vg_line( p100, p101, colour );
 422    vg_line( p101, p111, colour );
 423    vg_line( p111, p110, colour );
 424    vg_line( p110, p100, colour );
 425
 426    vg_line( p100, p000, colour );
 427    vg_line( p101, p001, colour );
 428    vg_line( p110, p010, colour );
 429    vg_line( p111, p011, colour );
 430
 431    vg_line( p000, p110, colour );
 432    vg_line( p100, p010, colour );
 433 }
 434
 435 #endif /* RIGIDBODY_H */