projects / carveJwlIkooP6JGAAIwe30JlM.git / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
history | raw | HEAD
compiler fun
[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 #include "distq.h"
14
15 #include <math.h>
16
17 VG_STATIC void rb_tangent_basis( v3f n, v3f tx, v3f ty );
18 VG_STATIC bh_system bh_system_rigidbodies;
19
20 #ifndef RIGIDBODY_H
21 #define RIGIDBODY_H
22
23 /*
24 * -----------------------------------------------------------------------------
25 * (K)onstants
26 * -----------------------------------------------------------------------------
27 */
28
29 VG_STATIC const float
30 k_rb_rate = (1.0/VG_TIMESTEP_FIXED),
31 k_rb_delta = (1.0/k_rb_rate),
32 k_friction = 0.6f,
33 k_damp_linear = 0.05f, /* scale velocity 1/(1+x) */
34 k_damp_angular = 0.1f, /* scale angular 1/(1+x) */
35 k_limit_bias = 0.02f,
36 k_joint_bias = 0.08f, /* positional joints */
37 k_joint_correction = 0.01f,
38 k_penetration_slop = 0.01f,
39 k_inertia_scale = 4.0f;
40
41 /*
42 * -----------------------------------------------------------------------------
43 * structure definitions
44 * -----------------------------------------------------------------------------
45 */
46
47 typedef struct rigidbody rigidbody;
48 typedef struct contact rb_ct;
49
50 struct rigidbody
51 {
52 v3f co, v, w;
53 v4f q;
54
55 enum rb_shape
56 {
57 k_rb_shape_box = 0,
58 k_rb_shape_sphere = 1,
59 k_rb_shape_capsule = 2,
60 k_rb_shape_scene = 3
61 }
62 type;
63
64 union
65 {
66 struct rb_sphere
67 {
68 float radius;
69 }
70 sphere;
71
72 struct rb_capsule
73 {
74 float height, radius;
75 }
76 capsule;
77
78 struct rb_scene
79 {
80 bh_tree *bh_scene;
81 }
82 scene;
83 }
84 inf;
85
86 v3f right, up, forward;
87
88 int is_world;
89
90 boxf bbx, bbx_world;
91 float inv_mass;
92
93 /* inertia model and inverse world tensor */
94 v3f I;
95 m3x3f iI, iIw;
96
97 m4x3f to_world, to_local;
98 };
99
100 VG_STATIC struct contact
101 {
102 rigidbody *rba, *rbb;
103 v3f co, n;
104 v3f t[2];
105 float p, bias, norm_impulse, tangent_impulse[2],
106 normal_mass, tangent_mass[2];
107
108 u32 element_id;
109
110 enum contact_type type;
111 }
112 rb_contact_buffer[256];
113 VG_STATIC int rb_contact_count = 0;
114
115 /*
116 * -----------------------------------------------------------------------------
117 * Math Utils
118 * -----------------------------------------------------------------------------
119 */
120
121 VG_STATIC float sphere_volume( float radius )
122 {
123 float r3 = radius*radius*radius;
124 return (4.0f/3.0f) * VG_PIf * r3;
125 }
126
127 VG_STATIC void rb_tangent_basis( v3f n, v3f tx, v3f ty )
128 {
129 /* Compute tangent basis (box2d) */
130 if( fabsf( n[0] ) >= 0.57735027f )
131 {
132 tx[0] = n[1];
133 tx[1] = -n[0];
134 tx[2] = 0.0f;
135 }
136 else
137 {
138 tx[0] = 0.0f;
139 tx[1] = n[2];
140 tx[2] = -n[1];
141 }
142
143 v3_normalize( tx );
144 v3_cross( n, tx, ty );
145 }
146
147 /*
148 * -----------------------------------------------------------------------------
149 * Debugging
150 * -----------------------------------------------------------------------------
151 */
152
153 VG_STATIC void rb_debug_contact( rb_ct *ct )
154 {
155 if( ct->type != k_contact_type_disabled )
156 {
157 v3f p1;
158 v3_muladds( ct->co, ct->n, 0.1f, p1 );
159 vg_line_pt3( ct->co, 0.025f, 0xff0000ff );
160 vg_line( ct->co, p1, 0xffffffff );
161 }
162 }
163
164 VG_STATIC void debug_sphere( m4x3f m, float radius, u32 colour )
165 {
166 v3f ly = { 0.0f, 0.0f, radius },
167 lx = { 0.0f, radius, 0.0f },
168 lz = { 0.0f, 0.0f, radius };
169
170 for( int i=0; i<16; i++ )
171 {
172 float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
173 s = sinf(t),
174 c = cosf(t);
175
176 v3f py = { s*radius, 0.0f, c*radius },
177 px = { s*radius, c*radius, 0.0f },
178 pz = { 0.0f, s*radius, c*radius };
179
180 v3f p0, p1, p2, p3, p4, p5;
181 m4x3_mulv( m, py, p0 );
182 m4x3_mulv( m, ly, p1 );
183 m4x3_mulv( m, px, p2 );
184 m4x3_mulv( m, lx, p3 );
185 m4x3_mulv( m, pz, p4 );
186 m4x3_mulv( m, lz, p5 );
187
188 vg_line( p0, p1, colour == 0x00? 0xff00ff00: colour );
189 vg_line( p2, p3, colour == 0x00? 0xff0000ff: colour );
190 vg_line( p4, p5, colour == 0x00? 0xffff0000: colour );
191
192 v3_copy( py, ly );
193 v3_copy( px, lx );
194 v3_copy( pz, lz );
195 }
196 }
197
198 VG_STATIC void debug_capsule( m4x3f m, float radius, float h, u32 colour )
199 {
200 v3f ly = { 0.0f, 0.0f, radius },
201 lx = { 0.0f, radius, 0.0f },
202 lz = { 0.0f, 0.0f, radius };
203
204 float s0 = sinf(0.0f)*radius,
205 c0 = cosf(0.0f)*radius;
206
207 v3f p0, p1, up, right, forward;
208 m3x3_mulv( m, (v3f){0.0f,1.0f,0.0f}, up );
209 m3x3_mulv( m, (v3f){1.0f,0.0f,0.0f}, right );
210 m3x3_mulv( m, (v3f){0.0f,0.0f,-1.0f}, forward );
211 v3_muladds( m[3], up, -h*0.5f+radius, p0 );
212 v3_muladds( m[3], up, h*0.5f-radius, p1 );
213
214 v3f a0, a1, b0, b1;
215 v3_muladds( p0, right, radius, a0 );
216 v3_muladds( p1, right, radius, a1 );
217 v3_muladds( p0, forward, radius, b0 );
218 v3_muladds( p1, forward, radius, b1 );
219 vg_line( a0, a1, colour );
220 vg_line( b0, b1, colour );
221
222 v3_muladds( p0, right, -radius, a0 );
223 v3_muladds( p1, right, -radius, a1 );
224 v3_muladds( p0, forward, -radius, b0 );
225 v3_muladds( p1, forward, -radius, b1 );
226 vg_line( a0, a1, colour );
227 vg_line( b0, b1, colour );
228
229 for( int i=0; i<16; i++ )
230 {
231 float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
232 s1 = sinf(t)*radius,
233 c1 = cosf(t)*radius;
234
235 v3f e0 = { s0, 0.0f, c0 },
236 e1 = { s1, 0.0f, c1 },
237 e2 = { s0, c0, 0.0f },
238 e3 = { s1, c1, 0.0f },
239 e4 = { 0.0f, c0, s0 },
240 e5 = { 0.0f, c1, s1 };
241
242 m3x3_mulv( m, e0, e0 );
243 m3x3_mulv( m, e1, e1 );
244 m3x3_mulv( m, e2, e2 );
245 m3x3_mulv( m, e3, e3 );
246 m3x3_mulv( m, e4, e4 );
247 m3x3_mulv( m, e5, e5 );
248
249 v3_add( p0, e0, a0 );
250 v3_add( p0, e1, a1 );
251 v3_add( p1, e0, b0 );
252 v3_add( p1, e1, b1 );
253
254 vg_line( a0, a1, colour );
255 vg_line( b0, b1, colour );
256
257 if( c0 < 0.0f )
258 {
259 v3_add( p0, e2, a0 );
260 v3_add( p0, e3, a1 );
261 v3_add( p0, e4, b0 );
262 v3_add( p0, e5, b1 );
263 }
264 else
265 {
266 v3_add( p1, e2, a0 );
267 v3_add( p1, e3, a1 );
268 v3_add( p1, e4, b0 );
269 v3_add( p1, e5, b1 );
270 }
271
272 vg_line( a0, a1, colour );
273 vg_line( b0, b1, colour );
274
275 s0 = s1;
276 c0 = c1;
277 }
278 }
279
280 VG_STATIC void rb_debug( rigidbody *rb, u32 colour )
281 {
282 if( rb->type == k_rb_shape_box )
283 {
284 v3f *box = rb->bbx;
285 vg_line_boxf_transformed( rb->to_world, rb->bbx, colour );
286 }
287 else if( rb->type == k_rb_shape_sphere )
288 {
289 debug_sphere( rb->to_world, rb->inf.sphere.radius, colour );
290 }
291 else if( rb->type == k_rb_shape_capsule )
292 {
293 m4x3f m0, m1;
294 float h = rb->inf.capsule.height,
295 r = rb->inf.capsule.radius;
296
297 debug_capsule( rb->to_world, r, h, colour );
298 }
299 else if( rb->type == k_rb_shape_scene )
300 {
301 vg_line_boxf( rb->bbx, colour );
302 }
303 }
304
305 /*
306 * -----------------------------------------------------------------------------
307 * Integration
308 * -----------------------------------------------------------------------------
309 */
310
311 /*
312 * Update world space bounding box based on local one
313 */
314 VG_STATIC void rb_update_bounds( rigidbody *rb )
315 {
316 box_copy( rb->bbx, rb->bbx_world );
317 m4x3_transform_aabb( rb->to_world, rb->bbx_world );
318 }
319
320 /*
321 * Commit transform to rigidbody. Updates matrices
322 */
323 VG_STATIC void rb_update_transform( rigidbody *rb )
324 {
325 q_normalize( rb->q );
326 q_m3x3( rb->q, rb->to_world );
327 v3_copy( rb->co, rb->to_world[3] );
328
329 m4x3_invert_affine( rb->to_world, rb->to_local );
330
331 m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f, 0.0f}, rb->right );
332 m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f, 0.0f}, rb->up );
333 m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,-1.0f}, rb->forward );
334
335 m3x3_mul( rb->iI, rb->to_local, rb->iIw );
336 m3x3_mul( rb->to_world, rb->iIw, rb->iIw );
337
338 rb_update_bounds( rb );
339 }
340
341 /*
342 * Extrapolate rigidbody into a transform based on vg accumulator.
343 * Useful for rendering
344 */
345 VG_STATIC void rb_extrapolate_transform( rigidbody *rb, m4x3f transform )
346 {
347 float substep = vg_clampf( vg.accumulator / k_rb_delta, 0.0f, 1.0f );
348
349 v3f co;
350 v4f q;
351
352 v3_muladds( rb->co, rb->v, k_rb_delta*substep, co );
353
354 if( v3_length2( rb->w ) > 0.0f )
355 {
356 v4f rotation;
357 v3f axis;
358 v3_copy( rb->w, axis );
359
360 float mag = v3_length( axis );
361 v3_divs( axis, mag, axis );
362 q_axis_angle( rotation, axis, mag*k_rb_delta*substep );
363 q_mul( rotation, rb->q, q );
364 q_normalize( q );
365 }
366 else
367 {
368 v4_copy( rb->q, q );
369 }
370
371 q_m3x3( q, transform );
372 v3_copy( co, transform[3] );
373 }
374
375 /*
376 * Initialize rigidbody and calculate masses, inertia
377 */
378 VG_STATIC void rb_init( rigidbody *rb )
379 {
380 float volume = 1.0f;
381
382 if( rb->type == k_rb_shape_box )
383 {
384 v3f dims;
385 v3_sub( rb->bbx[1], rb->bbx[0], dims );
386 volume = dims[0]*dims[1]*dims[2];
387 }
388 else if( rb->type == k_rb_shape_sphere )
389 {
390 volume = sphere_volume( rb->inf.sphere.radius );
391 v3_fill( rb->bbx[0], -rb->inf.sphere.radius );
392 v3_fill( rb->bbx[1], rb->inf.sphere.radius );
393 }
394 else if( rb->type == k_rb_shape_capsule )
395 {
396 float r = rb->inf.capsule.radius,
397 h = rb->inf.capsule.height;
398 volume = sphere_volume( r ) + VG_PIf * r*r * (h - r*2.0f);
399
400 v3_fill( rb->bbx[0], -rb->inf.sphere.radius );
401 v3_fill( rb->bbx[1], rb->inf.sphere.radius );
402 rb->bbx[0][1] = -h;
403 rb->bbx[1][1] = h;
404 }
405 else if( rb->type == k_rb_shape_scene )
406 {
407 rb->is_world = 1;
408 box_copy( rb->inf.scene.bh_scene->nodes[0].bbx, rb->bbx );
409 }
410
411 if( rb->is_world )
412 {
413 rb->inv_mass = 0.0f;
414 v3_zero( rb->I );
415 m3x3_zero(rb->iI);
416 }
417 else
418 {
419 float mass = 2.0f*volume;
420 rb->inv_mass = 1.0f/mass;
421
422 v3f extent;
423 v3_sub( rb->bbx[1], rb->bbx[0], extent );
424 v3_muls( extent, 0.5f, extent );
425
426 /* local intertia tensor */
427 float scale = k_inertia_scale;
428 float ex2 = scale*extent[0]*extent[0],
429 ey2 = scale*extent[1]*extent[1],
430 ez2 = scale*extent[2]*extent[2];
431
432 rb->I[0] = ((1.0f/12.0f) * mass * (ey2+ez2));
433 rb->I[1] = ((1.0f/12.0f) * mass * (ex2+ez2));
434 rb->I[2] = ((1.0f/12.0f) * mass * (ex2+ey2));
435
436 m3x3_identity( rb->iI );
437 rb->iI[0][0] = rb->I[0];
438 rb->iI[1][1] = rb->I[1];
439 rb->iI[2][2] = rb->I[2];
440 m3x3_inv( rb->iI, rb->iI );
441 }
442
443 v3_zero( rb->v );
444 v3_zero( rb->w );
445
446 rb_update_transform( rb );
447 }
448
449 VG_STATIC void rb_iter( rigidbody *rb )
450 {
451 if( !vg_validf( rb->v[0] ) ||
452 !vg_validf( rb->v[1] ) ||
453 !vg_validf( rb->v[2] ) )
454 {
455 vg_fatal_exit_loop( "NaN velocity" );
456 }
457
458 v3f gravity = { 0.0f, -9.8f, 0.0f };
459 v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
460
461 /* intergrate velocity */
462 v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
463 v3_lerp( rb->w, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->w );
464
465 /* inegrate inertia */
466 if( v3_length2( rb->w ) > 0.0f )
467 {
468 v4f rotation;
469 v3f axis;
470 v3_copy( rb->w, axis );
471
472 float mag = v3_length( axis );
473 v3_divs( axis, mag, axis );
474 q_axis_angle( rotation, axis, mag*k_rb_delta );
475 q_mul( rotation, rb->q, rb->q );
476 }
477
478 /* damping */
479 v3_muls( rb->v, 1.0f/(1.0f+k_rb_delta*k_damp_linear), rb->v );
480 v3_muls( rb->w, 1.0f/(1.0f+k_rb_delta*k_damp_angular), rb->w );
481 }
482
483
484 /*
485 * -----------------------------------------------------------------------------
486 * Boolean shape overlap functions
487 * -----------------------------------------------------------------------------
488 */
489
490 /*
491 * Project AABB, and triangle interval onto axis to check if they overlap
492 */
493 VG_STATIC int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] )
494 {
495 float
496
497 r = extent[0] * fabsf(axis[0]) +
498 extent[1] * fabsf(axis[1]) +
499 extent[2] * fabsf(axis[2]),
500
501 p0 = v3_dot( axis, tri[0] ),
502 p1 = v3_dot( axis, tri[1] ),
503 p2 = v3_dot( axis, tri[2] ),
504
505 e = vg_maxf(-vg_maxf(p0,vg_maxf(p1,p2)), vg_minf(p0,vg_minf(p1,p2)));
506
507 if( e > r ) return 0;
508 else return 1;
509 }
510
511 /*
512 * Seperating axis test box vs triangle
513 */
514 VG_STATIC int rb_box_triangle_sat( rigidbody *rba, v3f tri_src[3] )
515 {
516 v3f tri[3];
517
518 v3f extent, c;
519 v3_sub( rba->bbx[1], rba->bbx[0], extent );
520 v3_muls( extent, 0.5f, extent );
521 v3_add( rba->bbx[0], extent, c );
522
523 for( int i=0; i<3; i++ )
524 {
525 m4x3_mulv( rba->to_local, tri_src[i], tri[i] );
526 v3_sub( tri[i], c, tri[i] );
527 }
528
529 /* u0, u1, u2 */
530 if(!rb_box_triangle_interval( extent, (v3f){1.0f,0.0f,0.0f}, tri )) return 0;
531 if(!rb_box_triangle_interval( extent, (v3f){0.0f,1.0f,0.0f}, tri )) return 0;
532 if(!rb_box_triangle_interval( extent, (v3f){0.0f,0.0f,1.0f}, tri )) return 0;
533
534 v3f v0,v1,v2,n, e0,e1,e2;
535 v3_sub( tri[1], tri[0], v0 );
536 v3_sub( tri[2], tri[0], v1 );
537 v3_sub( tri[2], tri[1], v2 );
538 v3_normalize( v0 );
539 v3_normalize( v1 );
540 v3_normalize( v2 );
541 v3_cross( v0, v1, n );
542 v3_cross( v0, n, e0 );
543 v3_cross( n, v1, e1 );
544 v3_cross( v2, n, e2 );
545
546 /* normal */
547 if(!rb_box_triangle_interval( extent, n, tri )) return 0;
548
549 v3f axis[9];
550 v3_cross( e0, (v3f){1.0f,0.0f,0.0f}, axis[0] );
551 v3_cross( e0, (v3f){0.0f,1.0f,0.0f}, axis[1] );
552 v3_cross( e0, (v3f){0.0f,0.0f,1.0f}, axis[2] );
553 v3_cross( e1, (v3f){1.0f,0.0f,0.0f}, axis[3] );
554 v3_cross( e1, (v3f){0.0f,1.0f,0.0f}, axis[4] );
555 v3_cross( e1, (v3f){0.0f,0.0f,1.0f}, axis[5] );
556 v3_cross( e2, (v3f){1.0f,0.0f,0.0f}, axis[6] );
557 v3_cross( e2, (v3f){0.0f,1.0f,0.0f}, axis[7] );
558 v3_cross( e2, (v3f){0.0f,0.0f,1.0f}, axis[8] );
559
560 for( int i=0; i<9; i++ )
561 if(!rb_box_triangle_interval( extent, axis[i], tri )) return 0;
562
563 return 1;
564 }
565
566 /*
567 * -----------------------------------------------------------------------------
568 * Manifold
569 * -----------------------------------------------------------------------------
570 */
571
572 VG_STATIC int rb_manifold_apply_filtered( rb_ct *man, int len )
573 {
574 int k = 0;
575
576 for( int i=0; i<len; i++ )
577 {
578 rb_ct *ct = &man[i];
579
580 if( ct->type == k_contact_type_disabled )
581 continue;
582
583 man[k ++] = man[i];
584 }
585
586 return k;
587 }
588
589 VG_STATIC void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r )
590 {
591 for( int i=0; i<len-1; i++ )
592 {
593 rb_ct *ci = &man[i];
594 if( ci->type != k_contact_type_edge )
595 continue;
596
597 for( int j=i+1; j<len; j++ )
598 {
599 rb_ct *cj = &man[j];
600 if( cj->type != k_contact_type_edge )
601 continue;
602
603 if( v3_dist2( ci->co, cj->co ) < r*r )
604 {
605 cj->type = k_contact_type_disabled;
606 ci->p = (ci->p + cj->p) * 0.5f;
607
608 v3_add( ci->co, cj->co, ci->co );
609 v3_muls( ci->co, 0.5f, ci->co );
610
611 v3f delta;
612 v3_sub( ci->rba->co, ci->co, delta );
613
614 float c0 = v3_dot( ci->n, delta ),
615 c1 = v3_dot( cj->n, delta );
616
617 if( c0 < 0.0f || c1 < 0.0f )
618 {
619 /* error */
620 ci->type = k_contact_type_disabled;
621 }
622 else
623 {
624 v3f n;
625 v3_muls( ci->n, c0, n );
626 v3_muladds( n, cj->n, c1, n );
627 v3_normalize( n );
628 v3_copy( n, ci->n );
629 }
630 }
631 }
632 }
633 }
634
635 /*
636 * Resolve overlapping pairs
637 */
638 VG_STATIC void rb_manifold_filter_pairs( rb_ct *man, int len, float r )
639 {
640 for( int i=0; i<len-1; i++ )
641 {
642 rb_ct *ci = &man[i];
643 int similar = 0;
644
645 if( ci->type == k_contact_type_disabled ) continue;
646
647 for( int j=i+1; j<len; j++ )
648 {
649 rb_ct *cj = &man[j];
650
651 if( cj->type == k_contact_type_disabled ) continue;
652
653 if( v3_dist2( ci->co, cj->co ) < r*r )
654 {
655 cj->type = k_contact_type_disabled;
656 v3_add( cj->n, ci->n, ci->n );
657 ci->p += cj->p;
658 similar ++;
659 }
660 }
661
662 if( similar )
663 {
664 float n = 1.0f/((float)similar+1.0f);
665 v3_muls( ci->n, n, ci->n );
666 ci->p *= n;
667
668 if( v3_length2(ci->n) < 0.1f*0.1f )
669 ci->type = k_contact_type_disabled;
670 else
671 v3_normalize( ci->n );
672 }
673 }
674 }
675
676 /*
677 * Remove contacts that are facing away from A
678 */
679 VG_STATIC void rb_manifold_filter_backface( rb_ct *man, int len )
680 {
681 for( int i=0; i<len; i++ )
682 {
683 rb_ct *ct = &man[i];
684 if( ct->type == k_contact_type_disabled )
685 continue;
686
687 v3f delta;
688 v3_sub( ct->co, ct->rba->co, delta );
689
690 if( v3_dot( delta, ct->n ) > -0.001f )
691 ct->type = k_contact_type_disabled;
692 }
693 }
694
695 /*
696 * Filter out duplicate coplanar results. Good for spheres.
697 */
698 VG_STATIC void rb_manifold_filter_coplanar( rb_ct *man, int len, float w )
699 {
700 for( int i=0; i<len; i++ )
701 {
702 rb_ct *ci = &man[i];
703 if( ci->type == k_contact_type_disabled ||
704 ci->type == k_contact_type_edge )
705 continue;
706
707 float d1 = v3_dot( ci->co, ci->n );
708
709 for( int j=0; j<len; j++ )
710 {
711 if( j == i )
712 continue;
713
714 rb_ct *cj = &man[j];
715 if( cj->type == k_contact_type_disabled )
716 continue;
717
718 float d2 = v3_dot( cj->co, ci->n ),
719 d = d2-d1;
720
721 if( fabsf( d ) <= w )
722 {
723 cj->type = k_contact_type_disabled;
724 }
725 }
726 }
727 }
728
729 /*
730 * -----------------------------------------------------------------------------
731 * Collision matrix
732 * -----------------------------------------------------------------------------
733 */
734
735 /*
736 * Contact generators
737 *
738 * These do not automatically allocate contacts, an appropriately sized
739 * buffer must be supplied. The function returns the size of the manifold
740 * which was generated.
741 *
742 * The values set on the contacts are: n, co, p, rba, rbb
743 */
744
745 /*
746 * By collecting the minimum(time) and maximum(time) pairs of points, we
747 * build a reduced and stable exact manifold.
748 *
749 * tx: time at point
750 * rx: minimum distance of these points
751 * dx: the delta between the two points
752 *
753 * pairs will only ammend these if they are creating a collision
754 */
755 typedef struct capsule_manifold capsule_manifold;
756 struct capsule_manifold
757 {
758 float t0, t1;
759 float r0, r1;
760 v3f d0, d1;
761 };
762
763 /*
764 * Expand a line manifold with a new pair. t value is the time along segment
765 * on the oriented object which created this pair.
766 */
767 VG_STATIC void rb_capsule_manifold( v3f pa, v3f pb, float t, float r,
768 capsule_manifold *manifold )
769 {
770 v3f delta;
771 v3_sub( pa, pb, delta );
772
773 if( v3_length2(delta) < r*r )
774 {
775 if( t < manifold->t0 )
776 {
777 v3_copy( delta, manifold->d0 );
778 manifold->t0 = t;
779 manifold->r0 = r;
780 }
781
782 if( t > manifold->t1 )
783 {
784 v3_copy( delta, manifold->d1 );
785 manifold->t1 = t;
786 manifold->r1 = r;
787 }
788 }
789 }
790
791 VG_STATIC void rb_capsule_manifold_init( capsule_manifold *manifold )
792 {
793 manifold->t0 = INFINITY;
794 manifold->t1 = -INFINITY;
795 }
796
797 VG_STATIC int rb_capsule_manifold_done( rigidbody *rba, rigidbody *rbb,
798 capsule_manifold *manifold, rb_ct *buf )
799 {
800 float h = rba->inf.capsule.height,
801 ra = rba->inf.capsule.radius;
802
803 v3f p0, p1;
804 v3_muladds( rba->co, rba->up, -h*0.5f+ra, p0 );
805 v3_muladds( rba->co, rba->up, h*0.5f-ra, p1 );
806
807 int count = 0;
808 if( manifold->t0 <= 1.0f )
809 {
810 rb_ct *ct = buf;
811
812 v3f pa;
813 v3_muls( p0, 1.0f-manifold->t0, pa );
814 v3_muladds( pa, p1, manifold->t0, pa );
815
816 float d = v3_length( manifold->d0 );
817 v3_muls( manifold->d0, 1.0f/d, ct->n );
818 v3_muladds( pa, ct->n, -ra, ct->co );
819
820 ct->p = manifold->r0 - d;
821 ct->rba = rba;
822 ct->rbb = rbb;
823 ct->type = k_contact_type_default;
824
825 count ++;
826 }
827
828 if( (manifold->t1 >= 0.0f) && (manifold->t0 != manifold->t1) )
829 {
830 rb_ct *ct = buf+count;
831
832 v3f pa;
833 v3_muls( p0, 1.0f-manifold->t1, pa );
834 v3_muladds( pa, p1, manifold->t1, pa );
835
836 float d = v3_length( manifold->d1 );
837 v3_muls( manifold->d1, 1.0f/d, ct->n );
838 v3_muladds( pa, ct->n, -ra, ct->co );
839
840 ct->p = manifold->r1 - d;
841 ct->rba = rba;
842 ct->rbb = rbb;
843 ct->type = k_contact_type_default;
844
845 count ++;
846 }
847
848 /*
849 * Debugging
850 */
851
852 if( count == 2 )
853 vg_line( buf[0].co, buf[1].co, 0xff0000ff );
854
855 return count;
856 }
857
858 VG_STATIC int rb_capsule_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
859 {
860 float h = rba->inf.capsule.height,
861 ra = rba->inf.capsule.radius,
862 rb = rbb->inf.sphere.radius;
863
864 v3f p0, p1;
865 v3_muladds( rba->co, rba->up, -h*0.5f+ra, p0 );
866 v3_muladds( rba->co, rba->up, h*0.5f-ra, p1 );
867
868 v3f c, delta;
869 closest_point_segment( p0, p1, rbb->co, c );
870 v3_sub( c, rbb->co, delta );
871
872 float d2 = v3_length2(delta),
873 r = ra + rb;
874
875 if( d2 < r*r )
876 {
877 float d = sqrtf(d2);
878
879 rb_ct *ct = buf;
880 v3_muls( delta, 1.0f/d, ct->n );
881 ct->p = r-d;
882
883 v3f p0, p1;
884 v3_muladds( c, ct->n, -ra, p0 );
885 v3_muladds( rbb->co, ct->n, rb, p1 );
886 v3_add( p0, p1, ct->co );
887 v3_muls( ct->co, 0.5f, ct->co );
888
889 ct->rba = rba;
890 ct->rbb = rbb;
891 ct->type = k_contact_type_default;
892
893 return 1;
894 }
895
896 return 0;
897 }
898
899 VG_STATIC int rb_capsule_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
900 {
901 float ha = rba->inf.capsule.height,
902 hb = rbb->inf.capsule.height,
903 ra = rba->inf.capsule.radius,
904 rb = rbb->inf.capsule.radius,
905 r = ra+rb;
906
907 v3f p0, p1, p2, p3;
908 v3_muladds( rba->co, rba->up, -ha*0.5f+ra, p0 );
909 v3_muladds( rba->co, rba->up, ha*0.5f-ra, p1 );
910 v3_muladds( rbb->co, rbb->up, -hb*0.5f+rb, p2 );
911 v3_muladds( rbb->co, rbb->up, hb*0.5f-rb, p3 );
912
913 capsule_manifold manifold;
914 rb_capsule_manifold_init( &manifold );
915
916 v3f pa, pb;
917 float ta, tb;
918 closest_segment_segment( p0, p1, p2, p3, &ta, &tb, pa, pb );
919 rb_capsule_manifold( pa, pb, ta, r, &manifold );
920
921 ta = closest_point_segment( p0, p1, p2, pa );
922 tb = closest_point_segment( p0, p1, p3, pb );
923 rb_capsule_manifold( pa, p2, ta, r, &manifold );
924 rb_capsule_manifold( pb, p3, tb, r, &manifold );
925
926 closest_point_segment( p2, p3, p0, pa );
927 closest_point_segment( p2, p3, p1, pb );
928 rb_capsule_manifold( p0, pa, 0.0f, r, &manifold );
929 rb_capsule_manifold( p1, pb, 1.0f, r, &manifold );
930
931 return rb_capsule_manifold_done( rba, rbb, &manifold, buf );
932 }
933
934 /*
935 * Generates up to two contacts; optimised for the most stable manifold
936 */
937 VG_STATIC int rb_capsule_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
938 {
939 float h = rba->inf.capsule.height,
940 r = rba->inf.capsule.radius;
941
942 /*
943 * Solving this in symetric local space of the cube saves us some time and a
944 * couple branches when it comes to the quad stage.
945 */
946 v3f centroid;
947 v3_add( rbb->bbx[0], rbb->bbx[1], centroid );
948 v3_muls( centroid, 0.5f, centroid );
949
950 boxf bbx;
951 v3_sub( rbb->bbx[0], centroid, bbx[0] );
952 v3_sub( rbb->bbx[1], centroid, bbx[1] );
953
954 v3f pc, p0w, p1w, p0, p1;
955 v3_muladds( rba->co, rba->up, -h*0.5f+r, p0w );
956 v3_muladds( rba->co, rba->up, h*0.5f-r, p1w );
957
958 m4x3_mulv( rbb->to_local, p0w, p0 );
959 m4x3_mulv( rbb->to_local, p1w, p1 );
960 v3_sub( p0, centroid, p0 );
961 v3_sub( p1, centroid, p1 );
962 v3_add( p0, p1, pc );
963 v3_muls( pc, 0.5f, pc );
964
965 /*
966 * Finding an appropriate quad to collide lines with
967 */
968 v3f region;
969 v3_div( pc, bbx[1], region );
970
971 v3f quad[4];
972 if( (fabsf(region[0]) > fabsf(region[1])) &&
973 (fabsf(region[0]) > fabsf(region[2])) )
974 {
975 float px = vg_signf(region[0]) * bbx[1][0];
976 v3_copy( (v3f){ px, bbx[0][1], bbx[0][2] }, quad[0] );
977 v3_copy( (v3f){ px, bbx[1][1], bbx[0][2] }, quad[1] );
978 v3_copy( (v3f){ px, bbx[1][1], bbx[1][2] }, quad[2] );
979 v3_copy( (v3f){ px, bbx[0][1], bbx[1][2] }, quad[3] );
980 }
981 else if( fabsf(region[1]) > fabsf(region[2]) )
982 {
983 float py = vg_signf(region[1]) * bbx[1][1];
984 v3_copy( (v3f){ bbx[0][0], py, bbx[0][2] }, quad[0] );
985 v3_copy( (v3f){ bbx[1][0], py, bbx[0][2] }, quad[1] );
986 v3_copy( (v3f){ bbx[1][0], py, bbx[1][2] }, quad[2] );
987 v3_copy( (v3f){ bbx[0][0], py, bbx[1][2] }, quad[3] );
988 }
989 else
990 {
991 float pz = vg_signf(region[2]) * bbx[1][2];
992 v3_copy( (v3f){ bbx[0][0], bbx[0][1], pz }, quad[0] );
993 v3_copy( (v3f){ bbx[1][0], bbx[0][1], pz }, quad[1] );
994 v3_copy( (v3f){ bbx[1][0], bbx[1][1], pz }, quad[2] );
995 v3_copy( (v3f){ bbx[0][0], bbx[1][1], pz }, quad[3] );
996 }
997
998 capsule_manifold manifold;
999 rb_capsule_manifold_init( &manifold );
1000
1001 v3f c0, c1;
1002 closest_point_aabb( p0, bbx, c0 );
1003 closest_point_aabb( p1, bbx, c1 );
1004
1005 v3f d0, d1, da;
1006 v3_sub( c0, p0, d0 );
1007 v3_sub( c1, p1, d1 );
1008 v3_sub( p1, p0, da );
1009
1010 v3_normalize(d0);
1011 v3_normalize(d1);
1012 v3_normalize(da);
1013
1014 if( v3_dot( da, d0 ) <= 0.01f )
1015 rb_capsule_manifold( p0, c0, 0.0f, r, &manifold );
1016
1017 if( v3_dot( da, d1 ) >= -0.01f )
1018 rb_capsule_manifold( p1, c1, 1.0f, r, &manifold );
1019
1020 for( int i=0; i<4; i++ )
1021 {
1022 int i0 = i,
1023 i1 = (i+1)%4;
1024
1025 v3f ca, cb;
1026 float ta, tb;
1027 closest_segment_segment( p0, p1, quad[i0], quad[i1], &ta, &tb, ca, cb );
1028 rb_capsule_manifold( ca, cb, ta, r, &manifold );
1029 }
1030
1031 /*
1032 * Create final contacts based on line manifold
1033 */
1034 m3x3_mulv( rbb->to_world, manifold.d0, manifold.d0 );
1035 m3x3_mulv( rbb->to_world, manifold.d1, manifold.d1 );
1036
1037 /*
1038 * Debugging
1039 */
1040
1041 #if 0
1042 for( int i=0; i<4; i++ )
1043 {
1044 v3f q0, q1;
1045 int i0 = i,
1046 i1 = (i+1)%4;
1047
1048 v3_add( quad[i0], centroid, q0 );
1049 v3_add( quad[i1], centroid, q1 );
1050
1051 m4x3_mulv( rbb->to_world, q0, q0 );
1052 m4x3_mulv( rbb->to_world, q1, q1 );
1053
1054 vg_line( q0, q1, 0xffffffff );
1055 }
1056 #endif
1057
1058 return rb_capsule_manifold_done( rba, rbb, &manifold, buf );
1059 }
1060
1061 VG_STATIC int rb_sphere_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1062 {
1063 v3f co, delta;
1064
1065 closest_point_obb( rba->co, rbb->bbx, rbb->to_world, rbb->to_local, co );
1066 v3_sub( rba->co, co, delta );
1067
1068 float d2 = v3_length2(delta),
1069 r = rba->inf.sphere.radius;
1070
1071 if( d2 <= r*r )
1072 {
1073 float d;
1074
1075 rb_ct *ct = buf;
1076 if( d2 <= 0.0001f )
1077 {
1078 v3_sub( rba->co, rbb->co, delta );
1079
1080 /*
1081 * some extra testing is required to find the best axis to push the
1082 * object back outside the box. Since there isnt a clear seperating
1083 * vector already, especially on really high aspect boxes.
1084 */
1085 float lx = v3_dot( rbb->right, delta ),
1086 ly = v3_dot( rbb->up, delta ),
1087 lz = v3_dot( rbb->forward, delta ),
1088 px = rbb->bbx[1][0] - fabsf(lx),
1089 py = rbb->bbx[1][1] - fabsf(ly),
1090 pz = rbb->bbx[1][2] - fabsf(lz);
1091
1092 if( px < py && px < pz )
1093 v3_muls( rbb->right, vg_signf(lx), ct->n );
1094 else if( py < pz )
1095 v3_muls( rbb->up, vg_signf(ly), ct->n );
1096 else
1097 v3_muls( rbb->forward, vg_signf(lz), ct->n );
1098
1099 v3_muladds( rba->co, ct->n, -r, ct->co );
1100 ct->p = r;
1101 }
1102 else
1103 {
1104 d = sqrtf(d2);
1105 v3_muls( delta, 1.0f/d, ct->n );
1106 ct->p = r-d;
1107 v3_copy( co, ct->co );
1108 }
1109
1110 ct->rba = rba;
1111 ct->rbb = rbb;
1112 ct->type = k_contact_type_default;
1113 return 1;
1114 }
1115
1116 return 0;
1117 }
1118
1119 VG_STATIC int rb_sphere_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1120 {
1121 v3f delta;
1122 v3_sub( rba->co, rbb->co, delta );
1123
1124 float d2 = v3_length2(delta),
1125 r = rba->inf.sphere.radius + rbb->inf.sphere.radius;
1126
1127 if( d2 < r*r )
1128 {
1129 float d = sqrtf(d2);
1130
1131 rb_ct *ct = buf;
1132 v3_muls( delta, 1.0f/d, ct->n );
1133
1134 v3f p0, p1;
1135 v3_muladds( rba->co, ct->n,-rba->inf.sphere.radius, p0 );
1136 v3_muladds( rbb->co, ct->n, rbb->inf.sphere.radius, p1 );
1137 v3_add( p0, p1, ct->co );
1138 v3_muls( ct->co, 0.5f, ct->co );
1139 ct->type = k_contact_type_default;
1140 ct->p = r-d;
1141 ct->rba = rba;
1142 ct->rbb = rbb;
1143 return 1;
1144 }
1145
1146 return 0;
1147 }
1148
1149 //#define RIGIDBODY_DYNAMIC_MESH_EDGES
1150
1151 VG_STATIC int rb_sphere_triangle( rigidbody *rba, rigidbody *rbb,
1152 v3f tri[3], rb_ct *buf )
1153 {
1154 v3f delta, co;
1155
1156 #ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
1157 closest_on_triangle_1( rba->co, tri, co );
1158 #else
1159 enum contact_type type = closest_on_triangle_1( rba->co, tri, co );
1160 #endif
1161
1162 v3_sub( rba->co, co, delta );
1163
1164 float d2 = v3_length2( delta ),
1165 r = rba->inf.sphere.radius;
1166
1167 if( d2 < r*r )
1168 {
1169 rb_ct *ct = buf;
1170
1171 v3f ab, ac, tn;
1172 v3_sub( tri[2], tri[0], ab );
1173 v3_sub( tri[1], tri[0], ac );
1174 v3_cross( ac, ab, tn );
1175 v3_copy( tn, ct->n );
1176 v3_normalize( ct->n );
1177
1178 float d = sqrtf(d2);
1179
1180 v3_copy( co, ct->co );
1181 ct->type = type;
1182 ct->p = r-d;
1183 ct->rba = rba;
1184 ct->rbb = rbb;
1185 return 1;
1186 }
1187
1188 return 0;
1189 }
1190
1191
1192 VG_STATIC void rb_debug_sharp_scene_edges( rigidbody *rbb, float sharp_ang,
1193 boxf box, u32 colour )
1194 {
1195 sharp_ang = cosf( sharp_ang );
1196
1197 scene *sc = rbb->inf.scene.bh_scene->user;
1198 vg_line_boxf( box, 0xff00ff00 );
1199
1200 bh_iter it;
1201 bh_iter_init( 0, &it );
1202 int idx;
1203
1204 while( bh_next( rbb->inf.scene.bh_scene, &it, box, &idx ) )
1205 {
1206 u32 *ptri = &sc->arrindices[ idx*3 ];
1207 v3f tri[3];
1208
1209 for( int j=0; j<3; j++ )
1210 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1211
1212 for( int j=0; j<3; j++ )
1213 {
1214 #if 0
1215 v3f edir;
1216 v3_sub( tri[(j+1)%3], tri[j], edir );
1217
1218 if( v3_dot( edir, (v3f){ 0.5184758473652127f,
1219 0.2073903389460850f,
1220 -0.8295613557843402f } ) < 0.0f )
1221 continue;
1222 #endif
1223
1224 bh_iter jt;
1225 bh_iter_init( 0, &jt );
1226
1227 boxf region;
1228 float const k_r = 0.02f;
1229 v3_add( (v3f){ k_r, k_r, k_r }, tri[j], region[1] );
1230 v3_add( (v3f){ -k_r, -k_r, -k_r }, tri[j], region[0] );
1231
1232 int jdx;
1233 while( bh_next( rbb->inf.scene.bh_scene, &jt, region, &jdx ) )
1234 {
1235 if( idx <= jdx )
1236 continue;
1237
1238 u32 *ptrj = &sc->arrindices[ jdx*3 ];
1239 v3f trj[3];
1240
1241 for( int k=0; k<3; k++ )
1242 v3_copy( sc->arrvertices[ptrj[k]].co, trj[k] );
1243
1244 for( int k=0; k<3; k++ )
1245 {
1246 if( v3_dist2( tri[j], trj[k] ) <= k_r*k_r )
1247 {
1248 int jp1 = (j+1)%3,
1249 jp2 = (j+2)%3,
1250 km1 = (k+3-1)%3,
1251 km2 = (k+3-2)%3;
1252
1253 if( v3_dist2( tri[jp1], trj[km1] ) <= k_r*k_r )
1254 {
1255 v3f b0, b1, b2;
1256 v3_sub( tri[jp1], tri[j], b0 );
1257 v3_sub( tri[jp2], tri[j], b1 );
1258 v3_sub( trj[km2], tri[j], b2 );
1259
1260 v3f cx0, cx1;
1261 v3_cross( b0, b1, cx0 );
1262 v3_cross( b2, b0, cx1 );
1263
1264 float polarity = v3_dot( cx0, b2 );
1265
1266 if( polarity < 0.0f )
1267 {
1268 #if 0
1269 vg_line( tri[j], tri[jp1], 0xff00ff00 );
1270 float ang = v3_dot(cx0,cx1) /
1271 (v3_length(cx0)*v3_length(cx1));
1272 if( ang < sharp_ang )
1273 {
1274 vg_line( tri[j], tri[jp1], 0xff00ff00 );
1275 }
1276 #endif
1277 }
1278 }
1279 }
1280 }
1281 }
1282 }
1283 }
1284 }
1285
1286 VG_STATIC int rb_sphere_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1287 {
1288 scene *sc = rbb->inf.scene.bh_scene->user;
1289
1290 bh_iter it;
1291 bh_iter_init( 0, &it );
1292 int idx;
1293
1294 int count = 0;
1295
1296 while( bh_next( rbb->inf.scene.bh_scene, &it, rba->bbx_world, &idx ) )
1297 {
1298 u32 *ptri = &sc->arrindices[ idx*3 ];
1299 v3f tri[3];
1300
1301 for( int j=0; j<3; j++ )
1302 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1303
1304 buf[ count ].element_id = ptri[0];
1305
1306 vg_line( tri[0],tri[1],0x70ff6000 );
1307 vg_line( tri[1],tri[2],0x70ff6000 );
1308 vg_line( tri[2],tri[0],0x70ff6000 );
1309
1310 int contact = rb_sphere_triangle( rba, rbb, tri, buf+count );
1311 count += contact;
1312
1313 if( count == 16 )
1314 {
1315 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
1316 return count;
1317 }
1318 }
1319
1320 return count;
1321 }
1322
1323 VG_STATIC int rb_box_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1324 {
1325 scene *sc = rbb->inf.scene.bh_scene->user;
1326
1327 v3f tri[3];
1328
1329 bh_iter it;
1330 bh_iter_init( 0, &it );
1331 int idx;
1332
1333 int count = 0;
1334
1335 while( bh_next( rbb->inf.scene.bh_scene, &it, rba->bbx_world, &idx ) )
1336 {
1337 u32 *ptri = &sc->arrindices[ idx*3 ];
1338
1339 for( int j=0; j<3; j++ )
1340 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1341
1342 if( rb_box_triangle_sat( rba, tri ) )
1343 {
1344 vg_line(tri[0],tri[1],0xff50ff00 );
1345 vg_line(tri[1],tri[2],0xff50ff00 );
1346 vg_line(tri[2],tri[0],0xff50ff00 );
1347 }
1348 else
1349 {
1350 vg_line(tri[0],tri[1],0xff0000ff );
1351 vg_line(tri[1],tri[2],0xff0000ff );
1352 vg_line(tri[2],tri[0],0xff0000ff );
1353
1354 continue;
1355 }
1356
1357 v3f v0,v1,n;
1358 v3_sub( tri[1], tri[0], v0 );
1359 v3_sub( tri[2], tri[0], v1 );
1360 v3_cross( v0, v1, n );
1361 v3_normalize( n );
1362
1363 /* find best feature */
1364 float best = v3_dot( rba->right, n );
1365 int axis = 0;
1366
1367 float cy = v3_dot( rba->up, n );
1368 if( fabsf(cy) > fabsf(best) )
1369 {
1370 best = cy;
1371 axis = 1;
1372 }
1373
1374 float cz = -v3_dot( rba->forward, n );
1375 if( fabsf(cz) > fabsf(best) )
1376 {
1377 best = cz;
1378 axis = 2;
1379 }
1380
1381 v3f manifold[4];
1382
1383 if( axis == 0 )
1384 {
1385 float px = best > 0.0f? rba->bbx[0][0]: rba->bbx[1][0];
1386 manifold[0][0] = px;
1387 manifold[0][1] = rba->bbx[0][1];
1388 manifold[0][2] = rba->bbx[0][2];
1389 manifold[1][0] = px;
1390 manifold[1][1] = rba->bbx[1][1];
1391 manifold[1][2] = rba->bbx[0][2];
1392 manifold[2][0] = px;
1393 manifold[2][1] = rba->bbx[1][1];
1394 manifold[2][2] = rba->bbx[1][2];
1395 manifold[3][0] = px;
1396 manifold[3][1] = rba->bbx[0][1];
1397 manifold[3][2] = rba->bbx[1][2];
1398 }
1399 else if( axis == 1 )
1400 {
1401 float py = best > 0.0f? rba->bbx[0][1]: rba->bbx[1][1];
1402 manifold[0][0] = rba->bbx[0][0];
1403 manifold[0][1] = py;
1404 manifold[0][2] = rba->bbx[0][2];
1405 manifold[1][0] = rba->bbx[1][0];
1406 manifold[1][1] = py;
1407 manifold[1][2] = rba->bbx[0][2];
1408 manifold[2][0] = rba->bbx[1][0];
1409 manifold[2][1] = py;
1410 manifold[2][2] = rba->bbx[1][2];
1411 manifold[3][0] = rba->bbx[0][0];
1412 manifold[3][1] = py;
1413 manifold[3][2] = rba->bbx[1][2];
1414 }
1415 else
1416 {
1417 float pz = best > 0.0f? rba->bbx[0][2]: rba->bbx[1][2];
1418 manifold[0][0] = rba->bbx[0][0];
1419 manifold[0][1] = rba->bbx[0][1];
1420 manifold[0][2] = pz;
1421 manifold[1][0] = rba->bbx[1][0];
1422 manifold[1][1] = rba->bbx[0][1];
1423 manifold[1][2] = pz;
1424 manifold[2][0] = rba->bbx[1][0];
1425 manifold[2][1] = rba->bbx[1][1];
1426 manifold[2][2] = pz;
1427 manifold[3][0] = rba->bbx[0][0];
1428 manifold[3][1] = rba->bbx[1][1];
1429 manifold[3][2] = pz;
1430 }
1431
1432 for( int j=0; j<4; j++ )
1433 m4x3_mulv( rba->to_world, manifold[j], manifold[j] );
1434
1435 vg_line( manifold[0], manifold[1], 0xffffffff );
1436 vg_line( manifold[1], manifold[2], 0xffffffff );
1437 vg_line( manifold[2], manifold[3], 0xffffffff );
1438 vg_line( manifold[3], manifold[0], 0xffffffff );
1439
1440 for( int j=0; j<4; j++ )
1441 {
1442 rb_ct *ct = buf+count;
1443
1444 v3_copy( manifold[j], ct->co );
1445 v3_copy( n, ct->n );
1446
1447 float l0 = v3_dot( tri[0], n ),
1448 l1 = v3_dot( manifold[j], n );
1449
1450 ct->p = (l0-l1)*0.5f;
1451 if( ct->p < 0.0f )
1452 continue;
1453
1454 ct->type = k_contact_type_default;
1455 ct->rba = rba;
1456 ct->rbb = rbb;
1457 count ++;
1458
1459 if( count >= 12 )
1460 return count;
1461 }
1462 }
1463 return count;
1464 }
1465
1466 VG_STATIC int RB_MATRIX_ERROR( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1467 {
1468 vg_error( "Collision type is unimplemented between types %d and %d\n",
1469 rba->type, rbb->type );
1470
1471 return 0;
1472 }
1473
1474 VG_STATIC int rb_sphere_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1475 {
1476 return rb_capsule_sphere( rbb, rba, buf );
1477 }
1478
1479 VG_STATIC int rb_box_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1480 {
1481 return rb_capsule_box( rbb, rba, buf );
1482 }
1483
1484 VG_STATIC int rb_box_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1485 {
1486 return rb_sphere_box( rbb, rba, buf );
1487 }
1488
1489 VG_STATIC int rb_scene_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1490 {
1491 return rb_box_scene( rbb, rba, buf );
1492 }
1493
1494 VG_STATIC int (*rb_jump_table[4][4])( rigidbody *a, rigidbody *b, rb_ct *buf ) =
1495 {
1496 /* box */ /* Sphere */ /* Capsule */ /* Mesh */
1497 { RB_MATRIX_ERROR, rb_box_sphere, rb_box_capsule, rb_box_scene },
1498 { rb_sphere_box, rb_sphere_sphere, rb_sphere_capsule, rb_sphere_scene },
1499 { rb_capsule_box, rb_capsule_sphere, rb_capsule_capsule, RB_MATRIX_ERROR },
1500 { rb_scene_box, RB_MATRIX_ERROR, RB_MATRIX_ERROR, RB_MATRIX_ERROR }
1501 };
1502
1503 VG_STATIC int rb_collide( rigidbody *rba, rigidbody *rbb )
1504 {
1505 int (*collider_jump)(rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1506 = rb_jump_table[rba->type][rbb->type];
1507
1508 /*
1509 * 12 is the maximum manifold size we can generate, so we are forced to abort
1510 * potentially checking any more.
1511 */
1512 if( rb_contact_count + 12 > vg_list_size(rb_contact_buffer) )
1513 {
1514 vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
1515 rb_contact_count, vg_list_size(rb_contact_buffer) );
1516 return 0;
1517 }
1518
1519 /*
1520 * FUTURE: Replace this with a more dedicated broad phase pass
1521 */
1522 if( box_overlap( rba->bbx_world, rbb->bbx_world ) )
1523 {
1524 int count = collider_jump( rba, rbb, rb_contact_buffer+rb_contact_count);
1525 rb_contact_count += count;
1526 return count;
1527 }
1528 else
1529 return 0;
1530 }
1531
1532 /*
1533 * -----------------------------------------------------------------------------
1534 * Dynamics
1535 * -----------------------------------------------------------------------------
1536 */
1537
1538 VG_STATIC void rb_solver_reset(void)
1539 {
1540 rb_contact_count = 0;
1541 }
1542
1543 VG_STATIC rb_ct *rb_global_ct(void)
1544 {
1545 return rb_contact_buffer + rb_contact_count;
1546 }
1547
1548 /*
1549 * Initializing things like tangent vectors
1550 */
1551 VG_STATIC void rb_presolve_contacts( rb_ct *buffer, int len )
1552 {
1553 for( int i=0; i<len; i++ )
1554 {
1555 rb_ct *ct = &buffer[i];
1556
1557 ct->bias = -0.2f * k_rb_rate * vg_minf( 0.0f, -ct->p+k_penetration_slop );
1558 rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
1559
1560 #if 0
1561 ct->type = k_contact_type_default;
1562 #endif
1563 ct->norm_impulse = 0.0f;
1564 ct->tangent_impulse[0] = 0.0f;
1565 ct->tangent_impulse[1] = 0.0f;
1566
1567 v3f ra, rb, raCn, rbCn, raCt, rbCt;
1568 v3_sub( ct->co, ct->rba->co, ra );
1569 v3_sub( ct->co, ct->rbb->co, rb );
1570 v3_cross( ra, ct->n, raCn );
1571 v3_cross( rb, ct->n, rbCn );
1572
1573 /* orient inverse inertia tensors */
1574 v3f raCnI, rbCnI;
1575 m3x3_mulv( ct->rba->iIw, raCn, raCnI );
1576 m3x3_mulv( ct->rbb->iIw, rbCn, rbCnI );
1577
1578 ct->normal_mass = ct->rba->inv_mass + ct->rbb->inv_mass;
1579 ct->normal_mass += v3_dot( raCn, raCnI );
1580 ct->normal_mass += v3_dot( rbCn, rbCnI );
1581 ct->normal_mass = 1.0f/ct->normal_mass;
1582
1583 for( int j=0; j<2; j++ )
1584 {
1585 v3f raCtI, rbCtI;
1586 v3_cross( ct->t[j], ra, raCt );
1587 v3_cross( ct->t[j], rb, rbCt );
1588 m3x3_mulv( ct->rba->iIw, raCt, raCtI );
1589 m3x3_mulv( ct->rbb->iIw, rbCt, rbCtI );
1590
1591 ct->tangent_mass[j] = ct->rba->inv_mass + ct->rbb->inv_mass;
1592 ct->tangent_mass[j] += v3_dot( raCt, raCtI );
1593 ct->tangent_mass[j] += v3_dot( rbCt, rbCtI );
1594 ct->tangent_mass[j] = 1.0f/ct->tangent_mass[j];
1595 }
1596
1597 rb_debug_contact( ct );
1598 }
1599 }
1600
1601 /*
1602 * Creates relative contact velocity vector, and offsets between each body
1603 */
1604 VG_STATIC void rb_rcv( rb_ct *ct, v3f rv, v3f da, v3f db )
1605 {
1606 rigidbody *rba = ct->rba,
1607 *rbb = ct->rbb;
1608
1609 v3_sub( ct->co, rba->co, da );
1610 v3_sub( ct->co, rbb->co, db );
1611
1612 v3f rva, rvb;
1613 v3_cross( rba->w, da, rva );
1614 v3_add( rba->v, rva, rva );
1615 v3_cross( rbb->w, db, rvb );
1616 v3_add( rbb->v, rvb, rvb );
1617
1618 v3_sub( rva, rvb, rv );
1619 }
1620
1621 /*
1622 * Apply impulse to object
1623 */
1624 VG_STATIC void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse )
1625 {
1626 /* linear */
1627 v3_muladds( rb->v, impulse, rb->inv_mass, rb->v );
1628
1629 /* Angular velocity */
1630 v3f wa;
1631 v3_cross( delta, impulse, wa );
1632
1633 m3x3_mulv( rb->iIw, wa, wa );
1634 v3_add( rb->w, wa, rb->w );
1635 }
1636
1637 /*
1638 * One iteration to solve the contact constraint
1639 */
1640 VG_STATIC void rb_solve_contacts( rb_ct *buf, int len )
1641 {
1642 for( int i=0; i<len; i++ )
1643 {
1644 struct contact *ct = &buf[i];
1645
1646 rigidbody *rb = ct->rba;
1647
1648 v3f rv, da, db;
1649 rb_rcv( ct, rv, da, db );
1650
1651 /* Friction */
1652 for( int j=0; j<2; j++ )
1653 {
1654 float f = k_friction * ct->norm_impulse,
1655 vt = v3_dot( rv, ct->t[j] ),
1656 lambda = ct->tangent_mass[j] * -vt;
1657
1658 float temp = ct->tangent_impulse[j];
1659 ct->tangent_impulse[j] = vg_clampf( temp + lambda, -f, f );
1660 lambda = ct->tangent_impulse[j] - temp;
1661
1662 v3f impulse;
1663 v3_muls( ct->t[j], lambda, impulse );
1664 rb_linear_impulse( ct->rba, da, impulse );
1665
1666 v3_muls( ct->t[j], -lambda, impulse );
1667 rb_linear_impulse( ct->rbb, db, impulse );
1668 }
1669
1670 /* Normal */
1671 rb_rcv( ct, rv, da, db );
1672 float vn = v3_dot( rv, ct->n ),
1673 lambda = ct->normal_mass * (-vn + ct->bias);
1674
1675 float temp = ct->norm_impulse;
1676 ct->norm_impulse = vg_maxf( temp + lambda, 0.0f );
1677 lambda = ct->norm_impulse - temp;
1678
1679 v3f impulse;
1680 v3_muls( ct->n, lambda, impulse );
1681 rb_linear_impulse( ct->rba, da, impulse );
1682
1683 v3_muls( ct->n, -lambda, impulse );
1684 rb_linear_impulse( ct->rbb, db, impulse );
1685 }
1686 }
1687
1688 /*
1689 * -----------------------------------------------------------------------------
1690 * Constraints
1691 * -----------------------------------------------------------------------------
1692 */
1693
1694 VG_STATIC void draw_angle_limit( v3f c, v3f major, v3f minor,
1695 float amin, float amax, float measured,
1696 u32 colour )
1697 {
1698 float f = 0.05f;
1699 v3f ay, ax;
1700 v3_muls( major, f, ay );
1701 v3_muls( minor, f, ax );
1702
1703 for( int x=0; x<16; x++ )
1704 {
1705 float t0 = (float)x / 16.0f,
1706 t1 = (float)(x+1) / 16.0f,
1707 a0 = vg_lerpf( amin, amax, t0 ),
1708 a1 = vg_lerpf( amin, amax, t1 );
1709
1710 v3f p0, p1;
1711 v3_muladds( c, ay, cosf(a0), p0 );
1712 v3_muladds( p0, ax, sinf(a0), p0 );
1713 v3_muladds( c, ay, cosf(a1), p1 );
1714 v3_muladds( p1, ax, sinf(a1), p1 );
1715
1716 vg_line( p0, p1, colour );
1717
1718 if( x == 0 )
1719 vg_line( c, p0, colour );
1720 if( x == 15 )
1721 vg_line( c, p1, colour );
1722 }
1723
1724 v3f p2;
1725 v3_muladds( c, ay, cosf(measured)*1.2f, p2 );
1726 v3_muladds( p2, ax, sinf(measured)*1.2f, p2 );
1727 vg_line( c, p2, colour );
1728 }
1729
1730 VG_STATIC void rb_debug_constraint_limits( rigidbody *ra, rigidbody *rb, v3f lca,
1731 v3f limits[2] )
1732 {
1733 v3f ax, ay, az, bx, by, bz;
1734 m3x3_mulv( ra->to_world, (v3f){1.0f,0.0f,0.0f}, ax );
1735 m3x3_mulv( ra->to_world, (v3f){0.0f,1.0f,0.0f}, ay );
1736 m3x3_mulv( ra->to_world, (v3f){0.0f,0.0f,1.0f}, az );
1737 m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f,0.0f}, bx );
1738 m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f,0.0f}, by );
1739 m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,1.0f}, bz );
1740
1741 v2f px, py, pz;
1742 px[0] = v3_dot( ay, by );
1743 px[1] = v3_dot( az, by );
1744
1745 py[0] = v3_dot( az, bz );
1746 py[1] = v3_dot( ax, bz );
1747
1748 pz[0] = v3_dot( ax, bx );
1749 pz[1] = v3_dot( ay, bx );
1750
1751 float r0 = atan2f( px[1], px[0] ),
1752 r1 = atan2f( py[1], py[0] ),
1753 r2 = atan2f( pz[1], pz[0] );
1754
1755 v3f c;
1756 m4x3_mulv( ra->to_world, lca, c );
1757 draw_angle_limit( c, ay, az, limits[0][0], limits[1][0], r0, 0xff0000ff );
1758 draw_angle_limit( c, az, ax, limits[0][1], limits[1][1], r1, 0xff00ff00 );
1759 draw_angle_limit( c, ax, ay, limits[0][2], limits[1][2], r2, 0xffff0000 );
1760 }
1761
1762 VG_STATIC void rb_limit_cure( rigidbody *ra, rigidbody *rb, v3f axis, float d )
1763 {
1764 if( d != 0.0f )
1765 {
1766 float avx = v3_dot( ra->w, axis ) - v3_dot( rb->w, axis );
1767 float joint_mass = rb->inv_mass + ra->inv_mass;
1768 joint_mass = 1.0f/joint_mass;
1769
1770 float bias = (k_limit_bias * k_rb_rate) * d,
1771 lambda = -(avx + bias) * joint_mass;
1772
1773 /* Angular velocity */
1774 v3f wa, wb;
1775 v3_muls( axis, lambda * ra->inv_mass, wa );
1776 v3_muls( axis, -lambda * rb->inv_mass, wb );
1777
1778 v3_add( ra->w, wa, ra->w );
1779 v3_add( rb->w, wb, rb->w );
1780 }
1781 }
1782
1783 VG_STATIC void rb_constraint_limits( rigidbody *ra, v3f lca,
1784 rigidbody *rb, v3f lcb, v3f limits[2] )
1785 {
1786 v3f ax, ay, az, bx, by, bz;
1787 m3x3_mulv( ra->to_world, (v3f){1.0f,0.0f,0.0f}, ax );
1788 m3x3_mulv( ra->to_world, (v3f){0.0f,1.0f,0.0f}, ay );
1789 m3x3_mulv( ra->to_world, (v3f){0.0f,0.0f,1.0f}, az );
1790 m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f,0.0f}, bx );
1791 m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f,0.0f}, by );
1792 m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,1.0f}, bz );
1793
1794 v2f px, py, pz;
1795 px[0] = v3_dot( ay, by );
1796 px[1] = v3_dot( az, by );
1797
1798 py[0] = v3_dot( az, bz );
1799 py[1] = v3_dot( ax, bz );
1800
1801 pz[0] = v3_dot( ax, bx );
1802 pz[1] = v3_dot( ay, bx );
1803
1804 float r0 = atan2f( px[1], px[0] ),
1805 r1 = atan2f( py[1], py[0] ),
1806 r2 = atan2f( pz[1], pz[0] );
1807
1808 /* calculate angle deltas */
1809 float dx = 0.0f, dy = 0.0f, dz = 0.0f;
1810
1811 if( r0 < limits[0][0] ) dx = limits[0][0] - r0;
1812 if( r0 > limits[1][0] ) dx = limits[1][0] - r0;
1813 if( r1 < limits[0][1] ) dy = limits[0][1] - r1;
1814 if( r1 > limits[1][1] ) dy = limits[1][1] - r1;
1815 if( r2 < limits[0][2] ) dz = limits[0][2] - r2;
1816 if( r2 > limits[1][2] ) dz = limits[1][2] - r2;
1817
1818 v3f wca, wcb;
1819 m3x3_mulv( ra->to_world, lca, wca );
1820 m3x3_mulv( rb->to_world, lcb, wcb );
1821
1822 rb_limit_cure( ra, rb, ax, dx );
1823 rb_limit_cure( ra, rb, ay, dy );
1824 rb_limit_cure( ra, rb, az, dz );
1825 }
1826
1827 VG_STATIC void rb_debug_constraint_position( rigidbody *ra, v3f lca,
1828 rigidbody *rb, v3f lcb )
1829 {
1830 v3f wca, wcb;
1831 m3x3_mulv( ra->to_world, lca, wca );
1832 m3x3_mulv( rb->to_world, lcb, wcb );
1833
1834 v3f p0, p1;
1835 v3_add( wca, ra->co, p0 );
1836 v3_add( wcb, rb->co, p1 );
1837 vg_line_pt3( p0, 0.005f, 0xffffff00 );
1838 vg_line_pt3( p1, 0.005f, 0xffffff00 );
1839 vg_line( p0, p1, 0xffffff00 );
1840 }
1841
1842 VG_STATIC void rb_constraint_position( rigidbody *ra, v3f lca,
1843 rigidbody *rb, v3f lcb )
1844 {
1845 /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
1846 v3f wca, wcb;
1847 m3x3_mulv( ra->to_world, lca, wca );
1848 m3x3_mulv( rb->to_world, lcb, wcb );
1849
1850 v3f rcv;
1851 v3_sub( ra->v, rb->v, rcv );
1852
1853 v3f rcv_Ra, rcv_Rb;
1854 v3_cross( ra->w, wca, rcv_Ra );
1855 v3_cross( rb->w, wcb, rcv_Rb );
1856 v3_add( rcv_Ra, rcv, rcv );
1857 v3_sub( rcv, rcv_Rb, rcv );
1858
1859 v3f delta;
1860 v3f p0, p1;
1861 v3_add( wca, ra->co, p0 );
1862 v3_add( wcb, rb->co, p1 );
1863 v3_sub( p1, p0, delta );
1864
1865 float dist2 = v3_length2( delta );
1866
1867 if( dist2 > 0.00001f )
1868 {
1869 float dist = sqrtf(dist2);
1870 v3_muls( delta, 1.0f/dist, delta );
1871
1872 float joint_mass = rb->inv_mass + ra->inv_mass;
1873
1874 v3f raCn, rbCn, raCt, rbCt;
1875 v3_cross( wca, delta, raCn );
1876 v3_cross( wcb, delta, rbCn );
1877
1878 /* orient inverse inertia tensors */
1879 v3f raCnI, rbCnI;
1880 m3x3_mulv( ra->iIw, raCn, raCnI );
1881 m3x3_mulv( rb->iIw, rbCn, rbCnI );
1882 joint_mass += v3_dot( raCn, raCnI );
1883 joint_mass += v3_dot( rbCn, rbCnI );
1884 joint_mass = 1.0f/joint_mass;
1885
1886 float vd = v3_dot( rcv, delta ),
1887 bias = -(k_joint_bias * k_rb_rate) * dist,
1888 lambda = -(vd + bias) * joint_mass;
1889
1890 v3f impulse;
1891 v3_muls( delta, lambda, impulse );
1892 rb_linear_impulse( ra, wca, impulse );
1893 v3_muls( delta, -lambda, impulse );
1894 rb_linear_impulse( rb, wcb, impulse );
1895
1896 /* 'fake' snap */
1897 v3_muladds( ra->co, delta, dist * k_joint_correction, ra->co );
1898 v3_muladds( rb->co, delta, -dist * k_joint_correction, rb->co );
1899 }
1900 }
1901
1902 /*
1903 * Effectors
1904 */
1905
1906 VG_STATIC void rb_effect_simple_bouyency( rigidbody *ra, v4f plane,
1907 float amt, float drag )
1908 {
1909 /* float */
1910 float depth = v3_dot( plane, ra->co ) - plane[3],
1911 lambda = vg_clampf( -depth, 0.0f, 1.0f ) * amt;
1912
1913 v3_muladds( ra->v, plane, lambda * k_rb_delta, ra->v );
1914
1915 if( depth < 0.0f )
1916 v3_muls( ra->v, 1.0f-(drag*k_rb_delta), ra->v );
1917 }
1918
1919 /*
1920 * -----------------------------------------------------------------------------
1921 * BVH implementation, this is ONLY for VG_STATIC rigidbodies, its to slow for
1922 * realtime use.
1923 * -----------------------------------------------------------------------------
1924 */
1925
1926 VG_STATIC void rb_bh_expand_bound( void *user, boxf bound, u32 item_index )
1927 {
1928 rigidbody *rb = &((rigidbody *)user)[ item_index ];
1929 box_concat( bound, rb->bbx_world );
1930 }
1931
1932 VG_STATIC float rb_bh_centroid( void *user, u32 item_index, int axis )
1933 {
1934 rigidbody *rb = &((rigidbody *)user)[ item_index ];
1935 return (rb->bbx_world[axis][0] + rb->bbx_world[1][axis]) * 0.5f;
1936 }
1937
1938 VG_STATIC void rb_bh_swap( void *user, u32 ia, u32 ib )
1939 {
1940 rigidbody temp, *rba, *rbb;
1941 rba = &((rigidbody *)user)[ ia ];
1942 rbb = &((rigidbody *)user)[ ib ];
1943
1944 temp = *rba;
1945 *rba = *rbb;
1946 *rbb = temp;
1947 }
1948
1949 VG_STATIC void rb_bh_debug( void *user, u32 item_index )
1950 {
1951 rigidbody *rb = &((rigidbody *)user)[ item_index ];
1952 rb_debug( rb, 0xff00ffff );
1953 }
1954
1955 VG_STATIC bh_system bh_system_rigidbodies =
1956 {
1957 .expand_bound = rb_bh_expand_bound,
1958 .item_centroid = rb_bh_centroid,
1959 .item_swap = rb_bh_swap,
1960 .item_debug = rb_bh_debug,
1961 .cast_ray = NULL
1962 };
1963
1964 #endif /* RIGIDBODY_H */
Skaterift source tree