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