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