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