scene_rigidbody.h

   1 #pragma once
   2
   3 /*
   4  * Copyright (C) 2021-2024 Mt.ZERO Software - All Rights Reserved
   5  *
   6  * Describes intereactions between vg rigidbody objects and skaterift's scene
   7  * description
   8  */
   9
  10 #include "scene.h"
  11 #include "vg/vg_rigidbody.h"
  12 #include "vg/vg_rigidbody_collision.h"
  13
  14 static int rb_sphere__scene( m4x3f mtxA, f32 r,
  15                              m4x3f mtxB, bh_tree *scene_bh, rb_ct *buf,
  16                              u16 ignore ){
  17    scene_context *sc = scene_bh->user;
  18
  19    int count = 0;
  20
  21    boxf box;
  22    v3_sub( mtxA[3], (v3f){ r,r,r }, box[0] );
  23    v3_add( mtxA[3], (v3f){ r,r,r }, box[1] );
  24
  25    bh_iter it;
  26    i32 idx;
  27    bh_iter_init_box( 0, &it, box );
  28
  29    while( bh_next( scene_bh, &it, &idx ) ){
  30       u32 *ptri = &sc->arrindices[ idx*3 ];
  31       v3f tri[3];
  32
  33       if( sc->arrvertices[ptri[0]].flags & ignore ) continue;
  34
  35       for( int j=0; j<3; j++ )
  36          v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
  37
  38       buf[ count ].element_id = ptri[0];
  39
  40       vg_line( tri[0],tri[1],0x70ff6000 );
  41       vg_line( tri[1],tri[2],0x70ff6000 );
  42       vg_line( tri[2],tri[0],0x70ff6000 );
  43
  44       int contact = rb_sphere__triangle( mtxA, r, tri, &buf[count] );
  45       count += contact;
  46
  47       if( count == 16 ){
  48          vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
  49          return count;
  50       }
  51    }
  52
  53    return count;
  54 }
  55
  56 static int rb_box__scene( m4x3f mtxA, boxf bbx,
  57                           m4x3f mtxB, bh_tree *scene_bh,
  58                           rb_ct *buf, u16 ignore ){
  59    scene_context *sc = scene_bh->user;
  60    v3f tri[3];
  61
  62    v3f extent, center;
  63    v3_sub( bbx[1], bbx[0], extent );
  64    v3_muls( extent, 0.5f, extent );
  65    v3_add( bbx[0], extent, center );
  66
  67    f32 r = v3_length(extent);
  68    boxf world_bbx;
  69    v3_fill( world_bbx[0], -r );
  70    v3_fill( world_bbx[1],  r );
  71    for( int i=0; i<2; i++ ){
  72       v3_add( center, world_bbx[i], world_bbx[i] );
  73       v3_add( mtxA[3], world_bbx[i], world_bbx[i] );
  74    }
  75
  76    m4x3f to_local;
  77    m4x3_invert_affine( mtxA, to_local );
  78
  79    bh_iter it;
  80    bh_iter_init_box( 0, &it, world_bbx );
  81    int idx;
  82    int count = 0;
  83
  84    vg_line_boxf( world_bbx, VG__RED );
  85
  86    while( bh_next( scene_bh, &it, &idx ) ){
  87       u32 *ptri = &sc->arrindices[ idx*3 ];
  88       if( sc->arrvertices[ptri[0]].flags & ignore ) continue;
  89
  90       for( int j=0; j<3; j++ )
  91          v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
  92
  93       if( rb_box_triangle_sat( extent, center, to_local, tri ) ){
  94          vg_line(tri[0],tri[1],0xff50ff00 );
  95          vg_line(tri[1],tri[2],0xff50ff00 );
  96          vg_line(tri[2],tri[0],0xff50ff00 );
  97       }
  98       else{
  99          vg_line(tri[0],tri[1],0xff0000ff );
 100          vg_line(tri[1],tri[2],0xff0000ff );
 101          vg_line(tri[2],tri[0],0xff0000ff );
 102          continue;
 103       }
 104
 105       v3f v0,v1,n;
 106       v3_sub( tri[1], tri[0], v0 );
 107       v3_sub( tri[2], tri[0], v1 );
 108       v3_cross( v0, v1, n );
 109
 110       if( v3_length2( n ) <= 0.00001f ){
 111 #ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
 112          vg_error( "Zero area triangle!\n" );
 113 #endif
 114          return 0;
 115       }
 116
 117       v3_normalize( n );
 118
 119       /* find best feature */
 120       f32 best = v3_dot( mtxA[0], n );
 121       int axis = 0;
 122
 123       for( int i=1; i<3; i++ ){
 124          f32 c = v3_dot( mtxA[i], n );
 125
 126          if( fabsf(c) > fabsf(best) ){
 127             best = c;
 128             axis = i;
 129          }
 130       }
 131
 132       v3f manifold[4];
 133
 134       if( axis == 0 ){
 135          f32 px = best > 0.0f? bbx[0][0]: bbx[1][0];
 136          manifold[0][0] = px;
 137          manifold[0][1] = bbx[0][1];
 138          manifold[0][2] = bbx[0][2];
 139          manifold[1][0] = px;
 140          manifold[1][1] = bbx[1][1];
 141          manifold[1][2] = bbx[0][2];
 142          manifold[2][0] = px;
 143          manifold[2][1] = bbx[1][1];
 144          manifold[2][2] = bbx[1][2];
 145          manifold[3][0] = px;
 146          manifold[3][1] = bbx[0][1];
 147          manifold[3][2] = bbx[1][2];
 148       }
 149       else if( axis == 1 ){
 150          f32 py = best > 0.0f? bbx[0][1]: bbx[1][1];
 151          manifold[0][0] = bbx[0][0];
 152          manifold[0][1] = py;
 153          manifold[0][2] = bbx[0][2];
 154          manifold[1][0] = bbx[1][0];
 155          manifold[1][1] = py;
 156          manifold[1][2] = bbx[0][2];
 157          manifold[2][0] = bbx[1][0];
 158          manifold[2][1] = py;
 159          manifold[2][2] = bbx[1][2];
 160          manifold[3][0] = bbx[0][0];
 161          manifold[3][1] = py;
 162          manifold[3][2] = bbx[1][2];
 163       }
 164       else{
 165          f32 pz = best > 0.0f? bbx[0][2]: bbx[1][2];
 166          manifold[0][0] = bbx[0][0];
 167          manifold[0][1] = bbx[0][1];
 168          manifold[0][2] = pz;
 169          manifold[1][0] = bbx[1][0];
 170          manifold[1][1] = bbx[0][1];
 171          manifold[1][2] = pz;
 172          manifold[2][0] = bbx[1][0];
 173          manifold[2][1] = bbx[1][1];
 174          manifold[2][2] = pz;
 175          manifold[3][0] = bbx[0][0];
 176          manifold[3][1] = bbx[1][1];
 177          manifold[3][2] = pz;
 178       }
 179
 180       for( int j=0; j<4; j++ )
 181          m4x3_mulv( mtxA, manifold[j], manifold[j] );
 182
 183       vg_line( manifold[0], manifold[1], 0xffffffff );
 184       vg_line( manifold[1], manifold[2], 0xffffffff );
 185       vg_line( manifold[2], manifold[3], 0xffffffff );
 186       vg_line( manifold[3], manifold[0], 0xffffffff );
 187
 188       for( int j=0; j<4; j++ ){
 189          rb_ct *ct = buf+count;
 190
 191          v3_copy( manifold[j], ct->co );
 192          v3_copy( n, ct->n );
 193
 194          f32 l0 = v3_dot( tri[0], n ),
 195                l1 = v3_dot( manifold[j], n );
 196
 197          ct->p = (l0-l1)*0.5f;
 198          if( ct->p < 0.0f )
 199             continue;
 200
 201          ct->type = k_contact_type_default;
 202          count ++;
 203
 204          if( count >= 12 )
 205             return count;
 206       }
 207    }
 208    return count;
 209 }
 210
 211 /* mtxB is defined only for tradition; it is not used currently */
 212 static int rb_capsule__scene( m4x3f mtxA, rb_capsule *c,
 213                               m4x3f mtxB, bh_tree *scene_bh,
 214                               rb_ct *buf, u16 ignore ){
 215    int count = 0;
 216
 217    boxf bbx;
 218    v3_sub( mtxA[3], (v3f){ c->h, c->h, c->h }, bbx[0] );
 219    v3_add( mtxA[3], (v3f){ c->h, c->h, c->h }, bbx[1] );
 220
 221    scene_context *sc = scene_bh->user;
 222
 223    bh_iter it;
 224    bh_iter_init_box( 0, &it, bbx );
 225    i32 idx;
 226    while( bh_next( scene_bh, &it, &idx ) ){
 227       u32 *ptri = &sc->arrindices[ idx*3 ];
 228       if( sc->arrvertices[ptri[0]].flags & ignore ) continue;
 229
 230       v3f tri[3];
 231       for( int j=0; j<3; j++ )
 232          v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
 233
 234       buf[ count ].element_id = ptri[0];
 235
 236       int contact = rb_capsule__triangle( mtxA, c, tri, &buf[count] );
 237       count += contact;
 238
 239       if( count >= 16 ){
 240          vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
 241          return count;
 242       }
 243    }
 244
 245    return count;
 246 }
 247