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