stuff
[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_delta (1.0f/60.0f)
17
18 typedef struct rigidbody rigidbody;
19 struct rigidbody
20 {
21 v3f co, v, I;
22 v4f q;
23 boxf bbx, bbx_world;
24 float inv_mass;
25
26 struct contact
27 {
28 v3f co, n, delta;
29 v3f t[2];
30 float bias, norm_impulse, tangent_impulse[2];
31 }
32 manifold[12];
33 int manifold_count;
34
35 v3f delta; /* where is the origin of this in relation to a parent body */
36 m4x3f to_world, to_local;
37 };
38
39 static void rb_update_transform( rigidbody *rb )
40 {
41 q_normalize( rb->q );
42 q_m3x3( rb->q, rb->to_world );
43 v3_copy( rb->co, rb->to_world[3] );
44
45 m4x3_invert_affine( rb->to_world, rb->to_local );
46
47 box_copy( rb->bbx, rb->bbx_world );
48 m4x3_transform_aabb( rb->to_world, rb->bbx_world );
49 }
50
51 static void rb_init( rigidbody *rb )
52 {
53 q_identity( rb->q );
54 v3_zero( rb->v );
55 v3_zero( rb->I );
56
57 v3f dims;
58 v3_sub( rb->bbx[1], rb->bbx[0], dims );
59
60 rb->inv_mass = 1.0f/(8.0f*dims[0]*dims[1]*dims[2]);
61
62 rb_update_transform( rb );
63 }
64
65 static void rb_iter( rigidbody *rb )
66 {
67 v3f gravity = { 0.0f, -9.6f, 0.0f };
68 v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
69
70 /* intergrate velocity */
71 v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
72
73 v3_lerp( rb->I, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->I );
74
75 /* inegrate inertia */
76 if( v3_length2( rb->I ) > 0.0f )
77 {
78 v4f rotation;
79 v3f axis;
80 v3_copy( rb->I, axis );
81
82 float mag = v3_length( axis );
83 v3_divs( axis, mag, axis );
84 q_axis_angle( rotation, axis, mag*k_rb_delta );
85 q_mul( rotation, rb->q, rb->q );
86 }
87 }
88
89 static void rb_torque( rigidbody *rb, v3f axis, float mag )
90 {
91 v3_muladds( rb->I, axis, mag*k_rb_delta, rb->I );
92 }
93
94 static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
95 {
96 /* Compute tangent basis (box2d) */
97 if( fabsf( n[0] ) >= 0.57735027f )
98 {
99 tx[0] = n[1];
100 tx[1] = -n[0];
101 tx[2] = 0.0f;
102 }
103 else
104 {
105 tx[0] = 0.0f;
106 tx[1] = n[2];
107 tx[2] = -n[1];
108 }
109
110 v3_normalize( tx );
111 v3_cross( n, tx, ty );
112 }
113
114 #include "world.h"
115
116 static void rb_manifold_reset( rigidbody *rb )
117 {
118 rb->manifold_count = 0;
119 }
120
121 static void rb_build_manifold_terrain( rigidbody *rb )
122 {
123 v3f *box = rb->bbx;
124 v3f pts[8];
125 float *p000 = pts[0], *p001 = pts[1], *p010 = pts[2], *p011 = pts[3],
126 *p100 = pts[4], *p101 = pts[5], *p110 = pts[6], *p111 = pts[7];
127
128 p000[0]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2];
129 p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2];
130 p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2];
131 p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2];
132
133 p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2];
134 p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2];
135 p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2];
136 p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2];
137
138 m4x3_mulv( rb->to_world, p000, p000 );
139 m4x3_mulv( rb->to_world, p001, p001 );
140 m4x3_mulv( rb->to_world, p010, p010 );
141 m4x3_mulv( rb->to_world, p011, p011 );
142 m4x3_mulv( rb->to_world, p100, p100 );
143 m4x3_mulv( rb->to_world, p101, p101 );
144 m4x3_mulv( rb->to_world, p110, p110 );
145 m4x3_mulv( rb->to_world, p111, p111 );
146
147 int count = 0;
148
149 for( int i=0; i<8; i++ )
150 {
151 float *point = pts[i];
152 struct contact *ct = &rb->manifold[rb->manifold_count];
153
154 v3f surface;
155 v3_copy( point, surface );
156 surface[1] += 4.0f;
157
158 ray_hit hit;
159 hit.dist = INFINITY;
160 if( !ray_world( surface, (v3f){0.0f,-1.0f,0.0f}, &hit ))
161 continue;
162
163 v3_copy( hit.normal, ct->n );
164 v3_copy( hit.pos, surface );
165
166 float p = vg_minf( surface[1] - point[1], 1.0f );
167
168 if( p > 0.0f )
169 {
170 v3_add( point, surface, ct->co );
171 v3_muls( ct->co, 0.5f, ct->co );
172
173 //vg_line_pt3( ct->co, 0.0125f, 0xff0000ff );
174
175 v3_sub( ct->co, rb->co, ct->delta );
176 ct->bias = -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -p+0.04f );
177 rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
178
179 ct->norm_impulse = 0.0f;
180 ct->tangent_impulse[0] = 0.0f;
181 ct->tangent_impulse[1] = 0.0f;
182
183 rb->manifold_count ++;
184 count ++;
185 if( count == 4 )
186 break;
187 }
188 }
189 }
190
191 static void rb_constraint_manifold( rigidbody *rb )
192 {
193 float k_friction = 0.1f;
194
195 /* Friction Impulse */
196 for( int i=0; i<rb->manifold_count; i++ )
197 {
198 struct contact *ct = &rb->manifold[i];
199
200 v3f dv;
201 v3_cross( rb->I, ct->delta, dv );
202 v3_add( rb->v, dv, dv );
203
204 for( int j=0; j<2; j++ )
205 {
206 float vt = vg_clampf( -v3_dot( dv, ct->t[j] ),
207 -k_friction, k_friction );
208
209 vt = -v3_dot( dv, ct->t[j] );
210
211 float temp = ct->tangent_impulse[j];
212 ct->tangent_impulse[j] = vg_clampf( temp+vt, -k_friction, k_friction );
213 vt = ct->tangent_impulse[j] - temp;
214
215 v3f impulse;
216
217 v3_muls( ct->t[j], vt, impulse );
218 v3_add( impulse, rb->v, rb->v );
219 v3_cross( ct->delta, impulse, impulse );
220 v3_add( impulse, rb->I, rb->I );
221 }
222 }
223
224 /* Normal Impulse */
225 for( int i=0; i<rb->manifold_count; i++ )
226 {
227 struct contact *ct = &rb->manifold[i];
228
229 v3f dv;
230 v3_cross( rb->I, ct->delta, dv );
231 v3_add( rb->v, dv, dv );
232
233 float vn = -v3_dot( dv, ct->n );
234 vn += ct->bias;
235
236 float temp = ct->norm_impulse;
237 ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
238 vn = ct->norm_impulse - temp;
239
240 v3f impulse;
241
242 v3_muls( ct->n, vn, impulse );
243 v3_add( impulse, rb->v, rb->v );
244 v3_cross( ct->delta, impulse, impulse );
245 v3_add( impulse, rb->I, rb->I );
246 }
247 }
248
249 struct rb_angle_limit
250 {
251 rigidbody *rba, *rbb;
252 v3f axis;
253 float impulse, bias;
254 };
255
256 static int rb_angle_limit_force( rigidbody *rba, v3f va,
257 rigidbody *rbb, v3f vb,
258 float max )
259 {
260 v3f wva, wvb;
261 m3x3_mulv( rba->to_world, va, wva );
262 m3x3_mulv( rbb->to_world, vb, wvb );
263
264 float dt = v3_dot(wva,wvb)*0.999f,
265 ang = fabsf(dt);
266 ang = acosf( dt );
267 if( ang > max )
268 {
269 float correction = max-ang;
270
271 v3f axis;
272 v3_cross( wva, wvb, axis );
273
274 v4f rotation;
275 q_axis_angle( rotation, axis, -correction*0.25f );
276 q_mul( rotation, rba->q, rba->q );
277
278 q_axis_angle( rotation, axis, correction*0.25f );
279 q_mul( rotation, rbb->q, rbb->q );
280
281 return 1;
282 }
283
284 return 0;
285 }
286
287 static void rb_constraint_angle_limit( struct rb_angle_limit *limit )
288 {
289
290 }
291
292 RB_DEPR
293 static void rb_constraint_angle( rigidbody *rba, v3f va,
294 rigidbody *rbb, v3f vb,
295 float max, float spring )
296 {
297 v3f wva, wvb;
298 m3x3_mulv( rba->to_world, va, wva );
299 m3x3_mulv( rbb->to_world, vb, wvb );
300
301 float dt = v3_dot(wva,wvb)*0.999f,
302 ang = fabsf(dt);
303
304 v3f axis;
305 v3_cross( wva, wvb, axis );
306 v3_muladds( rba->I, axis, ang*spring*0.5f, rba->I );
307 v3_muladds( rbb->I, axis, -ang*spring*0.5f, rbb->I );
308
309 return;
310
311 /* TODO: convert max into the dot product value so we dont have to always
312 * evaluate acosf, only if its greater than the angle specified */
313 ang = acosf( dt );
314 if( ang > max )
315 {
316 float correction = max-ang;
317
318 v4f rotation;
319 q_axis_angle( rotation, axis, -correction*0.125f );
320 q_mul( rotation, rba->q, rba->q );
321
322 q_axis_angle( rotation, axis, correction*0.125f );
323 q_mul( rotation, rbb->q, rbb->q );
324 }
325 }
326
327 static void rb_relative_velocity( rigidbody *ra, v3f lca,
328 rigidbody *rb, v3f lcb, v3f rcv )
329 {
330 v3f wca, wcb;
331 m3x3_mulv( ra->to_world, lca, wca );
332 m3x3_mulv( rb->to_world, lcb, wcb );
333
334 v3_sub( ra->v, rb->v, rcv );
335
336 v3f rcv_Ra, rcv_Rb;
337 v3_cross( ra->I, wca, rcv_Ra );
338 v3_cross( rb->I, wcb, rcv_Rb );
339 v3_add( rcv_Ra, rcv, rcv );
340 v3_sub( rcv, rcv_Rb, rcv );
341 }
342
343 static void rb_constraint_position( rigidbody *ra, v3f lca,
344 rigidbody *rb, v3f lcb )
345 {
346 /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
347 v3f wca, wcb;
348 m3x3_mulv( ra->to_world, lca, wca );
349 m3x3_mulv( rb->to_world, lcb, wcb );
350
351 v3f delta;
352 v3_add( wcb, rb->co, delta );
353 v3_sub( delta, wca, delta );
354 v3_sub( delta, ra->co, delta );
355
356 v3_muladds( ra->co, delta, 0.5f, ra->co );
357 v3_muladds( rb->co, delta, -0.5f, rb->co );
358
359 v3f rcv;
360 v3_sub( ra->v, rb->v, rcv );
361
362 v3f rcv_Ra, rcv_Rb;
363 v3_cross( ra->I, wca, rcv_Ra );
364 v3_cross( rb->I, wcb, rcv_Rb );
365 v3_add( rcv_Ra, rcv, rcv );
366 v3_sub( rcv, rcv_Rb, rcv );
367
368 float nm = 0.5f/(rb->inv_mass + ra->inv_mass);
369
370 float mass_a = 1.0f/ra->inv_mass,
371 mass_b = 1.0f/rb->inv_mass,
372 total_mass = mass_a+mass_b;
373
374 v3f impulse;
375 v3_muls( rcv, 1.0f, impulse );
376 v3_muladds( rb->v, impulse, mass_b/total_mass, rb->v );
377 v3_cross( wcb, impulse, impulse );
378 v3_add( impulse, rb->I, rb->I );
379
380 v3_muls( rcv, -1.0f, impulse );
381 v3_muladds( ra->v, impulse, mass_a/total_mass, ra->v );
382 v3_cross( wca, impulse, impulse );
383 v3_add( impulse, ra->I, ra->I );
384
385 #if 0
386 /*
387 * this could be used for spring joints
388 * its not good for position constraint
389 */
390 v3f impulse;
391 v3_muls( delta, 0.5f*spring, impulse );
392
393 v3_add( impulse, ra->v, ra->v );
394 v3_cross( wca, impulse, impulse );
395 v3_add( impulse, ra->I, ra->I );
396
397 v3_muls( delta, -0.5f*spring, impulse );
398
399 v3_add( impulse, rb->v, rb->v );
400 v3_cross( wcb, impulse, impulse );
401 v3_add( impulse, rb->I, rb->I );
402 #endif
403 }
404
405 static void rb_debug( rigidbody *rb, u32 colour )
406 {
407 v3f *box = rb->bbx;
408 v3f p000, p001, p010, p011, p100, p101, p110, p111;
409
410 p000[0]=box[0][0];p000[1]=box[0][1];p000[2]=box[0][2];
411 p001[0]=box[0][0];p001[1]=box[0][1];p001[2]=box[1][2];
412 p010[0]=box[0][0];p010[1]=box[1][1];p010[2]=box[0][2];
413 p011[0]=box[0][0];p011[1]=box[1][1];p011[2]=box[1][2];
414
415 p100[0]=box[1][0];p100[1]=box[0][1];p100[2]=box[0][2];
416 p101[0]=box[1][0];p101[1]=box[0][1];p101[2]=box[1][2];
417 p110[0]=box[1][0];p110[1]=box[1][1];p110[2]=box[0][2];
418 p111[0]=box[1][0];p111[1]=box[1][1];p111[2]=box[1][2];
419
420 m4x3_mulv( rb->to_world, p000, p000 );
421 m4x3_mulv( rb->to_world, p001, p001 );
422 m4x3_mulv( rb->to_world, p010, p010 );
423 m4x3_mulv( rb->to_world, p011, p011 );
424 m4x3_mulv( rb->to_world, p100, p100 );
425 m4x3_mulv( rb->to_world, p101, p101 );
426 m4x3_mulv( rb->to_world, p110, p110 );
427 m4x3_mulv( rb->to_world, p111, p111 );
428
429 vg_line( p000, p001, colour );
430 vg_line( p001, p011, colour );
431 vg_line( p011, p010, colour );
432 vg_line( p010, p000, colour );
433
434 vg_line( p100, p101, colour );
435 vg_line( p101, p111, colour );
436 vg_line( p111, p110, colour );
437 vg_line( p110, p100, colour );
438
439 vg_line( p100, p000, colour );
440 vg_line( p101, p001, colour );
441 vg_line( p110, p010, colour );
442 vg_line( p111, p011, colour );
443
444 vg_line( p000, p110, colour );
445 vg_line( p100, p010, colour );
446 }
447
448 /*
449 * out penetration distance, normal
450 */
451 static int rb_point_in_body( rigidbody *rb, v3f pos, float *pen, v3f normal )
452 {
453 v3f local;
454 m4x3_mulv( rb->to_local, pos, local );
455
456 if( local[0] > rb->bbx[0][0] && local[0] < rb->bbx[1][0] &&
457 local[1] > rb->bbx[0][1] && local[1] < rb->bbx[1][1] &&
458 local[2] > rb->bbx[0][2] && local[2] < rb->bbx[1][2] )
459 {
460 v3f area, com, comrel;
461 v3_add( rb->bbx[0], rb->bbx[1], com );
462 v3_muls( com, 0.5f, com );
463
464 v3_sub( rb->bbx[1], rb->bbx[0], area );
465 v3_sub( local, com, comrel );
466 v3_div( comrel, area, comrel );
467
468 int axis = 0;
469 float max_mag = fabsf(comrel[0]);
470
471 if( fabsf(comrel[1]) > max_mag )
472 {
473 axis = 1;
474 max_mag = fabsf(comrel[1]);
475 }
476 if( fabsf(comrel[2]) > max_mag )
477 {
478 axis = 2;
479 max_mag = fabsf(comrel[2]);
480 }
481
482 v3_zero( normal );
483 normal[axis] = vg_signf(comrel[axis]);
484
485 if( normal[axis] < 0.0f )
486 *pen = local[axis] - rb->bbx[0][axis];
487 else
488 *pen = rb->bbx[1][axis] - local[axis];
489
490 m3x3_mulv( rb->to_world, normal, normal );
491 return 1;
492 }
493
494 return 0;
495 }
496
497 static void rb_build_manifold_rb_static( rigidbody *ra, rigidbody *rb_static )
498 {
499 v3f verts[8];
500
501 v3f a, b;
502 v3_copy( ra->bbx[0], a );
503 v3_copy( ra->bbx[1], b );
504
505 m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], a[2] }, verts[0] );
506 m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], a[2] }, verts[1] );
507 m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], a[2] }, verts[2] );
508 m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], a[2] }, verts[3] );
509 m4x3_mulv( ra->to_world, (v3f){ a[0], a[1], b[2] }, verts[4] );
510 m4x3_mulv( ra->to_world, (v3f){ a[0], b[1], b[2] }, verts[5] );
511 m4x3_mulv( ra->to_world, (v3f){ b[0], b[1], b[2] }, verts[6] );
512 m4x3_mulv( ra->to_world, (v3f){ b[0], a[1], b[2] }, verts[7] );
513
514 int count = 0;
515
516 for( int i=0; i<8; i++ )
517 {
518 if( ra->manifold_count == vg_list_size(ra->manifold) )
519 return;
520
521 struct contact *ct = &ra->manifold[ ra->manifold_count ];
522
523 float p;
524 v3f normal;
525
526 if( rb_point_in_body( rb_static, verts[i], &p, normal ))
527 {
528 v3_copy( normal, ct->n );
529 v3_muladds( verts[i], ct->n, p*0.5f, ct->co );
530 v3_sub( ct->co, ra->co, ct->delta );
531
532 vg_line_pt3( ct->co, 0.0125f, 0xffff00ff );
533
534 ct->bias = -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -p+0.04f );
535 rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
536
537 ct->norm_impulse = 0.0f;
538 ct->tangent_impulse[0] = 0.0f;
539 ct->tangent_impulse[1] = 0.0f;
540
541 ra->manifold_count ++;
542 count ++;
543 if( count == 4 )
544 return;
545 }
546 }
547 }
548
549 /*
550 * BVH implementation, this is ONLY for static rigidbodies, its to slow for
551 * realtime use.
552 */
553
554 static void rb_bh_expand_bound( void *user, boxf bound, u32 item_index )
555 {
556 rigidbody *rb = &((rigidbody *)user)[ item_index ];
557 box_concat( bound, rb->bbx_world );
558 }
559
560 static float rb_bh_centroid( void *user, u32 item_index, int axis )
561 {
562 rigidbody *rb = &((rigidbody *)user)[ item_index ];
563 return (rb->bbx_world[axis][0] + rb->bbx_world[1][axis]) * 0.5f;
564 }
565
566 static void rb_bh_swap( void *user, u32 ia, u32 ib )
567 {
568 rigidbody temp, *rba, *rbb;
569 rba = &((rigidbody *)user)[ ia ];
570 rbb = &((rigidbody *)user)[ ib ];
571
572 temp = *rba;
573 *rba = *rbb;
574 *rbb = temp;
575 }
576
577 static void rb_bh_debug( void *user, u32 item_index )
578 {
579 rigidbody *rb = &((rigidbody *)user)[ item_index ];
580 rb_debug( rb, 0xff00ffff );
581 }
582
583 static bh_system bh_system_rigidbodies =
584 {
585 .expand_bound = rb_bh_expand_bound,
586 .item_centroid = rb_bh_centroid,
587 .item_swap = rb_bh_swap,
588 .item_debug = rb_bh_debug,
589 .cast_ray = NULL
590 };
591
592 #endif /* RIGIDBODY_H */