more physics shapes
[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
9 static void rb_tangent_basis( v3f n, v3f tx, v3f ty );
10 static bh_system bh_system_rigidbodies;
11
12 #ifndef RIGIDBODY_H
13 #define RIGIDBODY_H
14
15 #define RB_DEPR
16 #define k_rb_rate 60.0f
17 #define k_rb_delta (1.0f/k_rb_rate)
18
19 typedef struct rigidbody rigidbody;
20 typedef struct contact rb_ct;
21
22 struct rigidbody
23 {
24 v3f co, v, w;
25 v4f q;
26
27 enum rb_shape
28 {
29 k_rb_shape_box,
30 k_rb_shape_sphere,
31 k_rb_shape_capsule
32 }
33 type;
34
35 union
36 {
37 struct rb_sphere
38 {
39 float radius;
40 }
41 sphere;
42
43 struct rb_capsule
44 {
45 float height, radius;
46 }
47 capsule;
48 }
49 inf;
50
51 v3f right, up, forward;
52
53 int is_world;
54
55 boxf bbx, bbx_world;
56 float inv_mass;
57
58 v3f delta; /* where is the origin of this in relation to a parent body */
59 m4x3f to_world, to_local;
60 };
61
62 static rigidbody rb_static_null =
63 {
64 .co={0.0f,0.0f,0.0f},
65 .q={0.0f,0.0f,0.0f,1.0f},
66 .v={0.0f,0.0f,0.0f},
67 .w={0.0f,0.0f,0.0f},
68 .is_world = 1,
69 .inv_mass = 0.0f
70 };
71
72 static void rb_debug( rigidbody *rb, u32 colour );
73
74 static struct contact
75 {
76 rigidbody *rba, *rbb;
77 v3f co, n;
78 v3f t[2];
79 float mass_total, p, bias, norm_impulse, tangent_impulse[2];
80 }
81 rb_contact_buffer[256];
82 static int rb_contact_count = 0;
83
84 static void rb_update_transform( rigidbody *rb )
85 {
86 q_normalize( rb->q );
87 q_m3x3( rb->q, rb->to_world );
88 v3_copy( rb->co, rb->to_world[3] );
89
90 m4x3_invert_affine( rb->to_world, rb->to_local );
91
92 box_copy( rb->bbx, rb->bbx_world );
93 m4x3_transform_aabb( rb->to_world, rb->bbx_world );
94
95 m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f, 0.0f}, rb->right );
96 m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f, 0.0f}, rb->up );
97 m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,-1.0f}, rb->forward );
98 }
99
100 static float sphere_volume( float radius )
101 {
102 float r3 = radius*radius*radius;
103 return (4.0f/3.0f) * VG_PIf * r3;
104 }
105
106 static void rb_init( rigidbody *rb )
107 {
108 float volume = 1.0f;
109
110 if( rb->type == k_rb_shape_box )
111 {
112 v3f dims;
113 v3_sub( rb->bbx[1], rb->bbx[0], dims );
114 volume = dims[0]*dims[1]*dims[2];
115 }
116 else if( rb->type == k_rb_shape_sphere )
117 {
118 volume = sphere_volume( rb->inf.sphere.radius );
119 v3_fill( rb->bbx[0], -rb->inf.sphere.radius );
120 v3_fill( rb->bbx[1], rb->inf.sphere.radius );
121 }
122 else if( rb->type == k_rb_shape_capsule )
123 {
124 float r = rb->inf.capsule.radius,
125 h = rb->inf.capsule.height;
126 volume = sphere_volume( r ) + VG_PIf * r*r * (h - r*2.0f);
127
128 v3_fill( rb->bbx[0], -rb->inf.sphere.radius );
129 v3_fill( rb->bbx[1], rb->inf.sphere.radius );
130 rb->bbx[0][1] = -h;
131 rb->bbx[1][1] = h;
132 }
133
134 if( rb->is_world )
135 {
136 rb->inv_mass = 0.0f;
137 }
138 else
139 {
140 rb->inv_mass = 1.0f/(8.0f*volume);
141 }
142
143 v3_zero( rb->v );
144 v3_zero( rb->w );
145
146 rb_update_transform( rb );
147 }
148
149 static void rb_iter( rigidbody *rb )
150 {
151 v3f gravity = { 0.0f, -9.6f, 0.0f };
152 v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
153
154 /* intergrate velocity */
155 v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
156 v3_lerp( rb->w, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->w );
157
158 /* inegrate inertia */
159 if( v3_length2( rb->w ) > 0.0f )
160 {
161 v4f rotation;
162 v3f axis;
163 v3_copy( rb->w, axis );
164
165 float mag = v3_length( axis );
166 v3_divs( axis, mag, axis );
167 q_axis_angle( rotation, axis, mag*k_rb_delta );
168 q_mul( rotation, rb->q, rb->q );
169 }
170 }
171
172 static void rb_torque( rigidbody *rb, v3f axis, float mag )
173 {
174 v3_muladds( rb->w, axis, mag*k_rb_delta, rb->w );
175 }
176
177 static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
178 {
179 /* Compute tangent basis (box2d) */
180 if( fabsf( n[0] ) >= 0.57735027f )
181 {
182 tx[0] = n[1];
183 tx[1] = -n[0];
184 tx[2] = 0.0f;
185 }
186 else
187 {
188 tx[0] = 0.0f;
189 tx[1] = n[2];
190 tx[2] = -n[1];
191 }
192
193 v3_normalize( tx );
194 v3_cross( n, tx, ty );
195 }
196
197 static void rb_solver_reset(void);
198 static void rb_build_manifold_terrain( rigidbody *rb );
199 static void rb_build_manifold_terrain_sphere( rigidbody *rb );
200 static void rb_solve_contacts( rb_ct *buf, int len );
201 static void rb_presolve_contacts( rb_ct *buffer, int len );
202
203 /*
204 * These closest point tests were learned from Real-Time Collision Detection by
205 * Christer Ericson
206 */
207 static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
208 float *s, float *t, v3f c1, v3f c2)
209 {
210 v3f d1,d2,r;
211 v3_sub( q1, p1, d1 );
212 v3_sub( q2, p2, d2 );
213 v3_sub( p1, p2, r );
214
215 float a = v3_length2( d1 ),
216 e = v3_length2( d2 ),
217 f = v3_dot( d2, r );
218
219 const float kEpsilon = 0.0001f;
220
221 if( a <= kEpsilon && e <= kEpsilon )
222 {
223 *s = 0.0f;
224 *t = 0.0f;
225 v3_copy( p1, c1 );
226 v3_copy( p2, c2 );
227
228 v3f v0;
229 v3_sub( c1, c2, v0 );
230
231 return v3_length2( v0 );
232 }
233
234 if( a<= kEpsilon )
235 {
236 *s = 0.0f;
237 *t = vg_clampf( f / e, 0.0f, 1.0f );
238 }
239 else
240 {
241 float c = v3_dot( d1, r );
242 if( e <= kEpsilon )
243 {
244 *t = 0.0f;
245 *s = vg_clampf( -c / a, 0.0f, 1.0f );
246 }
247 else
248 {
249 float b = v3_dot(d1,d2),
250 d = a*e-b*b;
251
252 if( d != 0.0f )
253 {
254 *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
255 }
256 else
257 {
258 *s = 0.0f;
259 }
260
261 *t = (b*(*s)+f) / e;
262
263 if( *t < 0.0f )
264 {
265 *t = 0.0f;
266 *s = vg_clampf( -c / a, 0.0f, 1.0f );
267 }
268 else if( *t > 1.0f )
269 {
270 *t = 1.0f;
271 *s = vg_clampf((b-c)/a,0.0f,1.0f);
272 }
273 }
274 }
275
276 v3_muladds( p1, d1, *s, c1 );
277 v3_muladds( p2, d2, *t, c2 );
278
279 v3f v0;
280 v3_sub( c1, c2, v0 );
281 return v3_length2( v0 );
282 }
283
284 static void closest_point_aabb( v3f p, boxf box, v3f dest )
285 {
286 v3_maxv( p, box[0], dest );
287 v3_minv( dest, box[1], dest );
288 }
289
290 static void closest_point_obb( v3f p, rigidbody *rb, v3f dest )
291 {
292 v3f local;
293 m4x3_mulv( rb->to_local, p, local );
294 closest_point_aabb( local, rb->bbx, local );
295 m4x3_mulv( rb->to_world, local, dest );
296 }
297
298 static float closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
299 {
300 v3f v0, v1;
301 v3_sub( b, a, v0 );
302 v3_sub( point, a, v1 );
303
304 float t = v3_dot( v1, v0 ) / v3_length2(v0);
305 t = vg_clampf(t,0.0f,1.0f);
306 v3_muladds( a, v0, t, dest );
307 return t;
308 }
309
310 static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
311 {
312 v3f ab, ac, ap;
313 float d1, d2;
314
315 /* Region outside A */
316 v3_sub( tri[1], tri[0], ab );
317 v3_sub( tri[2], tri[0], ac );
318 v3_sub( p, tri[0], ap );
319
320 d1 = v3_dot(ab,ap);
321 d2 = v3_dot(ac,ap);
322 if( d1 <= 0.0f && d2 <= 0.0f )
323 {
324 v3_copy( tri[0], dest );
325 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
326 return;
327 }
328
329 /* Region outside B */
330 v3f bp;
331 float d3, d4;
332
333 v3_sub( p, tri[1], bp );
334 d3 = v3_dot( ab, bp );
335 d4 = v3_dot( ac, bp );
336
337 if( d3 >= 0.0f && d4 <= d3 )
338 {
339 v3_copy( tri[1], dest );
340 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
341 return;
342 }
343
344 /* Edge region of AB */
345 float vc = d1*d4 - d3*d2;
346 if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
347 {
348 float v = d1 / (d1-d3);
349 v3_muladds( tri[0], ab, v, dest );
350 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
351 return;
352 }
353
354 /* Region outside C */
355 v3f cp;
356 float d5, d6;
357 v3_sub( p, tri[2], cp );
358 d5 = v3_dot(ab, cp);
359 d6 = v3_dot(ac, cp);
360
361 if( d6 >= 0.0f && d5 <= d6 )
362 {
363 v3_copy( tri[2], dest );
364 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
365 return;
366 }
367
368 /* Region of AC */
369 float vb = d5*d2 - d1*d6;
370 if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
371 {
372 float w = d2 / (d2-d6);
373 v3_muladds( tri[0], ac, w, dest );
374 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
375 return;
376 }
377
378 /* Region of BC */
379 float va = d3*d6 - d5*d4;
380 if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
381 {
382 float w = (d4-d3) / ((d4-d3) + (d5-d6));
383 v3f bc;
384 v3_sub( tri[2], tri[1], bc );
385 v3_muladds( tri[1], bc, w, dest );
386 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
387 return;
388 }
389
390 /* P inside region, Q via barycentric coordinates uvw */
391 float d = 1.0f/(va+vb+vc),
392 v = vb*d,
393 w = vc*d;
394
395 v3_muladds( tri[0], ab, v, dest );
396 v3_muladds( dest, ac, w, dest );
397 }
398
399 /*
400 * Contact generators
401 *
402 * These do not automatically allocate contacts, an appropriately sized
403 * buffer must be supplied. The function returns the size of the manifold
404 * which was generated.
405 *
406 * The values set on the contacts are: n, co, p, rba, rbb
407 */
408
409 static void rb_debug_contact( rb_ct *ct )
410 {
411 v3f p1;
412 v3_muladds( ct->co, ct->n, 0.2f, p1 );
413 vg_line_pt3( ct->co, 0.1f, 0xff0000ff );
414 vg_line( ct->co, p1, 0xffffffff );
415 }
416
417 /*
418 * By collecting the minimum(time) and maximum(time) pairs of points, we
419 * build a reduced and stable exact manifold.
420 *
421 * tx: time at point
422 * rx: minimum distance of these points
423 * dx: the delta between the two points
424 *
425 * pairs will only ammend these if they are creating a collision
426 */
427 typedef struct capsule_manifold capsule_manifold;
428 struct capsule_manifold
429 {
430 float t0, t1;
431 float r0, r1;
432 v3f d0, d1;
433 };
434
435 /*
436 * Expand a line manifold with a new pair. t value is the time along segment
437 * on the oriented object which created this pair.
438 */
439 static void rb_capsule_manifold( v3f pa, v3f pb, float t, float r,
440 capsule_manifold *manifold )
441 {
442 v3f delta;
443 v3_sub( pa, pb, delta );
444
445 if( v3_length2(delta) < r*r )
446 {
447 if( t < manifold->t0 )
448 {
449 v3_copy( delta, manifold->d0 );
450 manifold->t0 = t;
451 manifold->r0 = r;
452 }
453
454 if( t > manifold->t1 )
455 {
456 v3_copy( delta, manifold->d1 );
457 manifold->t1 = t;
458 manifold->r1 = r;
459 }
460 }
461 }
462
463 static void rb_capsule_manifold_init( capsule_manifold *manifold )
464 {
465 manifold->t0 = INFINITY;
466 manifold->t1 = -INFINITY;
467 }
468
469 static int rb_capsule_manifold_done( rigidbody *rba, rigidbody *rbb,
470 capsule_manifold *manifold, rb_ct *buf )
471 {
472 float h = rba->inf.capsule.height,
473 ra = rba->inf.capsule.radius;
474
475 v3f p0, p1;
476 v3_muladds( rba->co, rba->up, -h*0.5f+ra, p0 );
477 v3_muladds( rba->co, rba->up, h*0.5f-ra, p1 );
478
479 int count = 0;
480 if( manifold->t0 <= 1.0f )
481 {
482 rb_ct *ct = buf;
483
484 v3f pa;
485 v3_muls( p0, 1.0f-manifold->t0, pa );
486 v3_muladds( pa, p1, manifold->t0, pa );
487
488 float d = v3_length( manifold->d0 );
489 v3_muls( manifold->d0, 1.0f/d, ct->n );
490 v3_muladds( pa, ct->n, -ra, ct->co );
491
492 ct->p = manifold->r0 - d;
493 ct->rba = rba;
494 ct->rbb = rbb;
495
496 count ++;
497 }
498
499 if( (manifold->t1 >= 0.0f) && (manifold->t0 != manifold->t1) )
500 {
501 rb_ct *ct = buf+count;
502
503 v3f pa;
504 v3_muls( p0, 1.0f-manifold->t1, pa );
505 v3_muladds( pa, p1, manifold->t1, pa );
506
507 float d = v3_length( manifold->d1 );
508 v3_muls( manifold->d1, 1.0f/d, ct->n );
509 v3_muladds( pa, ct->n, -ra, ct->co );
510
511 ct->p = manifold->r1 - d;
512 ct->rba = rba;
513 ct->rbb = rbb;
514
515 count ++;
516 }
517
518 /*
519 * Debugging
520 */
521
522 if( count == 2 )
523 vg_line( buf[0].co, buf[1].co, 0xff0000ff );
524
525 return count;
526 }
527
528 static int rb_capsule_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
529 {
530 float h = rba->inf.capsule.height,
531 ra = rba->inf.capsule.radius,
532 rb = rbb->inf.sphere.radius;
533
534 v3f p0, p1;
535 v3_muladds( rba->co, rba->up, -h*0.5f+ra, p0 );
536 v3_muladds( rba->co, rba->up, h*0.5f-ra, p1 );
537
538 v3f c, delta;
539 closest_point_segment( p0, p1, rbb->co, c );
540 v3_sub( c, rbb->co, delta );
541
542 float d2 = v3_length2(delta),
543 r = ra + rb;
544
545 if( d2 < r*r )
546 {
547 float d = sqrtf(d2);
548
549 rb_ct *ct = buf;
550 v3_muls( delta, 1.0f/d, ct->n );
551 ct->p = r-d;
552
553 v3f p0, p1;
554 v3_muladds( c, ct->n, -ra, p0 );
555 v3_muladds( rbb->co, ct->n, rb, p1 );
556 v3_add( p0, p1, ct->co );
557 v3_muls( ct->co, 0.5f, ct->co );
558
559 ct->rba = rba;
560 ct->rbb = rbb;
561
562 return 1;
563 }
564
565 return 0;
566 }
567
568 static int rb_capsule_vs_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
569 {
570 float ha = rba->inf.capsule.height,
571 hb = rbb->inf.capsule.height,
572 ra = rba->inf.capsule.radius,
573 rb = rbb->inf.capsule.radius,
574 r = ra+rb;
575
576 v3f p0, p1, p2, p3;
577 v3_muladds( rba->co, rba->up, -ha*0.5f+ra, p0 );
578 v3_muladds( rba->co, rba->up, ha*0.5f-ra, p1 );
579 v3_muladds( rbb->co, rbb->up, -hb*0.5f+rb, p2 );
580 v3_muladds( rbb->co, rbb->up, hb*0.5f-rb, p3 );
581
582 capsule_manifold manifold;
583 rb_capsule_manifold_init( &manifold );
584
585 v3f pa, pb;
586 float ta, tb;
587 closest_segment_segment( p0, p1, p2, p3, &ta, &tb, pa, pb );
588 rb_capsule_manifold( pa, pb, ta, r, &manifold );
589
590 ta = closest_point_segment( p0, p1, p2, pa );
591 tb = closest_point_segment( p0, p1, p3, pb );
592 rb_capsule_manifold( pa, p2, ta, r, &manifold );
593 rb_capsule_manifold( pb, p3, tb, r, &manifold );
594
595 closest_point_segment( p2, p3, p0, pa );
596 closest_point_segment( p2, p3, p1, pb );
597 rb_capsule_manifold( p0, pa, 0.0f, r, &manifold );
598 rb_capsule_manifold( p1, pb, 1.0f, r, &manifold );
599
600 return rb_capsule_manifold_done( rba, rbb, &manifold, buf );
601 }
602
603 /*
604 * Generates up to two contacts; optimised for the most stable manifold
605 */
606 static int rb_capsule_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
607 {
608 float h = rba->inf.capsule.height,
609 r = rba->inf.capsule.radius;
610
611 /*
612 * Solving this in symetric local space of the cube saves us some time and a
613 * couple branches when it comes to the quad stage.
614 */
615 v3f centroid;
616 v3_add( rbb->bbx[0], rbb->bbx[1], centroid );
617 v3_muls( centroid, 0.5f, centroid );
618
619 boxf bbx;
620 v3_sub( rbb->bbx[0], centroid, bbx[0] );
621 v3_sub( rbb->bbx[1], centroid, bbx[1] );
622
623 v3f pc, p0w, p1w, p0, p1;
624 v3_muladds( rba->co, rba->up, -h*0.5f+r, p0w );
625 v3_muladds( rba->co, rba->up, h*0.5f-r, p1w );
626
627 m4x3_mulv( rbb->to_local, p0w, p0 );
628 m4x3_mulv( rbb->to_local, p1w, p1 );
629 v3_sub( p0, centroid, p0 );
630 v3_sub( p1, centroid, p1 );
631 v3_add( p0, p1, pc );
632 v3_muls( pc, 0.5f, pc );
633
634 /*
635 * Finding an appropriate quad to collide lines with
636 */
637 v3f region;
638 v3_div( pc, bbx[1], region );
639
640 v3f quad[4];
641 if( (fabsf(region[0]) > fabsf(region[1])) &&
642 (fabsf(region[0]) > fabsf(region[2])) )
643 {
644 float px = vg_signf(region[0]) * bbx[1][0];
645 v3_copy( (v3f){ px, bbx[0][1], bbx[0][2] }, quad[0] );
646 v3_copy( (v3f){ px, bbx[1][1], bbx[0][2] }, quad[1] );
647 v3_copy( (v3f){ px, bbx[1][1], bbx[1][2] }, quad[2] );
648 v3_copy( (v3f){ px, bbx[0][1], bbx[1][2] }, quad[3] );
649 }
650 else if( fabsf(region[1]) > fabsf(region[2]) )
651 {
652 float py = vg_signf(region[1]) * bbx[1][1];
653 v3_copy( (v3f){ bbx[0][0], py, bbx[0][2] }, quad[0] );
654 v3_copy( (v3f){ bbx[1][0], py, bbx[0][2] }, quad[1] );
655 v3_copy( (v3f){ bbx[1][0], py, bbx[1][2] }, quad[2] );
656 v3_copy( (v3f){ bbx[0][0], py, bbx[1][2] }, quad[3] );
657 }
658 else
659 {
660 float pz = vg_signf(region[2]) * bbx[1][2];
661 v3_copy( (v3f){ bbx[0][0], bbx[0][1], pz }, quad[0] );
662 v3_copy( (v3f){ bbx[1][0], bbx[0][1], pz }, quad[1] );
663 v3_copy( (v3f){ bbx[1][0], bbx[1][1], pz }, quad[2] );
664 v3_copy( (v3f){ bbx[0][0], bbx[1][1], pz }, quad[3] );
665 }
666
667 capsule_manifold manifold;
668 rb_capsule_manifold_init( &manifold );
669
670 v3f c0, c1;
671 closest_point_aabb( p0, bbx, c0 );
672 closest_point_aabb( p1, bbx, c1 );
673
674 v3f d0, d1, da;
675 v3_sub( c0, p0, d0 );
676 v3_sub( c1, p1, d1 );
677 v3_sub( p1, p0, da );
678
679 /* TODO: ? */
680 v3_normalize(d0);
681 v3_normalize(d1);
682 v3_normalize(da);
683
684 if( v3_dot( da, d0 ) <= 0.01f )
685 rb_capsule_manifold( p0, c0, 0.0f, r, &manifold );
686
687 if( v3_dot( da, d1 ) >= -0.01f )
688 rb_capsule_manifold( p1, c1, 1.0f, r, &manifold );
689
690 for( int i=0; i<4; i++ )
691 {
692 int i0 = i,
693 i1 = (i+1)%4;
694
695 v3f ca, cb;
696 float ta, tb;
697 closest_segment_segment( p0, p1, quad[i0], quad[i1], &ta, &tb, ca, cb );
698 rb_capsule_manifold( ca, cb, ta, r, &manifold );
699 }
700
701 /*
702 * Create final contacts based on line manifold
703 */
704 m3x3_mulv( rbb->to_world, manifold.d0, manifold.d0 );
705 m3x3_mulv( rbb->to_world, manifold.d1, manifold.d1 );
706
707 /*
708 * Debugging
709 */
710
711 #if 0
712 for( int i=0; i<4; i++ )
713 {
714 v3f q0, q1;
715 int i0 = i,
716 i1 = (i+1)%4;
717
718 v3_add( quad[i0], centroid, q0 );
719 v3_add( quad[i1], centroid, q1 );
720
721 m4x3_mulv( rbb->to_world, q0, q0 );
722 m4x3_mulv( rbb->to_world, q1, q1 );
723
724 vg_line( q0, q1, 0xffffffff );
725 }
726 #endif
727
728 return rb_capsule_manifold_done( rba, rbb, &manifold, buf );
729 }
730
731 static int rb_sphere_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
732 {
733 v3f co, delta;
734
735 closest_point_obb( rba->co, rbb, co );
736 v3_sub( rba->co, co, delta );
737
738 float d2 = v3_length2(delta),
739 r = rba->inf.sphere.radius;
740
741 if( d2 <= r*r )
742 {
743 float d;
744
745 rb_ct *ct = buf;
746 if( d2 <= 0.0001f )
747 {
748 v3_sub( rba->co, rbb->co, delta );
749
750 /*
751 * some extra testing is required to find the best axis to push the
752 * object back outside the box. Since there isnt a clear seperating
753 * vector already, especially on really high aspect boxes.
754 */
755 float lx = v3_dot( rbb->right, delta ),
756 ly = v3_dot( rbb->up, delta ),
757 lz = v3_dot( rbb->forward, delta ),
758 px = rbb->bbx[1][0] - fabsf(lx),
759 py = rbb->bbx[1][1] - fabsf(ly),
760 pz = rbb->bbx[1][2] - fabsf(lz);
761
762 if( px < py && px < pz )
763 v3_muls( rbb->right, vg_signf(lx), ct->n );
764 else if( py < pz )
765 v3_muls( rbb->up, vg_signf(ly), ct->n );
766 else
767 v3_muls( rbb->forward, vg_signf(lz), ct->n );
768
769 v3_muladds( rba->co, ct->n, -r, ct->co );
770 ct->p = r;
771 }
772 else
773 {
774 d = sqrtf(d2);
775 v3_muls( delta, 1.0f/d, ct->n );
776 ct->p = r-d;
777 v3_copy( co, ct->co );
778 }
779
780 ct->rba = rba;
781 ct->rbb = rbb;
782 return 1;
783 }
784
785 return 0;
786 }
787
788 static int rb_sphere_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
789 {
790 v3f delta;
791 v3_sub( rba->co, rbb->co, delta );
792
793 float d2 = v3_length2(delta),
794 r = rba->inf.sphere.radius + rbb->inf.sphere.radius;
795
796 if( d2 < r*r )
797 {
798 float d = sqrtf(d2);
799
800 rb_ct *ct = buf;
801 v3_muls( delta, 1.0f/d, ct->n );
802
803 v3f p0, p1;
804 v3_muladds( rba->co, ct->n,-rba->inf.sphere.radius, p0 );
805 v3_muladds( rbb->co, ct->n, rbb->inf.sphere.radius, p1 );
806 v3_add( p0, p1, ct->co );
807 v3_muls( ct->co, 0.5f, ct->co );
808 ct->p = r-d;
809 ct->rba = rba;
810 ct->rbb = rbb;
811 return 1;
812 }
813
814 return 0;
815 }
816
817 static int RB_MATRIX_ERROR( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
818 {
819 vg_error( "Collision type is unimplemented between types %d and %d\n",
820 rba->type, rbb->type );
821
822 return 0;
823 }
824
825 static int rb_sphere_vs_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
826 {
827 return rb_capsule_vs_sphere( rbb, rba, buf );
828 }
829
830 static int rb_box_vs_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
831 {
832 return rb_capsule_vs_box( rbb, rba, buf );
833 }
834
835 static int rb_box_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
836 {
837 return rb_sphere_vs_box( rbb, rba, buf );
838 }
839
840 static int (*rb_jump_table[4][4])( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
841 = {
842 /* box */ /* Sphere */ /* Capsule */
843 /*box */ { RB_MATRIX_ERROR, rb_box_vs_sphere, rb_box_vs_capsule, RB_MATRIX_ERROR },
844 /*sphere */ { rb_sphere_vs_box, rb_sphere_vs_sphere, rb_sphere_vs_capsule, RB_MATRIX_ERROR },
845 /*capsule*/ { rb_capsule_vs_box,rb_capsule_vs_sphere,rb_capsule_vs_capsule,RB_MATRIX_ERROR },
846 /*mesh */ { RB_MATRIX_ERROR, RB_MATRIX_ERROR, RB_MATRIX_ERROR, RB_MATRIX_ERROR }
847 };
848
849
850 /*
851 * Generic functions
852 */
853
854 /*
855 * This function does not accept triangle as a dynamic object, it is assumed
856 * to always be static.
857 *
858 * The triangle is also assumed to be one sided for better detection
859 */
860 static int rb_sphere_vs_triangle( rigidbody *rba, v3f tri[3], rb_ct *buf )
861 {
862 v3f delta, co;
863
864 closest_on_triangle( rba->co, tri, co );
865 v3_sub( rba->co, co, delta );
866
867 float d2 = v3_length2( delta ),
868 r = rba->inf.sphere.radius;
869
870 if( d2 < r*r )
871 {
872 v3f ab, ac, tn;
873 v3_sub( tri[1], tri[0], ab );
874 v3_sub( tri[2], tri[0], ac );
875 v3_cross( ac, ab, tn );
876
877 if( v3_dot( delta, tn ) > 0.0f )
878 v3_muls( delta, -1.0f, delta );
879
880 float d = sqrtf(d2);
881
882 rb_ct *ct = buf;
883 v3_muls( delta, 1.0f/d, ct->n );
884 v3_copy( co, ct->co );
885 ct->p = r-d;
886 ct->rba = rba;
887 ct->rbb = &rb_static_null;
888 return 1;
889 }
890
891 return 0;
892 }
893
894
895 RB_DEPR
896 static int sphere_vs_triangle( v3f c, float r, v3f tri[3],
897 v3f co, v3f norm, float *p )
898 {
899 v3f delta;
900 closest_on_triangle( c, tri, co );
901
902 v3_sub( c, co, delta );
903
904
905 float d = v3_length2( delta );
906 if( d < r*r )
907 {
908 v3f ab, ac, tn;
909 v3_sub( tri[1], tri[0], ab );
910 v3_sub( tri[2], tri[0], ac );
911 v3_cross( ac, ab, tn );
912
913 if( v3_dot( delta, tn ) > 0.0f )
914 v3_muls( delta, -1.0f, delta );
915
916 vg_line_pt3( co, 0.05f, 0xff00ff00 );
917
918 d = sqrtf(d);
919 v3_muls( delta, 1.0f/d, norm );
920
921 *p = r-d;
922 return 1;
923 }
924
925 return 0;
926 }
927
928 #include "world.h"
929
930 static void rb_solver_reset(void)
931 {
932 rb_contact_count = 0;
933 }
934
935 static rb_ct *rb_global_ct(void)
936 {
937 return rb_contact_buffer + rb_contact_count;
938 }
939
940 static struct contact *rb_start_contact(void)
941 {
942 if( rb_contact_count == vg_list_size(rb_contact_buffer) )
943 {
944 vg_error( "rigidbody: too many contacts generated (%u)\n",
945 rb_contact_count );
946 return NULL;
947 }
948
949 return &rb_contact_buffer[ rb_contact_count ];
950 }
951
952 static void rb_commit_contact( struct contact *ct, float p )
953 {
954 ct->bias = -0.2f*k_rb_rate*vg_minf(0.0f,-p+0.04f);
955 rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
956
957 ct->norm_impulse = 0.0f;
958 ct->tangent_impulse[0] = 0.0f;
959 ct->tangent_impulse[1] = 0.0f;
960
961 rb_contact_count ++;
962 }
963
964 static void rb_build_manifold_terrain_sphere( rigidbody *rb )
965 {
966 u32 geo[256];
967 v3f tri[3];
968 int len = bh_select( &world.geo.bhtris, rb->bbx_world, geo, 256 );
969
970 for( int i=0; i<len; i++ )
971 {
972 u32 *ptri = &world.geo.indices[ geo[i]*3 ];
973
974 for( int j=0; j<3; j++ )
975 v3_copy( world.geo.verts[ptri[j]].co, tri[j] );
976
977 vg_line(tri[0],tri[1],0xff00ff00 );
978 vg_line(tri[1],tri[2],0xff00ff00 );
979 vg_line(tri[2],tri[0],0xff00ff00 );
980
981 v3f co, norm;
982 float p;
983
984 for( int j=0; j<2; j++ )
985 {
986 if( sphere_vs_triangle( rb->co, rb->inf.sphere.radius, tri,co,norm,&p))
987 {
988 struct contact *ct = rb_start_contact();
989
990 if( !ct )
991 return;
992
993 v3f p1;
994 v3_muladds( rb->co, norm, p, p1 );
995 vg_line( rb->co, p1, 0xffffffff );
996
997 ct->rba = rb;
998 v3_copy( co, ct->co );
999 v3_copy( norm, ct->n );
1000 rb_commit_contact( ct, p );
1001 }
1002 }
1003 }
1004
1005 }
1006
1007 RB_DEPR
1008 static void rb_build_manifold_terrain( rigidbody *rb )
1009 {
1010 v3f *box = rb->bbx;
1011 v3f pts[8];
1012 float *p000 = pts[0], *p001 = pts[1], *p010 = pts[2], *p011 = pts[3],
1013 *p100 = pts[4], *p101 = pts[5], *p110 = pts[6], *p111 = pts[7];
1014
1015 p000[0]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2];
1016 p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2];
1017 p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2];
1018 p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2];
1019
1020 p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2];
1021 p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2];
1022 p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2];
1023 p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2];
1024
1025 m4x3_mulv( rb->to_world, p000, p000 );
1026 m4x3_mulv( rb->to_world, p001, p001 );
1027 m4x3_mulv( rb->to_world, p010, p010 );
1028 m4x3_mulv( rb->to_world, p011, p011 );
1029 m4x3_mulv( rb->to_world, p100, p100 );
1030 m4x3_mulv( rb->to_world, p101, p101 );
1031 m4x3_mulv( rb->to_world, p110, p110 );
1032 m4x3_mulv( rb->to_world, p111, p111 );
1033
1034 int count = 0;
1035
1036 for( int i=0; i<8; i++ )
1037 {
1038 float *point = pts[i];
1039 struct contact *ct = rb_start_contact();
1040
1041 if( !ct )
1042 return;
1043
1044 ct->rba = rb;
1045
1046 v3f surface;
1047 v3_copy( point, surface );
1048 surface[1] += 4.0f;
1049
1050 ray_hit hit;
1051 hit.dist = INFINITY;
1052 if( !ray_world( surface, (v3f){0.0f,-1.0f,0.0f}, &hit ))
1053 continue;
1054
1055 v3_copy( hit.pos, surface );
1056
1057 float p = vg_minf( surface[1] - point[1], 1.0f );
1058
1059 if( p > 0.0f )
1060 {
1061 v3_copy( hit.normal, ct->n );
1062 v3_add( point, surface, ct->co );
1063 v3_muls( ct->co, 0.5f, ct->co );
1064
1065 rb_commit_contact( ct, p );
1066 count ++;
1067 if( count == 4 )
1068 break;
1069 }
1070 }
1071 }
1072
1073 /*
1074 * Initializing things like tangent vectors
1075 */
1076
1077 static void rb_presolve_contacts( rb_ct *buffer, int len )
1078 {
1079 for( int i=0; i<len; i++ )
1080 {
1081 rb_ct *ct = &buffer[i];
1082 ct->bias = -0.2f * k_rb_rate * vg_minf(0.0f,-ct->p+0.04f);
1083 rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
1084
1085 ct->norm_impulse = 0.0f;
1086 ct->tangent_impulse[0] = 0.0f;
1087 ct->tangent_impulse[1] = 0.0f;
1088 ct->mass_total = 1.0f/(ct->rba->inv_mass + ct->rbb->inv_mass);
1089
1090 rb_debug_contact( ct );
1091 }
1092 }
1093
1094 /*
1095 * Creates relative contact velocity vector, and offsets between each body
1096 */
1097 static void rb_rcv( rb_ct *ct, v3f rv, v3f da, v3f db )
1098 {
1099 rigidbody *rba = ct->rba,
1100 *rbb = ct->rbb;
1101
1102 v3_sub( ct->co, rba->co, da );
1103 v3_sub( ct->co, rbb->co, db );
1104
1105 v3f rva, rvb;
1106 v3_cross( rba->w, da, rva );
1107 v3_add( rba->v, rva, rva );
1108 v3_cross( rbb->w, db, rvb );
1109 v3_add( rbb->v, rvb, rvb );
1110
1111 v3_sub( rva, rvb, rv );
1112 }
1113
1114 /*
1115 * Apply regular and angular velocity impulses to objects involved in contact
1116 */
1117 static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse )
1118 {
1119 rigidbody *rba = ct->rba,
1120 *rbb = ct->rbb;
1121
1122 v3f ia, ib;
1123 v3_muls( impulse, ct->mass_total*rba->inv_mass, ia );
1124 v3_muls( impulse, -ct->mass_total*rbb->inv_mass, ib );
1125
1126 /* response */
1127 v3_add( rba->v, ia, rba->v );
1128 v3_add( rbb->v, ib, rbb->v );
1129
1130 /* Angular velocity */
1131 v3f wa, wb;
1132 v3_cross( da, ia, wa );
1133 v3_cross( db, ib, wb );
1134 v3_add( rba->w, wa, rba->w );
1135 v3_add( rbb->w, wb, rbb->w );
1136 }
1137
1138 /*
1139 * One iteration to solve the contact constraint
1140 */
1141 static void rb_solve_contacts( rb_ct *buf, int len )
1142 {
1143 float k_friction = 0.1f;
1144
1145 /* Friction Impulse */
1146 for( int i=0; i<len; i++ )
1147 {
1148 struct contact *ct = &buf[i];
1149 rigidbody *rb = ct->rba;
1150
1151 v3f rv, da, db;
1152 rb_rcv( ct, rv, da, db );
1153
1154 for( int j=0; j<2; j++ )
1155 {
1156 float f = k_friction * ct->norm_impulse,
1157 vt = -v3_dot( rv, ct->t[j] );
1158
1159 float temp = ct->tangent_impulse[j];
1160 ct->tangent_impulse[j] = vg_clampf( temp+vt, -f, f );
1161 vt = ct->tangent_impulse[j] - temp;
1162
1163 v3f impulse;
1164 v3_muls( ct->t[j], vt, impulse );
1165 rb_standard_impulse( ct, da, db, impulse );
1166 }
1167 }
1168
1169 /* Normal Impulse */
1170 for( int i=0; i<len; i++ )
1171 {
1172 struct contact *ct = &buf[i];
1173 rigidbody *rba = ct->rba,
1174 *rbb = ct->rbb;
1175
1176 v3f rv, da, db;
1177 rb_rcv( ct, rv, da, db );
1178
1179 float vn = -v3_dot( rv, ct->n ) + ct->bias;
1180
1181 float temp = ct->norm_impulse;
1182 ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
1183 vn = ct->norm_impulse - temp;
1184
1185 v3f impulse;
1186 v3_muls( ct->n, vn, impulse );
1187 rb_standard_impulse( ct, da, db, impulse );
1188 }
1189 }
1190
1191 /*
1192 * The following ventures into not really very sophisticated at all maths
1193 */
1194
1195 struct rb_angle_limit
1196 {
1197 rigidbody *rba, *rbb;
1198 v3f axis;
1199 float impulse, bias;
1200 };
1201
1202 static int rb_angle_limit_force( rigidbody *rba, v3f va,
1203 rigidbody *rbb, v3f vb,
1204 float max )
1205 {
1206 v3f wva, wvb;
1207 m3x3_mulv( rba->to_world, va, wva );
1208 m3x3_mulv( rbb->to_world, vb, wvb );
1209
1210 float dt = v3_dot(wva,wvb)*0.999f,
1211 ang = fabsf(dt);
1212 ang = acosf( dt );
1213 if( ang > max )
1214 {
1215 float correction = max-ang;
1216
1217 v3f axis;
1218 v3_cross( wva, wvb, axis );
1219
1220 v4f rotation;
1221 q_axis_angle( rotation, axis, -correction*0.25f );
1222 q_mul( rotation, rba->q, rba->q );
1223
1224 q_axis_angle( rotation, axis, correction*0.25f );
1225 q_mul( rotation, rbb->q, rbb->q );
1226
1227 return 1;
1228 }
1229
1230 return 0;
1231 }
1232
1233 static void rb_constraint_angle_limit( struct rb_angle_limit *limit )
1234 {
1235
1236 }
1237
1238 RB_DEPR
1239 static void rb_constraint_angle( rigidbody *rba, v3f va,
1240 rigidbody *rbb, v3f vb,
1241 float max, float spring )
1242 {
1243 v3f wva, wvb;
1244 m3x3_mulv( rba->to_world, va, wva );
1245 m3x3_mulv( rbb->to_world, vb, wvb );
1246
1247 float dt = v3_dot(wva,wvb)*0.999f,
1248 ang = fabsf(dt);
1249
1250 v3f axis;
1251 v3_cross( wva, wvb, axis );
1252 v3_muladds( rba->w, axis, ang*spring*0.5f, rba->w );
1253 v3_muladds( rbb->w, axis, -ang*spring*0.5f, rbb->w );
1254
1255 return;
1256
1257 /* TODO: convert max into the dot product value so we dont have to always
1258 * evaluate acosf, only if its greater than the angle specified */
1259 ang = acosf( dt );
1260 if( ang > max )
1261 {
1262 float correction = max-ang;
1263
1264 v4f rotation;
1265 q_axis_angle( rotation, axis, -correction*0.125f );
1266 q_mul( rotation, rba->q, rba->q );
1267
1268 q_axis_angle( rotation, axis, correction*0.125f );
1269 q_mul( rotation, rbb->q, rbb->q );
1270 }
1271 }
1272
1273 static void rb_relative_velocity( rigidbody *ra, v3f lca,
1274 rigidbody *rb, v3f lcb, v3f rcv )
1275 {
1276 v3f wca, wcb;
1277 m3x3_mulv( ra->to_world, lca, wca );
1278 m3x3_mulv( rb->to_world, lcb, wcb );
1279
1280 v3_sub( ra->v, rb->v, rcv );
1281
1282 v3f rcv_Ra, rcv_Rb;
1283 v3_cross( ra->w, wca, rcv_Ra );
1284 v3_cross( rb->w, wcb, rcv_Rb );
1285 v3_add( rcv_Ra, rcv, rcv );
1286 v3_sub( rcv, rcv_Rb, rcv );
1287 }
1288
1289 static void rb_constraint_position( rigidbody *ra, v3f lca,
1290 rigidbody *rb, v3f lcb )
1291 {
1292 /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
1293 v3f wca, wcb;
1294 m3x3_mulv( ra->to_world, lca, wca );
1295 m3x3_mulv( rb->to_world, lcb, wcb );
1296
1297 v3f delta;
1298 v3_add( wcb, rb->co, delta );
1299 v3_sub( delta, wca, delta );
1300 v3_sub( delta, ra->co, delta );
1301
1302 v3_muladds( ra->co, delta, 0.5f, ra->co );
1303 v3_muladds( rb->co, delta, -0.5f, rb->co );
1304
1305 v3f rcv;
1306 v3_sub( ra->v, rb->v, rcv );
1307
1308 v3f rcv_Ra, rcv_Rb;
1309 v3_cross( ra->w, wca, rcv_Ra );
1310 v3_cross( rb->w, wcb, rcv_Rb );
1311 v3_add( rcv_Ra, rcv, rcv );
1312 v3_sub( rcv, rcv_Rb, rcv );
1313
1314 float nm = 0.5f/(rb->inv_mass + ra->inv_mass);
1315
1316 float mass_a = 1.0f/ra->inv_mass,
1317 mass_b = 1.0f/rb->inv_mass,
1318 total_mass = mass_a+mass_b;
1319
1320 v3f impulse;
1321 v3_muls( rcv, 1.0f, impulse );
1322 v3_muladds( rb->v, impulse, mass_b/total_mass, rb->v );
1323 v3_cross( wcb, impulse, impulse );
1324 v3_add( impulse, rb->w, rb->w );
1325
1326 v3_muls( rcv, -1.0f, impulse );
1327 v3_muladds( ra->v, impulse, mass_a/total_mass, ra->v );
1328 v3_cross( wca, impulse, impulse );
1329 v3_add( impulse, ra->w, ra->w );
1330
1331 #if 0
1332 /*
1333 * this could be used for spring joints
1334 * its not good for position constraint
1335 */
1336 v3f impulse;
1337 v3_muls( delta, 0.5f*spring, impulse );
1338
1339 v3_add( impulse, ra->v, ra->v );
1340 v3_cross( wca, impulse, impulse );
1341 v3_add( impulse, ra->w, ra->w );
1342
1343 v3_muls( delta, -0.5f*spring, impulse );
1344
1345 v3_add( impulse, rb->v, rb->v );
1346 v3_cross( wcb, impulse, impulse );
1347 v3_add( impulse, rb->w, rb->w );
1348 #endif
1349 }
1350
1351 static void debug_sphere( m4x3f m, float radius, u32 colour )
1352 {
1353 v3f ly = { 0.0f, 0.0f, radius },
1354 lx = { 0.0f, radius, 0.0f },
1355 lz = { 0.0f, 0.0f, radius };
1356
1357 for( int i=0; i<16; i++ )
1358 {
1359 float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
1360 s = sinf(t),
1361 c = cosf(t);
1362
1363 v3f py = { s*radius, 0.0f, c*radius },
1364 px = { s*radius, c*radius, 0.0f },
1365 pz = { 0.0f, s*radius, c*radius };
1366
1367 v3f p0, p1, p2, p3, p4, p5;
1368 m4x3_mulv( m, py, p0 );
1369 m4x3_mulv( m, ly, p1 );
1370 m4x3_mulv( m, px, p2 );
1371 m4x3_mulv( m, lx, p3 );
1372 m4x3_mulv( m, pz, p4 );
1373 m4x3_mulv( m, lz, p5 );
1374
1375 vg_line( p0, p1, colour == 0x00? 0xff00ff00: colour );
1376 vg_line( p2, p3, colour == 0x00? 0xff0000ff: colour );
1377 vg_line( p4, p5, colour == 0x00? 0xffff0000: colour );
1378
1379 v3_copy( py, ly );
1380 v3_copy( px, lx );
1381 v3_copy( pz, lz );
1382 }
1383 }
1384
1385 static void debug_capsule( m4x3f m, float radius, float h, u32 colour )
1386 {
1387 v3f ly = { 0.0f, 0.0f, radius },
1388 lx = { 0.0f, radius, 0.0f },
1389 lz = { 0.0f, 0.0f, radius };
1390
1391 float s0 = sinf(0.0f)*radius,
1392 c0 = cosf(0.0f)*radius;
1393
1394 v3f p0, p1, up, right, forward;
1395 m3x3_mulv( m, (v3f){0.0f,1.0f,0.0f}, up );
1396 m3x3_mulv( m, (v3f){1.0f,0.0f,0.0f}, right );
1397 m3x3_mulv( m, (v3f){0.0f,0.0f,-1.0f}, forward );
1398 v3_muladds( m[3], up, -h*0.5f+radius, p0 );
1399 v3_muladds( m[3], up, h*0.5f-radius, p1 );
1400
1401 v3f a0, a1, b0, b1;
1402 v3_muladds( p0, right, radius, a0 );
1403 v3_muladds( p1, right, radius, a1 );
1404 v3_muladds( p0, forward, radius, b0 );
1405 v3_muladds( p1, forward, radius, b1 );
1406 vg_line( a0, a1, colour );
1407 vg_line( b0, b1, colour );
1408
1409 v3_muladds( p0, right, -radius, a0 );
1410 v3_muladds( p1, right, -radius, a1 );
1411 v3_muladds( p0, forward, -radius, b0 );
1412 v3_muladds( p1, forward, -radius, b1 );
1413 vg_line( a0, a1, colour );
1414 vg_line( b0, b1, colour );
1415
1416 for( int i=0; i<16; i++ )
1417 {
1418 float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
1419 s1 = sinf(t)*radius,
1420 c1 = cosf(t)*radius;
1421
1422 v3f e0 = { s0, 0.0f, c0 },
1423 e1 = { s1, 0.0f, c1 },
1424 e2 = { s0, c0, 0.0f },
1425 e3 = { s1, c1, 0.0f },
1426 e4 = { 0.0f, c0, s0 },
1427 e5 = { 0.0f, c1, s1 };
1428
1429 m3x3_mulv( m, e0, e0 );
1430 m3x3_mulv( m, e1, e1 );
1431 m3x3_mulv( m, e2, e2 );
1432 m3x3_mulv( m, e3, e3 );
1433 m3x3_mulv( m, e4, e4 );
1434 m3x3_mulv( m, e5, e5 );
1435
1436 v3_add( p0, e0, a0 );
1437 v3_add( p0, e1, a1 );
1438 v3_add( p1, e0, b0 );
1439 v3_add( p1, e1, b1 );
1440
1441 vg_line( a0, a1, colour );
1442 vg_line( b0, b1, colour );
1443
1444 if( c0 < 0.0f )
1445 {
1446 v3_add( p0, e2, a0 );
1447 v3_add( p0, e3, a1 );
1448 v3_add( p0, e4, b0 );
1449 v3_add( p0, e5, b1 );
1450 }
1451 else
1452 {
1453 v3_add( p1, e2, a0 );
1454 v3_add( p1, e3, a1 );
1455 v3_add( p1, e4, b0 );
1456 v3_add( p1, e5, b1 );
1457 }
1458
1459 vg_line( a0, a1, colour );
1460 vg_line( b0, b1, colour );
1461
1462 s0 = s1;
1463 c0 = c1;
1464 }
1465 }
1466
1467 static void rb_debug( rigidbody *rb, u32 colour )
1468 {
1469 if( rb->type == k_rb_shape_box )
1470 {
1471 v3f *box = rb->bbx;
1472 vg_line_boxf_transformed( rb->to_world, rb->bbx, colour );
1473 }
1474 else if( rb->type == k_rb_shape_sphere )
1475 {
1476 debug_sphere( rb->to_world, rb->inf.sphere.radius, colour );
1477 }
1478 else if( rb->type == k_rb_shape_capsule )
1479 {
1480 m4x3f m0, m1;
1481 float h = rb->inf.capsule.height,
1482 r = rb->inf.capsule.radius;
1483
1484 debug_capsule( rb->to_world, r, h, colour );
1485 }
1486 }
1487
1488 /*
1489 * out penetration distance, normal
1490 */
1491 static int rb_point_in_body( rigidbody *rb, v3f pos, float *pen, v3f normal )
1492 {
1493 v3f local;
1494 m4x3_mulv( rb->to_local, pos, local );
1495
1496 if( local[0] > rb->bbx[0][0] && local[0] < rb->bbx[1][0] &&
1497 local[1] > rb->bbx[0][1] && local[1] < rb->bbx[1][1] &&
1498 local[2] > rb->bbx[0][2] && local[2] < rb->bbx[1][2] )
1499 {
1500 v3f area, com, comrel;
1501 v3_add( rb->bbx[0], rb->bbx[1], com );
1502 v3_muls( com, 0.5f, com );
1503
1504 v3_sub( rb->bbx[1], rb->bbx[0], area );
1505 v3_sub( local, com, comrel );
1506 v3_div( comrel, area, comrel );
1507
1508 int axis = 0;
1509 float max_mag = fabsf(comrel[0]);
1510
1511 if( fabsf(comrel[1]) > max_mag )
1512 {
1513 axis = 1;
1514 max_mag = fabsf(comrel[1]);
1515 }
1516 if( fabsf(comrel[2]) > max_mag )
1517 {
1518 axis = 2;
1519 max_mag = fabsf(comrel[2]);
1520 }
1521
1522 v3_zero( normal );
1523 normal[axis] = vg_signf(comrel[axis]);
1524
1525 if( normal[axis] < 0.0f )
1526 *pen = local[axis] - rb->bbx[0][axis];
1527 else
1528 *pen = rb->bbx[1][axis] - local[axis];
1529
1530 m3x3_mulv( rb->to_world, normal, normal );
1531 return 1;
1532 }
1533
1534 return 0;
1535 }
1536
1537 /*
1538 * BVH implementation, this is ONLY for static rigidbodies, its to slow for
1539 * realtime use.
1540 */
1541
1542 static void rb_bh_expand_bound( void *user, boxf bound, u32 item_index )
1543 {
1544 rigidbody *rb = &((rigidbody *)user)[ item_index ];
1545 box_concat( bound, rb->bbx_world );
1546 }
1547
1548 static float rb_bh_centroid( void *user, u32 item_index, int axis )
1549 {
1550 rigidbody *rb = &((rigidbody *)user)[ item_index ];
1551 return (rb->bbx_world[axis][0] + rb->bbx_world[1][axis]) * 0.5f;
1552 }
1553
1554 static void rb_bh_swap( void *user, u32 ia, u32 ib )
1555 {
1556 rigidbody temp, *rba, *rbb;
1557 rba = &((rigidbody *)user)[ ia ];
1558 rbb = &((rigidbody *)user)[ ib ];
1559
1560 temp = *rba;
1561 *rba = *rbb;
1562 *rbb = temp;
1563 }
1564
1565 static void rb_bh_debug( void *user, u32 item_index )
1566 {
1567 rigidbody *rb = &((rigidbody *)user)[ item_index ];
1568 rb_debug( rb, 0xff00ffff );
1569 }
1570
1571 static bh_system bh_system_rigidbodies =
1572 {
1573 .expand_bound = rb_bh_expand_bound,
1574 .item_centroid = rb_bh_centroid,
1575 .item_swap = rb_bh_swap,
1576 .item_debug = rb_bh_debug,
1577 .cast_ray = NULL
1578 };
1579
1580 #endif /* RIGIDBODY_H */