4 * Copyright (C) 2021-2024 Mt.ZERO Software - All Rights Reserved
6 * Describes intereactions between vg rigidbody objects and skaterift's scene
11 #include "vg/vg_rigidbody.h"
12 #include "vg/vg_rigidbody_collision.h"
14 static int rb_sphere__scene( m4x3f mtxA
, f32 r
,
15 m4x3f mtxB
, bh_tree
*scene_bh
, rb_ct
*buf
,
17 scene_context
*sc
= scene_bh
->user
;
22 v3_sub( mtxA
[3], (v3f
){ r
,r
,r
}, box
[0] );
23 v3_add( mtxA
[3], (v3f
){ r
,r
,r
}, box
[1] );
27 bh_iter_init_box( 0, &it
, box
);
29 while( bh_next( scene_bh
, &it
, &idx
) ){
30 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
33 if( sc
->arrvertices
[ptri
[0]].flags
& ignore
) continue;
35 for( int j
=0; j
<3; j
++ )
36 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
38 buf
[ count
].element_id
= ptri
[0];
40 vg_line( tri
[0],tri
[1],0x70ff6000 );
41 vg_line( tri
[1],tri
[2],0x70ff6000 );
42 vg_line( tri
[2],tri
[0],0x70ff6000 );
44 int contact
= rb_sphere__triangle( mtxA
, r
, tri
, &buf
[count
] );
48 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
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
;
63 v3_sub( bbx
[1], bbx
[0], extent
);
64 v3_muls( extent
, 0.5f
, extent
);
65 v3_add( bbx
[0], extent
, center
);
67 f32 r
= v3_length(extent
);
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
] );
77 m4x3_invert_affine( mtxA
, to_local
);
80 bh_iter_init_box( 0, &it
, world_bbx
);
84 vg_line_boxf( world_bbx
, VG__RED
);
86 while( bh_next( scene_bh
, &it
, &idx
) ){
87 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
88 if( sc
->arrvertices
[ptri
[0]].flags
& ignore
) continue;
90 for( int j
=0; j
<3; j
++ )
91 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
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 );
99 vg_line(tri
[0],tri
[1],0xff0000ff );
100 vg_line(tri
[1],tri
[2],0xff0000ff );
101 vg_line(tri
[2],tri
[0],0xff0000ff );
106 v3_sub( tri
[1], tri
[0], v0
);
107 v3_sub( tri
[2], tri
[0], v1
);
108 v3_cross( v0
, v1
, n
);
110 if( v3_length2( n
) <= 0.00001f
){
111 #ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
112 vg_error( "Zero area triangle!\n" );
119 /* find best feature */
120 f32 best
= v3_dot( mtxA
[0], n
);
123 for( int i
=1; i
<3; i
++ ){
124 f32 c
= v3_dot( mtxA
[i
], n
);
126 if( fabsf(c
) > fabsf(best
) ){
135 f32 px
= best
> 0.0f
? bbx
[0][0]: bbx
[1][0];
137 manifold
[0][1] = bbx
[0][1];
138 manifold
[0][2] = bbx
[0][2];
140 manifold
[1][1] = bbx
[1][1];
141 manifold
[1][2] = bbx
[0][2];
143 manifold
[2][1] = bbx
[1][1];
144 manifold
[2][2] = bbx
[1][2];
146 manifold
[3][1] = bbx
[0][1];
147 manifold
[3][2] = bbx
[1][2];
149 else if( axis
== 1 ){
150 f32 py
= best
> 0.0f
? bbx
[0][1]: bbx
[1][1];
151 manifold
[0][0] = bbx
[0][0];
153 manifold
[0][2] = bbx
[0][2];
154 manifold
[1][0] = bbx
[1][0];
156 manifold
[1][2] = bbx
[0][2];
157 manifold
[2][0] = bbx
[1][0];
159 manifold
[2][2] = bbx
[1][2];
160 manifold
[3][0] = bbx
[0][0];
162 manifold
[3][2] = bbx
[1][2];
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];
169 manifold
[1][0] = bbx
[1][0];
170 manifold
[1][1] = bbx
[0][1];
172 manifold
[2][0] = bbx
[1][0];
173 manifold
[2][1] = bbx
[1][1];
175 manifold
[3][0] = bbx
[0][0];
176 manifold
[3][1] = bbx
[1][1];
180 for( int j
=0; j
<4; j
++ )
181 m4x3_mulv( mtxA
, manifold
[j
], manifold
[j
] );
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 );
188 for( int j
=0; j
<4; j
++ ){
189 rb_ct
*ct
= buf
+count
;
191 v3_copy( manifold
[j
], ct
->co
);
194 f32 l0
= v3_dot( tri
[0], n
),
195 l1
= v3_dot( manifold
[j
], n
);
197 ct
->p
= (l0
-l1
)*0.5f
;
201 ct
->type
= k_contact_type_default
;
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
){
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] );
221 scene_context
*sc
= scene_bh
->user
;
224 bh_iter_init_box( 0, &it
, bbx
);
226 while( bh_next( scene_bh
, &it
, &idx
) ){
227 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
228 if( sc
->arrvertices
[ptri
[0]].flags
& ignore
) continue;
231 for( int j
=0; j
<3; j
++ )
232 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
234 buf
[ count
].element_id
= ptri
[0];
236 int contact
= rb_capsule__triangle( mtxA
, c
, tri
, &buf
[count
] );
240 vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");