rigidbody.h

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