--- /dev/null
+#pragma once
+
+/*
+ * Copyright (C) 2021-2024 Mt.ZERO Software - All Rights Reserved
+ *
+ * Describes intereactions between vg rigidbody objects and skaterift's scene
+ * description
+ */
+
+#include "scene.h"
+#include "vg/vg_rigidbody.h"
+#include "vg/vg_rigidbody_collision.h"
+
+static int rb_sphere__scene( m4x3f mtxA, f32 r,
+ m4x3f mtxB, bh_tree *scene_bh, rb_ct *buf,
+ u16 ignore ){
+ scene_context *sc = scene_bh->user;
+
+ int count = 0;
+
+ boxf box;
+ v3_sub( mtxA[3], (v3f){ r,r,r }, box[0] );
+ v3_add( mtxA[3], (v3f){ r,r,r }, box[1] );
+
+ bh_iter it;
+ i32 idx;
+ bh_iter_init_box( 0, &it, box );
+
+ while( bh_next( scene_bh, &it, &idx ) ){
+ u32 *ptri = &sc->arrindices[ idx*3 ];
+ v3f tri[3];
+
+ if( sc->arrvertices[ptri[0]].flags & ignore ) continue;
+
+ for( int j=0; j<3; j++ )
+ v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
+
+ buf[ count ].element_id = ptri[0];
+
+ vg_line( tri[0],tri[1],0x70ff6000 );
+ vg_line( tri[1],tri[2],0x70ff6000 );
+ vg_line( tri[2],tri[0],0x70ff6000 );
+
+ int contact = rb_sphere__triangle( mtxA, r, tri, &buf[count] );
+ count += contact;
+
+ if( count == 16 ){
+ vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
+ return count;
+ }
+ }
+
+ return count;
+}
+
+static int rb_box__scene( m4x3f mtxA, boxf bbx,
+ m4x3f mtxB, bh_tree *scene_bh,
+ rb_ct *buf, u16 ignore ){
+ scene_context *sc = scene_bh->user;
+ v3f tri[3];
+
+ v3f extent, center;
+ v3_sub( bbx[1], bbx[0], extent );
+ v3_muls( extent, 0.5f, extent );
+ v3_add( bbx[0], extent, center );
+
+ f32 r = v3_length(extent);
+ boxf world_bbx;
+ v3_fill( world_bbx[0], -r );
+ v3_fill( world_bbx[1], r );
+ for( int i=0; i<2; i++ ){
+ v3_add( center, world_bbx[i], world_bbx[i] );
+ v3_add( mtxA[3], world_bbx[i], world_bbx[i] );
+ }
+
+ m4x3f to_local;
+ m4x3_invert_affine( mtxA, to_local );
+
+ bh_iter it;
+ bh_iter_init_box( 0, &it, world_bbx );
+ int idx;
+ int count = 0;
+
+ vg_line_boxf( world_bbx, VG__RED );
+
+ while( bh_next( scene_bh, &it, &idx ) ){
+ u32 *ptri = &sc->arrindices[ idx*3 ];
+ if( sc->arrvertices[ptri[0]].flags & ignore ) continue;
+
+ for( int j=0; j<3; j++ )
+ v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
+
+ if( rb_box_triangle_sat( extent, center, to_local, tri ) ){
+ vg_line(tri[0],tri[1],0xff50ff00 );
+ vg_line(tri[1],tri[2],0xff50ff00 );
+ vg_line(tri[2],tri[0],0xff50ff00 );
+ }
+ else{
+ vg_line(tri[0],tri[1],0xff0000ff );
+ vg_line(tri[1],tri[2],0xff0000ff );
+ vg_line(tri[2],tri[0],0xff0000ff );
+ continue;
+ }
+
+ v3f v0,v1,n;
+ v3_sub( tri[1], tri[0], v0 );
+ v3_sub( tri[2], tri[0], v1 );
+ v3_cross( v0, v1, n );
+
+ if( v3_length2( n ) <= 0.00001f ){
+#ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
+ vg_error( "Zero area triangle!\n" );
+#endif
+ return 0;
+ }
+
+ v3_normalize( n );
+
+ /* find best feature */
+ f32 best = v3_dot( mtxA[0], n );
+ int axis = 0;
+
+ for( int i=1; i<3; i++ ){
+ f32 c = v3_dot( mtxA[i], n );
+
+ if( fabsf(c) > fabsf(best) ){
+ best = c;
+ axis = i;
+ }
+ }
+
+ v3f manifold[4];
+
+ if( axis == 0 ){
+ f32 px = best > 0.0f? bbx[0][0]: bbx[1][0];
+ manifold[0][0] = px;
+ manifold[0][1] = bbx[0][1];
+ manifold[0][2] = bbx[0][2];
+ manifold[1][0] = px;
+ manifold[1][1] = bbx[1][1];
+ manifold[1][2] = bbx[0][2];
+ manifold[2][0] = px;
+ manifold[2][1] = bbx[1][1];
+ manifold[2][2] = bbx[1][2];
+ manifold[3][0] = px;
+ manifold[3][1] = bbx[0][1];
+ manifold[3][2] = bbx[1][2];
+ }
+ else if( axis == 1 ){
+ f32 py = best > 0.0f? bbx[0][1]: bbx[1][1];
+ manifold[0][0] = bbx[0][0];
+ manifold[0][1] = py;
+ manifold[0][2] = bbx[0][2];
+ manifold[1][0] = bbx[1][0];
+ manifold[1][1] = py;
+ manifold[1][2] = bbx[0][2];
+ manifold[2][0] = bbx[1][0];
+ manifold[2][1] = py;
+ manifold[2][2] = bbx[1][2];
+ manifold[3][0] = bbx[0][0];
+ manifold[3][1] = py;
+ manifold[3][2] = bbx[1][2];
+ }
+ else{
+ f32 pz = best > 0.0f? bbx[0][2]: bbx[1][2];
+ manifold[0][0] = bbx[0][0];
+ manifold[0][1] = bbx[0][1];
+ manifold[0][2] = pz;
+ manifold[1][0] = bbx[1][0];
+ manifold[1][1] = bbx[0][1];
+ manifold[1][2] = pz;
+ manifold[2][0] = bbx[1][0];
+ manifold[2][1] = bbx[1][1];
+ manifold[2][2] = pz;
+ manifold[3][0] = bbx[0][0];
+ manifold[3][1] = bbx[1][1];
+ manifold[3][2] = pz;
+ }
+
+ for( int j=0; j<4; j++ )
+ m4x3_mulv( mtxA, manifold[j], manifold[j] );
+
+ vg_line( manifold[0], manifold[1], 0xffffffff );
+ vg_line( manifold[1], manifold[2], 0xffffffff );
+ vg_line( manifold[2], manifold[3], 0xffffffff );
+ vg_line( manifold[3], manifold[0], 0xffffffff );
+
+ for( int j=0; j<4; j++ ){
+ rb_ct *ct = buf+count;
+
+ v3_copy( manifold[j], ct->co );
+ v3_copy( n, ct->n );
+
+ f32 l0 = v3_dot( tri[0], n ),
+ l1 = v3_dot( manifold[j], n );
+
+ ct->p = (l0-l1)*0.5f;
+ if( ct->p < 0.0f )
+ continue;
+
+ ct->type = k_contact_type_default;
+ count ++;
+
+ if( count >= 12 )
+ return count;
+ }
+ }
+ return count;
+}
+
+/* mtxB is defined only for tradition; it is not used currently */
+static int rb_capsule__scene( m4x3f mtxA, rb_capsule *c,
+ m4x3f mtxB, bh_tree *scene_bh,
+ rb_ct *buf, u16 ignore ){
+ int count = 0;
+
+ boxf bbx;
+ v3_sub( mtxA[3], (v3f){ c->h, c->h, c->h }, bbx[0] );
+ v3_add( mtxA[3], (v3f){ c->h, c->h, c->h }, bbx[1] );
+
+ scene_context *sc = scene_bh->user;
+
+ bh_iter it;
+ bh_iter_init_box( 0, &it, bbx );
+ i32 idx;
+ while( bh_next( scene_bh, &it, &idx ) ){
+ u32 *ptri = &sc->arrindices[ idx*3 ];
+ if( sc->arrvertices[ptri[0]].flags & ignore ) continue;
+
+ v3f tri[3];
+ for( int j=0; j<3; j++ )
+ v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
+
+ buf[ count ].element_id = ptri[0];
+
+ int contact = rb_capsule__triangle( mtxA, c, tri, &buf[count] );
+ count += contact;
+
+ if( count >= 16 ){
+ vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
+ return count;
+ }
+ }
+
+ return count;
+}
+
projects / carveJwlIkooP6JGAAIwe30JlM.git / blobdiff