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