projects / carveJwlIkooP6JGAAIwe30JlM.git / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
history | raw | HEAD
longjump gates
[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h
1 /*
2 * Copyright (C) 2021-2022 Mt.ZERO Software, Harry Godden - All Rights Reserved
3 */
4
5 /*
6 * Resources: Box2D - Erin Catto
7 * qu3e - Randy Gaul
8 */
9
10 #include "common.h"
11 #include "bvh.h"
12 #include "scene.h"
13
14 #include <math.h>
15
16 VG_STATIC void rb_tangent_basis( v3f n, v3f tx, v3f ty );
17 VG_STATIC bh_system bh_system_rigidbodies;
18
19 #ifndef RIGIDBODY_H
20 #define RIGIDBODY_H
21
22 /*
23 * -----------------------------------------------------------------------------
24 * (K)onstants
25 * -----------------------------------------------------------------------------
26 */
27
28 VG_STATIC const float
29 k_rb_rate = (1.0/VG_TIMESTEP_FIXED),
30 k_rb_delta = (1.0/k_rb_rate),
31 k_friction = 0.4f,
32 k_damp_linear = 0.1f, /* scale velocity 1/(1+x) */
33 k_damp_angular = 0.1f, /* scale angular 1/(1+x) */
34 k_penetration_slop = 0.01f,
35 k_inertia_scale = 8.0f,
36 k_phys_baumgarte = 0.2f,
37 k_gravity = 9.6f;
38
39 VG_STATIC float
40 k_limit_bias = 0.02f,
41 k_joint_correction = 0.01f,
42 k_joint_impulse = 1.0f,
43 k_joint_bias = 0.08f; /* positional joints */
44
45 VG_STATIC void rb_register_cvar(void)
46 {
47 vg_var_push( (struct vg_var){
48 .name = "k_limit_bias", .data = &k_limit_bias,
49 .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1
50 });
51
52 vg_var_push( (struct vg_var){
53 .name = "k_joint_bias", .data = &k_joint_bias,
54 .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1
55 });
56
57 vg_var_push( (struct vg_var){
58 .name = "k_joint_correction", .data = &k_joint_correction,
59 .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1
60 });
61
62 vg_var_push( (struct vg_var){
63 .name = "k_joint_impulse", .data = &k_joint_impulse,
64 .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1
65 });
66 }
67
68 /*
69 * -----------------------------------------------------------------------------
70 * structure definitions
71 * -----------------------------------------------------------------------------
72 */
73
74 typedef struct rigidbody rigidbody;
75 typedef struct contact rb_ct;
76 typedef struct rb_sphere rb_sphere;
77 typedef struct rb_capsule rb_capsule;
78 typedef struct rb_scene rb_scene;
79
80 struct rb_sphere
81 {
82 float radius;
83 };
84
85 struct rb_capsule
86 {
87 float height, radius;
88 };
89
90 struct rb_scene
91 {
92 bh_tree *bh_scene;
93 };
94
95 struct rigidbody
96 {
97 v3f co, v, w;
98 v4f q;
99
100 enum rb_shape
101 {
102 k_rb_shape_box = 0,
103 k_rb_shape_sphere = 1,
104 k_rb_shape_capsule = 2,
105 k_rb_shape_scene = 3
106 }
107 type;
108
109 union
110 {
111 struct rb_sphere sphere;
112 struct rb_capsule capsule;
113 struct rb_scene scene;
114 }
115 inf;
116
117 v3f right, up, forward;
118
119 int is_world;
120
121 boxf bbx, bbx_world;
122 float inv_mass;
123
124 /* inertia model and inverse world tensor */
125 v3f I;
126 m3x3f iI, iIw;
127
128 m4x3f to_world, to_local;
129 };
130
131 VG_STATIC struct contact
132 {
133 rigidbody *rba, *rbb;
134 v3f co, n;
135 v3f t[2];
136 float p, bias, norm_impulse, tangent_impulse[2],
137 normal_mass, tangent_mass[2];
138
139 u32 element_id;
140
141 enum contact_type type;
142 }
143 rb_contact_buffer[256];
144 VG_STATIC int rb_contact_count = 0;
145
146 typedef struct rb_constr_pos rb_constr_pos;
147 typedef struct rb_constr_swingtwist rb_constr_swingtwist;
148
149 struct rb_constr_pos
150 {
151 rigidbody *rba, *rbb;
152 v3f lca, lcb;
153 };
154
155 struct rb_constr_swingtwist
156 {
157 rigidbody *rba, *rbb;
158
159 v4f conevx, conevy; /* relative to rba */
160 v3f view_offset, /* relative to rba */
161 coneva, conevxb;/* relative to rbb */
162
163 int tangent_violation, axis_violation;
164 v3f axis, tangent_axis, tangent_target, axis_target;
165
166 float conet;
167 float tangent_mass, axis_mass;
168 };
169
170 struct rb_constr_spring
171 {
172 int nothing;
173 };
174
175 /*
176 * -----------------------------------------------------------------------------
177 * Math Utils
178 * -----------------------------------------------------------------------------
179 */
180
181 VG_STATIC float sphere_volume( float radius )
182 {
183 float r3 = radius*radius*radius;
184 return (4.0f/3.0f) * VG_PIf * r3;
185 }
186
187 VG_STATIC void rb_tangent_basis( v3f n, v3f tx, v3f ty )
188 {
189 /* Compute tangent basis (box2d) */
190 if( fabsf( n[0] ) >= 0.57735027f )
191 {
192 tx[0] = n[1];
193 tx[1] = -n[0];
194 tx[2] = 0.0f;
195 }
196 else
197 {
198 tx[0] = 0.0f;
199 tx[1] = n[2];
200 tx[2] = -n[1];
201 }
202
203 v3_normalize( tx );
204 v3_cross( n, tx, ty );
205 }
206
207 /*
208 * -----------------------------------------------------------------------------
209 * Debugging
210 * -----------------------------------------------------------------------------
211 */
212
213 VG_STATIC void rb_debug_contact( rb_ct *ct )
214 {
215 v3f p1;
216 v3_muladds( ct->co, ct->n, 0.05f, p1 );
217
218 if( ct->type == k_contact_type_default )
219 {
220 vg_line_pt3( ct->co, 0.0125f, 0xff0000ff );
221 vg_line( ct->co, p1, 0xffffffff );
222 }
223 else if( ct->type == k_contact_type_edge )
224 {
225 vg_line_pt3( ct->co, 0.0125f, 0xff00ffc0 );
226 vg_line( ct->co, p1, 0xffffffff );
227 }
228 }
229
230 VG_STATIC void debug_sphere( m4x3f m, float radius, u32 colour )
231 {
232 v3f ly = { 0.0f, 0.0f, radius },
233 lx = { 0.0f, radius, 0.0f },
234 lz = { 0.0f, 0.0f, radius };
235
236 for( int i=0; i<16; i++ )
237 {
238 float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
239 s = sinf(t),
240 c = cosf(t);
241
242 v3f py = { s*radius, 0.0f, c*radius },
243 px = { s*radius, c*radius, 0.0f },
244 pz = { 0.0f, s*radius, c*radius };
245
246 v3f p0, p1, p2, p3, p4, p5;
247 m4x3_mulv( m, py, p0 );
248 m4x3_mulv( m, ly, p1 );
249 m4x3_mulv( m, px, p2 );
250 m4x3_mulv( m, lx, p3 );
251 m4x3_mulv( m, pz, p4 );
252 m4x3_mulv( m, lz, p5 );
253
254 vg_line( p0, p1, colour == 0x00? 0xff00ff00: colour );
255 vg_line( p2, p3, colour == 0x00? 0xff0000ff: colour );
256 vg_line( p4, p5, colour == 0x00? 0xffff0000: colour );
257
258 v3_copy( py, ly );
259 v3_copy( px, lx );
260 v3_copy( pz, lz );
261 }
262 }
263
264 VG_STATIC void debug_capsule( m4x3f m, float radius, float h, u32 colour )
265 {
266 v3f ly = { 0.0f, 0.0f, radius },
267 lx = { 0.0f, radius, 0.0f },
268 lz = { 0.0f, 0.0f, radius };
269
270 float s0 = sinf(0.0f)*radius,
271 c0 = cosf(0.0f)*radius;
272
273 v3f p0, p1, up, right, forward;
274 m3x3_mulv( m, (v3f){0.0f,1.0f,0.0f}, up );
275 m3x3_mulv( m, (v3f){1.0f,0.0f,0.0f}, right );
276 m3x3_mulv( m, (v3f){0.0f,0.0f,-1.0f}, forward );
277 v3_muladds( m[3], up, -h*0.5f+radius, p0 );
278 v3_muladds( m[3], up, h*0.5f-radius, p1 );
279
280 v3f a0, a1, b0, b1;
281 v3_muladds( p0, right, radius, a0 );
282 v3_muladds( p1, right, radius, a1 );
283 v3_muladds( p0, forward, radius, b0 );
284 v3_muladds( p1, forward, radius, b1 );
285 vg_line( a0, a1, colour );
286 vg_line( b0, b1, colour );
287
288 v3_muladds( p0, right, -radius, a0 );
289 v3_muladds( p1, right, -radius, a1 );
290 v3_muladds( p0, forward, -radius, b0 );
291 v3_muladds( p1, forward, -radius, b1 );
292 vg_line( a0, a1, colour );
293 vg_line( b0, b1, colour );
294
295 for( int i=0; i<16; i++ )
296 {
297 float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
298 s1 = sinf(t)*radius,
299 c1 = cosf(t)*radius;
300
301 v3f e0 = { s0, 0.0f, c0 },
302 e1 = { s1, 0.0f, c1 },
303 e2 = { s0, c0, 0.0f },
304 e3 = { s1, c1, 0.0f },
305 e4 = { 0.0f, c0, s0 },
306 e5 = { 0.0f, c1, s1 };
307
308 m3x3_mulv( m, e0, e0 );
309 m3x3_mulv( m, e1, e1 );
310 m3x3_mulv( m, e2, e2 );
311 m3x3_mulv( m, e3, e3 );
312 m3x3_mulv( m, e4, e4 );
313 m3x3_mulv( m, e5, e5 );
314
315 v3_add( p0, e0, a0 );
316 v3_add( p0, e1, a1 );
317 v3_add( p1, e0, b0 );
318 v3_add( p1, e1, b1 );
319
320 vg_line( a0, a1, colour );
321 vg_line( b0, b1, colour );
322
323 if( c0 < 0.0f )
324 {
325 v3_add( p0, e2, a0 );
326 v3_add( p0, e3, a1 );
327 v3_add( p0, e4, b0 );
328 v3_add( p0, e5, b1 );
329 }
330 else
331 {
332 v3_add( p1, e2, a0 );
333 v3_add( p1, e3, a1 );
334 v3_add( p1, e4, b0 );
335 v3_add( p1, e5, b1 );
336 }
337
338 vg_line( a0, a1, colour );
339 vg_line( b0, b1, colour );
340
341 s0 = s1;
342 c0 = c1;
343 }
344 }
345
346 VG_STATIC void rb_debug( rigidbody *rb, u32 colour )
347 {
348 if( rb->type == k_rb_shape_box )
349 {
350 v3f *box = rb->bbx;
351 vg_line_boxf_transformed( rb->to_world, rb->bbx, colour );
352 }
353 else if( rb->type == k_rb_shape_sphere )
354 {
355 debug_sphere( rb->to_world, rb->inf.sphere.radius, colour );
356 }
357 else if( rb->type == k_rb_shape_capsule )
358 {
359 m4x3f m0, m1;
360 float h = rb->inf.capsule.height,
361 r = rb->inf.capsule.radius;
362
363 debug_capsule( rb->to_world, r, h, colour );
364 }
365 else if( rb->type == k_rb_shape_scene )
366 {
367 vg_line_boxf( rb->bbx, colour );
368 }
369 }
370
371 /*
372 * -----------------------------------------------------------------------------
373 * Integration
374 * -----------------------------------------------------------------------------
375 */
376
377 /*
378 * Update world space bounding box based on local one
379 */
380 VG_STATIC void rb_update_bounds( rigidbody *rb )
381 {
382 box_copy( rb->bbx, rb->bbx_world );
383 m4x3_transform_aabb( rb->to_world, rb->bbx_world );
384 }
385
386 /*
387 * Commit transform to rigidbody. Updates matrices
388 */
389 VG_STATIC void rb_update_transform( rigidbody *rb )
390 {
391 q_normalize( rb->q );
392 q_m3x3( rb->q, rb->to_world );
393 v3_copy( rb->co, rb->to_world[3] );
394
395 m4x3_invert_affine( rb->to_world, rb->to_local );
396
397 m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f, 0.0f}, rb->right );
398 m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f, 0.0f}, rb->up );
399 m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,-1.0f}, rb->forward );
400
401 m3x3_mul( rb->iI, rb->to_local, rb->iIw );
402 m3x3_mul( rb->to_world, rb->iIw, rb->iIw );
403
404 rb_update_bounds( rb );
405 }
406
407 /*
408 * Extrapolate rigidbody into a transform based on vg accumulator.
409 * Useful for rendering
410 */
411 __attribute__ ((deprecated))
412 VG_STATIC void rb_extrapolate_transform( rigidbody *rb, m4x3f transform )
413 {
414 float substep = vg_clampf( vg.accumulator / k_rb_delta, 0.0f, 1.0f );
415
416 v3f co;
417 v4f q;
418
419 v3_muladds( rb->co, rb->v, k_rb_delta*substep, co );
420
421 if( v3_length2( rb->w ) > 0.0f )
422 {
423 v4f rotation;
424 v3f axis;
425 v3_copy( rb->w, axis );
426
427 float mag = v3_length( axis );
428 v3_divs( axis, mag, axis );
429 q_axis_angle( rotation, axis, mag*k_rb_delta*substep );
430 q_mul( rotation, rb->q, q );
431 q_normalize( q );
432 }
433 else
434 {
435 v4_copy( rb->q, q );
436 }
437
438 q_m3x3( q, transform );
439 v3_copy( co, transform[3] );
440 }
441
442 VG_STATIC void rb_extrapolate( rigidbody *rb, v3f co, v4f q )
443 {
444 float substep = vg_clampf( vg.accumulator / k_rb_delta, 0.0f, 1.0f );
445
446 v3_muladds( rb->co, rb->v, k_rb_delta*substep, co );
447
448 if( v3_length2( rb->w ) > 0.0f )
449 {
450 v4f rotation;
451 v3f axis;
452 v3_copy( rb->w, axis );
453
454 float mag = v3_length( axis );
455 v3_divs( axis, mag, axis );
456 q_axis_angle( rotation, axis, mag*k_rb_delta*substep );
457 q_mul( rotation, rb->q, q );
458 q_normalize( q );
459 }
460 else
461 {
462 v4_copy( rb->q, q );
463 }
464 }
465
466 /*
467 * Initialize rigidbody and calculate masses, inertia
468 */
469 VG_STATIC void rb_init( rigidbody *rb )
470 {
471 float volume = 1.0f;
472
473 if( rb->type == k_rb_shape_box )
474 {
475 v3f dims;
476 v3_sub( rb->bbx[1], rb->bbx[0], dims );
477 volume = dims[0]*dims[1]*dims[2];
478 }
479 else if( rb->type == k_rb_shape_sphere )
480 {
481 volume = sphere_volume( rb->inf.sphere.radius );
482 v3_fill( rb->bbx[0], -rb->inf.sphere.radius );
483 v3_fill( rb->bbx[1], rb->inf.sphere.radius );
484 }
485 else if( rb->type == k_rb_shape_capsule )
486 {
487 float r = rb->inf.capsule.radius,
488 h = rb->inf.capsule.height;
489 volume = sphere_volume( r ) + VG_PIf * r*r * (h - r*2.0f);
490
491 v3_fill( rb->bbx[0], -r );
492 v3_fill( rb->bbx[1], r );
493 rb->bbx[0][1] = -h;
494 rb->bbx[1][1] = h;
495 }
496 else if( rb->type == k_rb_shape_scene )
497 {
498 rb->is_world = 1;
499 box_copy( rb->inf.scene.bh_scene->nodes[0].bbx, rb->bbx );
500 }
501
502 if( rb->is_world )
503 {
504 rb->inv_mass = 0.0f;
505 v3_zero( rb->I );
506 m3x3_zero(rb->iI);
507 }
508 else
509 {
510 float mass = 2.0f*volume;
511 rb->inv_mass = 1.0f/mass;
512
513 v3f extent;
514 v3_sub( rb->bbx[1], rb->bbx[0], extent );
515 v3_muls( extent, 0.5f, extent );
516
517 /* local intertia tensor */
518 float scale = k_inertia_scale;
519 float ex2 = scale*extent[0]*extent[0],
520 ey2 = scale*extent[1]*extent[1],
521 ez2 = scale*extent[2]*extent[2];
522
523 rb->I[0] = ((1.0f/12.0f) * mass * (ey2+ez2));
524 rb->I[1] = ((1.0f/12.0f) * mass * (ex2+ez2));
525 rb->I[2] = ((1.0f/12.0f) * mass * (ex2+ey2));
526
527 m3x3_identity( rb->iI );
528 rb->iI[0][0] = rb->I[0];
529 rb->iI[1][1] = rb->I[1];
530 rb->iI[2][2] = rb->I[2];
531 m3x3_inv( rb->iI, rb->iI );
532 }
533
534 v3_zero( rb->v );
535 v3_zero( rb->w );
536
537 rb_update_transform( rb );
538 }
539
540 VG_STATIC void rb_iter( rigidbody *rb )
541 {
542 if( !vg_validf( rb->v[0] ) ||
543 !vg_validf( rb->v[1] ) ||
544 !vg_validf( rb->v[2] ) )
545 {
546 vg_fatal_exit_loop( "NaN velocity" );
547 }
548
549 v3f gravity = { 0.0f, -9.8f, 0.0f };
550 v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
551
552 /* intergrate velocity */
553 v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
554 v3_lerp( rb->w, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->w );
555
556 /* inegrate inertia */
557 if( v3_length2( rb->w ) > 0.0f )
558 {
559 v4f rotation;
560 v3f axis;
561 v3_copy( rb->w, axis );
562
563 float mag = v3_length( axis );
564 v3_divs( axis, mag, axis );
565 q_axis_angle( rotation, axis, mag*k_rb_delta );
566 q_mul( rotation, rb->q, rb->q );
567 }
568
569 /* damping */
570 v3_muls( rb->v, 1.0f/(1.0f+k_rb_delta*k_damp_linear), rb->v );
571 v3_muls( rb->w, 1.0f/(1.0f+k_rb_delta*k_damp_angular), rb->w );
572 }
573
574
575 /*
576 * -----------------------------------------------------------------------------
577 * Boolean shape overlap functions
578 * -----------------------------------------------------------------------------
579 */
580
581 /*
582 * Project AABB, and triangle interval onto axis to check if they overlap
583 */
584 VG_STATIC int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] )
585 {
586 float
587
588 r = extent[0] * fabsf(axis[0]) +
589 extent[1] * fabsf(axis[1]) +
590 extent[2] * fabsf(axis[2]),
591
592 p0 = v3_dot( axis, tri[0] ),
593 p1 = v3_dot( axis, tri[1] ),
594 p2 = v3_dot( axis, tri[2] ),
595
596 e = vg_maxf(-vg_maxf(p0,vg_maxf(p1,p2)), vg_minf(p0,vg_minf(p1,p2)));
597
598 if( e > r ) return 0;
599 else return 1;
600 }
601
602 /*
603 * Seperating axis test box vs triangle
604 */
605 VG_STATIC int rb_box_triangle_sat( rigidbody *rba, v3f tri_src[3] )
606 {
607 v3f tri[3];
608
609 v3f extent, c;
610 v3_sub( rba->bbx[1], rba->bbx[0], extent );
611 v3_muls( extent, 0.5f, extent );
612 v3_add( rba->bbx[0], extent, c );
613
614 for( int i=0; i<3; i++ )
615 {
616 m4x3_mulv( rba->to_local, tri_src[i], tri[i] );
617 v3_sub( tri[i], c, tri[i] );
618 }
619
620 /* u0, u1, u2 */
621 if(!rb_box_triangle_interval( extent, (v3f){1.0f,0.0f,0.0f}, tri )) return 0;
622 if(!rb_box_triangle_interval( extent, (v3f){0.0f,1.0f,0.0f}, tri )) return 0;
623 if(!rb_box_triangle_interval( extent, (v3f){0.0f,0.0f,1.0f}, tri )) return 0;
624
625 v3f v0,v1,v2,n, e0,e1,e2;
626 v3_sub( tri[1], tri[0], v0 );
627 v3_sub( tri[2], tri[0], v1 );
628 v3_sub( tri[2], tri[1], v2 );
629 v3_normalize( v0 );
630 v3_normalize( v1 );
631 v3_normalize( v2 );
632 v3_cross( v0, v1, n );
633 v3_cross( v0, n, e0 );
634 v3_cross( n, v1, e1 );
635 v3_cross( v2, n, e2 );
636
637 /* normal */
638 if(!rb_box_triangle_interval( extent, n, tri )) return 0;
639
640 v3f axis[9];
641 v3_cross( e0, (v3f){1.0f,0.0f,0.0f}, axis[0] );
642 v3_cross( e0, (v3f){0.0f,1.0f,0.0f}, axis[1] );
643 v3_cross( e0, (v3f){0.0f,0.0f,1.0f}, axis[2] );
644 v3_cross( e1, (v3f){1.0f,0.0f,0.0f}, axis[3] );
645 v3_cross( e1, (v3f){0.0f,1.0f,0.0f}, axis[4] );
646 v3_cross( e1, (v3f){0.0f,0.0f,1.0f}, axis[5] );
647 v3_cross( e2, (v3f){1.0f,0.0f,0.0f}, axis[6] );
648 v3_cross( e2, (v3f){0.0f,1.0f,0.0f}, axis[7] );
649 v3_cross( e2, (v3f){0.0f,0.0f,1.0f}, axis[8] );
650
651 for( int i=0; i<9; i++ )
652 if(!rb_box_triangle_interval( extent, axis[i], tri )) return 0;
653
654 return 1;
655 }
656
657 /*
658 * -----------------------------------------------------------------------------
659 * Manifold
660 * -----------------------------------------------------------------------------
661 */
662
663 VG_STATIC int rb_manifold_apply_filtered( rb_ct *man, int len )
664 {
665 int k = 0;
666
667 for( int i=0; i<len; i++ )
668 {
669 rb_ct *ct = &man[i];
670
671 if( ct->type == k_contact_type_disabled )
672 continue;
673
674 man[k ++] = man[i];
675 }
676
677 return k;
678 }
679
680 /*
681 * Merge two contacts if they are within radius(r) of eachother
682 */
683 VG_STATIC void rb_manifold_contact_weld( rb_ct *ci, rb_ct *cj, float r )
684 {
685 if( v3_dist2( ci->co, cj->co ) < r*r )
686 {
687 cj->type = k_contact_type_disabled;
688 ci->p = (ci->p + cj->p) * 0.5f;
689
690 v3_add( ci->co, cj->co, ci->co );
691 v3_muls( ci->co, 0.5f, ci->co );
692
693 v3f delta;
694 v3_sub( ci->rba->co, ci->co, delta );
695
696 float c0 = v3_dot( ci->n, delta ),
697 c1 = v3_dot( cj->n, delta );
698
699 if( c0 < 0.0f || c1 < 0.0f )
700 {
701 /* error */
702 ci->type = k_contact_type_disabled;
703 }
704 else
705 {
706 v3f n;
707 v3_muls( ci->n, c0, n );
708 v3_muladds( n, cj->n, c1, n );
709 v3_normalize( n );
710 v3_copy( n, ci->n );
711 }
712 }
713 }
714
715 /*
716 *
717 */
718 VG_STATIC void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r )
719 {
720 for( int i=0; i<len-1; i++ )
721 {
722 rb_ct *ci = &man[i];
723 if( ci->type != k_contact_type_edge )
724 continue;
725
726 for( int j=i+1; j<len; j++ )
727 {
728 rb_ct *cj = &man[j];
729 if( cj->type != k_contact_type_edge )
730 continue;
731
732 rb_manifold_contact_weld( ci, cj, r );
733 }
734 }
735 }
736
737 /*
738 * Resolve overlapping pairs
739 *
740 * TODO: Remove?
741 */
742 VG_STATIC void rb_manifold_filter_pairs( rb_ct *man, int len, float r )
743 {
744 for( int i=0; i<len-1; i++ )
745 {
746 rb_ct *ci = &man[i];
747 int similar = 0;
748
749 if( ci->type == k_contact_type_disabled ) continue;
750
751 for( int j=i+1; j<len; j++ )
752 {
753 rb_ct *cj = &man[j];
754
755 if( cj->type == k_contact_type_disabled ) continue;
756
757 if( v3_dist2( ci->co, cj->co ) < r*r )
758 {
759 cj->type = k_contact_type_disabled;
760 v3_add( cj->n, ci->n, ci->n );
761 ci->p += cj->p;
762 similar ++;
763 }
764 }
765
766 if( similar )
767 {
768 float n = 1.0f/((float)similar+1.0f);
769 v3_muls( ci->n, n, ci->n );
770 ci->p *= n;
771
772 if( v3_length2(ci->n) < 0.1f*0.1f )
773 ci->type = k_contact_type_disabled;
774 else
775 v3_normalize( ci->n );
776 }
777 }
778 }
779
780 /*
781 * Remove contacts that are facing away from A
782 */
783 VG_STATIC void rb_manifold_filter_backface( rb_ct *man, int len )
784 {
785 for( int i=0; i<len; i++ )
786 {
787 rb_ct *ct = &man[i];
788 if( ct->type == k_contact_type_disabled )
789 continue;
790
791 v3f delta;
792 v3_sub( ct->co, ct->rba->co, delta );
793
794 if( v3_dot( delta, ct->n ) > -0.001f )
795 ct->type = k_contact_type_disabled;
796 }
797 }
798
799 /*
800 * Filter out duplicate coplanar results. Good for spheres.
801 */
802 VG_STATIC void rb_manifold_filter_coplanar( rb_ct *man, int len, float w )
803 {
804 for( int i=0; i<len; i++ )
805 {
806 rb_ct *ci = &man[i];
807 if( ci->type == k_contact_type_disabled ||
808 ci->type == k_contact_type_edge )
809 continue;
810
811 float d1 = v3_dot( ci->co, ci->n );
812
813 for( int j=0; j<len; j++ )
814 {
815 if( j == i )
816 continue;
817
818 rb_ct *cj = &man[j];
819 if( cj->type == k_contact_type_disabled )
820 continue;
821
822 float d2 = v3_dot( cj->co, ci->n ),
823 d = d2-d1;
824
825 if( fabsf( d ) <= w )
826 {
827 cj->type = k_contact_type_disabled;
828 }
829 }
830 }
831 }
832
833 /*
834 * -----------------------------------------------------------------------------
835 * Collision matrix
836 * -----------------------------------------------------------------------------
837 */
838
839 /*
840 * Contact generators
841 *
842 * These do not automatically allocate contacts, an appropriately sized
843 * buffer must be supplied. The function returns the size of the manifold
844 * which was generated.
845 *
846 * The values set on the contacts are: n, co, p, rba, rbb
847 */
848
849 /*
850 * By collecting the minimum(time) and maximum(time) pairs of points, we
851 * build a reduced and stable exact manifold.
852 *
853 * tx: time at point
854 * rx: minimum distance of these points
855 * dx: the delta between the two points
856 *
857 * pairs will only ammend these if they are creating a collision
858 */
859 typedef struct capsule_manifold capsule_manifold;
860 struct capsule_manifold
861 {
862 float t0, t1;
863 float r0, r1;
864 v3f d0, d1;
865 };
866
867 /*
868 * Expand a line manifold with a new pair. t value is the time along segment
869 * on the oriented object which created this pair.
870 */
871 VG_STATIC void rb_capsule_manifold( v3f pa, v3f pb, float t, float r,
872 capsule_manifold *manifold )
873 {
874 v3f delta;
875 v3_sub( pa, pb, delta );
876
877 if( v3_length2(delta) < r*r )
878 {
879 if( t < manifold->t0 )
880 {
881 v3_copy( delta, manifold->d0 );
882 manifold->t0 = t;
883 manifold->r0 = r;
884 }
885
886 if( t > manifold->t1 )
887 {
888 v3_copy( delta, manifold->d1 );
889 manifold->t1 = t;
890 manifold->r1 = r;
891 }
892 }
893 }
894
895 VG_STATIC void rb_capsule_manifold_init( capsule_manifold *manifold )
896 {
897 manifold->t0 = INFINITY;
898 manifold->t1 = -INFINITY;
899 }
900
901 __attribute__ ((deprecated))
902 VG_STATIC int rb_capsule_manifold_done( rigidbody *rba, rigidbody *rbb,
903 capsule_manifold *manifold, rb_ct *buf )
904 {
905 float h = rba->inf.capsule.height,
906 ra = rba->inf.capsule.radius;
907
908 v3f p0, p1;
909 v3_muladds( rba->co, rba->up, -h*0.5f+ra, p0 );
910 v3_muladds( rba->co, rba->up, h*0.5f-ra, p1 );
911
912 int count = 0;
913 if( manifold->t0 <= 1.0f )
914 {
915 rb_ct *ct = buf;
916
917 v3f pa;
918 v3_muls( p0, 1.0f-manifold->t0, pa );
919 v3_muladds( pa, p1, manifold->t0, pa );
920
921 float d = v3_length( manifold->d0 );
922 v3_muls( manifold->d0, 1.0f/d, ct->n );
923 v3_muladds( pa, ct->n, -ra, ct->co );
924
925 ct->p = manifold->r0 - d;
926 ct->rba = rba;
927 ct->rbb = rbb;
928 ct->type = k_contact_type_default;
929
930 count ++;
931 }
932
933 if( (manifold->t1 >= 0.0f) && (manifold->t0 != manifold->t1) )
934 {
935 rb_ct *ct = buf+count;
936
937 v3f pa;
938 v3_muls( p0, 1.0f-manifold->t1, pa );
939 v3_muladds( pa, p1, manifold->t1, pa );
940
941 float d = v3_length( manifold->d1 );
942 v3_muls( manifold->d1, 1.0f/d, ct->n );
943 v3_muladds( pa, ct->n, -ra, ct->co );
944
945 ct->p = manifold->r1 - d;
946 ct->rba = rba;
947 ct->rbb = rbb;
948 ct->type = k_contact_type_default;
949
950 count ++;
951 }
952
953 /*
954 * Debugging
955 */
956
957 if( count == 2 )
958 vg_line( buf[0].co, buf[1].co, 0xff0000ff );
959
960 return count;
961 }
962
963 VG_STATIC int rb_capsule__manifold_done( m4x3f mtx, rb_capsule *c,
964 capsule_manifold *manifold,
965 rb_ct *buf )
966 {
967 v3f p0, p1;
968 v3_muladds( mtx[3], mtx[1], -c->height*0.5f+c->radius, p0 );
969 v3_muladds( mtx[3], mtx[1], c->height*0.5f-c->radius, p1 );
970
971 int count = 0;
972 if( manifold->t0 <= 1.0f )
973 {
974 rb_ct *ct = buf;
975
976 v3f pa;
977 v3_muls( p0, 1.0f-manifold->t0, pa );
978 v3_muladds( pa, p1, manifold->t0, pa );
979
980 float d = v3_length( manifold->d0 );
981 v3_muls( manifold->d0, 1.0f/d, ct->n );
982 v3_muladds( pa, ct->n, -c->radius, ct->co );
983
984 ct->p = manifold->r0 - d;
985 ct->type = k_contact_type_default;
986 count ++;
987 }
988
989 if( (manifold->t1 >= 0.0f) && (manifold->t0 != manifold->t1) )
990 {
991 rb_ct *ct = buf+count;
992
993 v3f pa;
994 v3_muls( p0, 1.0f-manifold->t1, pa );
995 v3_muladds( pa, p1, manifold->t1, pa );
996
997 float d = v3_length( manifold->d1 );
998 v3_muls( manifold->d1, 1.0f/d, ct->n );
999 v3_muladds( pa, ct->n, -c->radius, ct->co );
1000
1001 ct->p = manifold->r1 - d;
1002 ct->type = k_contact_type_default;
1003
1004 count ++;
1005 }
1006
1007 /*
1008 * Debugging
1009 */
1010
1011 if( count == 2 )
1012 vg_line( buf[0].co, buf[1].co, 0xff0000ff );
1013
1014 return count;
1015 }
1016
1017 VG_STATIC int rb_capsule_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1018 {
1019 float h = rba->inf.capsule.height,
1020 ra = rba->inf.capsule.radius,
1021 rb = rbb->inf.sphere.radius;
1022
1023 v3f p0, p1;
1024 v3_muladds( rba->co, rba->up, -h*0.5f+ra, p0 );
1025 v3_muladds( rba->co, rba->up, h*0.5f-ra, p1 );
1026
1027 v3f c, delta;
1028 closest_point_segment( p0, p1, rbb->co, c );
1029 v3_sub( c, rbb->co, delta );
1030
1031 float d2 = v3_length2(delta),
1032 r = ra + rb;
1033
1034 if( d2 < r*r )
1035 {
1036 float d = sqrtf(d2);
1037
1038 rb_ct *ct = buf;
1039 v3_muls( delta, 1.0f/d, ct->n );
1040 ct->p = r-d;
1041
1042 v3f p0, p1;
1043 v3_muladds( c, ct->n, -ra, p0 );
1044 v3_muladds( rbb->co, ct->n, rb, p1 );
1045 v3_add( p0, p1, ct->co );
1046 v3_muls( ct->co, 0.5f, ct->co );
1047
1048 ct->rba = rba;
1049 ct->rbb = rbb;
1050 ct->type = k_contact_type_default;
1051
1052 return 1;
1053 }
1054
1055 return 0;
1056 }
1057
1058 VG_STATIC int rb_capsule_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1059 {
1060 if( !box_overlap( rba->bbx_world, rbb->bbx_world ) )
1061 return 0;
1062
1063 float ha = rba->inf.capsule.height,
1064 hb = rbb->inf.capsule.height,
1065 ra = rba->inf.capsule.radius,
1066 rb = rbb->inf.capsule.radius,
1067 r = ra+rb;
1068
1069 v3f p0, p1, p2, p3;
1070 v3_muladds( rba->co, rba->up, -ha*0.5f+ra, p0 );
1071 v3_muladds( rba->co, rba->up, ha*0.5f-ra, p1 );
1072 v3_muladds( rbb->co, rbb->up, -hb*0.5f+rb, p2 );
1073 v3_muladds( rbb->co, rbb->up, hb*0.5f-rb, p3 );
1074
1075 capsule_manifold manifold;
1076 rb_capsule_manifold_init( &manifold );
1077
1078 v3f pa, pb;
1079 float ta, tb;
1080 closest_segment_segment( p0, p1, p2, p3, &ta, &tb, pa, pb );
1081 rb_capsule_manifold( pa, pb, ta, r, &manifold );
1082
1083 ta = closest_point_segment( p0, p1, p2, pa );
1084 tb = closest_point_segment( p0, p1, p3, pb );
1085 rb_capsule_manifold( pa, p2, ta, r, &manifold );
1086 rb_capsule_manifold( pb, p3, tb, r, &manifold );
1087
1088 closest_point_segment( p2, p3, p0, pa );
1089 closest_point_segment( p2, p3, p1, pb );
1090 rb_capsule_manifold( p0, pa, 0.0f, r, &manifold );
1091 rb_capsule_manifold( p1, pb, 1.0f, r, &manifold );
1092
1093 return rb_capsule_manifold_done( rba, rbb, &manifold, buf );
1094 }
1095
1096 /*
1097 * Generates up to two contacts; optimised for the most stable manifold
1098 */
1099 VG_STATIC int rb_capsule_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1100 {
1101 float h = rba->inf.capsule.height,
1102 r = rba->inf.capsule.radius;
1103
1104 /*
1105 * Solving this in symetric local space of the cube saves us some time and a
1106 * couple branches when it comes to the quad stage.
1107 */
1108 v3f centroid;
1109 v3_add( rbb->bbx[0], rbb->bbx[1], centroid );
1110 v3_muls( centroid, 0.5f, centroid );
1111
1112 boxf bbx;
1113 v3_sub( rbb->bbx[0], centroid, bbx[0] );
1114 v3_sub( rbb->bbx[1], centroid, bbx[1] );
1115
1116 v3f pc, p0w, p1w, p0, p1;
1117 v3_muladds( rba->co, rba->up, -h*0.5f+r, p0w );
1118 v3_muladds( rba->co, rba->up, h*0.5f-r, p1w );
1119
1120 m4x3_mulv( rbb->to_local, p0w, p0 );
1121 m4x3_mulv( rbb->to_local, p1w, p1 );
1122 v3_sub( p0, centroid, p0 );
1123 v3_sub( p1, centroid, p1 );
1124 v3_add( p0, p1, pc );
1125 v3_muls( pc, 0.5f, pc );
1126
1127 /*
1128 * Finding an appropriate quad to collide lines with
1129 */
1130 v3f region;
1131 v3_div( pc, bbx[1], region );
1132
1133 v3f quad[4];
1134 if( (fabsf(region[0]) > fabsf(region[1])) &&
1135 (fabsf(region[0]) > fabsf(region[2])) )
1136 {
1137 float px = vg_signf(region[0]) * bbx[1][0];
1138 v3_copy( (v3f){ px, bbx[0][1], bbx[0][2] }, quad[0] );
1139 v3_copy( (v3f){ px, bbx[1][1], bbx[0][2] }, quad[1] );
1140 v3_copy( (v3f){ px, bbx[1][1], bbx[1][2] }, quad[2] );
1141 v3_copy( (v3f){ px, bbx[0][1], bbx[1][2] }, quad[3] );
1142 }
1143 else if( fabsf(region[1]) > fabsf(region[2]) )
1144 {
1145 float py = vg_signf(region[1]) * bbx[1][1];
1146 v3_copy( (v3f){ bbx[0][0], py, bbx[0][2] }, quad[0] );
1147 v3_copy( (v3f){ bbx[1][0], py, bbx[0][2] }, quad[1] );
1148 v3_copy( (v3f){ bbx[1][0], py, bbx[1][2] }, quad[2] );
1149 v3_copy( (v3f){ bbx[0][0], py, bbx[1][2] }, quad[3] );
1150 }
1151 else
1152 {
1153 float pz = vg_signf(region[2]) * bbx[1][2];
1154 v3_copy( (v3f){ bbx[0][0], bbx[0][1], pz }, quad[0] );
1155 v3_copy( (v3f){ bbx[1][0], bbx[0][1], pz }, quad[1] );
1156 v3_copy( (v3f){ bbx[1][0], bbx[1][1], pz }, quad[2] );
1157 v3_copy( (v3f){ bbx[0][0], bbx[1][1], pz }, quad[3] );
1158 }
1159
1160 capsule_manifold manifold;
1161 rb_capsule_manifold_init( &manifold );
1162
1163 v3f c0, c1;
1164 closest_point_aabb( p0, bbx, c0 );
1165 closest_point_aabb( p1, bbx, c1 );
1166
1167 v3f d0, d1, da;
1168 v3_sub( c0, p0, d0 );
1169 v3_sub( c1, p1, d1 );
1170 v3_sub( p1, p0, da );
1171
1172 v3_normalize(d0);
1173 v3_normalize(d1);
1174 v3_normalize(da);
1175
1176 if( v3_dot( da, d0 ) <= 0.01f )
1177 rb_capsule_manifold( p0, c0, 0.0f, r, &manifold );
1178
1179 if( v3_dot( da, d1 ) >= -0.01f )
1180 rb_capsule_manifold( p1, c1, 1.0f, r, &manifold );
1181
1182 for( int i=0; i<4; i++ )
1183 {
1184 int i0 = i,
1185 i1 = (i+1)%4;
1186
1187 v3f ca, cb;
1188 float ta, tb;
1189 closest_segment_segment( p0, p1, quad[i0], quad[i1], &ta, &tb, ca, cb );
1190 rb_capsule_manifold( ca, cb, ta, r, &manifold );
1191 }
1192
1193 /*
1194 * Create final contacts based on line manifold
1195 */
1196 m3x3_mulv( rbb->to_world, manifold.d0, manifold.d0 );
1197 m3x3_mulv( rbb->to_world, manifold.d1, manifold.d1 );
1198
1199 /*
1200 * Debugging
1201 */
1202
1203 #if 0
1204 for( int i=0; i<4; i++ )
1205 {
1206 v3f q0, q1;
1207 int i0 = i,
1208 i1 = (i+1)%4;
1209
1210 v3_add( quad[i0], centroid, q0 );
1211 v3_add( quad[i1], centroid, q1 );
1212
1213 m4x3_mulv( rbb->to_world, q0, q0 );
1214 m4x3_mulv( rbb->to_world, q1, q1 );
1215
1216 vg_line( q0, q1, 0xffffffff );
1217 }
1218 #endif
1219
1220 return rb_capsule_manifold_done( rba, rbb, &manifold, buf );
1221 }
1222
1223 VG_STATIC int rb_sphere_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1224 {
1225 v3f co, delta;
1226
1227 closest_point_obb( rba->co, rbb->bbx, rbb->to_world, rbb->to_local, co );
1228 v3_sub( rba->co, co, delta );
1229
1230 float d2 = v3_length2(delta),
1231 r = rba->inf.sphere.radius;
1232
1233 if( d2 <= r*r )
1234 {
1235 float d;
1236
1237 rb_ct *ct = buf;
1238 if( d2 <= 0.0001f )
1239 {
1240 v3_sub( rba->co, rbb->co, delta );
1241
1242 /*
1243 * some extra testing is required to find the best axis to push the
1244 * object back outside the box. Since there isnt a clear seperating
1245 * vector already, especially on really high aspect boxes.
1246 */
1247 float lx = v3_dot( rbb->right, delta ),
1248 ly = v3_dot( rbb->up, delta ),
1249 lz = v3_dot( rbb->forward, delta ),
1250 px = rbb->bbx[1][0] - fabsf(lx),
1251 py = rbb->bbx[1][1] - fabsf(ly),
1252 pz = rbb->bbx[1][2] - fabsf(lz);
1253
1254 if( px < py && px < pz )
1255 v3_muls( rbb->right, vg_signf(lx), ct->n );
1256 else if( py < pz )
1257 v3_muls( rbb->up, vg_signf(ly), ct->n );
1258 else
1259 v3_muls( rbb->forward, vg_signf(lz), ct->n );
1260
1261 v3_muladds( rba->co, ct->n, -r, ct->co );
1262 ct->p = r;
1263 }
1264 else
1265 {
1266 d = sqrtf(d2);
1267 v3_muls( delta, 1.0f/d, ct->n );
1268 ct->p = r-d;
1269 v3_copy( co, ct->co );
1270 }
1271
1272 ct->rba = rba;
1273 ct->rbb = rbb;
1274 ct->type = k_contact_type_default;
1275 return 1;
1276 }
1277
1278 return 0;
1279 }
1280
1281 VG_STATIC int rb_sphere_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1282 {
1283 v3f delta;
1284 v3_sub( rba->co, rbb->co, delta );
1285
1286 float d2 = v3_length2(delta),
1287 r = rba->inf.sphere.radius + rbb->inf.sphere.radius;
1288
1289 if( d2 < r*r )
1290 {
1291 float d = sqrtf(d2);
1292
1293 rb_ct *ct = buf;
1294 v3_muls( delta, 1.0f/d, ct->n );
1295
1296 v3f p0, p1;
1297 v3_muladds( rba->co, ct->n,-rba->inf.sphere.radius, p0 );
1298 v3_muladds( rbb->co, ct->n, rbb->inf.sphere.radius, p1 );
1299 v3_add( p0, p1, ct->co );
1300 v3_muls( ct->co, 0.5f, ct->co );
1301 ct->type = k_contact_type_default;
1302 ct->p = r-d;
1303 ct->rba = rba;
1304 ct->rbb = rbb;
1305 return 1;
1306 }
1307
1308 return 0;
1309 }
1310
1311 //#define RIGIDBODY_DYNAMIC_MESH_EDGES
1312
1313 __attribute__ ((deprecated))
1314 VG_STATIC int rb_sphere_triangle( rigidbody *rba, rigidbody *rbb,
1315 v3f tri[3], rb_ct *buf )
1316 {
1317 v3f delta, co;
1318
1319 #ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
1320 closest_on_triangle_1( rba->co, tri, co );
1321 #else
1322 enum contact_type type = closest_on_triangle_1( rba->co, tri, co );
1323 #endif
1324
1325 v3_sub( rba->co, co, delta );
1326
1327 float d2 = v3_length2( delta ),
1328 r = rba->inf.sphere.radius;
1329
1330 if( d2 < r*r )
1331 {
1332 rb_ct *ct = buf;
1333
1334 v3f ab, ac, tn;
1335 v3_sub( tri[2], tri[0], ab );
1336 v3_sub( tri[1], tri[0], ac );
1337 v3_cross( ac, ab, tn );
1338 v3_copy( tn, ct->n );
1339
1340 if( v3_length2( ct->n ) <= 0.00001f )
1341 {
1342 vg_error( "Zero area triangle!\n" );
1343 return 0;
1344 }
1345
1346 v3_normalize( ct->n );
1347
1348 float d = sqrtf(d2);
1349
1350 v3_copy( co, ct->co );
1351 ct->type = type;
1352 ct->p = r-d;
1353 ct->rba = rba;
1354 ct->rbb = rbb;
1355 return 1;
1356 }
1357
1358 return 0;
1359 }
1360
1361 VG_STATIC int rb_sphere__triangle( m4x3f mtxA, rb_sphere *b,
1362 v3f tri[3], rb_ct *buf )
1363 {
1364 v3f delta, co;
1365 enum contact_type type = closest_on_triangle_1( mtxA[3], tri, co );
1366
1367 v3_sub( mtxA[3], co, delta );
1368
1369 float d2 = v3_length2( delta ),
1370 r = b->radius;
1371
1372 if( d2 <= r*r )
1373 {
1374 rb_ct *ct = buf;
1375
1376 v3f ab, ac, tn;
1377 v3_sub( tri[2], tri[0], ab );
1378 v3_sub( tri[1], tri[0], ac );
1379 v3_cross( ac, ab, tn );
1380 v3_copy( tn, ct->n );
1381
1382 if( v3_length2( ct->n ) <= 0.00001f )
1383 {
1384 vg_error( "Zero area triangle!\n" );
1385 return 0;
1386 }
1387
1388 v3_normalize( ct->n );
1389
1390 float d = sqrtf(d2);
1391
1392 v3_copy( co, ct->co );
1393 ct->type = type;
1394 ct->p = r-d;
1395 return 1;
1396 }
1397
1398 return 0;
1399 }
1400
1401 VG_STATIC void rb_debug_sharp_scene_edges( rigidbody *rbb, float sharp_ang,
1402 boxf box, u32 colour )
1403 {
1404 sharp_ang = cosf( sharp_ang );
1405
1406 scene *sc = rbb->inf.scene.bh_scene->user;
1407 vg_line_boxf( box, 0xff00ff00 );
1408
1409 bh_iter it;
1410 bh_iter_init( 0, &it );
1411 int idx;
1412
1413 while( bh_next( rbb->inf.scene.bh_scene, &it, box, &idx ) )
1414 {
1415 u32 *ptri = &sc->arrindices[ idx*3 ];
1416 v3f tri[3];
1417
1418 for( int j=0; j<3; j++ )
1419 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1420
1421 for( int j=0; j<3; j++ )
1422 {
1423 #if 0
1424 v3f edir;
1425 v3_sub( tri[(j+1)%3], tri[j], edir );
1426
1427 if( v3_dot( edir, (v3f){ 0.5184758473652127f,
1428 0.2073903389460850f,
1429 -0.8295613557843402f } ) < 0.0f )
1430 continue;
1431 #endif
1432
1433 bh_iter jt;
1434 bh_iter_init( 0, &jt );
1435
1436 boxf region;
1437 float const k_r = 0.02f;
1438 v3_add( (v3f){ k_r, k_r, k_r }, tri[j], region[1] );
1439 v3_add( (v3f){ -k_r, -k_r, -k_r }, tri[j], region[0] );
1440
1441 int jdx;
1442 while( bh_next( rbb->inf.scene.bh_scene, &jt, region, &jdx ) )
1443 {
1444 if( idx <= jdx )
1445 continue;
1446
1447 u32 *ptrj = &sc->arrindices[ jdx*3 ];
1448 v3f trj[3];
1449
1450 for( int k=0; k<3; k++ )
1451 v3_copy( sc->arrvertices[ptrj[k]].co, trj[k] );
1452
1453 for( int k=0; k<3; k++ )
1454 {
1455 if( v3_dist2( tri[j], trj[k] ) <= k_r*k_r )
1456 {
1457 int jp1 = (j+1)%3,
1458 jp2 = (j+2)%3,
1459 km1 = (k+3-1)%3,
1460 km2 = (k+3-2)%3;
1461
1462 if( v3_dist2( tri[jp1], trj[km1] ) <= k_r*k_r )
1463 {
1464 v3f b0, b1, b2;
1465 v3_sub( tri[jp1], tri[j], b0 );
1466 v3_sub( tri[jp2], tri[j], b1 );
1467 v3_sub( trj[km2], tri[j], b2 );
1468
1469 v3f cx0, cx1;
1470 v3_cross( b0, b1, cx0 );
1471 v3_cross( b2, b0, cx1 );
1472
1473 float polarity = v3_dot( cx0, b2 );
1474
1475 if( polarity < 0.0f )
1476 {
1477 #if 0
1478 vg_line( tri[j], tri[jp1], 0xff00ff00 );
1479 float ang = v3_dot(cx0,cx1) /
1480 (v3_length(cx0)*v3_length(cx1));
1481 if( ang < sharp_ang )
1482 {
1483 vg_line( tri[j], tri[jp1], 0xff00ff00 );
1484 }
1485 #endif
1486 }
1487 }
1488 }
1489 }
1490 }
1491 }
1492 }
1493 }
1494
1495 VG_STATIC int rb_sphere__scene( m4x3f mtxA, rb_sphere *b,
1496 m4x3f mtxB, rb_scene *s, rb_ct *buf )
1497 {
1498 scene *sc = s->bh_scene->user;
1499
1500 bh_iter it;
1501 bh_iter_init( 0, &it );
1502 int idx;
1503
1504 int count = 0;
1505
1506 float r = b->radius + 0.1f;
1507 boxf box;
1508 v3_sub( mtxA[3], (v3f){ r,r,r }, box[0] );
1509 v3_add( mtxA[3], (v3f){ r,r,r }, box[1] );
1510
1511 while( bh_next( s->bh_scene, &it, box, &idx ) )
1512 {
1513 u32 *ptri = &sc->arrindices[ idx*3 ];
1514 v3f tri[3];
1515
1516 for( int j=0; j<3; j++ )
1517 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1518
1519 buf[ count ].element_id = ptri[0];
1520
1521 vg_line( tri[0],tri[1],0x70ff6000 );
1522 vg_line( tri[1],tri[2],0x70ff6000 );
1523 vg_line( tri[2],tri[0],0x70ff6000 );
1524
1525 int contact = rb_sphere__triangle( mtxA, b, tri, &buf[count] );
1526 count += contact;
1527
1528 if( count == 16 )
1529 {
1530 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
1531 return count;
1532 }
1533 }
1534
1535 return count;
1536 }
1537
1538 __attribute__ ((deprecated))
1539 VG_STATIC int rb_sphere_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1540 {
1541 scene *sc = rbb->inf.scene.bh_scene->user;
1542
1543 bh_iter it;
1544 bh_iter_init( 0, &it );
1545 int idx;
1546
1547 int count = 0;
1548
1549 while( bh_next( rbb->inf.scene.bh_scene, &it, rba->bbx_world, &idx ) )
1550 {
1551 u32 *ptri = &sc->arrindices[ idx*3 ];
1552 v3f tri[3];
1553
1554 for( int j=0; j<3; j++ )
1555 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1556
1557 buf[ count ].element_id = ptri[0];
1558
1559 vg_line( tri[0],tri[1],0x70ff6000 );
1560 vg_line( tri[1],tri[2],0x70ff6000 );
1561 vg_line( tri[2],tri[0],0x70ff6000 );
1562
1563 int contact = rb_sphere_triangle( rba, rbb, tri, buf+count );
1564 count += contact;
1565
1566 if( count == 16 )
1567 {
1568 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
1569 return count;
1570 }
1571 }
1572
1573 return count;
1574 }
1575
1576 VG_STATIC int rb_box_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1577 {
1578 scene *sc = rbb->inf.scene.bh_scene->user;
1579
1580 v3f tri[3];
1581
1582 bh_iter it;
1583 bh_iter_init( 0, &it );
1584 int idx;
1585
1586 int count = 0;
1587
1588 while( bh_next( rbb->inf.scene.bh_scene, &it, rba->bbx_world, &idx ) )
1589 {
1590 u32 *ptri = &sc->arrindices[ idx*3 ];
1591
1592 for( int j=0; j<3; j++ )
1593 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1594
1595 if( rb_box_triangle_sat( rba, tri ) )
1596 {
1597 vg_line(tri[0],tri[1],0xff50ff00 );
1598 vg_line(tri[1],tri[2],0xff50ff00 );
1599 vg_line(tri[2],tri[0],0xff50ff00 );
1600 }
1601 else
1602 {
1603 vg_line(tri[0],tri[1],0xff0000ff );
1604 vg_line(tri[1],tri[2],0xff0000ff );
1605 vg_line(tri[2],tri[0],0xff0000ff );
1606
1607 continue;
1608 }
1609
1610 v3f v0,v1,n;
1611 v3_sub( tri[1], tri[0], v0 );
1612 v3_sub( tri[2], tri[0], v1 );
1613 v3_cross( v0, v1, n );
1614 v3_normalize( n );
1615
1616 /* find best feature */
1617 float best = v3_dot( rba->right, n );
1618 int axis = 0;
1619
1620 float cy = v3_dot( rba->up, n );
1621 if( fabsf(cy) > fabsf(best) )
1622 {
1623 best = cy;
1624 axis = 1;
1625 }
1626
1627 float cz = -v3_dot( rba->forward, n );
1628 if( fabsf(cz) > fabsf(best) )
1629 {
1630 best = cz;
1631 axis = 2;
1632 }
1633
1634 v3f manifold[4];
1635
1636 if( axis == 0 )
1637 {
1638 float px = best > 0.0f? rba->bbx[0][0]: rba->bbx[1][0];
1639 manifold[0][0] = px;
1640 manifold[0][1] = rba->bbx[0][1];
1641 manifold[0][2] = rba->bbx[0][2];
1642 manifold[1][0] = px;
1643 manifold[1][1] = rba->bbx[1][1];
1644 manifold[1][2] = rba->bbx[0][2];
1645 manifold[2][0] = px;
1646 manifold[2][1] = rba->bbx[1][1];
1647 manifold[2][2] = rba->bbx[1][2];
1648 manifold[3][0] = px;
1649 manifold[3][1] = rba->bbx[0][1];
1650 manifold[3][2] = rba->bbx[1][2];
1651 }
1652 else if( axis == 1 )
1653 {
1654 float py = best > 0.0f? rba->bbx[0][1]: rba->bbx[1][1];
1655 manifold[0][0] = rba->bbx[0][0];
1656 manifold[0][1] = py;
1657 manifold[0][2] = rba->bbx[0][2];
1658 manifold[1][0] = rba->bbx[1][0];
1659 manifold[1][1] = py;
1660 manifold[1][2] = rba->bbx[0][2];
1661 manifold[2][0] = rba->bbx[1][0];
1662 manifold[2][1] = py;
1663 manifold[2][2] = rba->bbx[1][2];
1664 manifold[3][0] = rba->bbx[0][0];
1665 manifold[3][1] = py;
1666 manifold[3][2] = rba->bbx[1][2];
1667 }
1668 else
1669 {
1670 float pz = best > 0.0f? rba->bbx[0][2]: rba->bbx[1][2];
1671 manifold[0][0] = rba->bbx[0][0];
1672 manifold[0][1] = rba->bbx[0][1];
1673 manifold[0][2] = pz;
1674 manifold[1][0] = rba->bbx[1][0];
1675 manifold[1][1] = rba->bbx[0][1];
1676 manifold[1][2] = pz;
1677 manifold[2][0] = rba->bbx[1][0];
1678 manifold[2][1] = rba->bbx[1][1];
1679 manifold[2][2] = pz;
1680 manifold[3][0] = rba->bbx[0][0];
1681 manifold[3][1] = rba->bbx[1][1];
1682 manifold[3][2] = pz;
1683 }
1684
1685 for( int j=0; j<4; j++ )
1686 m4x3_mulv( rba->to_world, manifold[j], manifold[j] );
1687
1688 vg_line( manifold[0], manifold[1], 0xffffffff );
1689 vg_line( manifold[1], manifold[2], 0xffffffff );
1690 vg_line( manifold[2], manifold[3], 0xffffffff );
1691 vg_line( manifold[3], manifold[0], 0xffffffff );
1692
1693 for( int j=0; j<4; j++ )
1694 {
1695 rb_ct *ct = buf+count;
1696
1697 v3_copy( manifold[j], ct->co );
1698 v3_copy( n, ct->n );
1699
1700 float l0 = v3_dot( tri[0], n ),
1701 l1 = v3_dot( manifold[j], n );
1702
1703 ct->p = (l0-l1)*0.5f;
1704 if( ct->p < 0.0f )
1705 continue;
1706
1707 ct->type = k_contact_type_default;
1708 ct->rba = rba;
1709 ct->rbb = rbb;
1710 count ++;
1711
1712 if( count >= 12 )
1713 return count;
1714 }
1715 }
1716 return count;
1717 }
1718
1719 VG_STATIC int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c,
1720 v3f tri[3], rb_ct *buf )
1721 {
1722 v3f pc, p0w, p1w;
1723 v3_muladds( mtxA[3], mtxA[1], -c->height*0.5f+c->radius, p0w );
1724 v3_muladds( mtxA[3], mtxA[1], c->height*0.5f-c->radius, p1w );
1725
1726 capsule_manifold manifold;
1727 rb_capsule_manifold_init( &manifold );
1728
1729 v3f c0, c1;
1730 closest_on_triangle_1( p0w, tri, c0 );
1731 closest_on_triangle_1( p1w, tri, c1 );
1732
1733 v3f d0, d1, da;
1734 v3_sub( c0, p0w, d0 );
1735 v3_sub( c1, p1w, d1 );
1736 v3_sub( p1w, p0w, da );
1737
1738 v3_normalize(d0);
1739 v3_normalize(d1);
1740 v3_normalize(da);
1741
1742 if( v3_dot( da, d0 ) <= 0.01f )
1743 rb_capsule_manifold( p0w, c0, 0.0f, c->radius, &manifold );
1744
1745 if( v3_dot( da, d1 ) >= -0.01f )
1746 rb_capsule_manifold( p1w, c1, 1.0f, c->radius, &manifold );
1747
1748 for( int i=0; i<3; i++ )
1749 {
1750 int i0 = i,
1751 i1 = (i+1)%3;
1752
1753 v3f ca, cb;
1754 float ta, tb;
1755 closest_segment_segment( p0w, p1w, tri[i0], tri[i1], &ta, &tb, ca, cb );
1756 rb_capsule_manifold( ca, cb, ta, c->radius, &manifold );
1757 }
1758
1759 v3f v0, v1, n;
1760 v3_sub( tri[1], tri[0], v0 );
1761 v3_sub( tri[2], tri[0], v1 );
1762 v3_cross( v0, v1, n );
1763 v3_normalize( n );
1764
1765 int count = rb_capsule__manifold_done( mtxA, c, &manifold, buf );
1766 for( int i=0; i<count; i++ )
1767 v3_copy( n, buf[i].n );
1768
1769 return count;
1770 }
1771
1772 /*
1773 * Generates up to two contacts; optimised for the most stable manifold
1774 */
1775 __attribute__ ((deprecated))
1776 VG_STATIC int rb_capsule_triangle( rigidbody *rba, rigidbody *rbb,
1777 v3f tri[3], rb_ct *buf )
1778 {
1779 float h = rba->inf.capsule.height,
1780 r = rba->inf.capsule.radius;
1781
1782 v3f pc, p0w, p1w;
1783 v3_muladds( rba->co, rba->up, -h*0.5f+r, p0w );
1784 v3_muladds( rba->co, rba->up, h*0.5f-r, p1w );
1785
1786 capsule_manifold manifold;
1787 rb_capsule_manifold_init( &manifold );
1788
1789 v3f c0, c1;
1790 closest_on_triangle_1( p0w, tri, c0 );
1791 closest_on_triangle_1( p1w, tri, c1 );
1792
1793 v3f d0, d1, da;
1794 v3_sub( c0, p0w, d0 );
1795 v3_sub( c1, p1w, d1 );
1796 v3_sub( p1w, p0w, da );
1797
1798 v3_normalize(d0);
1799 v3_normalize(d1);
1800 v3_normalize(da);
1801
1802 if( v3_dot( da, d0 ) <= 0.01f )
1803 rb_capsule_manifold( p0w, c0, 0.0f, r, &manifold );
1804
1805 if( v3_dot( da, d1 ) >= -0.01f )
1806 rb_capsule_manifold( p1w, c1, 1.0f, r, &manifold );
1807
1808 for( int i=0; i<3; i++ )
1809 {
1810 int i0 = i,
1811 i1 = (i+1)%3;
1812
1813 v3f ca, cb;
1814 float ta, tb;
1815 closest_segment_segment( p0w, p1w, tri[i0], tri[i1], &ta, &tb, ca, cb );
1816 rb_capsule_manifold( ca, cb, ta, r, &manifold );
1817 }
1818
1819 v3f v0, v1, n;
1820 v3_sub( tri[1], tri[0], v0 );
1821 v3_sub( tri[2], tri[0], v1 );
1822 v3_cross( v0, v1, n );
1823 v3_normalize( n );
1824
1825 int count = rb_capsule_manifold_done( rba, rbb, &manifold, buf );
1826 for( int i=0; i<count; i++ )
1827 v3_copy( n, buf[i].n );
1828
1829 return count;
1830 }
1831
1832 /* mtxB is defined only for tradition; it is not used currently */
1833 VG_STATIC int rb_capsule__scene( m4x3f mtxA, rb_capsule *c,
1834 m4x3f mtxB, rb_scene *s,
1835 rb_ct *buf )
1836 {
1837 bh_iter it;
1838 bh_iter_init( 0, &it );
1839 int idx;
1840 int count = 0;
1841
1842 boxf bbx;
1843 v3_sub( mtxA[3], (v3f){ c->height, c->height, c->height }, bbx[0] );
1844 v3_add( mtxA[3], (v3f){ c->height, c->height, c->height }, bbx[1] );
1845
1846 scene *sc = s->bh_scene->user;
1847
1848 while( bh_next( s->bh_scene, &it, bbx, &idx ) )
1849 {
1850 u32 *ptri = &sc->arrindices[ idx*3 ];
1851 v3f tri[3];
1852
1853 for( int j=0; j<3; j++ )
1854 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1855
1856 buf[ count ].element_id = ptri[0];
1857
1858 int contact = rb_capsule__triangle( mtxA, c, tri, &buf[count] );
1859 count += contact;
1860
1861 if( count >= 16 )
1862 {
1863 vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
1864 return count;
1865 }
1866 }
1867
1868 return count;
1869 }
1870
1871 __attribute__ ((deprecated))
1872 VG_STATIC int rb_capsule_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1873 {
1874 scene *sc = rbb->inf.scene.bh_scene->user;
1875
1876 bh_iter it;
1877 bh_iter_init( 0, &it );
1878 int idx;
1879
1880 int count = 0;
1881
1882 while( bh_next( rbb->inf.scene.bh_scene, &it, rba->bbx_world, &idx ) )
1883 {
1884 u32 *ptri = &sc->arrindices[ idx*3 ];
1885 v3f tri[3];
1886
1887 for( int j=0; j<3; j++ )
1888 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1889
1890 buf[ count ].element_id = ptri[0];
1891
1892 #if 0
1893 vg_line( tri[0],tri[1],0x70ff6000 );
1894 vg_line( tri[1],tri[2],0x70ff6000 );
1895 vg_line( tri[2],tri[0],0x70ff6000 );
1896 #endif
1897
1898 int contact = rb_capsule_triangle( rba, rbb, tri, buf+count );
1899 count += contact;
1900
1901 if( count == 16 )
1902 {
1903 vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
1904 return count;
1905 }
1906 }
1907
1908 return count;
1909 }
1910
1911 VG_STATIC int rb_scene_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1912 {
1913 return rb_capsule_scene( rbb, rba, buf );
1914 }
1915
1916 VG_STATIC int RB_MATRIX_ERROR( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1917 {
1918 #if 0
1919 vg_error( "Collision type is unimplemented between types %d and %d\n",
1920 rba->type, rbb->type );
1921 #endif
1922
1923 return 0;
1924 }
1925
1926 VG_STATIC int rb_sphere_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1927 {
1928 return rb_capsule_sphere( rbb, rba, buf );
1929 }
1930
1931 VG_STATIC int rb_box_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1932 {
1933 return rb_capsule_box( rbb, rba, buf );
1934 }
1935
1936 VG_STATIC int rb_box_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1937 {
1938 return rb_sphere_box( rbb, rba, buf );
1939 }
1940
1941 VG_STATIC int rb_scene_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1942 {
1943 return rb_box_scene( rbb, rba, buf );
1944 }
1945
1946 VG_STATIC int (*rb_jump_table[4][4])( rigidbody *a, rigidbody *b, rb_ct *buf ) =
1947 {
1948 /* box */ /* Sphere */ /* Capsule */ /* Mesh */
1949 { RB_MATRIX_ERROR, rb_box_sphere, rb_box_capsule, rb_box_scene },
1950 { rb_sphere_box, rb_sphere_sphere, rb_sphere_capsule, rb_sphere_scene },
1951 { rb_capsule_box, rb_capsule_sphere, rb_capsule_capsule, rb_capsule_scene },
1952 { rb_scene_box, RB_MATRIX_ERROR, rb_scene_capsule, RB_MATRIX_ERROR }
1953 };
1954
1955 VG_STATIC int rb_collide( rigidbody *rba, rigidbody *rbb )
1956 {
1957 int (*collider_jump)(rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1958 = rb_jump_table[rba->type][rbb->type];
1959
1960 /*
1961 * 12 is the maximum manifold size we can generate, so we are forced to abort
1962 * potentially checking any more.
1963 */
1964 if( rb_contact_count + 12 > vg_list_size(rb_contact_buffer) )
1965 {
1966 vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
1967 rb_contact_count, vg_list_size(rb_contact_buffer) );
1968 return 0;
1969 }
1970
1971 /*
1972 * FUTURE: Replace this with a more dedicated broad phase pass
1973 */
1974 if( box_overlap( rba->bbx_world, rbb->bbx_world ) )
1975 {
1976 int count = collider_jump( rba, rbb, rb_contact_buffer+rb_contact_count);
1977 rb_contact_count += count;
1978 return count;
1979 }
1980 else
1981 return 0;
1982 }
1983
1984 /*
1985 * -----------------------------------------------------------------------------
1986 * Dynamics
1987 * -----------------------------------------------------------------------------
1988 */
1989
1990 VG_STATIC void rb_solver_reset(void)
1991 {
1992 rb_contact_count = 0;
1993 }
1994
1995 VG_STATIC rb_ct *rb_global_ct(void)
1996 {
1997 return rb_contact_buffer + rb_contact_count;
1998 }
1999
2000 VG_STATIC void rb_prepare_contact( rb_ct *ct, float timestep )
2001 {
2002 ct->bias = -0.2f * (timestep*3600.0f)
2003 * vg_minf( 0.0f, -ct->p+k_penetration_slop );
2004
2005 rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
2006 ct->norm_impulse = 0.0f;
2007 ct->tangent_impulse[0] = 0.0f;
2008 ct->tangent_impulse[1] = 0.0f;
2009 }
2010
2011 /* calculate total move. manifold should belong to ONE object only */
2012 VG_STATIC void rb_depenetrate( rb_ct *manifold, int len, v3f dt )
2013 {
2014 v3_zero( dt );
2015
2016 for( int j=0; j<7; j++ )
2017 {
2018 for( int i=0; i<len; i++ )
2019 {
2020 struct contact *ct = &manifold[i];
2021
2022 float resolved_amt = v3_dot( ct->n, dt ),
2023 remaining = (ct->p-k_penetration_slop) - resolved_amt,
2024 apply = vg_maxf( remaining, 0.0f ) * 0.4f;
2025
2026 v3_muladds( dt, ct->n, apply, dt );
2027 }
2028 }
2029 }
2030
2031 /*
2032 * Initializing things like tangent vectors
2033 */
2034 VG_STATIC void rb_presolve_contacts( rb_ct *buffer, int len )
2035 {
2036 for( int i=0; i<len; i++ )
2037 {
2038 rb_ct *ct = &buffer[i];
2039 rb_prepare_contact( ct, k_rb_delta );
2040
2041 v3f ra, rb, raCn, rbCn, raCt, rbCt;
2042 v3_sub( ct->co, ct->rba->co, ra );
2043 v3_sub( ct->co, ct->rbb->co, rb );
2044 v3_cross( ra, ct->n, raCn );
2045 v3_cross( rb, ct->n, rbCn );
2046
2047 /* orient inverse inertia tensors */
2048 v3f raCnI, rbCnI;
2049 m3x3_mulv( ct->rba->iIw, raCn, raCnI );
2050 m3x3_mulv( ct->rbb->iIw, rbCn, rbCnI );
2051
2052 ct->normal_mass = ct->rba->inv_mass + ct->rbb->inv_mass;
2053 ct->normal_mass += v3_dot( raCn, raCnI );
2054 ct->normal_mass += v3_dot( rbCn, rbCnI );
2055 ct->normal_mass = 1.0f/ct->normal_mass;
2056
2057 for( int j=0; j<2; j++ )
2058 {
2059 v3f raCtI, rbCtI;
2060 v3_cross( ct->t[j], ra, raCt );
2061 v3_cross( ct->t[j], rb, rbCt );
2062 m3x3_mulv( ct->rba->iIw, raCt, raCtI );
2063 m3x3_mulv( ct->rbb->iIw, rbCt, rbCtI );
2064
2065 ct->tangent_mass[j] = ct->rba->inv_mass + ct->rbb->inv_mass;
2066 ct->tangent_mass[j] += v3_dot( raCt, raCtI );
2067 ct->tangent_mass[j] += v3_dot( rbCt, rbCtI );
2068 ct->tangent_mass[j] = 1.0f/ct->tangent_mass[j];
2069 }
2070
2071 rb_debug_contact( ct );
2072 }
2073 }
2074
2075 /*
2076 * Creates relative contact velocity vector
2077 */
2078 VG_STATIC void rb_rcv( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb, v3f rv )
2079 {
2080 v3f rva, rvb;
2081 v3_cross( rba->w, ra, rva );
2082 v3_add( rba->v, rva, rva );
2083 v3_cross( rbb->w, rb, rvb );
2084 v3_add( rbb->v, rvb, rvb );
2085
2086 v3_sub( rva, rvb, rv );
2087 }
2088
2089 /*
2090 * Apply impulse to object
2091 */
2092 VG_STATIC void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse )
2093 {
2094 /* linear */
2095 v3_muladds( rb->v, impulse, rb->inv_mass, rb->v );
2096
2097 /* Angular velocity */
2098 v3f wa;
2099 v3_cross( delta, impulse, wa );
2100
2101 m3x3_mulv( rb->iIw, wa, wa );
2102 v3_add( rb->w, wa, rb->w );
2103 }
2104
2105 /*
2106 * One iteration to solve the contact constraint
2107 */
2108 VG_STATIC void rb_solve_contacts( rb_ct *buf, int len )
2109 {
2110 for( int i=0; i<len; i++ )
2111 {
2112 struct contact *ct = &buf[i];
2113
2114 v3f rv, ra, rb;
2115 v3_sub( ct->co, ct->rba->co, ra );
2116 v3_sub( ct->co, ct->rbb->co, rb );
2117 rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
2118
2119 /* Friction */
2120 for( int j=0; j<2; j++ )
2121 {
2122 float f = k_friction * ct->norm_impulse,
2123 vt = v3_dot( rv, ct->t[j] ),
2124 lambda = ct->tangent_mass[j] * -vt;
2125
2126 float temp = ct->tangent_impulse[j];
2127 ct->tangent_impulse[j] = vg_clampf( temp + lambda, -f, f );
2128 lambda = ct->tangent_impulse[j] - temp;
2129
2130 v3f impulse;
2131 v3_muls( ct->t[j], lambda, impulse );
2132 rb_linear_impulse( ct->rba, ra, impulse );
2133
2134 v3_muls( ct->t[j], -lambda, impulse );
2135 rb_linear_impulse( ct->rbb, rb, impulse );
2136 }
2137
2138 /* Normal */
2139 rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
2140 float vn = v3_dot( rv, ct->n ),
2141 lambda = ct->normal_mass * (-vn + ct->bias);
2142
2143 float temp = ct->norm_impulse;
2144 ct->norm_impulse = vg_maxf( temp + lambda, 0.0f );
2145 lambda = ct->norm_impulse - temp;
2146
2147 v3f impulse;
2148 v3_muls( ct->n, lambda, impulse );
2149 rb_linear_impulse( ct->rba, ra, impulse );
2150
2151 v3_muls( ct->n, -lambda, impulse );
2152 rb_linear_impulse( ct->rbb, rb, impulse );
2153 }
2154 }
2155
2156 /*
2157 * -----------------------------------------------------------------------------
2158 * Constraints
2159 * -----------------------------------------------------------------------------
2160 */
2161
2162 VG_STATIC void rb_debug_position_constraints( rb_constr_pos *buffer, int len )
2163 {
2164 for( int i=0; i<len; i++ )
2165 {
2166 rb_constr_pos *constr = &buffer[i];
2167 rigidbody *rba = constr->rba, *rbb = constr->rbb;
2168
2169 v3f wca, wcb;
2170 m3x3_mulv( rba->to_world, constr->lca, wca );
2171 m3x3_mulv( rbb->to_world, constr->lcb, wcb );
2172
2173 v3f p0, p1;
2174 v3_add( wca, rba->co, p0 );
2175 v3_add( wcb, rbb->co, p1 );
2176 vg_line_pt3( p0, 0.0025f, 0xff000000 );
2177 vg_line_pt3( p1, 0.0025f, 0xffffffff );
2178 vg_line2( p0, p1, 0xff000000, 0xffffffff );
2179 }
2180 }
2181
2182 VG_STATIC void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf,
2183 int len )
2184 {
2185 float size = 0.12f;
2186
2187 for( int i=0; i<len; i++ )
2188 {
2189 rb_constr_swingtwist *st = &buf[ i ];
2190
2191 v3f vx, vy, va, vxb, axis, center;
2192
2193 m3x3_mulv( st->rba->to_world, st->conevx, vx );
2194 m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
2195 m3x3_mulv( st->rba->to_world, st->conevy, vy );
2196 m3x3_mulv( st->rbb->to_world, st->coneva, va );
2197 m4x3_mulv( st->rba->to_world, st->view_offset, center );
2198 v3_cross( vy, vx, axis );
2199
2200 /* Constraint violated ? */
2201 float fx = v3_dot( vx, va ), /* projection world */
2202 fy = v3_dot( vy, va ),
2203 fn = v3_dot( va, axis ),
2204
2205 rx = st->conevx[3], /* elipse radii */
2206 ry = st->conevy[3],
2207
2208 lx = fx/rx, /* projection local (fn==lz) */
2209 ly = fy/ry;
2210
2211 st->tangent_violation = ((lx*lx + ly*ly) > fn*fn) || (fn <= 0.0f);
2212 if( st->tangent_violation )
2213 {
2214 /* Calculate a good position and the axis to solve on */
2215 v2f closest, tangent,
2216 p = { fx/fabsf(fn), fy/fabsf(fn) };
2217
2218 closest_point_elipse( p, (v2f){rx,ry}, closest );
2219 tangent[0] = -closest[1] / (ry*ry);
2220 tangent[1] = closest[0] / (rx*rx);
2221 v2_normalize( tangent );
2222
2223 v3f v0, v1;
2224 v3_muladds( axis, vx, closest[0], v0 );
2225 v3_muladds( v0, vy, closest[1], v0 );
2226 v3_normalize( v0 );
2227
2228 v3_muls( vx, tangent[0], v1 );
2229 v3_muladds( v1, vy, tangent[1], v1 );
2230
2231 v3_copy( v0, st->tangent_target );
2232 v3_copy( v1, st->tangent_axis );
2233
2234 /* calculate mass */
2235 v3f aIw, bIw;
2236 m3x3_mulv( st->rba->iIw, st->tangent_axis, aIw );
2237 m3x3_mulv( st->rbb->iIw, st->tangent_axis, bIw );
2238 st->tangent_mass = 1.0f / (v3_dot( st->tangent_axis, aIw ) +
2239 v3_dot( st->tangent_axis, bIw ));
2240
2241 float angle = v3_dot( va, st->tangent_target );
2242 }
2243
2244 v3f refaxis;
2245 v3_cross( vy, va, refaxis ); /* our default rotation */
2246 v3_normalize( refaxis );
2247
2248 float angle = v3_dot( refaxis, vxb );
2249 st->axis_violation = fabsf(angle) < st->conet;
2250
2251 if( st->axis_violation )
2252 {
2253 v3f dir_test;
2254 v3_cross( refaxis, vxb, dir_test );
2255
2256 if( v3_dot(dir_test, va) < 0.0f )
2257 st->axis_violation = -st->axis_violation;
2258
2259 float newang = (float)st->axis_violation * acosf(st->conet-0.0001f);
2260
2261 v3f refaxis_up;
2262 v3_cross( va, refaxis, refaxis_up );
2263 v3_muls( refaxis_up, sinf(newang), st->axis_target );
2264 v3_muladds( st->axis_target, refaxis, -cosf(newang), st->axis_target );
2265
2266 /* calculate mass */
2267 v3_copy( va, st->axis );
2268 v3f aIw, bIw;
2269 m3x3_mulv( st->rba->iIw, st->axis, aIw );
2270 m3x3_mulv( st->rbb->iIw, st->axis, bIw );
2271 st->axis_mass = 1.0f / (v3_dot( st->axis, aIw ) +
2272 v3_dot( st->axis, bIw ));
2273 }
2274 }
2275 }
2276
2277 VG_STATIC void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf,
2278 int len )
2279 {
2280 float size = 0.12f;
2281
2282 for( int i=0; i<len; i++ )
2283 {
2284 rb_constr_swingtwist *st = &buf[ i ];
2285
2286 v3f vx, vxb, vy, va, axis, center;
2287
2288 m3x3_mulv( st->rba->to_world, st->conevx, vx );
2289 m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
2290 m3x3_mulv( st->rba->to_world, st->conevy, vy );
2291 m3x3_mulv( st->rbb->to_world, st->coneva, va );
2292 m4x3_mulv( st->rba->to_world, st->view_offset, center );
2293 v3_cross( vy, vx, axis );
2294
2295 float rx = st->conevx[3], /* elipse radii */
2296 ry = st->conevy[3];
2297
2298 v3f p0, p1;
2299 v3_muladds( center, va, size, p1 );
2300 vg_line( center, p1, 0xffffffff );
2301 vg_line_pt3( p1, 0.00025f, 0xffffffff );
2302
2303 if( st->tangent_violation )
2304 {
2305 v3_muladds( center, st->tangent_target, size, p0 );
2306
2307 vg_line( center, p0, 0xff00ff00 );
2308 vg_line_pt3( p0, 0.00025f, 0xff00ff00 );
2309 vg_line( p1, p0, 0xff000000 );
2310 }
2311
2312 for( int x=0; x<32; x++ )
2313 {
2314 float t0 = ((float)x * (1.0f/32.0f)) * VG_TAUf,
2315 t1 = (((float)x+1.0f) * (1.0f/32.0f)) * VG_TAUf,
2316 c0 = cosf( t0 ),
2317 s0 = sinf( t0 ),
2318 c1 = cosf( t1 ),
2319 s1 = sinf( t1 );
2320
2321 v3f v0, v1;
2322 v3_muladds( axis, vx, c0*rx, v0 );
2323 v3_muladds( v0, vy, s0*ry, v0 );
2324 v3_muladds( axis, vx, c1*rx, v1 );
2325 v3_muladds( v1, vy, s1*ry, v1 );
2326
2327 v3_normalize( v0 );
2328 v3_normalize( v1 );
2329
2330 v3_muladds( center, v0, size, p0 );
2331 v3_muladds( center, v1, size, p1 );
2332
2333 u32 col0r = fabsf(c0) * 255.0f,
2334 col0g = fabsf(s0) * 255.0f,
2335 col1r = fabsf(c1) * 255.0f,
2336 col1g = fabsf(s1) * 255.0f,
2337 col = st->tangent_violation? 0xff0000ff: 0xff000000,
2338 col0 = col | (col0r<<16) | (col0g << 8),
2339 col1 = col | (col1r<<16) | (col1g << 8);
2340
2341 vg_line2( center, p0, VG__NONE, col0 );
2342 vg_line2( p0, p1, col0, col1 );
2343 }
2344
2345 /* Draw twist */
2346 v3_muladds( center, va, size, p0 );
2347 v3_muladds( p0, vxb, size, p1 );
2348
2349 vg_line( p0, p1, 0xff0000ff );
2350
2351 if( st->axis_violation )
2352 {
2353 v3_muladds( p0, st->axis_target, size*1.25f, p1 );
2354 vg_line( p0, p1, 0xffffff00 );
2355 vg_line_pt3( p1, 0.0025f, 0xffffff80 );
2356 }
2357
2358 v3f refaxis;
2359 v3_cross( vy, va, refaxis ); /* our default rotation */
2360 v3_normalize( refaxis );
2361 v3f refaxis_up;
2362 v3_cross( va, refaxis, refaxis_up );
2363 float newang = acosf(st->conet-0.0001f);
2364
2365 v3_muladds( p0, refaxis_up, sinf(newang)*size, p1 );
2366 v3_muladds( p1, refaxis, -cosf(newang)*size, p1 );
2367 vg_line( p0, p1, 0xff000000 );
2368
2369 v3_muladds( p0, refaxis_up, sinf(-newang)*size, p1 );
2370 v3_muladds( p1, refaxis, -cosf(-newang)*size, p1 );
2371 vg_line( p0, p1, 0xff404040 );
2372 }
2373 }
2374
2375 /*
2376 * Solve a list of positional constraints
2377 */
2378 VG_STATIC void rb_solve_position_constraints( rb_constr_pos *buf, int len )
2379 {
2380 for( int i=0; i<len; i++ )
2381 {
2382 rb_constr_pos *constr = &buf[i];
2383 rigidbody *rba = constr->rba, *rbb = constr->rbb;
2384
2385 v3f wa, wb;
2386 m3x3_mulv( rba->to_world, constr->lca, wa );
2387 m3x3_mulv( rbb->to_world, constr->lcb, wb );
2388
2389 m3x3f ssra, ssrat, ssrb, ssrbt;
2390
2391 m3x3_skew_symetric( ssrat, wa );
2392 m3x3_skew_symetric( ssrbt, wb );
2393 m3x3_transpose( ssrat, ssra );
2394 m3x3_transpose( ssrbt, ssrb );
2395
2396 v3f b, b_wa, b_wb, b_a, b_b;
2397 m3x3_mulv( ssra, rba->w, b_wa );
2398 m3x3_mulv( ssrb, rbb->w, b_wb );
2399 v3_add( rba->v, b_wa, b );
2400 v3_sub( b, rbb->v, b );
2401 v3_sub( b, b_wb, b );
2402 v3_muls( b, -1.0f, b );
2403
2404 m3x3f invMa, invMb;
2405 m3x3_diagonal( invMa, rba->inv_mass );
2406 m3x3_diagonal( invMb, rbb->inv_mass );
2407
2408 m3x3f ia, ib;
2409 m3x3_mul( ssra, rba->iIw, ia );
2410 m3x3_mul( ia, ssrat, ia );
2411 m3x3_mul( ssrb, rbb->iIw, ib );
2412 m3x3_mul( ib, ssrbt, ib );
2413
2414 m3x3f cma, cmb;
2415 m3x3_add( invMa, ia, cma );
2416 m3x3_add( invMb, ib, cmb );
2417
2418 m3x3f A;
2419 m3x3_add( cma, cmb, A );
2420
2421 /* Solve Ax = b ( A^-1*b = x ) */
2422 v3f impulse;
2423 m3x3f invA;
2424 m3x3_inv( A, invA );
2425 m3x3_mulv( invA, b, impulse );
2426
2427 v3f delta_va, delta_wa, delta_vb, delta_wb;
2428 m3x3f iwa, iwb;
2429 m3x3_mul( rba->iIw, ssrat, iwa );
2430 m3x3_mul( rbb->iIw, ssrbt, iwb );
2431
2432 m3x3_mulv( invMa, impulse, delta_va );
2433 m3x3_mulv( invMb, impulse, delta_vb );
2434 m3x3_mulv( iwa, impulse, delta_wa );
2435 m3x3_mulv( iwb, impulse, delta_wb );
2436
2437 v3_add( rba->v, delta_va, rba->v );
2438 v3_add( rba->w, delta_wa, rba->w );
2439 v3_sub( rbb->v, delta_vb, rbb->v );
2440 v3_sub( rbb->w, delta_wb, rbb->w );
2441 }
2442 }
2443
2444 VG_STATIC void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf,
2445 int len )
2446 {
2447 float size = 0.12f;
2448
2449 for( int i=0; i<len; i++ )
2450 {
2451 rb_constr_swingtwist *st = &buf[ i ];
2452
2453 if( !st->axis_violation )
2454 continue;
2455
2456 float rv = v3_dot( st->axis, st->rbb->w ) -
2457 v3_dot( st->axis, st->rba->w );
2458
2459 if( rv * (float)st->axis_violation > 0.0f )
2460 continue;
2461
2462 v3f impulse, wa, wb;
2463 v3_muls( st->axis, rv*st->axis_mass, impulse );
2464 m3x3_mulv( st->rba->iIw, impulse, wa );
2465 v3_add( st->rba->w, wa, st->rba->w );
2466
2467 v3_muls( impulse, -1.0f, impulse );
2468 m3x3_mulv( st->rbb->iIw, impulse, wb );
2469 v3_add( st->rbb->w, wb, st->rbb->w );
2470
2471 float rv2 = v3_dot( st->axis, st->rbb->w ) -
2472 v3_dot( st->axis, st->rba->w );
2473 }
2474
2475 for( int i=0; i<len; i++ )
2476 {
2477 rb_constr_swingtwist *st = &buf[ i ];
2478
2479 if( !st->tangent_violation )
2480 continue;
2481
2482 float rv = v3_dot( st->tangent_axis, st->rbb->w ) -
2483 v3_dot( st->tangent_axis, st->rba->w );
2484
2485 if( rv > 0.0f )
2486 continue;
2487
2488 v3f impulse, wa, wb;
2489 v3_muls( st->tangent_axis, rv*st->tangent_mass, impulse );
2490 m3x3_mulv( st->rba->iIw, impulse, wa );
2491 v3_add( st->rba->w, wa, st->rba->w );
2492
2493 v3_muls( impulse, -1.0f, impulse );
2494 m3x3_mulv( st->rbb->iIw, impulse, wb );
2495 v3_add( st->rbb->w, wb, st->rbb->w );
2496
2497 float rv2 = v3_dot( st->tangent_axis, st->rbb->w ) -
2498 v3_dot( st->tangent_axis, st->rba->w );
2499 }
2500 }
2501
2502 VG_STATIC void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb,
2503 v3f ra, v3f rb )
2504 {
2505 m3x3f ssra, ssrb, ssrat, ssrbt;
2506 m3x3f cma, cmb;
2507
2508 m3x3_skew_symetric( ssrat, ra );
2509 m3x3_skew_symetric( ssrbt, rb );
2510 m3x3_transpose( ssrat, ssra );
2511 m3x3_transpose( ssrbt, ssrb );
2512
2513 m3x3_mul( ssra, rba->iIw, cma );
2514 m3x3_mul( cma, ssrat, cma );
2515 m3x3_mul( ssrb, rbb->iIw, cmb );
2516 m3x3_mul( cmb, ssrbt, cmb );
2517
2518 m3x3f A, invA;
2519 m3x3_add( cma, cmb, A );
2520 m3x3_inv( A, invA );
2521
2522 v3f b_wa, b_wb, b;
2523 m3x3_mulv( ssra, rba->w, b_wa );
2524 m3x3_mulv( ssrb, rbb->w, b_wb );
2525 v3_add( b_wa, b_wb, b );
2526 v3_negate( b, b );
2527
2528 v3f impulse;
2529 m3x3_mulv( invA, b, impulse );
2530
2531 v3f delta_wa, delta_wb;
2532 m3x3f iwa, iwb;
2533 m3x3_mul( rba->iIw, ssrat, iwa );
2534 m3x3_mul( rbb->iIw, ssrbt, iwb );
2535 m3x3_mulv( iwa, impulse, delta_wa );
2536 m3x3_mulv( iwb, impulse, delta_wb );
2537 v3_add( rba->w, delta_wa, rba->w );
2538 v3_sub( rbb->w, delta_wb, rbb->w );
2539 }
2540
2541 /*
2542 * Correct position constraint drift errors
2543 * [ 0.0 <= amt <= 1.0 ]: the correction amount
2544 */
2545 VG_STATIC void rb_correct_position_constraints( rb_constr_pos *buf, int len,
2546 float amt )
2547 {
2548 for( int i=0; i<len; i++ )
2549 {
2550 rb_constr_pos *constr = &buf[i];
2551 rigidbody *rba = constr->rba, *rbb = constr->rbb;
2552
2553 v3f p0, p1, d;
2554 m3x3_mulv( rba->to_world, constr->lca, p0 );
2555 m3x3_mulv( rbb->to_world, constr->lcb, p1 );
2556 v3_add( rba->co, p0, p0 );
2557 v3_add( rbb->co, p1, p1 );
2558 v3_sub( p1, p0, d );
2559
2560 v3_muladds( rbb->co, d, -1.0f * amt, rbb->co );
2561 rb_update_transform( rbb );
2562 }
2563 }
2564
2565 VG_STATIC void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf,
2566 int len, float amt )
2567 {
2568 for( int i=0; i<len; i++ )
2569 {
2570 rb_constr_swingtwist *st = &buf[i];
2571
2572 if( !st->tangent_violation )
2573 continue;
2574
2575 v3f va;
2576 m3x3_mulv( st->rbb->to_world, st->coneva, va );
2577
2578 float angle = v3_dot( va, st->tangent_target );
2579
2580 if( fabsf(angle) < 0.9999f )
2581 {
2582 v3f axis;
2583 v3_cross( va, st->tangent_target, axis );
2584
2585 v4f correction;
2586 q_axis_angle( correction, axis, acosf(angle) * amt );
2587 q_mul( correction, st->rbb->q, st->rbb->q );
2588 rb_update_transform( st->rbb );
2589 }
2590 }
2591
2592 for( int i=0; i<len; i++ )
2593 {
2594 rb_constr_swingtwist *st = &buf[i];
2595
2596 if( !st->axis_violation )
2597 continue;
2598
2599 v3f vxb;
2600 m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
2601
2602 float angle = v3_dot( vxb, st->axis_target );
2603
2604 if( fabsf(angle) < 0.9999f )
2605 {
2606 v3f axis;
2607 v3_cross( vxb, st->axis_target, axis );
2608
2609 v4f correction;
2610 q_axis_angle( correction, axis, acosf(angle) * amt );
2611 q_mul( correction, st->rbb->q, st->rbb->q );
2612 rb_update_transform( st->rbb );
2613 }
2614 }
2615 }
2616
2617 VG_STATIC void rb_correct_contact_constraints( rb_ct *buf, int len, float amt )
2618 {
2619 for( int i=0; i<len; i++ )
2620 {
2621 rb_ct *ct = &buf[i];
2622 rigidbody *rba = ct->rba,
2623 *rbb = ct->rbb;
2624
2625 float mass_total = 1.0f / (rba->inv_mass + rbb->inv_mass);
2626
2627 v3_muladds( rba->co, ct->n, -mass_total * rba->inv_mass, rba->co );
2628 v3_muladds( rbb->co, ct->n, mass_total * rbb->inv_mass, rbb->co );
2629 }
2630 }
2631
2632
2633 /*
2634 * Effectors
2635 */
2636
2637 VG_STATIC void rb_effect_simple_bouyency( rigidbody *ra, v4f plane,
2638 float amt, float drag )
2639 {
2640 /* float */
2641 float depth = v3_dot( plane, ra->co ) - plane[3],
2642 lambda = vg_clampf( -depth, 0.0f, 1.0f ) * amt;
2643
2644 v3_muladds( ra->v, plane, lambda * k_rb_delta, ra->v );
2645
2646 if( depth < 0.0f )
2647 v3_muls( ra->v, 1.0f-(drag*k_rb_delta), ra->v );
2648 }
2649
2650 /* apply a spring&dampener force to match ra(worldspace) on rigidbody, to
2651 * rt(worldspace)
2652 */
2653 VG_STATIC void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt,
2654 float spring, float dampening,
2655 float timestep )
2656 {
2657 float a = acosf( vg_clampf( v3_dot( rt, ra ), -1.0f, 1.0f ) );
2658
2659 v3f axis;
2660 v3_cross( rt, ra, axis );
2661
2662 float Fs = -a * spring,
2663 Fd = -v3_dot( rba->w, axis ) * dampening;
2664
2665 v3_muladds( rba->w, axis, (Fs+Fd) * timestep, rba->w );
2666 }
2667
2668 /*
2669 * -----------------------------------------------------------------------------
2670 * BVH implementation, this is ONLY for VG_STATIC rigidbodies, its to slow for
2671 * realtime use.
2672 * -----------------------------------------------------------------------------
2673 */
2674
2675 VG_STATIC void rb_bh_expand_bound( void *user, boxf bound, u32 item_index )
2676 {
2677 rigidbody *rb = &((rigidbody *)user)[ item_index ];
2678 box_concat( bound, rb->bbx_world );
2679 }
2680
2681 VG_STATIC float rb_bh_centroid( void *user, u32 item_index, int axis )
2682 {
2683 rigidbody *rb = &((rigidbody *)user)[ item_index ];
2684 return (rb->bbx_world[axis][0] + rb->bbx_world[1][axis]) * 0.5f;
2685 }
2686
2687 VG_STATIC void rb_bh_swap( void *user, u32 ia, u32 ib )
2688 {
2689 rigidbody temp, *rba, *rbb;
2690 rba = &((rigidbody *)user)[ ia ];
2691 rbb = &((rigidbody *)user)[ ib ];
2692
2693 temp = *rba;
2694 *rba = *rbb;
2695 *rbb = temp;
2696 }
2697
2698 VG_STATIC void rb_bh_debug( void *user, u32 item_index )
2699 {
2700 rigidbody *rb = &((rigidbody *)user)[ item_index ];
2701 rb_debug( rb, 0xff00ffff );
2702 }
2703
2704 VG_STATIC bh_system bh_system_rigidbodies =
2705 {
2706 .expand_bound = rb_bh_expand_bound,
2707 .item_centroid = rb_bh_centroid,
2708 .item_swap = rb_bh_swap,
2709 .item_debug = rb_bh_debug,
2710 .cast_ray = NULL
2711 };
2712
2713 #endif /* RIGIDBODY_H */
Skaterift source tree