vg_rigidbody_constraints.c

   1 #pragma once
   2 #include "vg_rigidbody.h"
   3 #include "vg_rigidbody_constraints.h"
   4 #include "vg_m.h"
   5 #include "vg_lines.h"
   6
   7 /*
   8  * -----------------------------------------------------------------------------
   9  *                               Constraints
  10  * -----------------------------------------------------------------------------
  11  */
  12
  13 void rb_debug_position_constraints( rb_constr_pos *buffer, int len )
  14 {
  15    for( int i=0; i<len; i++ ){
  16       rb_constr_pos *constr = &buffer[i];
  17       rigidbody *rba = constr->rba, *rbb = constr->rbb;
  18
  19       v3f wca, wcb;
  20       m3x3_mulv( rba->to_world, constr->lca, wca );
  21       m3x3_mulv( rbb->to_world, constr->lcb, wcb );
  22
  23       v3f p0, p1;
  24       v3_add( wca, rba->co, p0 );
  25       v3_add( wcb, rbb->co, p1 );
  26       vg_line_point( p0, 0.0025f, 0xff000000 );
  27       vg_line_point( p1, 0.0025f, 0xffffffff );
  28       vg_line2( p0, p1, 0xff000000, 0xffffffff );
  29    }
  30 }
  31
  32 void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf, int len )
  33 {
  34    for( int i=0; i<len; i++ ){
  35       rb_constr_swingtwist *st = &buf[ i ];
  36
  37       v3f vx, vy, va, vxb, axis, center;
  38
  39       m3x3_mulv( st->rba->to_world, st->conevx, vx );
  40       m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
  41       m3x3_mulv( st->rba->to_world, st->conevy, vy );
  42       m3x3_mulv( st->rbb->to_world, st->coneva, va );
  43       m4x3_mulv( st->rba->to_world, st->view_offset, center );
  44       v3_cross( vy, vx, axis );
  45
  46       /* Constraint violated ? */
  47       float fx = v3_dot( vx, va ),     /* projection world */
  48             fy = v3_dot( vy, va ),
  49             fn = v3_dot( va, axis ),
  50
  51             rx = st->conevx[3],        /* elipse radii */
  52             ry = st->conevy[3],
  53
  54             lx = fx/rx,                /* projection local (fn==lz) */
  55             ly = fy/ry;
  56
  57       st->tangent_violation = ((lx*lx + ly*ly) > fn*fn) || (fn <= 0.0f);
  58       if( st->tangent_violation ){
  59          /* Calculate a good position and the axis to solve on */
  60          v2f closest, tangent,
  61              p = { fx/fabsf(fn), fy/fabsf(fn) };
  62
  63          closest_point_elipse( p, (v2f){rx,ry}, closest );
  64          tangent[0] = -closest[1] / (ry*ry);
  65          tangent[1] =  closest[0] / (rx*rx);
  66          v2_normalize( tangent );
  67
  68          v3f v0, v1;
  69          v3_muladds( axis, vx, closest[0], v0 );
  70          v3_muladds( v0, vy, closest[1], v0 );
  71          v3_normalize( v0 );
  72
  73          v3_muls( vx, tangent[0], v1 );
  74          v3_muladds( v1, vy, tangent[1], v1 );
  75
  76          v3_copy( v0, st->tangent_target );
  77          v3_copy( v1, st->tangent_axis );
  78
  79          /* calculate mass */
  80          v3f aIw, bIw;
  81          m3x3_mulv( st->rba->iIw, st->tangent_axis, aIw );
  82          m3x3_mulv( st->rbb->iIw, st->tangent_axis, bIw );
  83          st->tangent_mass = 1.0f / (v3_dot( st->tangent_axis, aIw ) +
  84                                     v3_dot( st->tangent_axis, bIw ));
  85
  86          float angle = v3_dot( va, st->tangent_target );
  87       }
  88
  89       v3f refaxis;
  90       v3_cross( vy, va, refaxis );  /* our default rotation */
  91       v3_normalize( refaxis );
  92
  93       float angle = v3_dot( refaxis, vxb );
  94       st->axis_violation = fabsf(angle) < st->conet;
  95
  96       if( st->axis_violation ){
  97          v3f dir_test;
  98          v3_cross( refaxis, vxb, dir_test );
  99
 100          if( v3_dot(dir_test, va) < 0.0f )
 101             st->axis_violation = -st->axis_violation;
 102
 103          float newang = (float)st->axis_violation * acosf(st->conet-0.0001f);
 104
 105          v3f refaxis_up;
 106          v3_cross( va, refaxis, refaxis_up );
 107          v3_muls( refaxis_up, sinf(newang), st->axis_target );
 108          v3_muladds( st->axis_target, refaxis, -cosf(newang), st->axis_target );
 109
 110          /* calculate mass */
 111          v3_copy( va, st->axis );
 112          v3f aIw, bIw;
 113          m3x3_mulv( st->rba->iIw, st->axis, aIw );
 114          m3x3_mulv( st->rbb->iIw, st->axis, bIw );
 115          st->axis_mass = 1.0f / (v3_dot( st->axis, aIw ) +
 116                                  v3_dot( st->axis, bIw ));
 117       }
 118    }
 119 }
 120
 121 void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf, int len )
 122 {
 123    float size = 0.12f;
 124
 125    for( int i=0; i<len; i++ ){
 126       rb_constr_swingtwist *st = &buf[ i ];
 127
 128       v3f vx, vxb, vy, va, axis, center;
 129
 130       m3x3_mulv( st->rba->to_world, st->conevx, vx );
 131       m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
 132       m3x3_mulv( st->rba->to_world, st->conevy, vy );
 133       m3x3_mulv( st->rbb->to_world, st->coneva, va );
 134       m4x3_mulv( st->rba->to_world, st->view_offset, center );
 135       v3_cross( vy, vx, axis );
 136
 137       float rx = st->conevx[3],        /* elipse radii */
 138             ry = st->conevy[3];
 139
 140       v3f p0, p1;
 141       v3_muladds( center, va, size, p1 );
 142       vg_line( center, p1, 0xffffffff );
 143       vg_line_point( p1, 0.00025f, 0xffffffff );
 144
 145       if( st->tangent_violation ){
 146          v3_muladds( center, st->tangent_target, size, p0 );
 147
 148          vg_line( center, p0, 0xff00ff00 );
 149          vg_line_point( p0, 0.00025f, 0xff00ff00 );
 150          vg_line( p1, p0, 0xff000000 );
 151       }
 152
 153       for( int x=0; x<32; x++ ){
 154          float t0 = ((float)x * (1.0f/32.0f)) * VG_TAUf,
 155                t1 = (((float)x+1.0f) * (1.0f/32.0f)) * VG_TAUf,
 156                c0 = cosf( t0 ),
 157                s0 = sinf( t0 ),
 158                c1 = cosf( t1 ),
 159                s1 = sinf( t1 );
 160
 161          v3f v0, v1;
 162          v3_muladds( axis, vx, c0*rx, v0 );
 163          v3_muladds( v0,   vy, s0*ry, v0 );
 164          v3_muladds( axis, vx, c1*rx, v1 );
 165          v3_muladds( v1,   vy, s1*ry, v1 );
 166
 167          v3_normalize( v0 );
 168          v3_normalize( v1 );
 169
 170          v3_muladds( center, v0, size, p0 );
 171          v3_muladds( center, v1, size, p1 );
 172
 173          u32 col0r = fabsf(c0) * 255.0f,
 174              col0g = fabsf(s0) * 255.0f,
 175              col1r = fabsf(c1) * 255.0f,
 176              col1g = fabsf(s1) * 255.0f,
 177              col   = st->tangent_violation? 0xff0000ff: 0xff000000,
 178              col0  = col | (col0r<<16) | (col0g << 8),
 179              col1  = col | (col1r<<16) | (col1g << 8);
 180
 181          vg_line2( center, p0, VG__NONE, col0 );
 182          vg_line2( p0, p1, col0, col1 );
 183       }
 184
 185       /* Draw twist */
 186       v3_muladds( center, va, size, p0 );
 187       v3_muladds( p0, vxb, size, p1 );
 188
 189       vg_line( p0, p1, 0xff0000ff );
 190
 191       if( st->axis_violation ){
 192          v3_muladds( p0, st->axis_target, size*1.25f, p1 );
 193          vg_line( p0, p1, 0xffffff00 );
 194          vg_line_point( p1, 0.0025f, 0xffffff80 );
 195       }
 196
 197       v3f refaxis;
 198       v3_cross( vy, va, refaxis );  /* our default rotation */
 199       v3_normalize( refaxis );
 200       v3f refaxis_up;
 201       v3_cross( va, refaxis, refaxis_up );
 202       float newang = acosf(st->conet-0.0001f);
 203
 204       v3_muladds( p0, refaxis_up, sinf(newang)*size, p1 );
 205       v3_muladds( p1, refaxis, -cosf(newang)*size, p1 );
 206       vg_line( p0, p1, 0xff000000 );
 207
 208       v3_muladds( p0, refaxis_up, sinf(-newang)*size, p1 );
 209       v3_muladds( p1, refaxis, -cosf(-newang)*size, p1 );
 210       vg_line( p0, p1, 0xff404040 );
 211    }
 212 }
 213
 214 void rb_solve_position_constraints( rb_constr_pos *buf, int len )
 215 {
 216    for( int i=0; i<len; i++ ){
 217       rb_constr_pos *constr = &buf[i];
 218       rigidbody *rba = constr->rba, *rbb = constr->rbb;
 219
 220       v3f wa, wb;
 221       m3x3_mulv( rba->to_world, constr->lca, wa );
 222       m3x3_mulv( rbb->to_world, constr->lcb, wb );
 223
 224       m3x3f ssra, ssrat, ssrb, ssrbt;
 225
 226       m3x3_skew_symetric( ssrat, wa );
 227       m3x3_skew_symetric( ssrbt, wb );
 228       m3x3_transpose( ssrat, ssra );
 229       m3x3_transpose( ssrbt, ssrb );
 230
 231       v3f b, b_wa, b_wb, b_a, b_b;
 232       m3x3_mulv( ssra, rba->w, b_wa );
 233       m3x3_mulv( ssrb, rbb->w, b_wb );
 234       v3_add( rba->v, b_wa, b );
 235       v3_sub( b, rbb->v, b );
 236       v3_sub( b, b_wb, b );
 237       v3_muls( b, -1.0f, b );
 238
 239       m3x3f invMa, invMb;
 240       m3x3_diagonal( invMa, rba->inv_mass );
 241       m3x3_diagonal( invMb, rbb->inv_mass );
 242
 243       m3x3f ia, ib;
 244       m3x3_mul( ssra, rba->iIw, ia );
 245       m3x3_mul( ia, ssrat, ia );
 246       m3x3_mul( ssrb, rbb->iIw, ib );
 247       m3x3_mul( ib, ssrbt, ib );
 248
 249       m3x3f cma, cmb;
 250       m3x3_add( invMa, ia, cma );
 251       m3x3_add( invMb, ib, cmb );
 252
 253       m3x3f A;
 254       m3x3_add( cma, cmb, A );
 255
 256       /* Solve Ax = b ( A^-1*b = x ) */
 257       v3f impulse;
 258       m3x3f invA;
 259       m3x3_inv( A, invA );
 260       m3x3_mulv( invA, b, impulse );
 261
 262       v3f delta_va, delta_wa, delta_vb, delta_wb;
 263       m3x3f iwa, iwb;
 264       m3x3_mul( rba->iIw, ssrat, iwa );
 265       m3x3_mul( rbb->iIw, ssrbt, iwb );
 266
 267       m3x3_mulv( invMa, impulse, delta_va );
 268       m3x3_mulv( invMb, impulse, delta_vb );
 269       m3x3_mulv( iwa, impulse, delta_wa );
 270       m3x3_mulv( iwb, impulse, delta_wb );
 271
 272       v3_add( rba->v, delta_va, rba->v );
 273       v3_add( rba->w, delta_wa, rba->w );
 274       v3_sub( rbb->v, delta_vb, rbb->v );
 275       v3_sub( rbb->w, delta_wb, rbb->w );
 276    }
 277 }
 278
 279 void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf, int len )
 280 {
 281    for( int i=0; i<len; i++ ){
 282       rb_constr_swingtwist *st = &buf[ i ];
 283
 284       if( !st->axis_violation )
 285          continue;
 286
 287       float rv = v3_dot( st->axis, st->rbb->w ) -
 288                  v3_dot( st->axis, st->rba->w );
 289
 290       if( rv * (float)st->axis_violation > 0.0f )
 291          continue;
 292
 293       v3f impulse, wa, wb;
 294       v3_muls( st->axis, rv*st->axis_mass, impulse );
 295       m3x3_mulv( st->rba->iIw, impulse, wa );
 296       v3_add( st->rba->w, wa, st->rba->w );
 297
 298       v3_muls( impulse, -1.0f, impulse );
 299       m3x3_mulv( st->rbb->iIw, impulse, wb );
 300       v3_add( st->rbb->w, wb, st->rbb->w );
 301
 302       float rv2 = v3_dot( st->axis, st->rbb->w ) -
 303                   v3_dot( st->axis, st->rba->w );
 304    }
 305
 306    for( int i=0; i<len; i++ ){
 307       rb_constr_swingtwist *st = &buf[ i ];
 308
 309       if( !st->tangent_violation )
 310          continue;
 311
 312       float rv = v3_dot( st->tangent_axis, st->rbb->w ) -
 313                  v3_dot( st->tangent_axis, st->rba->w );
 314
 315       if( rv > 0.0f )
 316          continue;
 317
 318       v3f impulse, wa, wb;
 319       v3_muls( st->tangent_axis, rv*st->tangent_mass, impulse );
 320       m3x3_mulv( st->rba->iIw, impulse, wa );
 321       v3_add( st->rba->w, wa, st->rba->w );
 322
 323       v3_muls( impulse, -1.0f, impulse );
 324       m3x3_mulv( st->rbb->iIw, impulse, wb );
 325       v3_add( st->rbb->w, wb, st->rbb->w );
 326
 327       float rv2 = v3_dot( st->tangent_axis, st->rbb->w ) -
 328                   v3_dot( st->tangent_axis, st->rba->w );
 329    }
 330 }
 331
 332 /* debugging */
 333 void rb_postsolve_swingtwist_constraints( rb_constr_swingtwist *buf, u32 len )
 334 {
 335    for( int i=0; i<len; i++ ){
 336       rb_constr_swingtwist *st = &buf[ i ];
 337
 338       if( !st->axis_violation ){
 339          st->conv_axis = 0.0f;
 340          continue;
 341       }
 342
 343       f32 rv = v3_dot( st->axis, st->rbb->w ) -
 344                v3_dot( st->axis, st->rba->w );
 345
 346       if( rv * (f32)st->axis_violation > 0.0f )
 347          st->conv_axis = 0.0f;
 348       else
 349          st->conv_axis = rv;
 350    }
 351
 352    for( int i=0; i<len; i++ ){
 353       rb_constr_swingtwist *st = &buf[ i ];
 354
 355       if( !st->tangent_violation ){
 356          st->conv_tangent = 0.0f;
 357          continue;
 358       }
 359
 360       f32 rv = v3_dot( st->tangent_axis, st->rbb->w ) -
 361                v3_dot( st->tangent_axis, st->rba->w );
 362
 363       if( rv > 0.0f )
 364          st->conv_tangent = 0.0f;
 365       else
 366          st->conv_tangent = rv;
 367    }
 368 }
 369
 370 void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb )
 371 {
 372    m3x3f ssra, ssrb, ssrat, ssrbt;
 373    m3x3f cma, cmb;
 374
 375    m3x3_skew_symetric( ssrat, ra );
 376    m3x3_skew_symetric( ssrbt, rb );
 377    m3x3_transpose( ssrat, ssra );
 378    m3x3_transpose( ssrbt, ssrb );
 379
 380    m3x3_mul( ssra, rba->iIw, cma );
 381    m3x3_mul( cma, ssrat, cma );
 382    m3x3_mul( ssrb, rbb->iIw, cmb );
 383    m3x3_mul( cmb, ssrbt, cmb );
 384
 385    m3x3f A, invA;
 386    m3x3_add( cma, cmb, A );
 387    m3x3_inv( A, invA );
 388
 389    v3f b_wa, b_wb, b;
 390    m3x3_mulv( ssra, rba->w, b_wa );
 391    m3x3_mulv( ssrb, rbb->w, b_wb );
 392    v3_add( b_wa, b_wb, b );
 393    v3_negate( b, b );
 394
 395    v3f impulse;
 396    m3x3_mulv( invA, b, impulse );
 397
 398    v3f delta_wa, delta_wb;
 399    m3x3f iwa, iwb;
 400    m3x3_mul( rba->iIw, ssrat, iwa );
 401    m3x3_mul( rbb->iIw, ssrbt, iwb );
 402    m3x3_mulv( iwa, impulse, delta_wa );
 403    m3x3_mulv( iwb, impulse, delta_wb );
 404    v3_add( rba->w, delta_wa, rba->w );
 405    v3_sub( rbb->w, delta_wb, rbb->w );
 406 }
 407
 408 void rb_correct_position_constraints( rb_constr_pos *buf, int len, f32 amt )
 409 {
 410    for( int i=0; i<len; i++ ){
 411       rb_constr_pos *constr = &buf[i];
 412       rigidbody *rba = constr->rba, *rbb = constr->rbb;
 413
 414       v3f p0, p1, d;
 415       m3x3_mulv( rba->to_world, constr->lca, p0 );
 416       m3x3_mulv( rbb->to_world, constr->lcb, p1 );
 417       v3_add( rba->co, p0, p0 );
 418       v3_add( rbb->co, p1, p1 );
 419       v3_sub( p1, p0, d );
 420
 421 #if 1
 422       v3_muladds( rbb->co, d, -1.0f * amt, rbb->co );
 423       rb_update_matrices( rbb );
 424 #else
 425       f32 mt = 1.0f/(rba->inv_mass+rbb->inv_mass),
 426           a  = mt * (k_phys_baumgarte/k_rb_delta);
 427
 428       v3_muladds( rba->v, d, a* rba->inv_mass, rba->v );
 429       v3_muladds( rbb->v, d, a*-rbb->inv_mass, rbb->v );
 430 #endif
 431    }
 432 }
 433
 434 void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf,
 435                                         int len, float amt )
 436 {
 437    for( int i=0; i<len; i++ ){
 438       rb_constr_swingtwist *st = &buf[i];
 439
 440       if( !st->tangent_violation )
 441          continue;
 442
 443       v3f va;
 444       m3x3_mulv( st->rbb->to_world, st->coneva, va );
 445
 446       f32 angle = v3_dot( va, st->tangent_target );
 447
 448       if( fabsf(angle) < 0.9999f ){
 449          v3f axis;
 450          v3_cross( va, st->tangent_target, axis );
 451 #if 1
 452          angle = acosf(angle) * amt;
 453          v4f correction;
 454          q_axis_angle( correction, axis, angle );
 455          q_mul( correction, st->rbb->q, st->rbb->q );
 456          q_normalize( st->rbb->q );
 457          rb_update_matrices( st->rbb );
 458 #else
 459          f32 mt = 1.0f/(st->rba->inv_mass+st->rbb->inv_mass),
 460              wa = mt * acosf(angle) * (k_phys_baumgarte/k_rb_delta);
 461          //v3_muladds( st->rba->w, axis, wa*-st->rba->inv_mass, st->rba->w );
 462          v3_muladds( st->rbb->w, axis, wa* st->rbb->inv_mass, st->rbb->w );
 463 #endif
 464       }
 465    }
 466
 467    for( int i=0; i<len; i++ ){
 468       rb_constr_swingtwist *st = &buf[i];
 469
 470       if( !st->axis_violation )
 471          continue;
 472
 473       v3f vxb;
 474       m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
 475
 476       f32 angle = v3_dot( vxb, st->axis_target );
 477
 478       if( fabsf(angle) < 0.9999f ){
 479          v3f axis;
 480          v3_cross( vxb, st->axis_target, axis );
 481
 482 #if 1
 483          angle = acosf(angle) * amt;
 484          v4f correction;
 485          q_axis_angle( correction, axis, angle );
 486          q_mul( correction, st->rbb->q, st->rbb->q );
 487          q_normalize( st->rbb->q );
 488          rb_update_matrices( st->rbb );
 489 #else
 490          f32 mt = 1.0f/(st->rba->inv_mass+st->rbb->inv_mass),
 491              wa = mt * acosf(angle) * (k_phys_baumgarte/k_rb_delta);
 492          //v3_muladds( st->rba->w, axis, wa*-0.5f, st->rba->w );
 493          v3_muladds( st->rbb->w, axis, wa* st->rbb->inv_mass, st->rbb->w );
 494 #endif
 495       }
 496    }
 497 }