physics revision
[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(void);
201
202 /*
203 * These closest point tests were learned from Real-Time Collision Detection by
204 * Christer Ericson
205 */
206 static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
207 float *s, float *t, v3f c1, v3f c2)
208 {
209 v3f d1,d2,r;
210 v3_sub( q1, p1, d1 );
211 v3_sub( q2, p2, d2 );
212 v3_sub( p1, p2, r );
213
214 float a = v3_length2( d1 ),
215 e = v3_length2( d2 ),
216 f = v3_dot( d2, r );
217
218 const float kEpsilon = 0.0001f;
219
220 if( a <= kEpsilon && e <= kEpsilon )
221 {
222 *s = 0.0f;
223 *t = 0.0f;
224 v3_copy( p1, c1 );
225 v3_copy( p2, c2 );
226
227 v3f v0;
228 v3_sub( c1, c2, v0 );
229
230 return v3_length2( v0 );
231 }
232
233 if( a<= kEpsilon )
234 {
235 *s = 0.0f;
236 *t = vg_clampf( f / e, 0.0f, 1.0f );
237 }
238 else
239 {
240 float c = v3_dot( d1, r );
241 if( e <= kEpsilon )
242 {
243 *t = 0.0f;
244 *s = vg_clampf( -c / a, 0.0f, 1.0f );
245 }
246 else
247 {
248 float b = v3_dot(d1,d2),
249 d = a*e-b*b;
250
251 if( d != 0.0f )
252 {
253 *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
254 }
255 else
256 {
257 *s = 0.0f;
258 }
259
260 *t = (b*(*s)+f) / e;
261
262 if( *t < 0.0f )
263 {
264 *t = 0.0f;
265 *s = vg_clampf( -c / a, 0.0f, 1.0f );
266 }
267 else if( *t > 1.0f )
268 {
269 *t = 1.0f;
270 *s = vg_clampf((b-c)/a,0.0f,1.0f);
271 }
272 }
273 }
274
275 v3_muladds( p1, d1, *s, c1 );
276 v3_muladds( p2, d2, *t, c2 );
277
278 v3f v0;
279 v3_sub( c1, c2, v0 );
280 return v3_length2( v0 );
281 }
282
283 static void closest_point_aabb( v3f p, boxf box, v3f dest )
284 {
285 v3_maxv( p, box[0], dest );
286 v3_minv( dest, box[1], dest );
287 }
288
289 static void closest_point_obb( v3f p, rigidbody *rb, v3f dest )
290 {
291 v3f local;
292 m4x3_mulv( rb->to_local, p, local );
293 closest_point_aabb( local, rb->bbx, local );
294 m4x3_mulv( rb->to_world, local, dest );
295 }
296
297 static void closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
298 {
299 v3f v0, v1;
300 v3_sub( b, a, v0 );
301 v3_sub( point, a, v1 );
302
303 float t = v3_dot( v1, v0 ) / v3_length2(v0);
304 v3_muladds( a, v0, vg_clampf(t,0.0f,1.0f), dest );
305 }
306
307 static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
308 {
309 v3f ab, ac, ap;
310 float d1, d2;
311
312 /* Region outside A */
313 v3_sub( tri[1], tri[0], ab );
314 v3_sub( tri[2], tri[0], ac );
315 v3_sub( p, tri[0], ap );
316
317 d1 = v3_dot(ab,ap);
318 d2 = v3_dot(ac,ap);
319 if( d1 <= 0.0f && d2 <= 0.0f )
320 {
321 v3_copy( tri[0], dest );
322 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
323 return;
324 }
325
326 /* Region outside B */
327 v3f bp;
328 float d3, d4;
329
330 v3_sub( p, tri[1], bp );
331 d3 = v3_dot( ab, bp );
332 d4 = v3_dot( ac, bp );
333
334 if( d3 >= 0.0f && d4 <= d3 )
335 {
336 v3_copy( tri[1], dest );
337 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
338 return;
339 }
340
341 /* Edge region of AB */
342 float vc = d1*d4 - d3*d2;
343 if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
344 {
345 float v = d1 / (d1-d3);
346 v3_muladds( tri[0], ab, v, dest );
347 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
348 return;
349 }
350
351 /* Region outside C */
352 v3f cp;
353 float d5, d6;
354 v3_sub( p, tri[2], cp );
355 d5 = v3_dot(ab, cp);
356 d6 = v3_dot(ac, cp);
357
358 if( d6 >= 0.0f && d5 <= d6 )
359 {
360 v3_copy( tri[2], dest );
361 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
362 return;
363 }
364
365 /* Region of AC */
366 float vb = d5*d2 - d1*d6;
367 if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
368 {
369 float w = d2 / (d2-d6);
370 v3_muladds( tri[0], ac, w, dest );
371 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
372 return;
373 }
374
375 /* Region of BC */
376 float va = d3*d6 - d5*d4;
377 if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
378 {
379 float w = (d4-d3) / ((d4-d3) + (d5-d6));
380 v3f bc;
381 v3_sub( tri[2], tri[1], bc );
382 v3_muladds( tri[1], bc, w, dest );
383 v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
384 return;
385 }
386
387 /* P inside region, Q via barycentric coordinates uvw */
388 float d = 1.0f/(va+vb+vc),
389 v = vb*d,
390 w = vc*d;
391
392 v3_muladds( tri[0], ab, v, dest );
393 v3_muladds( dest, ac, w, dest );
394 }
395
396 /*
397 * Contact generators
398 *
399 * These do not automatically allocate contacts, an appropriately sized
400 * buffer must be supplied. The function returns the size of the manifold
401 * which was generated.
402 *
403 * The values set on the contacts are: n, co, p, rba, rbb
404 */
405
406 static void rb_debug_contact( rb_ct *ct )
407 {
408 v3f p1;
409 v3_muladds( ct->co, ct->n, 0.2f, p1 );
410 vg_line_pt3( ct->co, 0.1f, 0xff0000ff );
411 vg_line( ct->co, p1, 0xffffffff );
412 }
413
414 static int rb_sphere_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
415 {
416 v3f co, delta;
417
418 closest_point_obb( rba->co, rbb, co );
419 v3_sub( rba->co, co, delta );
420
421 float d2 = v3_length2(delta),
422 r = rba->inf.sphere.radius;
423
424 if( d2 <= r*r )
425 {
426 float d;
427 if( d2 <= 0.0001f )
428 {
429 v3_sub( rbb->co, rba->co, delta );
430 d2 = v3_length2(delta);
431 }
432
433 d = sqrtf(d2);
434
435 rb_ct *ct = buf;
436 v3_muls( delta, 1.0f/d, ct->n );
437 v3_copy( co, ct->co );
438 ct->p = r-d;
439 ct->rba = rba;
440 ct->rbb = rbb;
441 return 1;
442 }
443
444 return 0;
445 }
446
447 static int rb_sphere_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
448 {
449 v3f delta;
450 v3_sub( rba->co, rbb->co, delta );
451
452 float d2 = v3_length2(delta),
453 r = rba->inf.sphere.radius + rbb->inf.sphere.radius;
454
455 if( d2 < r*r )
456 {
457 float d = sqrtf(d2);
458
459 rb_ct *ct = buf;
460 v3_muls( delta, -1.0f/d, ct->n );
461
462 v3f p0, p1;
463 v3_muladds( rba->co, ct->n, rba->inf.sphere.radius, p0 );
464 v3_muladds( rbb->co, ct->n,-rbb->inf.sphere.radius, p1 );
465 v3_add( p0, p1, ct->co );
466 v3_muls( ct->co, 0.5f, ct->co );
467 ct->p = r-d;
468 ct->rba = rba;
469 ct->rbb = rbb;
470 return 1;
471 }
472
473 return 0;
474 }
475
476 static int rb_box_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
477 {
478 return rb_sphere_vs_box( rbb, rba, buf );
479 }
480
481 static int rb_box_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
482 {
483 /* TODO: Generating a stable quad manifold, lots of clipping */
484 return 0;
485 }
486
487 /*
488 * This function does not accept triangle as a dynamic object, it is assumed
489 * to always be static.
490 *
491 * The triangle is also assumed to be one sided for better detection
492 */
493 static int rb_sphere_vs_triangle( rigidbody *rba, v3f tri[3], rb_ct *buf )
494 {
495 v3f delta, co;
496
497 closest_on_triangle( rba->co, tri, co );
498 v3_sub( rba->co, co, delta );
499
500 float d2 = v3_length2( delta ),
501 r = rba->inf.sphere.radius;
502
503 if( d2 < r*r )
504 {
505 v3f ab, ac, tn;
506 v3_sub( tri[1], tri[0], ab );
507 v3_sub( tri[2], tri[0], ac );
508 v3_cross( ac, ab, tn );
509
510 if( v3_dot( delta, tn ) > 0.0f )
511 v3_muls( delta, -1.0f, delta );
512
513 float d = sqrtf(d2);
514
515 rb_ct *ct = buf;
516 v3_muls( delta, 1.0f/d, ct->n );
517 v3_copy( co, ct->co );
518 ct->p = r-d;
519 ct->rba = rba;
520 ct->rbb = &rb_static_null;
521 return 1;
522 }
523
524 return 0;
525 }
526
527
528 /*
529 * Generic functions
530 */
531
532 RB_DEPR
533 static int sphere_vs_triangle( v3f c, float r, v3f tri[3],
534 v3f co, v3f norm, float *p )
535 {
536 v3f delta;
537 closest_on_triangle( c, tri, co );
538
539 v3_sub( c, co, delta );
540
541
542 float d = v3_length2( delta );
543 if( d < r*r )
544 {
545 v3f ab, ac, tn;
546 v3_sub( tri[1], tri[0], ab );
547 v3_sub( tri[2], tri[0], ac );
548 v3_cross( ac, ab, tn );
549
550 if( v3_dot( delta, tn ) > 0.0f )
551 v3_muls( delta, -1.0f, delta );
552
553 vg_line_pt3( co, 0.05f, 0xff00ff00 );
554
555 d = sqrtf(d);
556 v3_muls( delta, 1.0f/d, norm );
557
558 *p = r-d;
559 return 1;
560 }
561
562 return 0;
563 }
564
565 #include "world.h"
566
567 static void rb_solver_reset(void)
568 {
569 rb_contact_count = 0;
570 }
571
572 static rb_ct *rb_global_ct(void)
573 {
574 return rb_contact_buffer + rb_contact_count;
575 }
576
577 static struct contact *rb_start_contact(void)
578 {
579 if( rb_contact_count == vg_list_size(rb_contact_buffer) )
580 {
581 vg_error( "rigidbody: too many contacts generated (%u)\n",
582 rb_contact_count );
583 return NULL;
584 }
585
586 return &rb_contact_buffer[ rb_contact_count ];
587 }
588
589 static void rb_commit_contact( struct contact *ct, float p )
590 {
591 ct->bias = -0.2f*k_rb_rate*vg_minf(0.0f,-p+0.04f);
592 rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
593
594 ct->norm_impulse = 0.0f;
595 ct->tangent_impulse[0] = 0.0f;
596 ct->tangent_impulse[1] = 0.0f;
597
598 rb_contact_count ++;
599 }
600
601 static void rb_build_manifold_terrain_sphere( rigidbody *rb )
602 {
603 u32 geo[256];
604 v3f tri[3];
605 int len = bh_select( &world.geo.bhtris, rb->bbx_world, geo, 256 );
606
607 for( int i=0; i<len; i++ )
608 {
609 u32 *ptri = &world.geo.indices[ geo[i]*3 ];
610
611 for( int j=0; j<3; j++ )
612 v3_copy( world.geo.verts[ptri[j]].co, tri[j] );
613
614 vg_line(tri[0],tri[1],0xff00ff00 );
615 vg_line(tri[1],tri[2],0xff00ff00 );
616 vg_line(tri[2],tri[0],0xff00ff00 );
617
618 v3f co, norm;
619 float p;
620
621 for( int j=0; j<2; j++ )
622 {
623 if( sphere_vs_triangle( rb->co, rb->inf.sphere.radius, tri,co,norm,&p))
624 {
625 struct contact *ct = rb_start_contact();
626
627 if( !ct )
628 return;
629
630 v3f p1;
631 v3_muladds( rb->co, norm, p, p1 );
632 vg_line( rb->co, p1, 0xffffffff );
633
634 ct->rba = rb;
635 v3_copy( co, ct->co );
636 v3_copy( norm, ct->n );
637 rb_commit_contact( ct, p );
638 }
639 }
640 }
641
642 }
643
644 RB_DEPR
645 static void rb_build_manifold_terrain( rigidbody *rb )
646 {
647 v3f *box = rb->bbx;
648 v3f pts[8];
649 float *p000 = pts[0], *p001 = pts[1], *p010 = pts[2], *p011 = pts[3],
650 *p100 = pts[4], *p101 = pts[5], *p110 = pts[6], *p111 = pts[7];
651
652 p000[0]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2];
653 p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2];
654 p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2];
655 p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2];
656
657 p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2];
658 p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2];
659 p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2];
660 p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2];
661
662 m4x3_mulv( rb->to_world, p000, p000 );
663 m4x3_mulv( rb->to_world, p001, p001 );
664 m4x3_mulv( rb->to_world, p010, p010 );
665 m4x3_mulv( rb->to_world, p011, p011 );
666 m4x3_mulv( rb->to_world, p100, p100 );
667 m4x3_mulv( rb->to_world, p101, p101 );
668 m4x3_mulv( rb->to_world, p110, p110 );
669 m4x3_mulv( rb->to_world, p111, p111 );
670
671 int count = 0;
672
673 for( int i=0; i<8; i++ )
674 {
675 float *point = pts[i];
676 struct contact *ct = rb_start_contact();
677
678 if( !ct )
679 return;
680
681 ct->rba = rb;
682
683 v3f surface;
684 v3_copy( point, surface );
685 surface[1] += 4.0f;
686
687 ray_hit hit;
688 hit.dist = INFINITY;
689 if( !ray_world( surface, (v3f){0.0f,-1.0f,0.0f}, &hit ))
690 continue;
691
692 v3_copy( hit.pos, surface );
693
694 float p = vg_minf( surface[1] - point[1], 1.0f );
695
696 if( p > 0.0f )
697 {
698 v3_copy( hit.normal, ct->n );
699 v3_add( point, surface, ct->co );
700 v3_muls( ct->co, 0.5f, ct->co );
701
702 rb_commit_contact( ct, p );
703 count ++;
704 if( count == 4 )
705 break;
706 }
707 }
708 }
709
710 /*
711 * Initializing things like tangent vectors
712 */
713 static void rb_presolve_contacts(void)
714 {
715 for( int i=0; i<rb_contact_count; i++ )
716 {
717 rb_ct *ct = &rb_contact_buffer[i];
718
719 ct->bias = -0.2f * k_rb_rate * vg_minf(0.0f,-ct->p+0.04f);
720 rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
721
722 ct->norm_impulse = 0.0f;
723 ct->tangent_impulse[0] = 0.0f;
724 ct->tangent_impulse[1] = 0.0f;
725 ct->mass_total = 1.0f/(ct->rba->inv_mass + ct->rbb->inv_mass);
726
727 rb_debug_contact( ct );
728 }
729 }
730
731 /*
732 * Creates relative contact velocity vector, and offsets between each body */
733 static void rb_rcv( rb_ct *ct, v3f rv, v3f da, v3f db )
734 {
735 rigidbody *rba = ct->rba,
736 *rbb = ct->rbb;
737
738 v3_sub( rba->co, ct->co, da );
739 v3_sub( rbb->co, ct->co, db );
740
741 v3f rva, rvb;
742 v3_cross( rba->w, da, rva );
743 v3_add( rba->v, rva, rva );
744
745 v3_cross( rbb->w, db, rvb );
746 v3_add( rbb->v, rvb, rvb );
747 v3_add( rva, rvb, rv );
748 }
749
750 static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse )
751 {
752 rigidbody *rba = ct->rba,
753 *rbb = ct->rbb;
754
755 /* response */
756 v3_muladds( rba->v, impulse, ct->mass_total * rba->inv_mass, rba->v );
757 v3_muladds( rbb->v, impulse, ct->mass_total * rbb->inv_mass, rbb->v );
758
759 /* Angular velocity */
760 v3f wa, wb;
761 v3_cross( da, impulse, wa );
762 v3_cross( db, impulse, wb );
763 v3_muladds( rba->w, wa, ct->mass_total * rba->inv_mass, rba->w );
764 v3_muladds( rbb->w, wb, ct->mass_total * rbb->inv_mass, rbb->w );
765 }
766
767 static void rb_solve_contacts(void)
768 {
769 float k_friction = 0.1f;
770
771 /* TODO: second object
772 * Static objects route to static element */
773
774 /* Friction Impulse */
775 for( int i=0; i<rb_contact_count; i++ )
776 {
777 struct contact *ct = &rb_contact_buffer[i];
778 rigidbody *rb = ct->rba;
779
780 v3f rv, da, db;
781 rb_rcv( ct, rv, da, db );
782
783 for( int j=0; j<2; j++ )
784 {
785 float f = k_friction * ct->norm_impulse,
786 vt = -v3_dot( rv, ct->t[j] );
787
788 float temp = ct->tangent_impulse[j];
789 ct->tangent_impulse[j] = vg_clampf( temp+vt, -f, f );
790 vt = ct->tangent_impulse[j] - temp;
791
792 v3f impulse;
793 v3_muls( ct->t[j], vt, impulse );
794 rb_standard_impulse( ct, da, db, impulse );
795 }
796 }
797
798 /* Normal Impulse */
799 for( int i=0; i<rb_contact_count; i++ )
800 {
801 struct contact *ct = &rb_contact_buffer[i];
802 rigidbody *rba = ct->rba,
803 *rbb = ct->rbb;
804
805 v3f rv, da, db;
806 rb_rcv( ct, rv, da, db );
807
808 float vn = -v3_dot( rv, ct->n );
809 vn += ct->bias;
810
811 float temp = ct->norm_impulse;
812 ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
813 vn = ct->norm_impulse - temp;
814
815 v3f impulse;
816 v3_muls( ct->n, vn, impulse );
817 rb_standard_impulse( ct, da, db, impulse );
818 }
819 }
820
821 struct rb_angle_limit
822 {
823 rigidbody *rba, *rbb;
824 v3f axis;
825 float impulse, bias;
826 };
827
828 static int rb_angle_limit_force( rigidbody *rba, v3f va,
829 rigidbody *rbb, v3f vb,
830 float max )
831 {
832 v3f wva, wvb;
833 m3x3_mulv( rba->to_world, va, wva );
834 m3x3_mulv( rbb->to_world, vb, wvb );
835
836 float dt = v3_dot(wva,wvb)*0.999f,
837 ang = fabsf(dt);
838 ang = acosf( dt );
839 if( ang > max )
840 {
841 float correction = max-ang;
842
843 v3f axis;
844 v3_cross( wva, wvb, axis );
845
846 v4f rotation;
847 q_axis_angle( rotation, axis, -correction*0.25f );
848 q_mul( rotation, rba->q, rba->q );
849
850 q_axis_angle( rotation, axis, correction*0.25f );
851 q_mul( rotation, rbb->q, rbb->q );
852
853 return 1;
854 }
855
856 return 0;
857 }
858
859 static void rb_constraint_angle_limit( struct rb_angle_limit *limit )
860 {
861
862 }
863
864 RB_DEPR
865 static void rb_constraint_angle( rigidbody *rba, v3f va,
866 rigidbody *rbb, v3f vb,
867 float max, float spring )
868 {
869 v3f wva, wvb;
870 m3x3_mulv( rba->to_world, va, wva );
871 m3x3_mulv( rbb->to_world, vb, wvb );
872
873 float dt = v3_dot(wva,wvb)*0.999f,
874 ang = fabsf(dt);
875
876 v3f axis;
877 v3_cross( wva, wvb, axis );
878 v3_muladds( rba->w, axis, ang*spring*0.5f, rba->w );
879 v3_muladds( rbb->w, axis, -ang*spring*0.5f, rbb->w );
880
881 return;
882
883 /* TODO: convert max into the dot product value so we dont have to always
884 * evaluate acosf, only if its greater than the angle specified */
885 ang = acosf( dt );
886 if( ang > max )
887 {
888 float correction = max-ang;
889
890 v4f rotation;
891 q_axis_angle( rotation, axis, -correction*0.125f );
892 q_mul( rotation, rba->q, rba->q );
893
894 q_axis_angle( rotation, axis, correction*0.125f );
895 q_mul( rotation, rbb->q, rbb->q );
896 }
897 }
898
899 static void rb_relative_velocity( rigidbody *ra, v3f lca,
900 rigidbody *rb, v3f lcb, v3f rcv )
901 {
902 v3f wca, wcb;
903 m3x3_mulv( ra->to_world, lca, wca );
904 m3x3_mulv( rb->to_world, lcb, wcb );
905
906 v3_sub( ra->v, rb->v, rcv );
907
908 v3f rcv_Ra, rcv_Rb;
909 v3_cross( ra->w, wca, rcv_Ra );
910 v3_cross( rb->w, wcb, rcv_Rb );
911 v3_add( rcv_Ra, rcv, rcv );
912 v3_sub( rcv, rcv_Rb, rcv );
913 }
914
915 static void rb_constraint_position( rigidbody *ra, v3f lca,
916 rigidbody *rb, v3f lcb )
917 {
918 /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
919 v3f wca, wcb;
920 m3x3_mulv( ra->to_world, lca, wca );
921 m3x3_mulv( rb->to_world, lcb, wcb );
922
923 v3f delta;
924 v3_add( wcb, rb->co, delta );
925 v3_sub( delta, wca, delta );
926 v3_sub( delta, ra->co, delta );
927
928 v3_muladds( ra->co, delta, 0.5f, ra->co );
929 v3_muladds( rb->co, delta, -0.5f, rb->co );
930
931 v3f rcv;
932 v3_sub( ra->v, rb->v, rcv );
933
934 v3f rcv_Ra, rcv_Rb;
935 v3_cross( ra->w, wca, rcv_Ra );
936 v3_cross( rb->w, wcb, rcv_Rb );
937 v3_add( rcv_Ra, rcv, rcv );
938 v3_sub( rcv, rcv_Rb, rcv );
939
940 float nm = 0.5f/(rb->inv_mass + ra->inv_mass);
941
942 float mass_a = 1.0f/ra->inv_mass,
943 mass_b = 1.0f/rb->inv_mass,
944 total_mass = mass_a+mass_b;
945
946 v3f impulse;
947 v3_muls( rcv, 1.0f, impulse );
948 v3_muladds( rb->v, impulse, mass_b/total_mass, rb->v );
949 v3_cross( wcb, impulse, impulse );
950 v3_add( impulse, rb->w, rb->w );
951
952 v3_muls( rcv, -1.0f, impulse );
953 v3_muladds( ra->v, impulse, mass_a/total_mass, ra->v );
954 v3_cross( wca, impulse, impulse );
955 v3_add( impulse, ra->w, ra->w );
956
957 #if 0
958 /*
959 * this could be used for spring joints
960 * its not good for position constraint
961 */
962 v3f impulse;
963 v3_muls( delta, 0.5f*spring, impulse );
964
965 v3_add( impulse, ra->v, ra->v );
966 v3_cross( wca, impulse, impulse );
967 v3_add( impulse, ra->w, ra->w );
968
969 v3_muls( delta, -0.5f*spring, impulse );
970
971 v3_add( impulse, rb->v, rb->v );
972 v3_cross( wcb, impulse, impulse );
973 v3_add( impulse, rb->w, rb->w );
974 #endif
975 }
976
977 static void debug_sphere( m4x3f m, float radius, u32 colour )
978 {
979 v3f ly = { 0.0f, 0.0f, radius },
980 lx = { 0.0f, radius, 0.0f },
981 lz = { 0.0f, 0.0f, radius };
982
983 for( int i=0; i<16; i++ )
984 {
985 float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
986 s = sinf(t),
987 c = cosf(t);
988
989 v3f py = { s*radius, 0.0f, c*radius },
990 px = { s*radius, c*radius, 0.0f },
991 pz = { 0.0f, s*radius, c*radius };
992
993 v3f p0, p1, p2, p3, p4, p5;
994 m4x3_mulv( m, py, p0 );
995 m4x3_mulv( m, ly, p1 );
996 m4x3_mulv( m, px, p2 );
997 m4x3_mulv( m, lx, p3 );
998 m4x3_mulv( m, pz, p4 );
999 m4x3_mulv( m, lz, p5 );
1000
1001 vg_line( p0, p1, colour == 0x00? 0xff00ff00: colour );
1002 vg_line( p2, p3, colour == 0x00? 0xff0000ff: colour );
1003 vg_line( p4, p5, colour == 0x00? 0xffff0000: colour );
1004
1005 v3_copy( py, ly );
1006 v3_copy( px, lx );
1007 v3_copy( pz, lz );
1008 }
1009 }
1010
1011 static void rb_debug( rigidbody *rb, u32 colour )
1012 {
1013 if( rb->type == k_rb_shape_box )
1014 {
1015 v3f *box = rb->bbx;
1016 vg_line_boxf_transformed( rb->to_world, rb->bbx, colour );
1017 }
1018 else if( rb->type == k_rb_shape_sphere )
1019 {
1020 debug_sphere( rb->to_world, rb->inf.sphere.radius, colour );
1021 }
1022 }
1023
1024 /*
1025 * out penetration distance, normal
1026 */
1027 static int rb_point_in_body( rigidbody *rb, v3f pos, float *pen, v3f normal )
1028 {
1029 v3f local;
1030 m4x3_mulv( rb->to_local, pos, local );
1031
1032 if( local[0] > rb->bbx[0][0] && local[0] < rb->bbx[1][0] &&
1033 local[1] > rb->bbx[0][1] && local[1] < rb->bbx[1][1] &&
1034 local[2] > rb->bbx[0][2] && local[2] < rb->bbx[1][2] )
1035 {
1036 v3f area, com, comrel;
1037 v3_add( rb->bbx[0], rb->bbx[1], com );
1038 v3_muls( com, 0.5f, com );
1039
1040 v3_sub( rb->bbx[1], rb->bbx[0], area );
1041 v3_sub( local, com, comrel );
1042 v3_div( comrel, area, comrel );
1043
1044 int axis = 0;
1045 float max_mag = fabsf(comrel[0]);
1046
1047 if( fabsf(comrel[1]) > max_mag )
1048 {
1049 axis = 1;
1050 max_mag = fabsf(comrel[1]);
1051 }
1052 if( fabsf(comrel[2]) > max_mag )
1053 {
1054 axis = 2;
1055 max_mag = fabsf(comrel[2]);
1056 }
1057
1058 v3_zero( normal );
1059 normal[axis] = vg_signf(comrel[axis]);
1060
1061 if( normal[axis] < 0.0f )
1062 *pen = local[axis] - rb->bbx[0][axis];
1063 else
1064 *pen = rb->bbx[1][axis] - local[axis];
1065
1066 m3x3_mulv( rb->to_world, normal, normal );
1067 return 1;
1068 }
1069
1070 return 0;
1071 }
1072
1073 #if 0
1074 static void rb_build_manifold_rb_static( rigidbody *ra, rigidbody *rb_static )
1075 {
1076 v3f verts[8];
1077
1078 v3f a, b;
1079 v3_copy( ra->bbx[0], a );
1080 v3_copy( ra->bbx[1], b );
1081
1082 m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], a[2] }, verts[0] );
1083 m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], a[2] }, verts[1] );
1084 m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], a[2] }, verts[2] );
1085 m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], a[2] }, verts[3] );
1086 m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], b[2] }, verts[4] );
1087 m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], b[2] }, verts[5] );
1088 m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], b[2] }, verts[6] );
1089 m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], b[2] }, verts[7] );
1090
1091 vg_line_boxf_transformed( rb_static->to_world, rb_static->bbx, 0xff0000ff );
1092
1093 int count = 0;
1094
1095 for( int i=0; i<8; i++ )
1096 {
1097 if( ra->manifold_count == vg_list_size(ra->manifold) )
1098 return;
1099
1100 struct contact *ct = &ra->manifold[ ra->manifold_count ];
1101
1102 float p;
1103 v3f normal;
1104
1105 if( rb_point_in_body( rb_static, verts[i], &p, normal ))
1106 {
1107 v3_copy( normal, ct->n );
1108 v3_muladds( verts[i], ct->n, p*0.5f, ct->co );
1109 v3_sub( ct->co, ra->co, ct->delta );
1110
1111 vg_line_pt3( ct->co, 0.0125f, 0xffff00ff );
1112
1113 ct->bias = -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -p+0.04f );
1114 rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
1115
1116 ct->norm_impulse = 0.0f;
1117 ct->tangent_impulse[0] = 0.0f;
1118 ct->tangent_impulse[1] = 0.0f;
1119
1120 ra->manifold_count ++;
1121 count ++;
1122 if( count == 4 )
1123 return;
1124 }
1125 }
1126 }
1127 #endif
1128
1129 /*
1130 * Capsule phyics
1131 */
1132
1133 static void debug_capsule( m4x3f m, float height, float radius, u32 colour )
1134 {
1135 v3f last = { 0.0f, 0.0f, radius };
1136 m4x3f lower, upper;
1137 m3x3_copy( m, lower );
1138 m3x3_copy( m, upper );
1139 m4x3_mulv( m, (v3f){0.0f,-height*0.5f+radius,0.0f}, lower[3] );
1140 m4x3_mulv( m, (v3f){0.0f, height*0.5f-radius,0.0f}, upper[3] );
1141
1142 for( int i=0; i<16; i++ )
1143 {
1144 float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
1145 s = sinf(t),
1146 c = cosf(t);
1147
1148 v3f p = { s*radius, 0.0f, c*radius };
1149
1150 v3f p0, p1;
1151 m4x3_mulv( lower, p, p0 );
1152 m4x3_mulv( lower, last, p1 );
1153 vg_line( p0, p1, colour );
1154
1155 m4x3_mulv( upper, p, p0 );
1156 m4x3_mulv( upper, last, p1 );
1157 vg_line( p0, p1, colour );
1158
1159 v3_copy( p, last );
1160 }
1161
1162 for( int i=0; i<4; i++ )
1163 {
1164 float t = ((float)(i) * (1.0f/4.0f)) * VG_PIf * 2.0f,
1165 s = sinf(t),
1166 c = cosf(t);
1167
1168 v3f p = { s*radius, 0.0f, c*radius };
1169
1170 v3f p0, p1;
1171 m4x3_mulv( lower, p, p0 );
1172 m4x3_mulv( upper, p, p1 );
1173 vg_line( p0, p1, colour );
1174
1175 m4x3_mulv( lower, (v3f){0.0f,-radius,0.0f}, p0 );
1176 m4x3_mulv( upper, (v3f){0.0f, radius,0.0f}, p1 );
1177 vg_line( p0, p1, colour );
1178 }
1179 }
1180
1181 /*
1182 * BVH implementation, this is ONLY for static rigidbodies, its to slow for
1183 * realtime use.
1184 */
1185
1186 static void rb_bh_expand_bound( void *user, boxf bound, u32 item_index )
1187 {
1188 rigidbody *rb = &((rigidbody *)user)[ item_index ];
1189 box_concat( bound, rb->bbx_world );
1190 }
1191
1192 static float rb_bh_centroid( void *user, u32 item_index, int axis )
1193 {
1194 rigidbody *rb = &((rigidbody *)user)[ item_index ];
1195 return (rb->bbx_world[axis][0] + rb->bbx_world[1][axis]) * 0.5f;
1196 }
1197
1198 static void rb_bh_swap( void *user, u32 ia, u32 ib )
1199 {
1200 rigidbody temp, *rba, *rbb;
1201 rba = &((rigidbody *)user)[ ia ];
1202 rbb = &((rigidbody *)user)[ ib ];
1203
1204 temp = *rba;
1205 *rba = *rbb;
1206 *rbb = temp;
1207 }
1208
1209 static void rb_bh_debug( void *user, u32 item_index )
1210 {
1211 rigidbody *rb = &((rigidbody *)user)[ item_index ];
1212 rb_debug( rb, 0xff00ffff );
1213 }
1214
1215 static bh_system bh_system_rigidbodies =
1216 {
1217 .expand_bound = rb_bh_expand_bound,
1218 .item_centroid = rb_bh_centroid,
1219 .item_swap = rb_bh_swap,
1220 .item_debug = rb_bh_debug,
1221 .cast_ray = NULL
1222 };
1223
1224 #endif /* RIGIDBODY_H */