vg_rigidbody_constraints.h

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