projects / carveJwlIkooP6JGAAIwe30JlM.git / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
history | raw | HEAD
phys adjustments
[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 /*
590 * Merge two contacts if they are within radius(r) of eachother
591 */
592 VG_STATIC void rb_manifold_contact_weld( rb_ct *ci, rb_ct *cj, float r )
593 {
594 if( v3_dist2( ci->co, cj->co ) < r*r )
595 {
596 cj->type = k_contact_type_disabled;
597 ci->p = (ci->p + cj->p) * 0.5f;
598
599 v3_add( ci->co, cj->co, ci->co );
600 v3_muls( ci->co, 0.5f, ci->co );
601
602 v3f delta;
603 v3_sub( ci->rba->co, ci->co, delta );
604
605 float c0 = v3_dot( ci->n, delta ),
606 c1 = v3_dot( cj->n, delta );
607
608 if( c0 < 0.0f || c1 < 0.0f )
609 {
610 /* error */
611 ci->type = k_contact_type_disabled;
612 }
613 else
614 {
615 v3f n;
616 v3_muls( ci->n, c0, n );
617 v3_muladds( n, cj->n, c1, n );
618 v3_normalize( n );
619 v3_copy( n, ci->n );
620 }
621 }
622 }
623
624 /*
625 *
626 */
627 VG_STATIC void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r )
628 {
629 for( int i=0; i<len-1; i++ )
630 {
631 rb_ct *ci = &man[i];
632 if( ci->type != k_contact_type_edge )
633 continue;
634
635 for( int j=i+1; j<len; j++ )
636 {
637 rb_ct *cj = &man[j];
638 if( cj->type != k_contact_type_edge )
639 continue;
640
641 rb_manifold_contact_weld( ci, cj, r );
642 }
643 }
644 }
645
646 /*
647 * Resolve overlapping pairs
648 *
649 * TODO: Remove?
650 */
651 VG_STATIC void rb_manifold_filter_pairs( rb_ct *man, int len, float r )
652 {
653 for( int i=0; i<len-1; i++ )
654 {
655 rb_ct *ci = &man[i];
656 int similar = 0;
657
658 if( ci->type == k_contact_type_disabled ) continue;
659
660 for( int j=i+1; j<len; j++ )
661 {
662 rb_ct *cj = &man[j];
663
664 if( cj->type == k_contact_type_disabled ) continue;
665
666 if( v3_dist2( ci->co, cj->co ) < r*r )
667 {
668 cj->type = k_contact_type_disabled;
669 v3_add( cj->n, ci->n, ci->n );
670 ci->p += cj->p;
671 similar ++;
672 }
673 }
674
675 if( similar )
676 {
677 float n = 1.0f/((float)similar+1.0f);
678 v3_muls( ci->n, n, ci->n );
679 ci->p *= n;
680
681 if( v3_length2(ci->n) < 0.1f*0.1f )
682 ci->type = k_contact_type_disabled;
683 else
684 v3_normalize( ci->n );
685 }
686 }
687 }
688
689 /*
690 * Remove contacts that are facing away from A
691 */
692 VG_STATIC void rb_manifold_filter_backface( rb_ct *man, int len )
693 {
694 for( int i=0; i<len; i++ )
695 {
696 rb_ct *ct = &man[i];
697 if( ct->type == k_contact_type_disabled )
698 continue;
699
700 v3f delta;
701 v3_sub( ct->co, ct->rba->co, delta );
702
703 if( v3_dot( delta, ct->n ) > -0.001f )
704 ct->type = k_contact_type_disabled;
705 }
706 }
707
708 /*
709 * Filter out duplicate coplanar results. Good for spheres.
710 */
711 VG_STATIC void rb_manifold_filter_coplanar( rb_ct *man, int len, float w )
712 {
713 for( int i=0; i<len; i++ )
714 {
715 rb_ct *ci = &man[i];
716 if( ci->type == k_contact_type_disabled ||
717 ci->type == k_contact_type_edge )
718 continue;
719
720 float d1 = v3_dot( ci->co, ci->n );
721
722 for( int j=0; j<len; j++ )
723 {
724 if( j == i )
725 continue;
726
727 rb_ct *cj = &man[j];
728 if( cj->type == k_contact_type_disabled )
729 continue;
730
731 float d2 = v3_dot( cj->co, ci->n ),
732 d = d2-d1;
733
734 if( fabsf( d ) <= w )
735 {
736 cj->type = k_contact_type_disabled;
737 }
738 }
739 }
740 }
741
742 /*
743 * -----------------------------------------------------------------------------
744 * Collision matrix
745 * -----------------------------------------------------------------------------
746 */
747
748 /*
749 * Contact generators
750 *
751 * These do not automatically allocate contacts, an appropriately sized
752 * buffer must be supplied. The function returns the size of the manifold
753 * which was generated.
754 *
755 * The values set on the contacts are: n, co, p, rba, rbb
756 */
757
758 /*
759 * By collecting the minimum(time) and maximum(time) pairs of points, we
760 * build a reduced and stable exact manifold.
761 *
762 * tx: time at point
763 * rx: minimum distance of these points
764 * dx: the delta between the two points
765 *
766 * pairs will only ammend these if they are creating a collision
767 */
768 typedef struct capsule_manifold capsule_manifold;
769 struct capsule_manifold
770 {
771 float t0, t1;
772 float r0, r1;
773 v3f d0, d1;
774 };
775
776 /*
777 * Expand a line manifold with a new pair. t value is the time along segment
778 * on the oriented object which created this pair.
779 */
780 VG_STATIC void rb_capsule_manifold( v3f pa, v3f pb, float t, float r,
781 capsule_manifold *manifold )
782 {
783 v3f delta;
784 v3_sub( pa, pb, delta );
785
786 if( v3_length2(delta) < r*r )
787 {
788 if( t < manifold->t0 )
789 {
790 v3_copy( delta, manifold->d0 );
791 manifold->t0 = t;
792 manifold->r0 = r;
793 }
794
795 if( t > manifold->t1 )
796 {
797 v3_copy( delta, manifold->d1 );
798 manifold->t1 = t;
799 manifold->r1 = r;
800 }
801 }
802 }
803
804 VG_STATIC void rb_capsule_manifold_init( capsule_manifold *manifold )
805 {
806 manifold->t0 = INFINITY;
807 manifold->t1 = -INFINITY;
808 }
809
810 VG_STATIC int rb_capsule_manifold_done( rigidbody *rba, rigidbody *rbb,
811 capsule_manifold *manifold, rb_ct *buf )
812 {
813 float h = rba->inf.capsule.height,
814 ra = rba->inf.capsule.radius;
815
816 v3f p0, p1;
817 v3_muladds( rba->co, rba->up, -h*0.5f+ra, p0 );
818 v3_muladds( rba->co, rba->up, h*0.5f-ra, p1 );
819
820 int count = 0;
821 if( manifold->t0 <= 1.0f )
822 {
823 rb_ct *ct = buf;
824
825 v3f pa;
826 v3_muls( p0, 1.0f-manifold->t0, pa );
827 v3_muladds( pa, p1, manifold->t0, pa );
828
829 float d = v3_length( manifold->d0 );
830 v3_muls( manifold->d0, 1.0f/d, ct->n );
831 v3_muladds( pa, ct->n, -ra, ct->co );
832
833 ct->p = manifold->r0 - d;
834 ct->rba = rba;
835 ct->rbb = rbb;
836 ct->type = k_contact_type_default;
837
838 count ++;
839 }
840
841 if( (manifold->t1 >= 0.0f) && (manifold->t0 != manifold->t1) )
842 {
843 rb_ct *ct = buf+count;
844
845 v3f pa;
846 v3_muls( p0, 1.0f-manifold->t1, pa );
847 v3_muladds( pa, p1, manifold->t1, pa );
848
849 float d = v3_length( manifold->d1 );
850 v3_muls( manifold->d1, 1.0f/d, ct->n );
851 v3_muladds( pa, ct->n, -ra, ct->co );
852
853 ct->p = manifold->r1 - d;
854 ct->rba = rba;
855 ct->rbb = rbb;
856 ct->type = k_contact_type_default;
857
858 count ++;
859 }
860
861 /*
862 * Debugging
863 */
864
865 if( count == 2 )
866 vg_line( buf[0].co, buf[1].co, 0xff0000ff );
867
868 return count;
869 }
870
871 VG_STATIC int rb_capsule_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
872 {
873 float h = rba->inf.capsule.height,
874 ra = rba->inf.capsule.radius,
875 rb = rbb->inf.sphere.radius;
876
877 v3f p0, p1;
878 v3_muladds( rba->co, rba->up, -h*0.5f+ra, p0 );
879 v3_muladds( rba->co, rba->up, h*0.5f-ra, p1 );
880
881 v3f c, delta;
882 closest_point_segment( p0, p1, rbb->co, c );
883 v3_sub( c, rbb->co, delta );
884
885 float d2 = v3_length2(delta),
886 r = ra + rb;
887
888 if( d2 < r*r )
889 {
890 float d = sqrtf(d2);
891
892 rb_ct *ct = buf;
893 v3_muls( delta, 1.0f/d, ct->n );
894 ct->p = r-d;
895
896 v3f p0, p1;
897 v3_muladds( c, ct->n, -ra, p0 );
898 v3_muladds( rbb->co, ct->n, rb, p1 );
899 v3_add( p0, p1, ct->co );
900 v3_muls( ct->co, 0.5f, ct->co );
901
902 ct->rba = rba;
903 ct->rbb = rbb;
904 ct->type = k_contact_type_default;
905
906 return 1;
907 }
908
909 return 0;
910 }
911
912 VG_STATIC int rb_capsule_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
913 {
914 float ha = rba->inf.capsule.height,
915 hb = rbb->inf.capsule.height,
916 ra = rba->inf.capsule.radius,
917 rb = rbb->inf.capsule.radius,
918 r = ra+rb;
919
920 v3f p0, p1, p2, p3;
921 v3_muladds( rba->co, rba->up, -ha*0.5f+ra, p0 );
922 v3_muladds( rba->co, rba->up, ha*0.5f-ra, p1 );
923 v3_muladds( rbb->co, rbb->up, -hb*0.5f+rb, p2 );
924 v3_muladds( rbb->co, rbb->up, hb*0.5f-rb, p3 );
925
926 capsule_manifold manifold;
927 rb_capsule_manifold_init( &manifold );
928
929 v3f pa, pb;
930 float ta, tb;
931 closest_segment_segment( p0, p1, p2, p3, &ta, &tb, pa, pb );
932 rb_capsule_manifold( pa, pb, ta, r, &manifold );
933
934 ta = closest_point_segment( p0, p1, p2, pa );
935 tb = closest_point_segment( p0, p1, p3, pb );
936 rb_capsule_manifold( pa, p2, ta, r, &manifold );
937 rb_capsule_manifold( pb, p3, tb, r, &manifold );
938
939 closest_point_segment( p2, p3, p0, pa );
940 closest_point_segment( p2, p3, p1, pb );
941 rb_capsule_manifold( p0, pa, 0.0f, r, &manifold );
942 rb_capsule_manifold( p1, pb, 1.0f, r, &manifold );
943
944 return rb_capsule_manifold_done( rba, rbb, &manifold, buf );
945 }
946
947 /*
948 * Generates up to two contacts; optimised for the most stable manifold
949 */
950 VG_STATIC int rb_capsule_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
951 {
952 float h = rba->inf.capsule.height,
953 r = rba->inf.capsule.radius;
954
955 /*
956 * Solving this in symetric local space of the cube saves us some time and a
957 * couple branches when it comes to the quad stage.
958 */
959 v3f centroid;
960 v3_add( rbb->bbx[0], rbb->bbx[1], centroid );
961 v3_muls( centroid, 0.5f, centroid );
962
963 boxf bbx;
964 v3_sub( rbb->bbx[0], centroid, bbx[0] );
965 v3_sub( rbb->bbx[1], centroid, bbx[1] );
966
967 v3f pc, p0w, p1w, p0, p1;
968 v3_muladds( rba->co, rba->up, -h*0.5f+r, p0w );
969 v3_muladds( rba->co, rba->up, h*0.5f-r, p1w );
970
971 m4x3_mulv( rbb->to_local, p0w, p0 );
972 m4x3_mulv( rbb->to_local, p1w, p1 );
973 v3_sub( p0, centroid, p0 );
974 v3_sub( p1, centroid, p1 );
975 v3_add( p0, p1, pc );
976 v3_muls( pc, 0.5f, pc );
977
978 /*
979 * Finding an appropriate quad to collide lines with
980 */
981 v3f region;
982 v3_div( pc, bbx[1], region );
983
984 v3f quad[4];
985 if( (fabsf(region[0]) > fabsf(region[1])) &&
986 (fabsf(region[0]) > fabsf(region[2])) )
987 {
988 float px = vg_signf(region[0]) * bbx[1][0];
989 v3_copy( (v3f){ px, bbx[0][1], bbx[0][2] }, quad[0] );
990 v3_copy( (v3f){ px, bbx[1][1], bbx[0][2] }, quad[1] );
991 v3_copy( (v3f){ px, bbx[1][1], bbx[1][2] }, quad[2] );
992 v3_copy( (v3f){ px, bbx[0][1], bbx[1][2] }, quad[3] );
993 }
994 else if( fabsf(region[1]) > fabsf(region[2]) )
995 {
996 float py = vg_signf(region[1]) * bbx[1][1];
997 v3_copy( (v3f){ bbx[0][0], py, bbx[0][2] }, quad[0] );
998 v3_copy( (v3f){ bbx[1][0], py, bbx[0][2] }, quad[1] );
999 v3_copy( (v3f){ bbx[1][0], py, bbx[1][2] }, quad[2] );
1000 v3_copy( (v3f){ bbx[0][0], py, bbx[1][2] }, quad[3] );
1001 }
1002 else
1003 {
1004 float pz = vg_signf(region[2]) * bbx[1][2];
1005 v3_copy( (v3f){ bbx[0][0], bbx[0][1], pz }, quad[0] );
1006 v3_copy( (v3f){ bbx[1][0], bbx[0][1], pz }, quad[1] );
1007 v3_copy( (v3f){ bbx[1][0], bbx[1][1], pz }, quad[2] );
1008 v3_copy( (v3f){ bbx[0][0], bbx[1][1], pz }, quad[3] );
1009 }
1010
1011 capsule_manifold manifold;
1012 rb_capsule_manifold_init( &manifold );
1013
1014 v3f c0, c1;
1015 closest_point_aabb( p0, bbx, c0 );
1016 closest_point_aabb( p1, bbx, c1 );
1017
1018 v3f d0, d1, da;
1019 v3_sub( c0, p0, d0 );
1020 v3_sub( c1, p1, d1 );
1021 v3_sub( p1, p0, da );
1022
1023 v3_normalize(d0);
1024 v3_normalize(d1);
1025 v3_normalize(da);
1026
1027 if( v3_dot( da, d0 ) <= 0.01f )
1028 rb_capsule_manifold( p0, c0, 0.0f, r, &manifold );
1029
1030 if( v3_dot( da, d1 ) >= -0.01f )
1031 rb_capsule_manifold( p1, c1, 1.0f, r, &manifold );
1032
1033 for( int i=0; i<4; i++ )
1034 {
1035 int i0 = i,
1036 i1 = (i+1)%4;
1037
1038 v3f ca, cb;
1039 float ta, tb;
1040 closest_segment_segment( p0, p1, quad[i0], quad[i1], &ta, &tb, ca, cb );
1041 rb_capsule_manifold( ca, cb, ta, r, &manifold );
1042 }
1043
1044 /*
1045 * Create final contacts based on line manifold
1046 */
1047 m3x3_mulv( rbb->to_world, manifold.d0, manifold.d0 );
1048 m3x3_mulv( rbb->to_world, manifold.d1, manifold.d1 );
1049
1050 /*
1051 * Debugging
1052 */
1053
1054 #if 0
1055 for( int i=0; i<4; i++ )
1056 {
1057 v3f q0, q1;
1058 int i0 = i,
1059 i1 = (i+1)%4;
1060
1061 v3_add( quad[i0], centroid, q0 );
1062 v3_add( quad[i1], centroid, q1 );
1063
1064 m4x3_mulv( rbb->to_world, q0, q0 );
1065 m4x3_mulv( rbb->to_world, q1, q1 );
1066
1067 vg_line( q0, q1, 0xffffffff );
1068 }
1069 #endif
1070
1071 return rb_capsule_manifold_done( rba, rbb, &manifold, buf );
1072 }
1073
1074 VG_STATIC int rb_sphere_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1075 {
1076 v3f co, delta;
1077
1078 closest_point_obb( rba->co, rbb->bbx, rbb->to_world, rbb->to_local, co );
1079 v3_sub( rba->co, co, delta );
1080
1081 float d2 = v3_length2(delta),
1082 r = rba->inf.sphere.radius;
1083
1084 if( d2 <= r*r )
1085 {
1086 float d;
1087
1088 rb_ct *ct = buf;
1089 if( d2 <= 0.0001f )
1090 {
1091 v3_sub( rba->co, rbb->co, delta );
1092
1093 /*
1094 * some extra testing is required to find the best axis to push the
1095 * object back outside the box. Since there isnt a clear seperating
1096 * vector already, especially on really high aspect boxes.
1097 */
1098 float lx = v3_dot( rbb->right, delta ),
1099 ly = v3_dot( rbb->up, delta ),
1100 lz = v3_dot( rbb->forward, delta ),
1101 px = rbb->bbx[1][0] - fabsf(lx),
1102 py = rbb->bbx[1][1] - fabsf(ly),
1103 pz = rbb->bbx[1][2] - fabsf(lz);
1104
1105 if( px < py && px < pz )
1106 v3_muls( rbb->right, vg_signf(lx), ct->n );
1107 else if( py < pz )
1108 v3_muls( rbb->up, vg_signf(ly), ct->n );
1109 else
1110 v3_muls( rbb->forward, vg_signf(lz), ct->n );
1111
1112 v3_muladds( rba->co, ct->n, -r, ct->co );
1113 ct->p = r;
1114 }
1115 else
1116 {
1117 d = sqrtf(d2);
1118 v3_muls( delta, 1.0f/d, ct->n );
1119 ct->p = r-d;
1120 v3_copy( co, ct->co );
1121 }
1122
1123 ct->rba = rba;
1124 ct->rbb = rbb;
1125 ct->type = k_contact_type_default;
1126 return 1;
1127 }
1128
1129 return 0;
1130 }
1131
1132 VG_STATIC int rb_sphere_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1133 {
1134 v3f delta;
1135 v3_sub( rba->co, rbb->co, delta );
1136
1137 float d2 = v3_length2(delta),
1138 r = rba->inf.sphere.radius + rbb->inf.sphere.radius;
1139
1140 if( d2 < r*r )
1141 {
1142 float d = sqrtf(d2);
1143
1144 rb_ct *ct = buf;
1145 v3_muls( delta, 1.0f/d, ct->n );
1146
1147 v3f p0, p1;
1148 v3_muladds( rba->co, ct->n,-rba->inf.sphere.radius, p0 );
1149 v3_muladds( rbb->co, ct->n, rbb->inf.sphere.radius, p1 );
1150 v3_add( p0, p1, ct->co );
1151 v3_muls( ct->co, 0.5f, ct->co );
1152 ct->type = k_contact_type_default;
1153 ct->p = r-d;
1154 ct->rba = rba;
1155 ct->rbb = rbb;
1156 return 1;
1157 }
1158
1159 return 0;
1160 }
1161
1162 //#define RIGIDBODY_DYNAMIC_MESH_EDGES
1163
1164 VG_STATIC int rb_sphere_triangle( rigidbody *rba, rigidbody *rbb,
1165 v3f tri[3], rb_ct *buf )
1166 {
1167 v3f delta, co;
1168
1169 #ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
1170 closest_on_triangle_1( rba->co, tri, co );
1171 #else
1172 enum contact_type type = closest_on_triangle_1( rba->co, tri, co );
1173 #endif
1174
1175 v3_sub( rba->co, co, delta );
1176
1177 float d2 = v3_length2( delta ),
1178 r = rba->inf.sphere.radius;
1179
1180 if( d2 < r*r )
1181 {
1182 rb_ct *ct = buf;
1183
1184 v3f ab, ac, tn;
1185 v3_sub( tri[2], tri[0], ab );
1186 v3_sub( tri[1], tri[0], ac );
1187 v3_cross( ac, ab, tn );
1188 v3_copy( tn, ct->n );
1189
1190 if( v3_length2( ct->n ) <= 0.00001f )
1191 {
1192 vg_error( "Zero area triangle!\n" );
1193 return 0;
1194 }
1195
1196 v3_normalize( ct->n );
1197
1198 float d = sqrtf(d2);
1199
1200 v3_copy( co, ct->co );
1201 ct->type = type;
1202 ct->p = r-d;
1203 ct->rba = rba;
1204 ct->rbb = rbb;
1205 return 1;
1206 }
1207
1208 return 0;
1209 }
1210
1211
1212 VG_STATIC void rb_debug_sharp_scene_edges( rigidbody *rbb, float sharp_ang,
1213 boxf box, u32 colour )
1214 {
1215 sharp_ang = cosf( sharp_ang );
1216
1217 scene *sc = rbb->inf.scene.bh_scene->user;
1218 vg_line_boxf( box, 0xff00ff00 );
1219
1220 bh_iter it;
1221 bh_iter_init( 0, &it );
1222 int idx;
1223
1224 while( bh_next( rbb->inf.scene.bh_scene, &it, box, &idx ) )
1225 {
1226 u32 *ptri = &sc->arrindices[ idx*3 ];
1227 v3f tri[3];
1228
1229 for( int j=0; j<3; j++ )
1230 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1231
1232 for( int j=0; j<3; j++ )
1233 {
1234 #if 0
1235 v3f edir;
1236 v3_sub( tri[(j+1)%3], tri[j], edir );
1237
1238 if( v3_dot( edir, (v3f){ 0.5184758473652127f,
1239 0.2073903389460850f,
1240 -0.8295613557843402f } ) < 0.0f )
1241 continue;
1242 #endif
1243
1244 bh_iter jt;
1245 bh_iter_init( 0, &jt );
1246
1247 boxf region;
1248 float const k_r = 0.02f;
1249 v3_add( (v3f){ k_r, k_r, k_r }, tri[j], region[1] );
1250 v3_add( (v3f){ -k_r, -k_r, -k_r }, tri[j], region[0] );
1251
1252 int jdx;
1253 while( bh_next( rbb->inf.scene.bh_scene, &jt, region, &jdx ) )
1254 {
1255 if( idx <= jdx )
1256 continue;
1257
1258 u32 *ptrj = &sc->arrindices[ jdx*3 ];
1259 v3f trj[3];
1260
1261 for( int k=0; k<3; k++ )
1262 v3_copy( sc->arrvertices[ptrj[k]].co, trj[k] );
1263
1264 for( int k=0; k<3; k++ )
1265 {
1266 if( v3_dist2( tri[j], trj[k] ) <= k_r*k_r )
1267 {
1268 int jp1 = (j+1)%3,
1269 jp2 = (j+2)%3,
1270 km1 = (k+3-1)%3,
1271 km2 = (k+3-2)%3;
1272
1273 if( v3_dist2( tri[jp1], trj[km1] ) <= k_r*k_r )
1274 {
1275 v3f b0, b1, b2;
1276 v3_sub( tri[jp1], tri[j], b0 );
1277 v3_sub( tri[jp2], tri[j], b1 );
1278 v3_sub( trj[km2], tri[j], b2 );
1279
1280 v3f cx0, cx1;
1281 v3_cross( b0, b1, cx0 );
1282 v3_cross( b2, b0, cx1 );
1283
1284 float polarity = v3_dot( cx0, b2 );
1285
1286 if( polarity < 0.0f )
1287 {
1288 #if 0
1289 vg_line( tri[j], tri[jp1], 0xff00ff00 );
1290 float ang = v3_dot(cx0,cx1) /
1291 (v3_length(cx0)*v3_length(cx1));
1292 if( ang < sharp_ang )
1293 {
1294 vg_line( tri[j], tri[jp1], 0xff00ff00 );
1295 }
1296 #endif
1297 }
1298 }
1299 }
1300 }
1301 }
1302 }
1303 }
1304 }
1305
1306 VG_STATIC int rb_sphere_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1307 {
1308 scene *sc = rbb->inf.scene.bh_scene->user;
1309
1310 bh_iter it;
1311 bh_iter_init( 0, &it );
1312 int idx;
1313
1314 int count = 0;
1315
1316 while( bh_next( rbb->inf.scene.bh_scene, &it, rba->bbx_world, &idx ) )
1317 {
1318 u32 *ptri = &sc->arrindices[ idx*3 ];
1319 v3f tri[3];
1320
1321 for( int j=0; j<3; j++ )
1322 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1323
1324 buf[ count ].element_id = ptri[0];
1325
1326 vg_line( tri[0],tri[1],0x70ff6000 );
1327 vg_line( tri[1],tri[2],0x70ff6000 );
1328 vg_line( tri[2],tri[0],0x70ff6000 );
1329
1330 int contact = rb_sphere_triangle( rba, rbb, tri, buf+count );
1331 count += contact;
1332
1333 if( count == 16 )
1334 {
1335 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
1336 return count;
1337 }
1338 }
1339
1340 return count;
1341 }
1342
1343 VG_STATIC int rb_box_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1344 {
1345 scene *sc = rbb->inf.scene.bh_scene->user;
1346
1347 v3f tri[3];
1348
1349 bh_iter it;
1350 bh_iter_init( 0, &it );
1351 int idx;
1352
1353 int count = 0;
1354
1355 while( bh_next( rbb->inf.scene.bh_scene, &it, rba->bbx_world, &idx ) )
1356 {
1357 u32 *ptri = &sc->arrindices[ idx*3 ];
1358
1359 for( int j=0; j<3; j++ )
1360 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1361
1362 if( rb_box_triangle_sat( rba, tri ) )
1363 {
1364 vg_line(tri[0],tri[1],0xff50ff00 );
1365 vg_line(tri[1],tri[2],0xff50ff00 );
1366 vg_line(tri[2],tri[0],0xff50ff00 );
1367 }
1368 else
1369 {
1370 vg_line(tri[0],tri[1],0xff0000ff );
1371 vg_line(tri[1],tri[2],0xff0000ff );
1372 vg_line(tri[2],tri[0],0xff0000ff );
1373
1374 continue;
1375 }
1376
1377 v3f v0,v1,n;
1378 v3_sub( tri[1], tri[0], v0 );
1379 v3_sub( tri[2], tri[0], v1 );
1380 v3_cross( v0, v1, n );
1381 v3_normalize( n );
1382
1383 /* find best feature */
1384 float best = v3_dot( rba->right, n );
1385 int axis = 0;
1386
1387 float cy = v3_dot( rba->up, n );
1388 if( fabsf(cy) > fabsf(best) )
1389 {
1390 best = cy;
1391 axis = 1;
1392 }
1393
1394 float cz = -v3_dot( rba->forward, n );
1395 if( fabsf(cz) > fabsf(best) )
1396 {
1397 best = cz;
1398 axis = 2;
1399 }
1400
1401 v3f manifold[4];
1402
1403 if( axis == 0 )
1404 {
1405 float px = best > 0.0f? rba->bbx[0][0]: rba->bbx[1][0];
1406 manifold[0][0] = px;
1407 manifold[0][1] = rba->bbx[0][1];
1408 manifold[0][2] = rba->bbx[0][2];
1409 manifold[1][0] = px;
1410 manifold[1][1] = rba->bbx[1][1];
1411 manifold[1][2] = rba->bbx[0][2];
1412 manifold[2][0] = px;
1413 manifold[2][1] = rba->bbx[1][1];
1414 manifold[2][2] = rba->bbx[1][2];
1415 manifold[3][0] = px;
1416 manifold[3][1] = rba->bbx[0][1];
1417 manifold[3][2] = rba->bbx[1][2];
1418 }
1419 else if( axis == 1 )
1420 {
1421 float py = best > 0.0f? rba->bbx[0][1]: rba->bbx[1][1];
1422 manifold[0][0] = rba->bbx[0][0];
1423 manifold[0][1] = py;
1424 manifold[0][2] = rba->bbx[0][2];
1425 manifold[1][0] = rba->bbx[1][0];
1426 manifold[1][1] = py;
1427 manifold[1][2] = rba->bbx[0][2];
1428 manifold[2][0] = rba->bbx[1][0];
1429 manifold[2][1] = py;
1430 manifold[2][2] = rba->bbx[1][2];
1431 manifold[3][0] = rba->bbx[0][0];
1432 manifold[3][1] = py;
1433 manifold[3][2] = rba->bbx[1][2];
1434 }
1435 else
1436 {
1437 float pz = best > 0.0f? rba->bbx[0][2]: rba->bbx[1][2];
1438 manifold[0][0] = rba->bbx[0][0];
1439 manifold[0][1] = rba->bbx[0][1];
1440 manifold[0][2] = pz;
1441 manifold[1][0] = rba->bbx[1][0];
1442 manifold[1][1] = rba->bbx[0][1];
1443 manifold[1][2] = pz;
1444 manifold[2][0] = rba->bbx[1][0];
1445 manifold[2][1] = rba->bbx[1][1];
1446 manifold[2][2] = pz;
1447 manifold[3][0] = rba->bbx[0][0];
1448 manifold[3][1] = rba->bbx[1][1];
1449 manifold[3][2] = pz;
1450 }
1451
1452 for( int j=0; j<4; j++ )
1453 m4x3_mulv( rba->to_world, manifold[j], manifold[j] );
1454
1455 vg_line( manifold[0], manifold[1], 0xffffffff );
1456 vg_line( manifold[1], manifold[2], 0xffffffff );
1457 vg_line( manifold[2], manifold[3], 0xffffffff );
1458 vg_line( manifold[3], manifold[0], 0xffffffff );
1459
1460 for( int j=0; j<4; j++ )
1461 {
1462 rb_ct *ct = buf+count;
1463
1464 v3_copy( manifold[j], ct->co );
1465 v3_copy( n, ct->n );
1466
1467 float l0 = v3_dot( tri[0], n ),
1468 l1 = v3_dot( manifold[j], n );
1469
1470 ct->p = (l0-l1)*0.5f;
1471 if( ct->p < 0.0f )
1472 continue;
1473
1474 ct->type = k_contact_type_default;
1475 ct->rba = rba;
1476 ct->rbb = rbb;
1477 count ++;
1478
1479 if( count >= 12 )
1480 return count;
1481 }
1482 }
1483 return count;
1484 }
1485
1486 VG_STATIC int RB_MATRIX_ERROR( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1487 {
1488 vg_error( "Collision type is unimplemented between types %d and %d\n",
1489 rba->type, rbb->type );
1490
1491 return 0;
1492 }
1493
1494 VG_STATIC int rb_sphere_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1495 {
1496 return rb_capsule_sphere( rbb, rba, buf );
1497 }
1498
1499 VG_STATIC int rb_box_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1500 {
1501 return rb_capsule_box( rbb, rba, buf );
1502 }
1503
1504 VG_STATIC int rb_box_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1505 {
1506 return rb_sphere_box( rbb, rba, buf );
1507 }
1508
1509 VG_STATIC int rb_scene_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1510 {
1511 return rb_box_scene( rbb, rba, buf );
1512 }
1513
1514 VG_STATIC int (*rb_jump_table[4][4])( rigidbody *a, rigidbody *b, rb_ct *buf ) =
1515 {
1516 /* box */ /* Sphere */ /* Capsule */ /* Mesh */
1517 { RB_MATRIX_ERROR, rb_box_sphere, rb_box_capsule, rb_box_scene },
1518 { rb_sphere_box, rb_sphere_sphere, rb_sphere_capsule, rb_sphere_scene },
1519 { rb_capsule_box, rb_capsule_sphere, rb_capsule_capsule, RB_MATRIX_ERROR },
1520 { rb_scene_box, RB_MATRIX_ERROR, RB_MATRIX_ERROR, RB_MATRIX_ERROR }
1521 };
1522
1523 VG_STATIC int rb_collide( rigidbody *rba, rigidbody *rbb )
1524 {
1525 int (*collider_jump)(rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1526 = rb_jump_table[rba->type][rbb->type];
1527
1528 /*
1529 * 12 is the maximum manifold size we can generate, so we are forced to abort
1530 * potentially checking any more.
1531 */
1532 if( rb_contact_count + 12 > vg_list_size(rb_contact_buffer) )
1533 {
1534 vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
1535 rb_contact_count, vg_list_size(rb_contact_buffer) );
1536 return 0;
1537 }
1538
1539 /*
1540 * FUTURE: Replace this with a more dedicated broad phase pass
1541 */
1542 if( box_overlap( rba->bbx_world, rbb->bbx_world ) )
1543 {
1544 int count = collider_jump( rba, rbb, rb_contact_buffer+rb_contact_count);
1545 rb_contact_count += count;
1546 return count;
1547 }
1548 else
1549 return 0;
1550 }
1551
1552 /*
1553 * -----------------------------------------------------------------------------
1554 * Dynamics
1555 * -----------------------------------------------------------------------------
1556 */
1557
1558 VG_STATIC void rb_solver_reset(void)
1559 {
1560 rb_contact_count = 0;
1561 }
1562
1563 VG_STATIC rb_ct *rb_global_ct(void)
1564 {
1565 return rb_contact_buffer + rb_contact_count;
1566 }
1567
1568 /*
1569 * Initializing things like tangent vectors
1570 */
1571 VG_STATIC void rb_presolve_contacts( rb_ct *buffer, int len )
1572 {
1573 for( int i=0; i<len; i++ )
1574 {
1575 rb_ct *ct = &buffer[i];
1576
1577 ct->bias = -0.2f * k_rb_rate * vg_minf( 0.0f, -ct->p+k_penetration_slop );
1578 rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
1579
1580 #if 0
1581 ct->type = k_contact_type_default;
1582 #endif
1583 ct->norm_impulse = 0.0f;
1584 ct->tangent_impulse[0] = 0.0f;
1585 ct->tangent_impulse[1] = 0.0f;
1586
1587 v3f ra, rb, raCn, rbCn, raCt, rbCt;
1588 v3_sub( ct->co, ct->rba->co, ra );
1589 v3_sub( ct->co, ct->rbb->co, rb );
1590 v3_cross( ra, ct->n, raCn );
1591 v3_cross( rb, ct->n, rbCn );
1592
1593 /* orient inverse inertia tensors */
1594 v3f raCnI, rbCnI;
1595 m3x3_mulv( ct->rba->iIw, raCn, raCnI );
1596 m3x3_mulv( ct->rbb->iIw, rbCn, rbCnI );
1597
1598 ct->normal_mass = ct->rba->inv_mass + ct->rbb->inv_mass;
1599 ct->normal_mass += v3_dot( raCn, raCnI );
1600 ct->normal_mass += v3_dot( rbCn, rbCnI );
1601 ct->normal_mass = 1.0f/ct->normal_mass;
1602
1603 for( int j=0; j<2; j++ )
1604 {
1605 v3f raCtI, rbCtI;
1606 v3_cross( ct->t[j], ra, raCt );
1607 v3_cross( ct->t[j], rb, rbCt );
1608 m3x3_mulv( ct->rba->iIw, raCt, raCtI );
1609 m3x3_mulv( ct->rbb->iIw, rbCt, rbCtI );
1610
1611 ct->tangent_mass[j] = ct->rba->inv_mass + ct->rbb->inv_mass;
1612 ct->tangent_mass[j] += v3_dot( raCt, raCtI );
1613 ct->tangent_mass[j] += v3_dot( rbCt, rbCtI );
1614 ct->tangent_mass[j] = 1.0f/ct->tangent_mass[j];
1615 }
1616
1617 rb_debug_contact( ct );
1618 }
1619 }
1620
1621 /*
1622 * Creates relative contact velocity vector, and offsets between each body
1623 */
1624 VG_STATIC void rb_rcv( rb_ct *ct, v3f rv, v3f da, v3f db )
1625 {
1626 rigidbody *rba = ct->rba,
1627 *rbb = ct->rbb;
1628
1629 v3_sub( ct->co, rba->co, da );
1630 v3_sub( ct->co, rbb->co, db );
1631
1632 v3f rva, rvb;
1633 v3_cross( rba->w, da, rva );
1634 v3_add( rba->v, rva, rva );
1635 v3_cross( rbb->w, db, rvb );
1636 v3_add( rbb->v, rvb, rvb );
1637
1638 v3_sub( rva, rvb, rv );
1639 }
1640
1641 /*
1642 * Apply impulse to object
1643 */
1644 VG_STATIC void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse )
1645 {
1646 /* linear */
1647 v3_muladds( rb->v, impulse, rb->inv_mass, rb->v );
1648
1649 /* Angular velocity */
1650 v3f wa;
1651 v3_cross( delta, impulse, wa );
1652
1653 m3x3_mulv( rb->iIw, wa, wa );
1654 v3_add( rb->w, wa, rb->w );
1655 }
1656
1657 /*
1658 * One iteration to solve the contact constraint
1659 */
1660 VG_STATIC void rb_solve_contacts( rb_ct *buf, int len )
1661 {
1662 for( int i=0; i<len; i++ )
1663 {
1664 struct contact *ct = &buf[i];
1665
1666 rigidbody *rb = ct->rba;
1667
1668 v3f rv, da, db;
1669 rb_rcv( ct, rv, da, db );
1670
1671 /* Friction */
1672 for( int j=0; j<2; j++ )
1673 {
1674 float f = k_friction * ct->norm_impulse,
1675 vt = v3_dot( rv, ct->t[j] ),
1676 lambda = ct->tangent_mass[j] * -vt;
1677
1678 float temp = ct->tangent_impulse[j];
1679 ct->tangent_impulse[j] = vg_clampf( temp + lambda, -f, f );
1680 lambda = ct->tangent_impulse[j] - temp;
1681
1682 v3f impulse;
1683 v3_muls( ct->t[j], lambda, impulse );
1684 rb_linear_impulse( ct->rba, da, impulse );
1685
1686 v3_muls( ct->t[j], -lambda, impulse );
1687 rb_linear_impulse( ct->rbb, db, impulse );
1688 }
1689
1690 /* Normal */
1691 rb_rcv( ct, rv, da, db );
1692 float vn = v3_dot( rv, ct->n ),
1693 lambda = ct->normal_mass * (-vn + ct->bias);
1694
1695 float temp = ct->norm_impulse;
1696 ct->norm_impulse = vg_maxf( temp + lambda, 0.0f );
1697 lambda = ct->norm_impulse - temp;
1698
1699 v3f impulse;
1700 v3_muls( ct->n, lambda, impulse );
1701 rb_linear_impulse( ct->rba, da, impulse );
1702
1703 v3_muls( ct->n, -lambda, impulse );
1704 rb_linear_impulse( ct->rbb, db, impulse );
1705 }
1706 }
1707
1708 /*
1709 * -----------------------------------------------------------------------------
1710 * Constraints
1711 * -----------------------------------------------------------------------------
1712 */
1713
1714 VG_STATIC void draw_angle_limit( v3f c, v3f major, v3f minor,
1715 float amin, float amax, float measured,
1716 u32 colour )
1717 {
1718 float f = 0.05f;
1719 v3f ay, ax;
1720 v3_muls( major, f, ay );
1721 v3_muls( minor, f, ax );
1722
1723 for( int x=0; x<16; x++ )
1724 {
1725 float t0 = (float)x / 16.0f,
1726 t1 = (float)(x+1) / 16.0f,
1727 a0 = vg_lerpf( amin, amax, t0 ),
1728 a1 = vg_lerpf( amin, amax, t1 );
1729
1730 v3f p0, p1;
1731 v3_muladds( c, ay, cosf(a0), p0 );
1732 v3_muladds( p0, ax, sinf(a0), p0 );
1733 v3_muladds( c, ay, cosf(a1), p1 );
1734 v3_muladds( p1, ax, sinf(a1), p1 );
1735
1736 vg_line( p0, p1, colour );
1737
1738 if( x == 0 )
1739 vg_line( c, p0, colour );
1740 if( x == 15 )
1741 vg_line( c, p1, colour );
1742 }
1743
1744 v3f p2;
1745 v3_muladds( c, ay, cosf(measured)*1.2f, p2 );
1746 v3_muladds( p2, ax, sinf(measured)*1.2f, p2 );
1747 vg_line( c, p2, colour );
1748 }
1749
1750 VG_STATIC void rb_debug_constraint_limits( rigidbody *ra, rigidbody *rb, v3f lca,
1751 v3f limits[2] )
1752 {
1753 v3f ax, ay, az, bx, by, bz;
1754 m3x3_mulv( ra->to_world, (v3f){1.0f,0.0f,0.0f}, ax );
1755 m3x3_mulv( ra->to_world, (v3f){0.0f,1.0f,0.0f}, ay );
1756 m3x3_mulv( ra->to_world, (v3f){0.0f,0.0f,1.0f}, az );
1757 m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f,0.0f}, bx );
1758 m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f,0.0f}, by );
1759 m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,1.0f}, bz );
1760
1761 v2f px, py, pz;
1762 px[0] = v3_dot( ay, by );
1763 px[1] = v3_dot( az, by );
1764
1765 py[0] = v3_dot( az, bz );
1766 py[1] = v3_dot( ax, bz );
1767
1768 pz[0] = v3_dot( ax, bx );
1769 pz[1] = v3_dot( ay, bx );
1770
1771 float r0 = atan2f( px[1], px[0] ),
1772 r1 = atan2f( py[1], py[0] ),
1773 r2 = atan2f( pz[1], pz[0] );
1774
1775 v3f c;
1776 m4x3_mulv( ra->to_world, lca, c );
1777 draw_angle_limit( c, ay, az, limits[0][0], limits[1][0], r0, 0xff0000ff );
1778 draw_angle_limit( c, az, ax, limits[0][1], limits[1][1], r1, 0xff00ff00 );
1779 draw_angle_limit( c, ax, ay, limits[0][2], limits[1][2], r2, 0xffff0000 );
1780 }
1781
1782 VG_STATIC void rb_limit_cure( rigidbody *ra, rigidbody *rb, v3f axis, float d )
1783 {
1784 if( d != 0.0f )
1785 {
1786 float avx = v3_dot( ra->w, axis ) - v3_dot( rb->w, axis );
1787 float joint_mass = rb->inv_mass + ra->inv_mass;
1788 joint_mass = 1.0f/joint_mass;
1789
1790 float bias = (k_limit_bias * k_rb_rate) * d,
1791 lambda = -(avx + bias) * joint_mass;
1792
1793 /* Angular velocity */
1794 v3f wa, wb;
1795 v3_muls( axis, lambda * ra->inv_mass, wa );
1796 v3_muls( axis, -lambda * rb->inv_mass, wb );
1797
1798 v3_add( ra->w, wa, ra->w );
1799 v3_add( rb->w, wb, rb->w );
1800 }
1801 }
1802
1803 VG_STATIC void rb_constraint_limits( rigidbody *ra, v3f lca,
1804 rigidbody *rb, v3f lcb, v3f limits[2] )
1805 {
1806 v3f ax, ay, az, bx, by, bz;
1807 m3x3_mulv( ra->to_world, (v3f){1.0f,0.0f,0.0f}, ax );
1808 m3x3_mulv( ra->to_world, (v3f){0.0f,1.0f,0.0f}, ay );
1809 m3x3_mulv( ra->to_world, (v3f){0.0f,0.0f,1.0f}, az );
1810 m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f,0.0f}, bx );
1811 m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f,0.0f}, by );
1812 m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,1.0f}, bz );
1813
1814 v2f px, py, pz;
1815 px[0] = v3_dot( ay, by );
1816 px[1] = v3_dot( az, by );
1817
1818 py[0] = v3_dot( az, bz );
1819 py[1] = v3_dot( ax, bz );
1820
1821 pz[0] = v3_dot( ax, bx );
1822 pz[1] = v3_dot( ay, bx );
1823
1824 float r0 = atan2f( px[1], px[0] ),
1825 r1 = atan2f( py[1], py[0] ),
1826 r2 = atan2f( pz[1], pz[0] );
1827
1828 /* calculate angle deltas */
1829 float dx = 0.0f, dy = 0.0f, dz = 0.0f;
1830
1831 if( r0 < limits[0][0] ) dx = limits[0][0] - r0;
1832 if( r0 > limits[1][0] ) dx = limits[1][0] - r0;
1833 if( r1 < limits[0][1] ) dy = limits[0][1] - r1;
1834 if( r1 > limits[1][1] ) dy = limits[1][1] - r1;
1835 if( r2 < limits[0][2] ) dz = limits[0][2] - r2;
1836 if( r2 > limits[1][2] ) dz = limits[1][2] - r2;
1837
1838 v3f wca, wcb;
1839 m3x3_mulv( ra->to_world, lca, wca );
1840 m3x3_mulv( rb->to_world, lcb, wcb );
1841
1842 rb_limit_cure( ra, rb, ax, dx );
1843 rb_limit_cure( ra, rb, ay, dy );
1844 rb_limit_cure( ra, rb, az, dz );
1845 }
1846
1847 VG_STATIC void rb_debug_constraint_position( rigidbody *ra, v3f lca,
1848 rigidbody *rb, v3f lcb )
1849 {
1850 v3f wca, wcb;
1851 m3x3_mulv( ra->to_world, lca, wca );
1852 m3x3_mulv( rb->to_world, lcb, wcb );
1853
1854 v3f p0, p1;
1855 v3_add( wca, ra->co, p0 );
1856 v3_add( wcb, rb->co, p1 );
1857 vg_line_pt3( p0, 0.005f, 0xffffff00 );
1858 vg_line_pt3( p1, 0.005f, 0xffffff00 );
1859 vg_line( p0, p1, 0xffffff00 );
1860 }
1861
1862 VG_STATIC void rb_constraint_position( rigidbody *ra, v3f lca,
1863 rigidbody *rb, v3f lcb )
1864 {
1865 /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
1866 v3f wca, wcb;
1867 m3x3_mulv( ra->to_world, lca, wca );
1868 m3x3_mulv( rb->to_world, lcb, wcb );
1869
1870 v3f rcv;
1871 v3_sub( ra->v, rb->v, rcv );
1872
1873 v3f rcv_Ra, rcv_Rb;
1874 v3_cross( ra->w, wca, rcv_Ra );
1875 v3_cross( rb->w, wcb, rcv_Rb );
1876 v3_add( rcv_Ra, rcv, rcv );
1877 v3_sub( rcv, rcv_Rb, rcv );
1878
1879 v3f delta;
1880 v3f p0, p1;
1881 v3_add( wca, ra->co, p0 );
1882 v3_add( wcb, rb->co, p1 );
1883 v3_sub( p1, p0, delta );
1884
1885 float dist2 = v3_length2( delta );
1886
1887 if( dist2 > 0.00001f )
1888 {
1889 float dist = sqrtf(dist2);
1890 v3_muls( delta, 1.0f/dist, delta );
1891
1892 float joint_mass = rb->inv_mass + ra->inv_mass;
1893
1894 v3f raCn, rbCn, raCt, rbCt;
1895 v3_cross( wca, delta, raCn );
1896 v3_cross( wcb, delta, rbCn );
1897
1898 /* orient inverse inertia tensors */
1899 v3f raCnI, rbCnI;
1900 m3x3_mulv( ra->iIw, raCn, raCnI );
1901 m3x3_mulv( rb->iIw, rbCn, rbCnI );
1902 joint_mass += v3_dot( raCn, raCnI );
1903 joint_mass += v3_dot( rbCn, rbCnI );
1904 joint_mass = 1.0f/joint_mass;
1905
1906 float vd = v3_dot( rcv, delta ),
1907 bias = -(k_joint_bias * k_rb_rate) * dist,
1908 lambda = -(vd + bias) * joint_mass;
1909
1910 v3f impulse;
1911 v3_muls( delta, lambda, impulse );
1912 rb_linear_impulse( ra, wca, impulse );
1913 v3_muls( delta, -lambda, impulse );
1914 rb_linear_impulse( rb, wcb, impulse );
1915
1916 /* 'fake' snap */
1917 v3_muladds( ra->co, delta, dist * k_joint_correction, ra->co );
1918 v3_muladds( rb->co, delta, -dist * k_joint_correction, rb->co );
1919 }
1920 }
1921
1922 /*
1923 * Effectors
1924 */
1925
1926 VG_STATIC void rb_effect_simple_bouyency( rigidbody *ra, v4f plane,
1927 float amt, float drag )
1928 {
1929 /* float */
1930 float depth = v3_dot( plane, ra->co ) - plane[3],
1931 lambda = vg_clampf( -depth, 0.0f, 1.0f ) * amt;
1932
1933 v3_muladds( ra->v, plane, lambda * k_rb_delta, ra->v );
1934
1935 if( depth < 0.0f )
1936 v3_muls( ra->v, 1.0f-(drag*k_rb_delta), ra->v );
1937 }
1938
1939 /*
1940 * -----------------------------------------------------------------------------
1941 * BVH implementation, this is ONLY for VG_STATIC rigidbodies, its to slow for
1942 * realtime use.
1943 * -----------------------------------------------------------------------------
1944 */
1945
1946 VG_STATIC void rb_bh_expand_bound( void *user, boxf bound, u32 item_index )
1947 {
1948 rigidbody *rb = &((rigidbody *)user)[ item_index ];
1949 box_concat( bound, rb->bbx_world );
1950 }
1951
1952 VG_STATIC float rb_bh_centroid( void *user, u32 item_index, int axis )
1953 {
1954 rigidbody *rb = &((rigidbody *)user)[ item_index ];
1955 return (rb->bbx_world[axis][0] + rb->bbx_world[1][axis]) * 0.5f;
1956 }
1957
1958 VG_STATIC void rb_bh_swap( void *user, u32 ia, u32 ib )
1959 {
1960 rigidbody temp, *rba, *rbb;
1961 rba = &((rigidbody *)user)[ ia ];
1962 rbb = &((rigidbody *)user)[ ib ];
1963
1964 temp = *rba;
1965 *rba = *rbb;
1966 *rbb = temp;
1967 }
1968
1969 VG_STATIC void rb_bh_debug( void *user, u32 item_index )
1970 {
1971 rigidbody *rb = &((rigidbody *)user)[ item_index ];
1972 rb_debug( rb, 0xff00ffff );
1973 }
1974
1975 VG_STATIC bh_system bh_system_rigidbodies =
1976 {
1977 .expand_bound = rb_bh_expand_bound,
1978 .item_centroid = rb_bh_centroid,
1979 .item_swap = rb_bh_swap,
1980 .item_debug = rb_bh_debug,
1981 .cast_ray = NULL
1982 };
1983
1984 #endif /* RIGIDBODY_H */
Skaterift source tree